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 84% rename from admittance_controller/include/admittance_controller/incremental_ik_calculator.hpp rename to admittance_controller/include/admittance_controller/incremental_kinematics.hpp index b547275840..a3a168c5ba 100644 --- a/admittance_controller/include/admittance_controller/incremental_ik_calculator.hpp +++ b/admittance_controller/include/admittance_controller/incremental_kinematics.hpp @@ -25,34 +25,34 @@ 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. * \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/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 52% rename from admittance_controller/src/incremental_ik_calculator.cpp rename to admittance_controller/src/incremental_kinematics.cpp index 9f278769b3..48bc7f6bc7 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 "Eigen/VectorXd" #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,23 +34,25 @@ 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_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 + // 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_trafo); + 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,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(); @@ -73,7 +75,7 @@ bool IncrementalIKCalculator::convertCartesianDeltasToJointDeltas(const std::vec 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,9 +83,51 @@ 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_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