Skip to content

Commit

Permalink
Add the ability to change KDL parameters from kinematics configuration (
Browse files Browse the repository at this point in the history 
#843)

Co-authored-by: Levi Armstrong <[email protected]>
  • Loading branch information
@seanmc602 @Levi-Armstrong
seanmc602 and Levi-Armstrong authored Jun 2, 2024
1 parent f59b78c commit 3124cd9
Show file tree
Hide file tree
Showing 7 changed files with 135 additions and 15 deletions.
20 changes: 20 additions & 0 deletions tesseract_kinematics/kdl/include/tesseract_kinematics/kdl/kdl_inv_kin_chain_lma.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -29,6 +29,7 @@
TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
#include <kdl/chainiksolverpos_lma.hpp>
#include <unordered_map>
#include <array>
#include <mutex>
TESSERACT_COMMON_IGNORE_WARNINGS_POP

Expand All @@ -54,6 +55,22 @@ class KDLInvKinChainLMA : public InverseKinematics
using UPtr = std::unique_ptr<KDLInvKinChainLMA>;
using ConstUPtr = std::unique_ptr<const KDLInvKinChainLMA>;

/**
* @brief The Config struct
*
* This contains parameters that can be used to customize the KDL solver for your application.
* They are ultimately passed to the constuctor of the undelying ChainIkSolver.
*
* The defaults provided here are the same defaults imposed by the KDL library.
*/
struct Config
{
std::array<double, 6> task_weights{ 1.0, 1.0, 1.0, 0.1, 0.1, 0.1 };
double eps{ 1E-5 };
int max_iterations{ 500 };
double eps_joints{ 1E-15 };
};

~KDLInvKinChainLMA() override = default;
KDLInvKinChainLMA(const KDLInvKinChainLMA& other);
KDLInvKinChainLMA& operator=(const KDLInvKinChainLMA& other);
Expand All @@ -71,6 +88,7 @@ class KDLInvKinChainLMA : public InverseKinematics
KDLInvKinChainLMA(const tesseract_scene_graph::SceneGraph& scene_graph,
const std::string& base_link,
const std::string& tip_link,
Config kdl_config,
std::string solver_name = KDL_INV_KIN_CHAIN_LMA_SOLVER_NAME);

/**
Expand All @@ -82,6 +100,7 @@ class KDLInvKinChainLMA : public InverseKinematics
*/
KDLInvKinChainLMA(const tesseract_scene_graph::SceneGraph& scene_graph,
const std::vector<std::pair<std::string, std::string> >& chains,
Config kdl_config,
std::string solver_name = KDL_INV_KIN_CHAIN_LMA_SOLVER_NAME);

IKSolutions calcInvKin(const tesseract_common::TransformMap& tip_link_poses,
Expand All @@ -97,6 +116,7 @@ class KDLInvKinChainLMA : public InverseKinematics

private:
KDLChainData kdl_data_; /**< @brief KDL data parsed from Scene Graph */
Config kdl_config_; /**< @brief KDL configuration data parsed from YAML */
std::unique_ptr<KDL::ChainIkSolverPos_LMA> ik_solver_; /**< @brief KDL Inverse kinematic solver */
std::string solver_name_{ KDL_INV_KIN_CHAIN_LMA_SOLVER_NAME }; /**< @brief Name of this solver */
mutable std::mutex mutex_; /**< @brief KDL is not thread safe due to mutable variables in Joint Class */
Expand Down
20 changes: 20 additions & 0 deletions tesseract_kinematics/kdl/include/tesseract_kinematics/kdl/kdl_inv_kin_chain_nr.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -56,6 +56,23 @@ class KDLInvKinChainNR : public InverseKinematics
using UPtr = std::unique_ptr<KDLInvKinChainNR>;
using ConstUPtr = std::unique_ptr<const KDLInvKinChainNR>;

/**
* @brief The Config struct
*
* This contains parameters that can be used to customize the KDL solver for your application.
* They are ultimately passed to the constuctors of the undelying ChainIkSolver.
* The NR version creates both position and velocity solvers with different defaults for each.
*
* The defaults provided here are the same defaults imposed by the KDL library.
*/
struct Config
{
double vel_eps{ 0.00001 };
int vel_iterations{ 150 };
double pos_eps{ 1e-6 };
int pos_iterations{ 100 };
};

~KDLInvKinChainNR() override = default;
KDLInvKinChainNR(const KDLInvKinChainNR& other);
KDLInvKinChainNR& operator=(const KDLInvKinChainNR& other);
Expand All @@ -73,6 +90,7 @@ class KDLInvKinChainNR : public InverseKinematics
KDLInvKinChainNR(const tesseract_scene_graph::SceneGraph& scene_graph,
const std::string& base_link,
const std::string& tip_link,
Config kdl_config,
std::string solver_name = KDL_INV_KIN_CHAIN_NR_SOLVER_NAME);

/**
Expand All @@ -84,6 +102,7 @@ class KDLInvKinChainNR : public InverseKinematics
*/
KDLInvKinChainNR(const tesseract_scene_graph::SceneGraph& scene_graph,
const std::vector<std::pair<std::string, std::string> >& chains,
Config kdl_config,
std::string solver_name = KDL_INV_KIN_CHAIN_NR_SOLVER_NAME);

IKSolutions calcInvKin(const tesseract_common::TransformMap& tip_link_poses,
Expand All @@ -99,6 +118,7 @@ class KDLInvKinChainNR : public InverseKinematics

private:
KDLChainData kdl_data_; /**< @brief KDL data parsed from Scene Graph */
Config kdl_config_; /**< @brief KDL configuration data parsed from YAML */
std::unique_ptr<KDL::ChainFkSolverPos_recursive> fk_solver_; /**< @brief KDL Forward Kinematic Solver */
std::unique_ptr<KDL::ChainIkSolverVel_pinv> ik_vel_solver_; /**< @brief KDL Inverse kinematic velocity solver */
std::unique_ptr<KDL::ChainIkSolverPos_NR> ik_solver_; /**< @brief KDL Inverse kinematic solver */
Expand Down
38 changes: 36 additions & 2 deletions tesseract_kinematics/kdl/src/kdl_factories.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -75,6 +75,7 @@ KDLInvKinChainLMAFactory::create(const std::string& solver_name,
{
std::string base_link;
std::string tip_link;
KDLInvKinChainLMA::Config kdl_config;

try
{
Expand All @@ -87,14 +88,33 @@ KDLInvKinChainLMAFactory::create(const std::string& solver_name,
tip_link = n.as<std::string>();
else
throw std::runtime_error("KDLInvKinChainLMAFactory, missing 'tip_link' entry");

// Optional configuration parameters
if (YAML::Node n = config["task_weights"])
{
// Make sure the length matches the constructor interface
if (n.size() != 6)
throw std::runtime_error("KDLInvKinChainLMAFactory, size of task_weights needs to be 6");

kdl_config.task_weights = n.as<std::array<double, 6>>();
}

if (YAML::Node n = config["eps"])
kdl_config.eps = n.as<double>();

if (YAML::Node n = config["max_iterations"])
kdl_config.max_iterations = n.as<int>();

if (YAML::Node n = config["eps_joints"])
kdl_config.eps_joints = n.as<double>();
}
catch (const std::exception& e)
{
CONSOLE_BRIDGE_logError("KDLInvKinChainLMAFactory: Failed to parse yaml config data! Details: %s", e.what());
return nullptr;
}

return std::make_unique<KDLInvKinChainLMA>(scene_graph, base_link, tip_link, solver_name);
return std::make_unique<KDLInvKinChainLMA>(scene_graph, base_link, tip_link, kdl_config, solver_name);
}

std::unique_ptr<InverseKinematics>
Expand All @@ -106,6 +126,7 @@ KDLInvKinChainNRFactory::create(const std::string& solver_name,
{
std::string base_link;
std::string tip_link;
KDLInvKinChainNR::Config kdl_config;

try
{
Expand All @@ -118,14 +139,27 @@ KDLInvKinChainNRFactory::create(const std::string& solver_name,
tip_link = n.as<std::string>();
else
throw std::runtime_error("KDLInvKinChainNRFactory, missing 'tip_link' entry");

// Optional configuration parameters
if (YAML::Node n = config["velocity_eps"])
kdl_config.vel_eps = n.as<double>();

if (YAML::Node n = config["velocity_iterations"])
kdl_config.vel_iterations = n.as<int>();

if (YAML::Node n = config["position_eps"])
kdl_config.pos_eps = n.as<double>();

if (YAML::Node n = config["position_iterations"])
kdl_config.pos_iterations = n.as<int>();
}
catch (const std::exception& e)
{
CONSOLE_BRIDGE_logError("KDLInvKinChainNRFactory: Failed to parse yaml config data! Details: %s", e.what());
return nullptr;
}

return std::make_unique<KDLInvKinChainNR>(scene_graph, base_link, tip_link, solver_name);
return std::make_unique<KDLInvKinChainNR>(scene_graph, base_link, tip_link, kdl_config, solver_name);
}

std::unique_ptr<InverseKinematics>
Expand Down
24 changes: 20 additions & 4 deletions tesseract_kinematics/kdl/src/kdl_inv_kin_chain_lma.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -40,8 +40,9 @@ using Eigen::VectorXd;

KDLInvKinChainLMA::KDLInvKinChainLMA(const tesseract_scene_graph::SceneGraph& scene_graph,
const std::vector<std::pair<std::string, std::string>>& chains,
Config kdl_config,
std::string solver_name)
: solver_name_(std::move(solver_name))
: kdl_config_(std::move(kdl_config)), solver_name_(std::move(solver_name))
{
if (!scene_graph.getLink(scene_graph.getRoot()))
throw std::runtime_error("The scene graph has an invalid root.");
Expand All @@ -50,14 +51,23 @@ KDLInvKinChainLMA::KDLInvKinChainLMA(const tesseract_scene_graph::SceneGraph& sc
throw std::runtime_error("Failed to parse KDL data from Scene Graph");

// Create KDL IK Solver
ik_solver_ = std::make_unique<KDL::ChainIkSolverPos_LMA>(kdl_data_.robot_chain);
ik_solver_ =
std::make_unique<KDL::ChainIkSolverPos_LMA>(kdl_data_.robot_chain,
Eigen::Matrix<double, 6, 1>{ kdl_config_.task_weights.data() },
kdl_config_.eps,
kdl_config_.max_iterations,
kdl_config_.eps_joints);
}

KDLInvKinChainLMA::KDLInvKinChainLMA(const tesseract_scene_graph::SceneGraph& scene_graph,
const std::string& base_link,
const std::string& tip_link,
Config kdl_config,
std::string solver_name)
: KDLInvKinChainLMA(scene_graph, { std::make_pair(base_link, tip_link) }, std::move(solver_name))
: KDLInvKinChainLMA(scene_graph,
{ std::make_pair(base_link, tip_link) },
std::move(kdl_config),
std::move(solver_name))
{
}

Expand All @@ -68,7 +78,13 @@ KDLInvKinChainLMA::KDLInvKinChainLMA(const KDLInvKinChainLMA& other) { *this = o
KDLInvKinChainLMA& KDLInvKinChainLMA::operator=(const KDLInvKinChainLMA& other)
{
kdl_data_ = other.kdl_data_;
ik_solver_ = std::make_unique<KDL::ChainIkSolverPos_LMA>(kdl_data_.robot_chain);
kdl_config_ = other.kdl_config_;
ik_solver_ =
std::make_unique<KDL::ChainIkSolverPos_LMA>(kdl_data_.robot_chain,
Eigen::Matrix<double, 6, 1>{ kdl_config_.task_weights.data() },
kdl_config_.eps,
kdl_config_.max_iterations,
kdl_config_.eps_joints);
solver_name_ = other.solver_name_;

return *this;
Expand Down
22 changes: 16 additions & 6 deletions tesseract_kinematics/kdl/src/kdl_inv_kin_chain_nr.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -40,8 +40,9 @@ using Eigen::VectorXd;

KDLInvKinChainNR::KDLInvKinChainNR(const tesseract_scene_graph::SceneGraph& scene_graph,
const std::vector<std::pair<std::string, std::string>>& chains,
Config kdl_config,
std::string solver_name)
: solver_name_(std::move(solver_name))
: kdl_config_(std::move(kdl_config)), solver_name_(std::move(solver_name))
{
if (!scene_graph.getLink(scene_graph.getRoot()))
throw std::runtime_error("The scene graph has an invalid root.");
Expand All @@ -51,15 +52,21 @@ KDLInvKinChainNR::KDLInvKinChainNR(const tesseract_scene_graph::SceneGraph& scen

// Create KDL FK and IK Solver
fk_solver_ = std::make_unique<KDL::ChainFkSolverPos_recursive>(kdl_data_.robot_chain);
ik_vel_solver_ = std::make_unique<KDL::ChainIkSolverVel_pinv>(kdl_data_.robot_chain);
ik_solver_ = std::make_unique<KDL::ChainIkSolverPos_NR>(kdl_data_.robot_chain, *fk_solver_, *ik_vel_solver_);
ik_vel_solver_ = std::make_unique<KDL::ChainIkSolverVel_pinv>(
kdl_data_.robot_chain, kdl_config_.vel_eps, kdl_config_.vel_iterations);
ik_solver_ = std::make_unique<KDL::ChainIkSolverPos_NR>(
kdl_data_.robot_chain, *fk_solver_, *ik_vel_solver_, kdl_config_.pos_iterations, kdl_config_.pos_eps);
}

KDLInvKinChainNR::KDLInvKinChainNR(const tesseract_scene_graph::SceneGraph& scene_graph,
const std::string& base_link,
const std::string& tip_link,
Config kdl_config,
std::string solver_name)
: KDLInvKinChainNR(scene_graph, { std::make_pair(base_link, tip_link) }, std::move(solver_name))
: KDLInvKinChainNR(scene_graph,
{ std::make_pair(base_link, tip_link) },
std::move(kdl_config),
std::move(solver_name))
{
}

Expand All @@ -70,9 +77,12 @@ KDLInvKinChainNR::KDLInvKinChainNR(const KDLInvKinChainNR& other) { *this = othe
KDLInvKinChainNR& KDLInvKinChainNR::operator=(const KDLInvKinChainNR& other)
{
kdl_data_ = other.kdl_data_;
kdl_config_ = other.kdl_config_;
fk_solver_ = std::make_unique<KDL::ChainFkSolverPos_recursive>(kdl_data_.robot_chain);
ik_vel_solver_ = std::make_unique<KDL::ChainIkSolverVel_pinv>(kdl_data_.robot_chain);
ik_solver_ = std::make_unique<KDL::ChainIkSolverPos_NR>(kdl_data_.robot_chain, *fk_solver_, *ik_vel_solver_);
ik_vel_solver_ = std::make_unique<KDL::ChainIkSolverVel_pinv>(
kdl_data_.robot_chain, kdl_config_.vel_eps, kdl_config_.vel_iterations);
ik_solver_ = std::make_unique<KDL::ChainIkSolverPos_NR>(
kdl_data_.robot_chain, *fk_solver_, *ik_vel_solver_, kdl_config_.pos_iterations, kdl_config_.pos_eps);
solver_name_ = other.solver_name_;

return *this;
Expand Down
6 changes: 4 additions & 2 deletions tesseract_kinematics/test/kdl_kinematics_unit.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -17,7 +17,8 @@ TEST(TesseractKinematicsUnit, KDLKinChainLMAInverseKinematicUnit) // NOLINT
{
auto scene_graph = getSceneGraphIIWA();

tesseract_kinematics::KDLInvKinChainLMA derived_kin(*scene_graph, "base_link", "tool0");
tesseract_kinematics::KDLInvKinChainLMA::Config config;
tesseract_kinematics::KDLInvKinChainLMA derived_kin(*scene_graph, "base_link", "tool0", config);

tesseract_kinematics::KinematicsPluginFactory factory;
runInvKinIIWATest(factory, "KDLInvKinChainLMAFactory", "KDLFwdKinChainFactory");
Expand All @@ -27,7 +28,8 @@ TEST(TesseractKinematicsUnit, KDLKinChainNRInverseKinematicUnit) // NOLINT
{
auto scene_graph = getSceneGraphIIWA();

tesseract_kinematics::KDLInvKinChainNR derived_kin(*scene_graph, "base_link", "tool0");
tesseract_kinematics::KDLInvKinChainNR::Config config;
tesseract_kinematics::KDLInvKinChainNR derived_kin(*scene_graph, "base_link", "tool0", config);

tesseract_kinematics::KinematicsPluginFactory factory;
runInvKinIIWATest(factory, "KDLInvKinChainNRFactory", "KDLFwdKinChainFactory");
Expand Down
20 changes: 19 additions & 1 deletion tesseract_kinematics/test/kinematics_factory_unit.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -1138,7 +1138,25 @@ TEST(TesseractKinematicsFactoryUnit, LoadKDLKinematicsUnit) // NOLINT
class: KDLInvKinChainNR_JLFactory
config:
base_link: base_link
tip_link: tool0)";
tip_link: tool0
KDLInvKinChainLMA_AllParams:
class: KDLInvKinChainLMAFactory
config:
base_link: base_link
tip_link: tool0
task_weights: [1, 1, 1, 0.1, 0.1, 0.1]
eps: 1e-5
max_iterations: 500
eps_joints: 1e-15
KDLInvKinChainNR_AllParams:
class: KDLInvKinChainNRFactory
config:
base_link: base_link
tip_link: tool0
velocity_eps: 0.00001
velocity_iterations: 150
position_eps: 1e-6
position_iterations: 100)";

{
KinematicsPluginFactory factory(YAML::Load(yaml_string));
Expand Down

0 comments on commit 3124cd9

Please sign in to comment.