Finish the method for forward kinematics (ros-controls#4)

* Rename incremental_ik_calculator -> incremental_kinematics

* Use Eigen double types. Rename _trafo transform variables

* Fix typo in transformation matrix

* Implement incremental forward kinematics

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

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<IncrementalIKCalculator> ik_;
+  std::shared_ptr<IncrementalKinematics> ik_;
   Eigen::VectorXd delta_x_;
   Eigen::VectorXd delta_theta_;
 

admittance_controller/include/admittance_controller/incremental_ik_calculator.hpp → 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<rclcpp::Node> & node, const std::string & group_name);
+  IncrementalKinematics(const std::shared_ptr<rclcpp::Node> & 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<double> & delta_x_vec, const geometry_msgs::msg::TransformStamped & ik_base_to_tip_trafo, std::vector<double> & delta_theta_vec);
+  convertCartesianDeltasToJointDeltas(const std::vector<double> & delta_x_vec, const geometry_msgs::msg::TransformStamped & ik_base_to_tip_tf, std::vector<double> & 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<double> &  delta_theta_vec, const geometry_msgs::msg::TransformStamped & ik_base_to_tip_trafo, std::vector<double> & delta_x_vec);
+  convertJointDeltasToCartesianDeltas(const std::vector<double> &  delta_theta_vec, const geometry_msgs::msg::TransformStamped & ik_base_to_tip_tf, std::vector<double> & delta_x_vec);
 
 private:
   // MoveIt setup, required to retrieve the Jacobian

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"
 

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<IncrementalIKCalculator>(node, ik_group_name_);
+  ik_ = std::make_shared<IncrementalKinematics>(node, ik_group_name_);
 
   return controller_interface::return_type::OK;
 }

admittance_controller/src/incremental_ik_calculator.cpp → 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<rclcpp::Node> & node, const std::string & group_name) : node_(node)
+IncrementalKinematics::IncrementalKinematics(const std::shared_ptr<rclcpp::Node> & node, const std::string & group_name) : node_(node)
 {
   // TODO(andyz): Parameterize robot description and joint group
   std::unique_ptr<robot_model_loader::RobotModelLoader> model_loader_ptr =
@@ -34,23 +34,25 @@ IncrementalIKCalculator::IncrementalIKCalculator(const std::shared_ptr<rclcpp::N
   // By default, the MoveIt Jacobian frame is the last link
 }
 
-bool IncrementalIKCalculator::convertCartesianDeltasToJointDeltas(const std::vector<double> & delta_x_vec, const geometry_msgs::msg::TransformStamped & ik_base_to_tip_trafo, std::vector<double> & delta_theta_vec)
+bool IncrementalKinematics::convertCartesianDeltasToJointDeltas(const std::vector<double> & delta_x_vec, const geometry_msgs::msg::TransformStamped & ik_base_to_tip_tf, std::vector<double> & delta_theta_vec)
 {
   // see here for this conversion: https://stackoverflow.com/questions/26094379/typecasting-eigenvectorxd-to-stdvector
-  Eigen::VectorXf delta_x = Eigen::Map<Eigen::VectorXf, Eigen::Unaligned>(&delta_x_vec[0], delta_x_vec.size());
+  Eigen::VectorXd delta_x = Eigen::Map<Eigen::VectorXd, Eigen::Unaligned>(&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,17 +75,59 @@ bool IncrementalIKCalculator::convertCartesianDeltasToJointDeltas(const std::vec
   svd_ = Eigen::JacobiSVD<Eigen::MatrixXd>(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<double> delta_theta_v(&delta_theta[0], delta_theta.data() + delta_theta.cols() * delta_theta.rows());
   delta_theta_vec = delta_theta_v;
 
   return true;
 }
 
-bool IncrementalIKCalculator::convertJointDeltasToCartesianDeltas(const std::vector<double> &  delta_theta_vec, const geometry_msgs::msg::TransformStamped & ik_base_to_tip_trafo, std::vector<double> & delta_x_vec)
+bool IncrementalKinematics::convertJointDeltasToCartesianDeltas(const std::vector<double> &  delta_theta_vec, const geometry_msgs::msg::TransformStamped & ik_base_to_tip_tf, std::vector<double> & 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<Eigen::VectorXd, Eigen::Unaligned>(&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<double> 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