Try to integrate IK

admittance_controller/include/admittance_controller/admittance_rule.hpp

@@ -34,21 +34,22 @@ class AdmittanceRule
 public:
   AdmittanceRule() = default;
 
-  controller_interface::return_type configure(rclcpp::Clock::SharedPtr clock);
+  controller_interface::return_type configure(rclcpp::Node::SharedPtr node);
 
   controller_interface::return_type update(
     const std::vector<double> & current_joint_state,
     const geometry_msgs::msg::Wrench & measured_force,
     const geometry_msgs::msg::PoseStamped & target_pose,
     const geometry_msgs::msg::WrenchStamped & target_force,
-    const rclcpp::Duration & period
-  );
+    const rclcpp::Duration & period,
+    std::array<double, 6> desired_joint_states);
 
   controller_interface::return_type update(
-    const std::vector<double> & current_joint_state,
+    const std::array<double, 6> & /*current_joint_state*/,
     const geometry_msgs::msg::Wrench & measured_force,
     const geometry_msgs::msg::PoseStamped & target_pose,
-    const rclcpp::Duration & period);
+    const rclcpp::Duration & period,
+    std::array<double, 6> desired_joint_states);
 
 //   controller_interface::return_type update(
 //     const geometry_msgs::msg::WrenchStamped & measured_force,
@@ -118,6 +119,8 @@ class AdmittanceRule
   std::array<double, 6> desired_velocity_previous_vec_;
   std::array<double, 6> desired_acceleration_previous_vec_;
 
+  std::vector<double> relative_desired_joint_state_vec_;
+
 private:
   // TODO: implement doTransform for WrenchStamped
   template<typename MsgType>

admittance_controller/include/admittance_controller/incremental_ik_calculator.hpp

@@ -16,10 +16,11 @@
 
 #pragma once
 
-#include <Eigen/Core>
-#include <moveit/robot_model_loader/robot_model_loader.h>
-#include <moveit/robot_state/robot_state.h>
-#include <tf2_ros/transform_listener.h>
+#include "Eigen/Core"
+#include "geometry_msgs/msg/transform_stamped.hpp"
+#include "moveit/robot_model_loader/robot_model_loader.h"
+#include "moveit/robot_state/robot_state.h"
+#include "rclcpp/utilities.hpp"
 
 namespace admittance_controller
 {
@@ -31,16 +32,27 @@ class IncrementalIKCalculator
    * \brief Create an object which takes Cartesian delta-x and converts to joint delta-theta.
    * It uses the Jacobian from MoveIt.
    */
-  IncrementalIKCalculator(std::shared_ptr<rclcpp::Node> & node);
+  IncrementalIKCalculator(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 input Cartesian deltas
-   * \param delta_x_frame input name of the delta_x tf frame
-   * \param delta_theta output
+   * \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 delta_theta_vec output vector with joint states
    * \return true if successful
    */
-  bool convertCartesianDeltasToJointDeltas(Eigen::VectorXd delta_x, std::string& delta_x_frame, Eigen::VectorXd& delta_theta);
+  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);
+
+  /**
+   * \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[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);
 
 private:
   // MoveIt setup, required to retrieve the Jacobian
@@ -53,12 +65,6 @@ class IncrementalIKCalculator
   Eigen::JacobiSVD<Eigen::MatrixXd> svd_;
   Eigen::MatrixXd matrix_s_;
   Eigen::MatrixXd pseudo_inverse_;
-
-  // TF frames
-  std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
-  std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
-  std::string moveit_jacobian_frame_;
-  geometry_msgs::msg::TransformStamped delta_x_to_jacobian_transform_;
 };
 
 }  // namespace admittance_controller

admittance_controller/src/admittance_controller.cpp

@@ -22,6 +22,7 @@
 // #include "Eigen/Core"
 
 #include "admittance_controller/admittance_controller.hpp"
+#include "admittance_controller/incremental_ik_calculator.hpp"
 #include "controller_interface/helpers.hpp"
 #include "hardware_interface/types/hardware_interface_type_values.hpp"
 
@@ -47,9 +48,9 @@ controller_interface::return_type AdmittanceController::init(const std::string &
 
     get_node()->declare_parameter<std::string>("IK.base", "");
     get_node()->declare_parameter<std::string>("IK.tip", "");
-    // TODO(destogl): enable when IK support is added
+    // TODO(destogl): enable when IK-plugin support is added
 //     get_node()->declare_parameter<std::string>("IK.plugin", "");
-//     get_node()->declare_parameter<std::string>("IK.group_name", "");
+    get_node()->declare_parameter<std::string>("IK.group_name", "");
 
     get_node()->declare_parameter<std::string>("control_frame", "");
     get_node()->declare_parameter<std::string>("endeffector_frame", "");
@@ -97,10 +98,6 @@ controller_interface::return_type AdmittanceController::init(const std::string &
 
   admittance_ = std::make_unique<admittance_controller::AdmittanceRule>();
 
-  auto node = get_node();
-//   ik_ = std::make_shared<IncrementalIKCalculator>(node);
-//   delta_x_ = Eigen::VectorXd(6);
-
   return controller_interface::return_type::OK;
 }
 
@@ -151,8 +148,7 @@ CallbackReturn AdmittanceController::on_configure(
     get_string_param_and_error_if_empty(ft_sensor_name_, "ft_sensor_name") ||
     get_string_param_and_error_if_empty(admittance_->ik_base_frame_, "IK.base") ||
     get_string_param_and_error_if_empty(admittance_->ik_tip_frame_, "IK.tip") ||
-    // TODO: Enable this when IK plugin use is implemented
-    //     get_string_param_and_error_if_empty(admittance_->ik_group_name_, "IK.group_name") ||
+    get_string_param_and_error_if_empty(admittance_->ik_group_name_, "IK.group_name") ||
     get_string_param_and_error_if_empty(admittance_->control_frame_, "control_frame") ||
     get_string_param_and_error_if_empty(admittance_->endeffector_frame_, "endeffector_frame") ||
     get_string_param_and_error_if_empty(admittance_->fixed_world_frame_, "fixed_world_frame") ||
@@ -281,7 +277,7 @@ CallbackReturn AdmittanceController::on_configure(
   state_publisher_ = std::make_unique<ControllerStatePublisher>(s_publisher_);
 
   // Configure AdmittanceRule
-  admittance_->configure(get_node()->get_clock());
+  admittance_->configure(get_node());
 
   RCLCPP_INFO_STREAM(get_node()->get_logger(), "configure successful");
   return CallbackReturn::SUCCESS;

admittance_controller/src/admittance_rule.cpp

@@ -26,6 +26,7 @@
 #include "rclcpp/duration.hpp"
 #include "rclcpp/utilities.hpp"
 #include "tf2/utils.h"
+#include <tf2_eigen/tf2_eigen.h>
 
 namespace {  // Utility namespace
 
@@ -138,9 +139,9 @@ void convert_array_to_message(const std::array<double, 6> & vector, geometry_msg
 namespace admittance_controller
 {
 
-controller_interface::return_type AdmittanceRule::configure(rclcpp::Clock::SharedPtr clock)
+controller_interface::return_type AdmittanceRule::configure(rclcpp::Node::SharedPtr node)
 {
-  tf_buffer_ = std::make_shared<tf2_ros::Buffer>(clock);
+  tf_buffer_ = std::make_shared<tf2_ros::Buffer>(node->get_clock());
   tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);
 
   // Initialize variables used in the update loop
@@ -152,6 +153,11 @@ controller_interface::return_type AdmittanceRule::configure(rclcpp::Clock::Share
 
   relative_desired_pose_.header.frame_id = control_frame_;
 
+  relative_desired_joint_state_vec_.reserve(6);
+
+  // Initialize IK
+  ik_ = std::make_shared<IncrementalIKCalculator>(node, ik_group_name_);
+
   return controller_interface::return_type::OK;
 }
 
@@ -175,25 +181,28 @@ controller_interface::return_type AdmittanceRule::reset()
 }
 
 controller_interface::return_type AdmittanceRule::update(
-  const std::vector<double> & /*current_joint_state*/,
+  const std::array<double, 6> & /*current_joint_states*/,
   const geometry_msgs::msg::Wrench & /*measured_force*/,
   const geometry_msgs::msg::PoseStamped & /*target_pose*/,
   const geometry_msgs::msg::WrenchStamped & /*target_force*/,
-  const rclcpp::Duration & /*period*/
+  const rclcpp::Duration & /*period*/,
+  std::array<double, 6> /*desired_joint_states*/
 )
 {
   // TODO(destogl): Implement this update
   //  transform_message_to_control_frame(**input_force_cmd, force_cmd_ctrl_frame_);
-  // TODO(destogl) reuse ting from other update
+  // TODO(destogl) reuse things from other update
 
   return controller_interface::return_type::OK;
 }
 
 controller_interface::return_type AdmittanceRule::update(
-  const std::vector<double> & /*current_joint_state*/,
+  const std::array<double, 6> & current_joint_states,
   const geometry_msgs::msg::Wrench & measured_force,
   const geometry_msgs::msg::PoseStamped & target_pose,
-  const rclcpp::Duration & period)
+  const rclcpp::Duration & period,
+  std::array<double, 6> desired_joint_states
+)
 {
   // Convert inputs to control frame
   transform_message_to_control_frame(target_pose, target_pose_control_frame_);
@@ -240,22 +249,25 @@ controller_interface::return_type AdmittanceRule::update(
     }
   }
 
+  // TODO: Add here desired vector at the IK tip and not tool!!!!
+
   // Do clean conversion to the goal pose using transform and not messing with Euler angles
   convert_array_to_message(relative_desired_pose_vec_, relative_desired_pose_);
   tf2::doTransform(current_pose_control_frame_, desired_pose_, relative_desired_pose_);
 
   // Use Jacobian-based IK
   // Calculate desired Cartesian displacement of the robot
-  //delta_x_ = relative_desired_pose_;
-
-  // Get current robot joint angles
-
-  // Convert Cartesian deltas to joint angle deltas via Jacobian
-  //   if (!ik_->convertCartesianDeltasToJointDeltas(delta_x_, sensor_frame_, delta_theta_))
-  //   {
-  //     RCLCPP_ERROR(get_node()->get_logger(), "Conversion of Cartesian deltas to joint deltas failed.");
-  //     return controller_interface::return_type::ERROR;
-  //   }
+  transform = tf_buffer_->lookupTransform(ik_base_frame_, ik_tip_frame_, tf2::TimePointZero);
+  std::vector<double> relative_desired_pose_v_(relative_desired_pose_vec_.begin(), relative_desired_pose_vec_.end());
+  if (ik_->convertCartesianDeltasToJointDeltas(
+    relative_desired_pose_v_, transform, relative_desired_joint_state_vec_)){
+      for (auto i = 0u; i < desired_joint_states.size(); ++i) {
+        desired_joint_states[i] = current_joint_states[i] + relative_desired_joint_state_vec_[i];
+      }
+    } else {
+      RCLCPP_ERROR(rclcpp::get_logger("AdmittanceRule"), "Conversion of Cartesian deltas to joint deltas failed. Sending current joint values to the robot.");
+      desired_joint_states = current_joint_states;
+    }
 
   // TODO(anyone: enable this when enabling use of IK directly
   // transform = tf_buffer_->lookupTransform(endeffector_frame_, ik_base_frame_, tf2::TimePointZero);
@@ -276,8 +288,8 @@ controller_interface::return_type AdmittanceRule::get_controller_state(
   state_message.measured_force_endeffector_frame = measured_force_control_frame_;
   state_message.desired_pose = desired_pose_;
   state_message.relative_desired_pose = relative_desired_pose_;
-  //   state_message.desired_joint_states = desired_pose_;
-  //   state_message.actual_joint_states = desired_pose_;
+//   state_message.desired_joint_states = desired_pose_;
+//   state_message.actual_joint_states.positi = desired_pose_;
   //   state_message.error_joint_state = desired_pose_;
 
   return controller_interface::return_type::OK;

admittance_controller/src/incremental_ik_calculator.cpp

@@ -16,35 +16,34 @@
 
 #include "admittance_controller/incremental_ik_calculator.hpp"
 
+#include "Eigen/VectorXf"
 #include "tf2_eigen/tf2_eigen.h"
 
 namespace admittance_controller
 {
-IncrementalIKCalculator::IncrementalIKCalculator(std::shared_ptr<rclcpp::Node>& node) : node_(node)
+IncrementalIKCalculator::IncrementalIKCalculator(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 =
       std::unique_ptr<robot_model_loader::RobotModelLoader>(new robot_model_loader::RobotModelLoader(node_, "/robot_description", false /* do not load kinematics plugins */));
   const moveit::core::RobotModelPtr& kinematic_model = model_loader_ptr->getModel();
   // TODO(andyz): joint_model_group_ is a raw pointer. Is it thread safe?
-  joint_model_group_ = kinematic_model->getJointModelGroup("gemini");
+  joint_model_group_ = kinematic_model->getJointModelGroup(group_name);
   kinematic_state_ = std::make_shared<moveit::core::RobotState>(kinematic_model);
 
   // By default, the MoveIt Jacobian frame is the last link
-  // TODO(andyz): parameterize this or retrieve it via API
-  moveit_jacobian_frame_ = "end_effector";
-  tf_buffer_ = std::make_shared<tf2_ros::Buffer>(node_->get_clock());
-  tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);
 }
 
-bool IncrementalIKCalculator::convertCartesianDeltasToJointDeltas(Eigen::VectorXd delta_x, std::string& delta_x_frame, Eigen::VectorXd& delta_theta)
+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)
 {
+  // 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());
+
   // Transform delta_x to the moveit_jacobian_frame
   try
   {
     // 4x4 transformation matrix
-    delta_x_to_jacobian_transform_ = tf_buffer_->lookupTransform(delta_x_frame, moveit_jacobian_frame_, tf2::TimePointZero);
-    Eigen::Isometry3d affine_transform = tf2::transformToEigen(delta_x_to_jacobian_transform_);
+    const Eigen::Isometry3d affine_transform = tf2::transformToEigen(ik_base_to_tip_trafo);
 
     // Build the 6x6 transformation matrix
     Eigen::MatrixXd twist_transform(6,6);
@@ -74,8 +73,17 @@ bool IncrementalIKCalculator::convertCartesianDeltasToJointDeltas(Eigen::VectorX
   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();
-  delta_theta = pseudo_inverse_ * delta_x;
+  Eigen::VectorXf  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)
+{
+  // TODO(andyz): Please add here FK implementation
+}
+
 }  // namespace admittance_controller

admittance_controller/test/test_admittance_controller.cpp

@@ -58,10 +58,10 @@ INSTANTIATE_TEST_CASE_P(
       std::string("IK.tip"),
       rclcpp::ParameterValue("")
     ),
-//     std::make_tuple(
-//       std::string("IK.group_name"),
-//       rclcpp::ParameterValue("")
-//     ),
+    std::make_tuple(
+      std::string("IK.group_name"),
+      rclcpp::ParameterValue("")
+    ),
     std::make_tuple(
       std::string("control_frame"),
       rclcpp::ParameterValue("")

admittance_controller/test/test_admittance_controller.hpp

@@ -28,6 +28,7 @@
 #include "hardware_interface/loaned_state_interface.hpp"
 #include "hardware_interface/loaned_command_interface.hpp"
 #include "hardware_interface/types/hardware_interface_return_values.hpp"
+#include "ros2_control_test_assets/6d_robot_description.hpp"
 #include "semantic_components/force_torque_sensor.hpp"
 #include "rclcpp/parameter_value.hpp"
 #include "rclcpp/utilities.hpp"
@@ -155,7 +156,8 @@ class AdmittanceControllerTest : public ::testing::Test
       controller_->get_node()->set_parameter({"IK.tip", ik_tip_frame_});
       // TODO(destogl): enable when IK support is added
 //       controller_->get_node()->set_parameter({"IK.plugin", ik_group_name_});
-//       controller_->get_node()->set_parameter({"IK.group_name", ik_group_name_});
+      controller_->get_node()->set_parameter({"IK.group_name", ik_group_name_});
+      controller_->get_node()->set_parameter({"robot_description", robot_description_});
 
       controller_->get_node()->set_parameter({"control_frame", control_frame_});
       controller_->get_node()->set_parameter({"endeffector_frame", endeffector_frame_});
@@ -330,9 +332,12 @@ class AdmittanceControllerTest : public ::testing::Test
   const std::vector<std::string> command_interface_types_ = {"position"};
   const std::vector<std::string> state_interface_types_ = {"position"};
   const std::string ft_sensor_name_ = "ft_sensor_name";
+
   const std::string ik_base_frame_ = "IK_base";
   const std::string ik_tip_frame_ = "IK.tip";
   const std::string ik_group_name_ = "IK.group_name";
+  const std::string robot_description_ = ros2_control_test_assets::6d_robot_urdf;
+
   const std::string control_frame_ = "control_frame";
   const std::string endeffector_frame_ = "endeffector_frame";
   const std::string fixed_world_frame_ = "fixed_world_frame";