Skip to content

Commit

Permalink
Fix unit tests
Browse files Browse the repository at this point in the history
  • Loading branch information
@Levi-Armstrong
Levi-Armstrong committed Jun 2, 2024
1 parent 000c18f commit d7dda31
Show file tree
Hide file tree
Showing 7 changed files with 59 additions and 32 deletions.
10 changes: 4 additions & 6 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 Down Expand Up @@ -64,13 +65,10 @@ class KDLInvKinChainLMA : public InverseKinematics
*/
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() };
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 };

public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};

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

/**
Expand All @@ -102,7 +100,7 @@ class KDLInvKinChainLMA : public InverseKinematics
*/
KDLInvKinChainLMA(const tesseract_scene_graph::SceneGraph& scene_graph,
const std::vector<std::pair<std::string, std::string> >& chains,
const Config& kdl_config,
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
4 changes: 2 additions & 2 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 @@ -90,7 +90,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,
Config kdl_config,
std::string solver_name = KDL_INV_KIN_CHAIN_NR_SOLVER_NAME);

/**
Expand All @@ -102,7 +102,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,
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
14 changes: 4 additions & 10 deletions tesseract_kinematics/kdl/src/kdl_factories.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -93,16 +93,10 @@ 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() != 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.task_weights(w++) = v.as<double>();
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"])
Expand Down
26 changes: 19 additions & 7 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,9 +40,9 @@ using Eigen::VectorXd;

KDLInvKinChainLMA::KDLInvKinChainLMA(const tesseract_scene_graph::SceneGraph& scene_graph,
const std::vector<std::pair<std::string, std::string>>& chains,
const Config& kdl_config,
Config kdl_config,
std::string solver_name)
: kdl_config_(kdl_config), 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,16 +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, kdl_config_.task_weights, kdl_config_.eps, kdl_config_.max_iterations, kdl_config_.eps_joints);
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,
const Config& kdl_config,
Config kdl_config,
std::string solver_name)
: KDLInvKinChainLMA(scene_graph, { std::make_pair(base_link, tip_link) }, kdl_config, std::move(solver_name))
: KDLInvKinChainLMA(scene_graph,
{ std::make_pair(base_link, tip_link) },
std::move(kdl_config),
std::move(solver_name))
{
}

Expand All @@ -72,7 +79,12 @@ KDLInvKinChainLMA& KDLInvKinChainLMA::operator=(const KDLInvKinChainLMA& other)
{
kdl_data_ = other.kdl_data_;
kdl_config_ = other.kdl_config_;
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);
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
11 changes: 7 additions & 4 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,9 +40,9 @@ 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,
Config kdl_config,
std::string solver_name)
: kdl_config_(kdl_config), 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 @@ -61,9 +61,12 @@ KDLInvKinChainNR::KDLInvKinChainNR(const tesseract_scene_graph::SceneGraph& scen
KDLInvKinChainNR::KDLInvKinChainNR(const tesseract_scene_graph::SceneGraph& scene_graph,
const std::string& base_link,
const std::string& tip_link,
const Config& kdl_config,
Config kdl_config,
std::string solver_name)
: KDLInvKinChainNR(scene_graph, { std::make_pair(base_link, tip_link) }, kdl_config, std::move(solver_name))
: KDLInvKinChainNR(scene_graph,
{ std::make_pair(base_link, tip_link) },
std::move(kdl_config),
std::move(solver_name))
{
}

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 d7dda31

Please sign in to comment.