From 6cb886b156ca057ad0030aefb40b084c379cdfd4 Mon Sep 17 00:00:00 2001 From: Paul Gesel Date: Thu, 4 Aug 2022 18:28:26 -0600 Subject: [PATCH] add verification for all inputs --- .../kinematics_interface_kdl.hpp | 28 ++-- .../src/kinematics_interface_kdl.cpp | 137 ++++++++++++------ .../test/test_kinematics_interface_kdl.cpp | 86 ++++++++--- 3 files changed, 172 insertions(+), 79 deletions(-) diff --git a/kinematics_interface_kdl/include/kinematics_interface_kdl/kinematics_interface_kdl.hpp b/kinematics_interface_kdl/include/kinematics_interface_kdl/kinematics_interface_kdl.hpp index 85721cd..d02825d 100644 --- a/kinematics_interface_kdl/include/kinematics_interface_kdl/kinematics_interface_kdl.hpp +++ b/kinematics_interface_kdl/include/kinematics_interface_kdl/kinematics_interface_kdl.hpp @@ -37,8 +37,8 @@ namespace kinematics_interface_kdl /** * \brief KDL implementation of ros2_control kinematics interface */ - virtual bool - initialize(std::shared_ptr node, const std::string &end_effector_name); + bool + initialize(std::shared_ptr node, const std::string &end_effector_name) override; /** * \brief Convert Cartesian delta-x to joint delta-theta, using the Jacobian. @@ -48,11 +48,11 @@ namespace kinematics_interface_kdl * \param[out] delta_theta_vec output vector with joint states * \return true if successful */ - virtual bool + bool convert_cartesian_deltas_to_joint_deltas(const std::vector &joint_pos, const std::vector &delta_x_vec, const std::string &link_name, - std::vector &delta_theta_vec); + std::vector &delta_theta_vec) override; /** * \brief Convert joint delta-theta to Cartesian delta-x. @@ -62,11 +62,11 @@ namespace kinematics_interface_kdl * \param[out] delta_x_vec Cartesian deltas (x, y, z, wx, wy, wz) * \return true if successful */ - virtual bool + bool convert_joint_deltas_to_cartesian_deltas(const std::vector &joint_pos, const std::vector &delta_theta_vec, const std::string &link_name, - std::vector &delta_x_vec); + std::vector &delta_x_vec) override; /** * \brief Calculates the joint transform for a specified link using provided joint positions. @@ -75,9 +75,9 @@ namespace kinematics_interface_kdl * \param[out] transform_vec transformation matrix of the specified link in column major format. * \return true if successful */ - virtual bool + bool calculate_link_transform(const std::vector &joint_pos, const std::string &link_name, - std::vector &transform_vec); + std::vector &transform_vec) override; /** * \brief Calculates the joint transform for a specified link using provided joint positions. @@ -86,17 +86,23 @@ namespace kinematics_interface_kdl * \param[out] jacobian Jacobian matrix of the specified link in column major format. * \return true if successful */ - virtual bool + bool calculate_jacobian(const std::vector &joint_pos, const std::string &link_name, - std::vector &jacobian); + std::vector &jacobian) override; private: bool update_joint_array(const std::vector& joint_pos); + + //verification methods + bool verify_initialized(); bool verify_link_name(const std::string& link_name); + bool verify_transform_vector(const std::vector &transform); + bool verify_cartesian_vector(const std::vector &cartesian_vector); + bool verify_joint_vector(const std::vector &joint_vector); + bool verify_jacobian(const std::vector &jacobian_vector); bool initialized = false; - std::string end_effector_name_; std::string root_name_; size_t num_joints_; KDL::Chain chain_; diff --git a/kinematics_interface_kdl/src/kinematics_interface_kdl.cpp b/kinematics_interface_kdl/src/kinematics_interface_kdl.cpp index efc58ed..5b255f0 100644 --- a/kinematics_interface_kdl/src/kinematics_interface_kdl.cpp +++ b/kinematics_interface_kdl/src/kinematics_interface_kdl.cpp @@ -22,8 +22,9 @@ namespace kinematics_interface_kdl { bool KDLKinematics::initialize(std::shared_ptr node, const std::string &end_effector_name) { + // track initialization plugin initialized = true; - end_effector_name_ = end_effector_name; + // get robot description auto robot_param = rclcpp::Parameter(); if (!node->get_parameter("robot_description", robot_param)) { @@ -69,20 +70,22 @@ namespace kinematics_interface_kdl { const std::vector &delta_theta_vec, const std::string &link_name, std::vector &delta_x_vec) { - // get joint array and check dimensions - if (!update_joint_array(joint_pos)) { - return false; - } - if (!verify_link_name(link_name)){ + // verify inputs + if (!verify_initialized() || !verify_joint_vector(joint_pos) || !verify_link_name(link_name) || + !verify_cartesian_vector(delta_x_vec) || !verify_joint_vector(delta_theta_vec)) { return false; } + + // get joint array + update_joint_array(joint_pos); + // copy vector to eigen type - memcpy(delta_theta.data(), delta_theta_vec.data(), delta_theta_vec.size()*sizeof(double)); + memcpy(delta_theta.data(), delta_theta_vec.data(), delta_theta_vec.size() * sizeof(double)); // calculate Jacobian jac_solver_->JntToJac(q_, *jacobian_, link_name_map_[link_name]); delta_x = jacobian_->data * delta_theta; // copy eigen type to vector - memcpy(delta_x_vec.data(), delta_x.data(), 6*sizeof(double)); + memcpy(delta_x_vec.data(), delta_x.data(), 6 * sizeof(double)); return true; } @@ -90,15 +93,17 @@ namespace kinematics_interface_kdl { const std::vector &delta_x_vec, const std::string &link_name, std::vector &delta_theta_vec) { - // get joint array and check dimensions - if (!update_joint_array(joint_pos)) { - return false; - } - if (!verify_link_name(link_name)){ + // verify inputs + if (!verify_initialized() || !verify_joint_vector(joint_pos) || !verify_link_name(link_name) || + !verify_cartesian_vector(delta_x_vec) || !verify_joint_vector(delta_theta_vec)) { return false; } + + // get joint array + update_joint_array(joint_pos); + // copy vector to eigen type - memcpy(delta_x.data(), delta_x_vec.data(), delta_x_vec.size()*sizeof(double)); + memcpy(delta_x.data(), delta_x_vec.data(), delta_x_vec.size() * sizeof(double)); // calculate Jacobian jac_solver_->JntToJac(q_, *jacobian_, link_name_map_[link_name]); // TODO this dynamic allocation needs to be replaced @@ -107,48 +112,44 @@ namespace kinematics_interface_kdl { Eigen::Matrix J_inverse = (J.transpose() * J + alpha * I).inverse() * J.transpose(); delta_theta = J_inverse * delta_x; // copy eigen type to vector - memcpy(delta_theta_vec.data(), delta_theta.data(), num_joints_*sizeof(double)); + memcpy(delta_theta_vec.data(), delta_theta.data(), num_joints_ * sizeof(double)); return true; } bool KDLKinematics::calculate_jacobian(const std::vector &joint_pos, - const std::string &link_name, - std::vector &jacobian) { - // get joint array and check dimensions - if (!update_joint_array(joint_pos)) { - return false; - } - if (!verify_link_name(link_name)){ - return false; - } - if (jacobian.size() != 6*num_joints_) { - RCLCPP_ERROR(LOGGER, "The size of the jacobian argument (%zu) does not match the required size of (%zu)", - jacobian.size(), 6*num_joints_); + const std::string &link_name, + std::vector &jacobian_vector) { + // verify inputs + if (!verify_initialized() || !verify_joint_vector(joint_pos) || !verify_link_name(link_name) || + !verify_jacobian(jacobian_vector)) { return false; } + // get joint array + update_joint_array(joint_pos); + // calculate Jacobian jac_solver_->JntToJac(q_, *jacobian_, link_name_map_[link_name]); - memcpy(jacobian.data(), jacobian_->data.data(), 6*num_joints_*sizeof(double)); + memcpy(jacobian_vector.data(), jacobian_->data.data(), 6 * num_joints_ * sizeof(double)); return true; } bool KDLKinematics::calculate_link_transform(const std::vector &joint_pos, const std::string &link_name, - std::vector &transform_vec - ) { - // get joint array and check dimensions - if (!update_joint_array(joint_pos)) { - return false; - } - if (!verify_link_name(link_name)){ + std::vector &transform_vec) { + // verify inputs + if (!verify_initialized() || !verify_joint_vector(joint_pos) || !verify_link_name(link_name) || + !verify_transform_vector(transform_vec)) { return false; } + // get joint array + update_joint_array(joint_pos); + // reset transform_vec - memset(transform_vec.data(), 0, 16*sizeof(double)); + memset(transform_vec.data(), 0, 16 * sizeof(double)); // special case: since the root is not in the robot tree, need to return identity transform if (link_name == root_name_) { @@ -158,6 +159,8 @@ namespace kinematics_interface_kdl { transform_vec[15] = 1.0; return true; } + + // create forward kinematics solver fk_pos_solver_->JntToCart(q_, frame_, link_name_map_[link_name]); double tmp[] = {frame_.p.x(), frame_.p.y(), frame_.p.z()}; // KDL::Rotation stores data in row-major format. e.g Xx, Yx, Zx, Xy... = data index at 0, 1, 2, 3, 4... @@ -172,18 +175,7 @@ namespace kinematics_interface_kdl { } bool KDLKinematics::update_joint_array(const std::vector &joint_pos) { - // check if interface is initialized - if (!initialized) { - RCLCPP_ERROR(LOGGER, "The KDL kinematics plugin was not initialized. Ensure you called the initialize method."); - return false; - } - // check that joint_pos_ is the correct size - if (joint_pos.size() != num_joints_) { - RCLCPP_ERROR(LOGGER, "The size of joint_pos_ (%zu) does not match that of the robot model (%zu)", - joint_pos.size(), num_joints_); - return false; - } - memcpy(q_.data.data(), joint_pos.data(), joint_pos.size()*sizeof(double)); + memcpy(q_.data.data(), joint_pos.data(), joint_pos.size() * sizeof(double)); return true; } @@ -202,6 +194,55 @@ namespace kinematics_interface_kdl { } return true; } + + bool KDLKinematics::verify_transform_vector(const std::vector &transform) { + if (transform.size() != 16) { + RCLCPP_ERROR(LOGGER, + "Invalid size (%zu) for the transformation vector. Expected size is 16.", + transform.size()); + return false; + } + return true; + } + + bool KDLKinematics::verify_cartesian_vector(const std::vector &cartesian_vector) { + if (cartesian_vector.size() != jacobian_->rows()) { + RCLCPP_ERROR(LOGGER, + "Invalid size (%zu) for the cartesian vector. Expected size is %d.", + cartesian_vector.size(), jacobian_->rows()); + return false; + } + return true; + } + + bool KDLKinematics::verify_joint_vector(const std::vector &joint_vector) { + if (joint_vector.size() != num_joints_) { + RCLCPP_ERROR(LOGGER, + "Invalid size (%zu) for the joint vector. Expected size is %d.", + joint_vector.size(), num_joints_); + return false; + } + return true; + } + + bool KDLKinematics::verify_initialized() { + // check if interface is initialized + if (!initialized) { + RCLCPP_ERROR(LOGGER, "The KDL kinematics plugin was not initialized. Ensure you called the initialize method."); + return false; + } + return true; + } + + bool KDLKinematics::verify_jacobian(const std::vector &jacobian_vector) { + if (jacobian_vector.size() != jacobian_->rows() * jacobian_->columns()) { + RCLCPP_ERROR(LOGGER, "The size of the jacobian argument (%zu) does not match the required size of (%zu)", + jacobian_vector.size(), jacobian_->rows() * jacobian_->columns()); + return false; + } + return true; + } + } #include "pluginlib/class_list_macros.hpp" diff --git a/kinematics_interface_kdl/test/test_kinematics_interface_kdl.cpp b/kinematics_interface_kdl/test/test_kinematics_interface_kdl.cpp index 734d6fb..cf1c603 100644 --- a/kinematics_interface_kdl/test/test_kinematics_interface_kdl.cpp +++ b/kinematics_interface_kdl/test/test_kinematics_interface_kdl.cpp @@ -20,22 +20,21 @@ #include "pluginlib/class_loader.hpp" #include "ros2_control_test_assets/descriptions.hpp" - class TestKDLPlugin : public ::testing::Test { public: std::shared_ptr> ik_loader_; std::shared_ptr ik_; - std::shared_ptr node; + std::shared_ptr node_; std::string end_effector = "link2"; void SetUp() { // init ros rclcpp::init(0, nullptr); - node = std::make_shared("test_node"); - + node_ = std::make_shared("test_node"); std::string plugin_name = "kinematics_interface_kdl/KDLKinematics"; - ik_loader_ = std::make_shared>( - "kinematics_interface_kdl", "kinematics_interface::KinematicsBaseClass"); + ik_loader_ = + std::make_shared>( + "kinematics_interface_kdl", "kinematics_interface::KinematicsBaseClass"); ik_ = std::unique_ptr( ik_loader_->createUnmanagedInstance(plugin_name)); } @@ -46,43 +45,90 @@ class TestKDLPlugin : public ::testing::Test { } void loadURDFParameter() { - auto urdf = std::string(ros2_control_test_assets::urdf_head) + std::string(ros2_control_test_assets::urdf_tail); + auto urdf = std::string(ros2_control_test_assets::urdf_head) + + std::string(ros2_control_test_assets::urdf_tail); rclcpp::Parameter param("robot_description", urdf); - node->declare_parameter("robot_description", ""); - node->set_parameter(param); + node_->declare_parameter("robot_description", ""); + node_->set_parameter(param); } void loadAlphaParameter() { rclcpp::Parameter param("alpha", 0.005); - node->declare_parameter("alpha", 0.005); - node->set_parameter(param); + node_->declare_parameter("alpha", 0.005); + node_->set_parameter(param); } }; TEST_F(TestKDLPlugin, KDL_plugin_function) { - std::vector pos = {0, 0}; // load robot description and alpha to parameter server loadURDFParameter(); loadAlphaParameter(); - ASSERT_TRUE(ik_->initialize(node, end_effector)); + // initialize the plugin + ASSERT_TRUE(ik_->initialize(node_, end_effector)); // calculate end effector transform - std::vector end_effector_transform(3 + 9); + std::vector pos = {0, 0}; + std::vector end_effector_transform(16); ASSERT_TRUE(ik_->calculate_link_transform(pos, end_effector, end_effector_transform)); // convert cartesian delta to joint delta std::vector delta_x = {0, 0, 1, 0, 0, 0}; - std::vector delta_theta(6); - ASSERT_TRUE(ik_->convert_cartesian_deltas_to_joint_deltas(pos, delta_x, end_effector, delta_theta)); + std::vector delta_theta(2); + ASSERT_TRUE( + ik_->convert_cartesian_deltas_to_joint_deltas(pos, delta_x, end_effector, delta_theta)); // convert joint delta to cartesian delta std::vector delta_x_est(6); - ASSERT_TRUE(ik_->convert_joint_deltas_to_cartesian_deltas(pos, delta_theta, end_effector, delta_x_est)); + ASSERT_TRUE( + ik_->convert_joint_deltas_to_cartesian_deltas(pos, delta_theta, end_effector, delta_x_est)); } -TEST_F(TestKDLPlugin, KDL_plugin_no_robot_description) { +TEST_F(TestKDLPlugin, incorrect_input_sizes) { + // load robot description and alpha to parameter server + loadURDFParameter(); + loadAlphaParameter(); + + // initialize the plugin + ASSERT_TRUE(ik_->initialize(node_, end_effector)); + + // define correct values + std::vector pos = {0, 0}; + std::vector end_effector_transform(16); + std::vector delta_x = {0, 0, 1, 0, 0, 0}; + std::vector delta_theta(2); + std::vector delta_x_est(6); + + // wrong size input vector + std::vector vec_5 = {1.0, 2.0, 3.0, 4.0, 5.0}; + + // calculate transform + ASSERT_FALSE(ik_->calculate_link_transform(vec_5, end_effector, end_effector_transform)); + ASSERT_FALSE(ik_->calculate_link_transform(pos, "link_not_in_model", end_effector_transform)); + ASSERT_FALSE(ik_->calculate_link_transform(pos, end_effector, vec_5)); + + // convert cartesian delta to joint delta + ASSERT_FALSE( + ik_->convert_cartesian_deltas_to_joint_deltas(vec_5, delta_x, end_effector, delta_theta)); + ASSERT_FALSE( + ik_->convert_cartesian_deltas_to_joint_deltas(pos, vec_5, end_effector, delta_theta)); + ASSERT_FALSE( + ik_->convert_cartesian_deltas_to_joint_deltas(pos, delta_x, "link_not_in_model", delta_theta)); + ASSERT_FALSE(ik_->convert_cartesian_deltas_to_joint_deltas(pos, delta_x, end_effector, vec_5)); + // convert joint delta to cartesian delta + ASSERT_FALSE( + ik_->convert_joint_deltas_to_cartesian_deltas(vec_5, delta_theta, end_effector, delta_x_est)); + ASSERT_FALSE( + ik_->convert_joint_deltas_to_cartesian_deltas(pos, vec_5, end_effector, delta_x_est)); + ASSERT_FALSE(ik_->convert_joint_deltas_to_cartesian_deltas( + pos, delta_theta, "link_not_in_model", delta_x_est)); + ASSERT_FALSE( + ik_->convert_joint_deltas_to_cartesian_deltas(pos, delta_theta, end_effector, vec_5)); +} + +TEST_F(TestKDLPlugin, KDL_plugin_no_robot_description) { + // load alpha to parameter server loadAlphaParameter(); - ASSERT_FALSE(ik_->initialize(node, end_effector)); -} \ No newline at end of file + ASSERT_FALSE(ik_->initialize(node_, end_effector)); +}