KDL based Movement in Cartesian Space

lbr_demos/lbr_demos_fri_ros2_cpp/CMakeLists.txt

@@ -15,6 +15,51 @@ find_package(fri_vendor REQUIRED)
 find_package(FRIClient REQUIRED)
 find_package(lbr_fri_msgs REQUIRED)
 find_package(rclcpp REQUIRED)
+find_package(tf2_ros REQUIRED)
+find_package(sensor_msgs REQUIRED)
+find_package(urdf REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(orocos_kdl_vendor REQUIRED)
+find_package(kdl_parser REQUIRED)
+
+# cartesian_pose_node
+add_executable(cartesian_pose_node
+  src/cartesian_pose_node.cpp
+)
+
+ament_target_dependencies(cartesian_pose_node
+  fri_vendor
+  lbr_fri_msgs
+  rclcpp
+  tf2_ros
+  urdf
+  sensor_msgs
+  orocos_kdl_vendor
+  kdl_parser
+  geometry_msgs
+)
+
+target_link_libraries(cartesian_pose_node
+  FRIClient::FRIClient
+)
+
+# cartesian_path_planning_node
+add_executable(cartesian_path_planning_node
+  src/cartesian_path_planning_node.cpp
+)
+
+ament_target_dependencies(cartesian_path_planning_node
+  fri_vendor
+  lbr_fri_msgs
+  rclcpp
+  tf2_ros
+  sensor_msgs
+  geometry_msgs
+)
+
+target_link_libraries(cartesian_path_planning_node
+  FRIClient::FRIClient
+)
 
 # joint sine overlay
 add_executable(joint_sine_overlay_node
@@ -62,6 +107,8 @@ target_link_libraries(wrench_sine_overlay_node
 )
 
 install(TARGETS
+  cartesian_pose_node
+  cartesian_path_planning_node
   joint_sine_overlay_node
   torque_sine_overlay_node
   wrench_sine_overlay_node

lbr_demos/lbr_demos_fri_ros2_cpp/package.xml

@@ -12,6 +12,12 @@
   <depend>fri_vendor</depend>
   <depend>lbr_fri_msgs</depend>
   <depend>rclcpp</depend>
+  <depend>tf2_ros</depend>
+  <depend>urdf</depend>
+  <depend>geometry_msgs</depend>
+  <depend>sensor_msgs</depend>
+  <depend>orocos_kdl_vendor</depend>
+  <depend>kdl_parser</depend>
 
   <exec_depend>lbr_fri_ros2</exec_depend>
 
@@ -21,4 +27,4 @@
   <export>
     <build_type>ament_cmake</build_type>
   </export>
-</package>
+</package>

lbr_demos/lbr_demos_fri_ros2_cpp/src/cartesian_path_planning_node.cpp

@@ -0,0 +1,74 @@
+#include "rclcpp/rclcpp.hpp"
+#include "cmath"
+#include <iostream>
+#include <string>
+#include "geometry_msgs/msg/pose.hpp"
+
+using std::placeholders::_1;
+
+class CartesianPosePublisherNode:public rclcpp::Node
+{
+  private:
+    rclcpp::Publisher<geometry_msgs::msg::Pose>::SharedPtr cartesian_pose_publisher_;
+    rclcpp::Subscription<geometry_msgs::msg::Pose>::SharedPtr cartesian_pose_subscriber_;
+
+    geometry_msgs::msg::Pose initial_cartesian_pose_; // robot starts from this pose
+    bool is_init_; 
+
+    double amplitude_; // rad
+    double frequency_; // Hz
+    double sampling_time_; // sampling time for sending position command
+    double phase_; // initial phase
+
+
+  private:
+    /**
+     * @function: callback function for Cartesian Pose Subscriber
+     * @param msg Cartesian Pose of the robot
+    */
+    void topic_callback(const geometry_msgs::msg::Pose& msg)
+    {            
+      if(!is_init_) 
+      {
+        initial_cartesian_pose_ = msg;
+        is_init_ = true;
+      }
+      else
+      {
+        geometry_msgs::msg::Pose cartesian_pose_command = initial_cartesian_pose_;
+
+        phase_ = phase_ + 2 * M_PI * frequency_ * sampling_time_;
+        cartesian_pose_command.position.z += amplitude_ * sin(phase_);
+
+        cartesian_pose_publisher_->publish(cartesian_pose_command);
+      }
+
+      return;
+    }
+
+  public:
+    CartesianPosePublisherNode():Node("cartesian_pose_publisher_node")
+    {
+      is_init_ = false;
+      amplitude_ = 0.05;
+      frequency_ = 0.5;
+      sampling_time_ = 0.01;
+      phase_ = 0.0;
+
+      cartesian_pose_publisher_ = this->create_publisher<geometry_msgs::msg::Pose>(
+          "/lbr/command/cartesian_pose", 10);
+      cartesian_pose_subscriber_ = this->create_subscription<geometry_msgs::msg::Pose>(
+          "/lbr/state/cartesian_pose", 10, 
+          std::bind(&CartesianPosePublisherNode::topic_callback, this, _1));
+    }
+};
+
+int main(int argc, char** argv)
+{
+  rclcpp::init(argc, argv);
+
+  rclcpp::spin(std::make_shared<CartesianPosePublisherNode>()); 
+
+  rclcpp::shutdown(); 
+  return 0;
+}

lbr_demos/lbr_demos_fri_ros2_cpp/src/cartesian_pose_node.cpp

@@ -0,0 +1,225 @@
+#include "rclcpp/rclcpp.hpp"
+#include <iostream>
+#include <fstream>
+#include <sstream>
+#include <string>
+#include "geometry_msgs/msg/pose.hpp" // for describing Cartesian Pose
+#include "kdl_parser/kdl_parser.hpp"
+#include "kdl/chainfksolverpos_recursive.hpp" // for forward kinematics
+#include "kdl/chainiksolverpos_lma.hpp" // for inverse kinematics
+#include "lbr_fri_msgs/msg/lbr_position_command.hpp"
+#include "lbr_fri_msgs/msg/lbr_state.hpp"
+#include "friClientIf.h"
+
+using std::placeholders::_1;
+using namespace std::chrono_literals;
+
+class CartesianPoseNode:public rclcpp::Node
+{
+  private:
+    rclcpp::Publisher<lbr_fri_msgs::msg::LBRPositionCommand>::SharedPtr joint_position_publisher_;
+    rclcpp::Subscription<lbr_fri_msgs::msg::LBRState>::SharedPtr joint_position_subscriber_;
+    rclcpp::Publisher<geometry_msgs::msg::Pose>::SharedPtr cartesian_pose_publisher_;
+    rclcpp::Subscription<geometry_msgs::msg::Pose>::SharedPtr cartesian_pose_subscriber_;
+
+    rclcpp::SyncParametersClient::SharedPtr parameter_client_;
+
+    lbr_fri_msgs::msg::LBRState current_robot_state_; // robot state, including joint positions
+    geometry_msgs::msg::Pose current_cartesian_position_; // current cartesian pose of robot
+
+    KDL::Chain chain_; // robot kinematics chain exetracted from robot URDF file
+
+  private:
+    /**
+     * @function: callback function for Joint Position Subscriber
+     * @param msg joint position of the robot
+    */
+    void joint_position_sub_callback(const lbr_fri_msgs::msg::LBRState& msg)
+    {
+      current_robot_state_ = msg;
+
+      double joint_position[7];
+      for(int i = 0; i < 7; i++)
+      {
+        joint_position[i] = current_robot_state_.measured_joint_position[i]; 
+      }
+      current_cartesian_position_ = computeForwardKinematics(joint_position);
+      cartesian_pose_publisher_->publish(current_cartesian_position_);
+
+      return;
+    }
+
+    /**
+     * @function: callback function for Cartesian Pose Subscriber
+     * @param msg cartesian pose command
+    */
+    void cartesian_pose_sub_callback(const geometry_msgs::msg::Pose& msg)
+    {
+      if(current_robot_state_.session_state == KUKA::FRI::COMMANDING_ACTIVE) 
+      {
+        unsigned int joint_number = chain_.getNrOfJoints(); // for kuka robot, 7 joints 
+        KDL::JntArray current_joint_positions(joint_number);
+        lbr_fri_msgs::msg::LBRPositionCommand joint_position_command;
+
+        for(unsigned int i = 0; i < joint_number; i++)
+        {
+          current_joint_positions(i) = current_robot_state_.measured_joint_position[i];
+        }
+
+        joint_position_command = computeInverseKinematics(msg, current_joint_positions);
+        joint_position_publisher_->publish(joint_position_command);
+      }
+
+      return;
+    }
+
+  public:
+    CartesianPoseNode():Node("cartesian_pose_node")
+    {
+      /* "/lbr/app" is the name of Parameter Server node, in which robot URDF file stores.
+         The node whose name is "/lbr/app" appears after we run the command
+	 (ros2 launch lbr_fri_ros2 app.launch.py) in the terminal.
+      */
+      parameter_client_ = std::make_shared<rclcpp::SyncParametersClient>(this, "/lbr/app"); 
+
+      while(!parameter_client_->wait_for_service(1s))
+      {
+        if(!rclcpp::ok())
+        {
+          return;
+        }
+        RCLCPP_INFO(this->get_logger(), "this node has not connected with Parameter Server Node.");
+      }
+      std::string robot_description_string = parameter_client_->get_parameter<std::string>("robot_description");
+
+      KDL::Tree robot_tree;
+      if(!kdl_parser::treeFromString(robot_description_string, robot_tree))
+      {
+        std::cout << "Failed to construct kdl tree." << std::endl;
+      }
+
+      std::string root_link = "link_0"; // adjust with your URDF‘s root link
+      std::string tip_link = "link_ee"; // adjust with your URDF‘s tip link
+      if(!robot_tree.getChain(root_link, tip_link, chain_))
+      {
+        std::cerr << "Failed to get chain from tree." << std::endl;
+      }
+      else
+      {
+        std::cout << "Get chain from tree successfully." << std::endl;
+      }
+
+      joint_position_publisher_ = this->create_publisher<lbr_fri_msgs::msg::LBRPositionCommand>(
+          "/lbr/command/joint_position", 10);
+      joint_position_subscriber_ = this->create_subscription<lbr_fri_msgs::msg::LBRState>(
+          "/lbr/state", 10, 
+          std::bind(&CartesianPoseNode::joint_position_sub_callback, this, _1));
+      cartesian_pose_publisher_ = this->create_publisher<geometry_msgs::msg::Pose>(
+          "/lbr/state/cartesian_pose", 10);
+      cartesian_pose_subscriber_ = this->create_subscription<geometry_msgs::msg::Pose>(
+          "/lbr/command/cartesian_pose", 10, 
+          std::bind(&CartesianPoseNode::cartesian_pose_sub_callback, this, _1));
+    }
+
+    /**
+     * @function: calculate forward kinematics of robot
+     * @param position_array_ptr store seven joint positions of robot 
+     * @return cartesian pose of the robot
+    */
+    geometry_msgs::msg::Pose computeForwardKinematics(double* position_array_ptr)
+    {
+      KDL::ChainFkSolverPos_recursive fk_solver = KDL::ChainFkSolverPos_recursive(chain_);
+
+      unsigned int joint_number = chain_.getNrOfJoints();
+      KDL::JntArray joint_positions = KDL::JntArray(joint_number);
+
+      for(unsigned int i = 0; i < joint_number; i++)
+      {
+        joint_positions(i) = position_array_ptr[i];
+      }
+
+      KDL::Frame cartesian_pose_temp; // Cartesian Pose described in data type KDL::Frame
+      geometry_msgs::msg::Pose cartesian_pose; // described in geometry_msgs::msg::Pose
+
+      if(fk_solver.JntToCart(joint_positions, cartesian_pose_temp) < 0)
+      {
+        std::cerr << "FK Solver to calculate JointToCartesian failed." << std::endl;
+      }
+      else
+      {
+        // Position
+        cartesian_pose.position.x = cartesian_pose_temp.p.x();
+        cartesian_pose.position.y = cartesian_pose_temp.p.y();
+        cartesian_pose.position.z = cartesian_pose_temp.p.z();
+
+        // Orientation
+        double x, y, z, w;
+        cartesian_pose_temp.M.GetQuaternion(x, y, z, w); // get quaternion
+        cartesian_pose.orientation.x = x;
+        cartesian_pose.orientation.y = y;
+        cartesian_pose.orientation.z = z;
+        cartesian_pose.orientation.w = w;
+      }
+
+      return cartesian_pose;
+    }
+
+    /**
+     * @function: calculate inverse kinematics of robot
+     * @param desired_cartesian_pose target cartesian pose we want to transform to joint space
+     * @param current_joint_positions current joint positions
+     * @return joint positions command 
+    */
+    lbr_fri_msgs::msg::LBRPositionCommand computeInverseKinematics(
+        const geometry_msgs::msg::Pose& desired_cartesian_pose, 
+        KDL::JntArray& current_joint_positions)
+    {
+      KDL::ChainIkSolverPos_LMA ik_solver(chain_);
+      KDL::JntArray result_joint_positions = KDL::JntArray(chain_.getNrOfJoints());
+      lbr_fri_msgs::msg::LBRPositionCommand joint_position_command;
+
+      // transfer data type 'geometry::msg::Pose' to be 'KDL::Frame'
+      KDL::Vector position(desired_cartesian_pose.position.x, 
+                           desired_cartesian_pose.position.y, 
+                           desired_cartesian_pose.position.z); 
+      KDL::Rotation rotation =KDL::Rotation::Quaternion(desired_cartesian_pose.orientation.x,
+                                                        desired_cartesian_pose.orientation.y,
+                                                        desired_cartesian_pose.orientation.z,
+                                                        desired_cartesian_pose.orientation.w);
+      KDL::Frame desired_cartesian_pose_temp(rotation, position); 
+
+      //auto start = std::chrono::high_resolution_clock::now();
+      int ik_result = ik_solver.CartToJnt(current_joint_positions, 
+                                          desired_cartesian_pose_temp, 
+                                          result_joint_positions); 
+      //auto end = std::chrono::high_resolution_clock::now();
+      //std::chrono::duration<double, std::milli> execution_time = end - start;
+      //std::cout << "IK solver execution time: "<< execution_time.count()<< "ms"<<std::endl;
+
+      if(ik_result < 0) // if solving failed, 'ik_result' would be less than 0
+      {
+        std::cerr << "Inverse kinematics failed." << std::endl;
+      }
+      else 
+      {
+        //std::cout << "Inverse kinematics solution:" << std::endl;
+        for(unsigned int i = 0; i < result_joint_positions.data.size(); i++) 
+        {
+          joint_position_command.joint_position[i] = result_joint_positions(i);
+          //std::cout << "Joint " << i << ": "<<result_joint_positions(i) << std::endl;
+        }
+      }
+
+      return joint_position_command;
+    }
+};
+
+int main(int argc, char** argv)
+{
+  rclcpp::init(argc, argv);
+
+  rclcpp::spin(std::make_shared<CartesianPoseNode>()); 
+
+  rclcpp::shutdown(); 
+  return 0;
+}