Skip to content

Commit

Permalink
- Move config struct into class
Browse files Browse the repository at this point in the history 
- Add Eigen alignment
- Trivialize assignment operators to avoid storing kdl_config (though I can do this if preferred)
- Format
  • Loading branch information
@Levi-Armstrong
Sean Cardello authored and Levi-Armstrong committed Jun 2, 2024
1 parent 08db716 commit 896c1d0
Show file tree
Hide file tree
Showing 5 changed files with 84 additions and 18 deletions.
23 changes: 21 additions & 2 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 @@ -54,6 +54,25 @@ 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
{
Eigen::Matrix<double, 6, 1> task_weights{ std::vector<double>{ 1.0, 1.0, 1.0, 0.1, 0.1, 0.1 }.data() };
double eps{ 1E-5 };
int max_iterations{ 500 };
double eps_joints{ 1E-15 };

public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};

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

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

IKSolutions calcInvKin(const tesseract_common::TransformMap& tip_link_poses,
Expand Down
22 changes: 22 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,26 @@ 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 };

public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};

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

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

IKSolutions calcInvKin(const tesseract_common::TransformMap& tip_link_poses,
Expand Down
30 changes: 24 additions & 6 deletions tesseract_kinematics/kdl/src/kdl_factories.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -75,7 +75,7 @@ KDLInvKinChainLMAFactory::create(const std::string& solver_name,
{
std::string base_link;
std::string tip_link;
KDLConfig kdl_config;
KDLInvKinChainLMA::Config kdl_config;

try
{
Expand All @@ -93,12 +93,16 @@ KDLInvKinChainLMAFactory::create(const std::string& solver_name,
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");
if (n.size() != kdl_config.task_weights.size())
{
std::ostringstream what;
what << "KDLInvKinChainLMAFactory, size of task_weights needs to be " << kdl_config.task_weights.size();
throw std::runtime_error(what.str());
}

auto w = 0;
for (auto const &v : n)
kdl_config.weights(w++) = v.as<double>();
for (auto const& v : n)
kdl_config.task_weights(w++) = v.as<double>();
}

if (YAML::Node n = config["eps"])
Expand Down Expand Up @@ -128,6 +132,7 @@ KDLInvKinChainNRFactory::create(const std::string& solver_name,
{
std::string base_link;
std::string tip_link;
KDLInvKinChainNR::Config kdl_config;

try
{
Expand All @@ -140,14 +145,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
11 changes: 7 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,7 +40,8 @@ using Eigen::VectorXd;

KDLInvKinChainLMA::KDLInvKinChainLMA(const tesseract_scene_graph::SceneGraph& scene_graph,
const std::vector<std::pair<std::string, std::string>>& chains,
const KDLConfig& kdl_config, std::string solver_name)
const Config& kdl_config,
std::string solver_name)
: solver_name_(std::move(solver_name))
{
if (!scene_graph.getLink(scene_graph.getRoot()))
Expand All @@ -50,13 +51,15 @@ 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, kdl_config.weights, kdl_config.eps, kdl_config.max_iterations, kdl_config.eps_joints);
ik_solver_ = std::make_unique<KDL::ChainIkSolverPos_LMA>(
kdl_data_.robot_chain, kdl_config.task_weights, 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,
const KDLConfig& kdl_config, std::string solver_name)
const Config& kdl_config,
std::string solver_name)
: KDLInvKinChainLMA(scene_graph, { std::make_pair(base_link, tip_link) }, kdl_config, std::move(solver_name))
{
}
Expand All @@ -68,7 +71,7 @@ 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);
ik_solver_ = other.ik_solver_;
solver_name_ = other.solver_name_;

return *this;
Expand Down
16 changes: 10 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,6 +40,7 @@ using Eigen::VectorXd;

KDLInvKinChainNR::KDLInvKinChainNR(const tesseract_scene_graph::SceneGraph& scene_graph,
const std::vector<std::pair<std::string, std::string>>& chains,
const Config& kdl_config,
std::string solver_name)
: solver_name_(std::move(solver_name))
{
Expand All @@ -51,15 +52,18 @@ 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,
const 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) }, kdl_config, std::move(solver_name))
{
}

Expand All @@ -70,9 +74,9 @@ KDLInvKinChainNR::KDLInvKinChainNR(const KDLInvKinChainNR& other) { *this = othe
KDLInvKinChainNR& KDLInvKinChainNR::operator=(const KDLInvKinChainNR& other)
{
kdl_data_ = other.kdl_data_;
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_);
fk_solver_ = other.fk_solver_;
ik_vel_solver_ = other.ik_vel_solver_;
ik_solver_ = other.ik_solver_;
solver_name_ = other.solver_name_;

return *this;
Expand Down

0 comments on commit 896c1d0

Please sign in to comment.