From b4957ca59555ae7c881c777d26dc6f573d60d23c Mon Sep 17 00:00:00 2001 From: AndyZe Date: Thu, 6 May 2021 20:43:40 -0500 Subject: [PATCH 1/4] Rename incremental_ik_calculator -> incremental_kinematics --- admittance_controller/CMakeLists.txt | 2 +- .../include/admittance_controller/admittance_rule.hpp | 4 ++-- ...ental_ik_calculator.hpp => incremental_kinematics.hpp} | 4 ++-- admittance_controller/src/admittance_controller.cpp | 2 +- admittance_controller/src/admittance_rule.cpp | 2 +- ...ental_ik_calculator.cpp => incremental_kinematics.cpp} | 8 ++++---- 6 files changed, 11 insertions(+), 11 deletions(-) rename admittance_controller/include/admittance_controller/{incremental_ik_calculator.hpp => incremental_kinematics.hpp} (94%) rename admittance_controller/src/{incremental_ik_calculator.cpp => incremental_kinematics.cpp} (85%) diff --git a/admittance_controller/CMakeLists.txt b/admittance_controller/CMakeLists.txt index 797942911c..f5c31bad99 100644 --- a/admittance_controller/CMakeLists.txt +++ b/admittance_controller/CMakeLists.txt @@ -33,7 +33,7 @@ add_library( SHARED src/admittance_controller.cpp src/admittance_rule.cpp - src/incremental_ik_calculator.cpp + src/incremental_kinematics.cpp ) target_include_directories( admittance_controller diff --git a/admittance_controller/include/admittance_controller/admittance_rule.hpp b/admittance_controller/include/admittance_controller/admittance_rule.hpp index a9d5554327..6f7fa867c4 100644 --- a/admittance_controller/include/admittance_controller/admittance_rule.hpp +++ b/admittance_controller/include/admittance_controller/admittance_rule.hpp @@ -17,7 +17,7 @@ #ifndef ADMITTANCE_CONTROLLER__ADMITTANCE_RULE_HPP_ #define ADMITTANCE_CONTROLLER__ADMITTANCE_RULE_HPP_ -#include "admittance_controller/incremental_ik_calculator.hpp" +#include "admittance_controller/incremental_kinematics.hpp" #include "control_msgs/msg/admittance_controller_state.hpp" #include "controller_interface/controller_interface.hpp" #include "geometry_msgs/msg/quaternion.hpp" @@ -86,7 +86,7 @@ class AdmittanceRule protected: // IK variables - std::shared_ptr ik_; + std::shared_ptr ik_; Eigen::VectorXd delta_x_; Eigen::VectorXd delta_theta_; diff --git a/admittance_controller/include/admittance_controller/incremental_ik_calculator.hpp b/admittance_controller/include/admittance_controller/incremental_kinematics.hpp similarity index 94% rename from admittance_controller/include/admittance_controller/incremental_ik_calculator.hpp rename to admittance_controller/include/admittance_controller/incremental_kinematics.hpp index b547275840..e17f736716 100644 --- a/admittance_controller/include/admittance_controller/incremental_ik_calculator.hpp +++ b/admittance_controller/include/admittance_controller/incremental_kinematics.hpp @@ -25,14 +25,14 @@ namespace admittance_controller { -class IncrementalIKCalculator +class IncrementalKinematics { public: /** * \brief Create an object which takes Cartesian delta-x and converts to joint delta-theta. * It uses the Jacobian from MoveIt. */ - IncrementalIKCalculator(const std::shared_ptr & node, const std::string & group_name); + IncrementalKinematics(const std::shared_ptr & node, const std::string & group_name); /** * \brief Convert Cartesian delta-x to joint delta-theta, using the Jacobian. diff --git a/admittance_controller/src/admittance_controller.cpp b/admittance_controller/src/admittance_controller.cpp index 010ac3d3c5..4e2335b350 100644 --- a/admittance_controller/src/admittance_controller.cpp +++ b/admittance_controller/src/admittance_controller.cpp @@ -22,7 +22,7 @@ // #include "Eigen/Core" #include "admittance_controller/admittance_controller.hpp" -#include "admittance_controller/incremental_ik_calculator.hpp" +#include "admittance_controller/incremental_kinematics.hpp" #include "controller_interface/helpers.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" diff --git a/admittance_controller/src/admittance_rule.cpp b/admittance_controller/src/admittance_rule.cpp index 1effcb6e7d..fc87049fdf 100644 --- a/admittance_controller/src/admittance_rule.cpp +++ b/admittance_controller/src/admittance_rule.cpp @@ -156,7 +156,7 @@ controller_interface::return_type AdmittanceRule::configure(rclcpp::Node::Shared relative_desired_joint_state_vec_.reserve(6); // Initialize IK - ik_ = std::make_shared(node, ik_group_name_); + ik_ = std::make_shared(node, ik_group_name_); return controller_interface::return_type::OK; } diff --git a/admittance_controller/src/incremental_ik_calculator.cpp b/admittance_controller/src/incremental_kinematics.cpp similarity index 85% rename from admittance_controller/src/incremental_ik_calculator.cpp rename to admittance_controller/src/incremental_kinematics.cpp index 9f278769b3..1d8949dd9f 100644 --- a/admittance_controller/src/incremental_ik_calculator.cpp +++ b/admittance_controller/src/incremental_kinematics.cpp @@ -14,14 +14,14 @@ // /// \author: Andy Zelenak -#include "admittance_controller/incremental_ik_calculator.hpp" +#include "admittance_controller/incremental_kinematics.hpp" #include "Eigen/VectorXf" #include "tf2_eigen/tf2_eigen.h" namespace admittance_controller { -IncrementalIKCalculator::IncrementalIKCalculator(const std::shared_ptr & node, const std::string & group_name) : node_(node) +IncrementalKinematics::IncrementalKinematics(const std::shared_ptr & node, const std::string & group_name) : node_(node) { // TODO(andyz): Parameterize robot description and joint group std::unique_ptr model_loader_ptr = @@ -34,7 +34,7 @@ IncrementalIKCalculator::IncrementalIKCalculator(const std::shared_ptr & delta_x_vec, const geometry_msgs::msg::TransformStamped & ik_base_to_tip_trafo, std::vector & delta_theta_vec) +bool IncrementalKinematics::convertCartesianDeltasToJointDeltas(const std::vector & delta_x_vec, const geometry_msgs::msg::TransformStamped & ik_base_to_tip_trafo, std::vector & delta_theta_vec) { // see here for this conversion: https://stackoverflow.com/questions/26094379/typecasting-eigenvectorxd-to-stdvector Eigen::VectorXf delta_x = Eigen::Map(&delta_x_vec[0], delta_x_vec.size()); @@ -81,7 +81,7 @@ bool IncrementalIKCalculator::convertCartesianDeltasToJointDeltas(const std::vec return true; } -bool IncrementalIKCalculator::convertJointDeltasToCartesianDeltas(const std::vector & delta_theta_vec, const geometry_msgs::msg::TransformStamped & ik_base_to_tip_trafo, std::vector & delta_x_vec) +bool IncrementalKinematics::convertJointDeltasToCartesianDeltas(const std::vector & delta_theta_vec, const geometry_msgs::msg::TransformStamped & ik_base_to_tip_trafo, std::vector & delta_x_vec) { // TODO(andyz): Please add here FK implementation } From 0e5f9dc86dac92a9b818a5baf884f6d346ee0823 Mon Sep 17 00:00:00 2001 From: AndyZe Date: Thu, 6 May 2021 20:48:32 -0500 Subject: [PATCH 2/4] Use Eigen double types. Rename _trafo transform variables --- .../admittance_controller/incremental_kinematics.hpp | 8 ++++---- admittance_controller/src/incremental_kinematics.cpp | 12 ++++++------ 2 files changed, 10 insertions(+), 10 deletions(-) diff --git a/admittance_controller/include/admittance_controller/incremental_kinematics.hpp b/admittance_controller/include/admittance_controller/incremental_kinematics.hpp index e17f736716..a3a168c5ba 100644 --- a/admittance_controller/include/admittance_controller/incremental_kinematics.hpp +++ b/admittance_controller/include/admittance_controller/incremental_kinematics.hpp @@ -37,22 +37,22 @@ class IncrementalKinematics /** * \brief Convert Cartesian delta-x to joint delta-theta, using the Jacobian. * \param delta_x_vec input Cartesian deltas (x, y, z, rx, ry, rz) - * \param ik_base_to_tip_trafo transformation between ik base and ik tip + * \param ik_base_to_tip_tf transformation between ik base and ik tip * \param delta_theta_vec output vector with joint states * \return true if successful */ bool - convertCartesianDeltasToJointDeltas(const std::vector & delta_x_vec, const geometry_msgs::msg::TransformStamped & ik_base_to_tip_trafo, std::vector & delta_theta_vec); + convertCartesianDeltasToJointDeltas(const std::vector & delta_x_vec, const geometry_msgs::msg::TransformStamped & ik_base_to_tip_tf, std::vector & delta_theta_vec); /** * \brief Convert joint delta-theta to Cartesian delta-x, using the Jacobian. * \param[in] delta_theta_vec vector with joint states - * \param[in] ik_base_to_tip_trafo transformation between ik base to ik tip + * \param[in] ik_base_to_tip_tf transformation between ik base to ik tip * \param[out] delta_x_vec Cartesian deltas (x, y, z, rx, ry, rz) * \return true if successful */ bool - convertJointDeltasToCartesianDeltas(const std::vector & delta_theta_vec, const geometry_msgs::msg::TransformStamped & ik_base_to_tip_trafo, std::vector & delta_x_vec); + convertJointDeltasToCartesianDeltas(const std::vector & delta_theta_vec, const geometry_msgs::msg::TransformStamped & ik_base_to_tip_tf, std::vector & delta_x_vec); private: // MoveIt setup, required to retrieve the Jacobian diff --git a/admittance_controller/src/incremental_kinematics.cpp b/admittance_controller/src/incremental_kinematics.cpp index 1d8949dd9f..9986e4d3cf 100644 --- a/admittance_controller/src/incremental_kinematics.cpp +++ b/admittance_controller/src/incremental_kinematics.cpp @@ -16,7 +16,7 @@ #include "admittance_controller/incremental_kinematics.hpp" -#include "Eigen/VectorXf" +#include "Eigen/VectorXd" #include "tf2_eigen/tf2_eigen.h" namespace admittance_controller @@ -34,16 +34,16 @@ IncrementalKinematics::IncrementalKinematics(const std::shared_ptr // By default, the MoveIt Jacobian frame is the last link } -bool IncrementalKinematics::convertCartesianDeltasToJointDeltas(const std::vector & delta_x_vec, const geometry_msgs::msg::TransformStamped & ik_base_to_tip_trafo, std::vector & delta_theta_vec) +bool IncrementalKinematics::convertCartesianDeltasToJointDeltas(const std::vector & delta_x_vec, const geometry_msgs::msg::TransformStamped & ik_base_to_tip_tf, std::vector & delta_theta_vec) { // see here for this conversion: https://stackoverflow.com/questions/26094379/typecasting-eigenvectorxd-to-stdvector - Eigen::VectorXf delta_x = Eigen::Map(&delta_x_vec[0], delta_x_vec.size()); + Eigen::VectorXd delta_x = Eigen::Map(&delta_x_vec[0], delta_x_vec.size()); // Transform delta_x to the moveit_jacobian_frame try { // 4x4 transformation matrix - const Eigen::Isometry3d affine_transform = tf2::transformToEigen(ik_base_to_tip_trafo); + const Eigen::Isometry3d affine_transform = tf2::transformToEigen(ik_base_to_tip_tf); // Build the 6x6 transformation matrix Eigen::MatrixXd twist_transform(6,6); @@ -73,7 +73,7 @@ bool IncrementalKinematics::convertCartesianDeltasToJointDeltas(const std::vecto svd_ = Eigen::JacobiSVD(jacobian_, Eigen::ComputeThinU | Eigen::ComputeThinV); matrix_s_ = svd_.singularValues().asDiagonal(); pseudo_inverse_ = svd_.matrixV() * matrix_s_.inverse() * svd_.matrixU().transpose(); - Eigen::VectorXf delta_theta = pseudo_inverse_ * delta_x; + Eigen::VectorXd delta_theta = pseudo_inverse_ * delta_x; std::vector delta_theta_v(&delta_theta[0], delta_theta.data() + delta_theta.cols() * delta_theta.rows()); delta_theta_vec = delta_theta_v; @@ -81,7 +81,7 @@ bool IncrementalKinematics::convertCartesianDeltasToJointDeltas(const std::vecto return true; } -bool IncrementalKinematics::convertJointDeltasToCartesianDeltas(const std::vector & delta_theta_vec, const geometry_msgs::msg::TransformStamped & ik_base_to_tip_trafo, std::vector & delta_x_vec) +bool IncrementalKinematics::convertJointDeltasToCartesianDeltas(const std::vector & delta_theta_vec, const geometry_msgs::msg::TransformStamped & ik_base_to_tip_tf, std::vector & delta_x_vec) { // TODO(andyz): Please add here FK implementation } From 07da5c498fc673398f3c5ca858bb3e12ddd54405 Mon Sep 17 00:00:00 2001 From: AndyZe Date: Thu, 6 May 2021 21:30:27 -0500 Subject: [PATCH 3/4] Fix typo in transformation matrix --- admittance_controller/src/incremental_kinematics.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/admittance_controller/src/incremental_kinematics.cpp b/admittance_controller/src/incremental_kinematics.cpp index 9986e4d3cf..a999535849 100644 --- a/admittance_controller/src/incremental_kinematics.cpp +++ b/admittance_controller/src/incremental_kinematics.cpp @@ -46,11 +46,13 @@ bool IncrementalKinematics::convertCartesianDeltasToJointDeltas(const std::vecto const Eigen::Isometry3d affine_transform = tf2::transformToEigen(ik_base_to_tip_tf); // Build the 6x6 transformation matrix + // TODO: replace when this PR to tf2_eigen is merged + // https://github.com/ros2/geometry2/pull/406 Eigen::MatrixXd twist_transform(6,6); // upper left 3x3 block is the rotation part twist_transform.block(0,0,3,3) = affine_transform.rotation(); // upper right 3x3 block is all zeros - twist_transform.block(0,4,3,3) = Eigen::MatrixXd::Zero(3,3); + twist_transform.block(0,3,3,3) = Eigen::MatrixXd::Zero(3,3); // lower left 3x3 block is tricky. See https://core.ac.uk/download/pdf/154240607.pdf Eigen::MatrixXd pos_vector_3x3(3,3); pos_vector_3x3(0,0) = 0; pos_vector_3x3(0,1) = -affine_transform.translation().z(); pos_vector_3x3(0,2) = affine_transform.translation().y(); From 1be6233b209ff94bf5ee62e11247f9bbe53001a2 Mon Sep 17 00:00:00 2001 From: AndyZe Date: Thu, 6 May 2021 21:36:36 -0500 Subject: [PATCH 4/4] Implement incremental forward kinematics --- .../src/incremental_kinematics.cpp | 48 +++++++++++++++++-- 1 file changed, 45 insertions(+), 3 deletions(-) diff --git a/admittance_controller/src/incremental_kinematics.cpp b/admittance_controller/src/incremental_kinematics.cpp index a999535849..48bc7f6bc7 100644 --- a/admittance_controller/src/incremental_kinematics.cpp +++ b/admittance_controller/src/incremental_kinematics.cpp @@ -40,14 +40,14 @@ bool IncrementalKinematics::convertCartesianDeltasToJointDeltas(const std::vecto Eigen::VectorXd delta_x = Eigen::Map(&delta_x_vec[0], delta_x_vec.size()); // Transform delta_x to the moveit_jacobian_frame + // TODO: replace when this PR to tf2_eigen is merged + // https://github.com/ros2/geometry2/pull/406 try { // 4x4 transformation matrix const Eigen::Isometry3d affine_transform = tf2::transformToEigen(ik_base_to_tip_tf); // Build the 6x6 transformation matrix - // TODO: replace when this PR to tf2_eigen is merged - // https://github.com/ros2/geometry2/pull/406 Eigen::MatrixXd twist_transform(6,6); // upper left 3x3 block is the rotation part twist_transform.block(0,0,3,3) = affine_transform.rotation(); @@ -85,7 +85,49 @@ bool IncrementalKinematics::convertCartesianDeltasToJointDeltas(const std::vecto bool IncrementalKinematics::convertJointDeltasToCartesianDeltas(const std::vector & delta_theta_vec, const geometry_msgs::msg::TransformStamped & ik_base_to_tip_tf, std::vector & delta_x_vec) { - // TODO(andyz): Please add here FK implementation + // see here for this conversion: https://stackoverflow.com/questions/26094379/typecasting-eigenvectorxd-to-stdvector + Eigen::VectorXd delta_theta = Eigen::Map(&delta_theta_vec[0], delta_theta_vec.size()); + + // Multiply with the Jacobian to get delta_x + jacobian_ = kinematic_state_->getJacobian(joint_model_group_); + // delta_x will be in the working frame of MoveIt + Eigen::VectorXd delta_x = jacobian_ * delta_theta; + + // Transform delta_x to the tip frame + // TODO: replace when this PR to tf2_eigen is merged + // https://github.com/ros2/geometry2/pull/406 + try + { + // 4x4 transformation matrix + const Eigen::Isometry3d affine_transform = tf2::transformToEigen(ik_base_to_tip_tf); + + // Build the 6x6 transformation matrix + Eigen::MatrixXd twist_transform(6,6); + // upper left 3x3 block is the rotation part + twist_transform.block(0,0,3,3) = affine_transform.rotation(); + // upper right 3x3 block is all zeros + twist_transform.block(0,3,3,3) = Eigen::MatrixXd::Zero(3,3); + // lower left 3x3 block is tricky. See https://core.ac.uk/download/pdf/154240607.pdf + Eigen::MatrixXd pos_vector_3x3(3,3); + pos_vector_3x3(0,0) = 0; pos_vector_3x3(0,1) = -affine_transform.translation().z(); pos_vector_3x3(0,2) = affine_transform.translation().y(); + pos_vector_3x3(1, 0) = affine_transform.translation().z(); pos_vector_3x3(1,1) = 0; pos_vector_3x3(1,2) = -affine_transform.translation().x(); + pos_vector_3x3(2, 0) = -affine_transform.translation().y(); pos_vector_3x3(2,1) = affine_transform.translation().x(); pos_vector_3x3(1,2) = 0; + twist_transform.block(3,0,3,3) = pos_vector_3x3 * affine_transform.rotation(); + // lower right 3x3 block is the rotation part + twist_transform.block(3,3,3,3) = affine_transform.rotation(); + + delta_x = twist_transform * delta_x; + } + catch (tf2::TransformException & ex) + { + RCLCPP_ERROR(node_->get_logger(), "Transformation of wrench failed."); + return false; + } + + std::vector delta_x_v(&delta_x[0], delta_x.data() + delta_x.cols() * delta_x.rows()); + delta_x_vec = delta_x_v; + + return true; } } // namespace admittance_controller