Cartesian control

[vopsi99]

Hello, I tried using this repository but my knowledge in ros is rather mediocre. I am curious if this stack has cartesian control implementation or not.

It may be a stupid question, but I am stuck for quite some time and I could really use some help..

[mhubii]

no worries @vopsi99 and thank you for raising this issue. In principle yes, in practise users have to put in some effort to do this right now. I can pinpoint you to some examples and try to improve the demos.

What are you trying to achieve:

• Move to an absolute pose?
• Specify an end-effector velocity?

[vopsi99]

I managed to move the end-effector incrementally but it is really unstable, so I need to move to an absolute pose with a certain velocity. I supposed this kind of motion would be the best to further integrate a stereo camera to position the robot in correlation to some markers in the workspace.

[mhubii]

so there is a demo here: https://lbr-stack.readthedocs.io/en/latest/lbr_fri_ros2_stack/lbr_demos/lbr_demos_advanced_cpp/doc/lbr_demos_advanced_cpp.html#pose-controller

You'd have to add a node that sends your desired pose. This can be done in Python, similar to this e.g. https://github.com/lbr-stack/lbr_fri_ros2_stack/blob/humble/lbr_demos/lbr_demos_advanced_cpp/src/pose_planning_node.cpp

[vopsi99]

So I should only send from another node my desired pose to this demo? This is the demo that I modified to send incremental values for x,y,z on position and orientation, but on Y orientation, for example, it always went crazy even if I was doing really small movement increments.

[mhubii]

okay I'll have to double check this demo, as it was community contributed. But this demo sends an absolute pose.

You will have to make sure you:

• Get the initial pose
• Send a small (like really small) increment on top of the initial pose

[mhubii]

I think this demo needs an update and should really allow users to control a velocity rather than absolute poses maybe

[vopsi99]

Yes, I realised that a really small increment (0.001) would make the movement smoother.For positioning on x,y and z everything seems fine, but when I am trying to orient it, it just breaks everything.

After sending the increment on Y for longer it tends to jump into a position and i get the inverse kinematics failed error.

I think velocity control would be really useful for everyone.

[mhubii]

thank you for the feedback. Let's add support for this then.

[BoWangFromMars]

Hello, I am back. I am the contributor for "pose control". The initial pose and position in this demo is singular to some extent, so that it can only support the movement in z axis. However, if we want to orient it, the inverse kinematics would be calculated in a wrong way.
The solution is setting a non-singular pose and position as the initial one. For example, I set the initial joint position as
ros2 topic pub --once /forward_position_controller/commands std_msgs/msg/Float64MultiArray "{data: [2.175727, 0.928842, -0.975961, -1.3158, 0.3883, 1.2622, 0.01274]}"
And I use the following code to orient it, please take it as reference code (interpolation of quaternion, rotating the end effector around its x-axis, y-axis, z-axis)

#include "rclcpp/rclcpp.hpp"
#include "cmath"
#include <iostream>
#include <string>
#include "geometry_msgs/msg/pose.hpp"

#include "tf2/transform_datatypes.h"
#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
#include "tf2/convert.h"
#include "tf2/LinearMath/Quaternion.h"
#include "tf2/LinearMath/Matrix3x3.h"

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
    geometry_msgs::msg::Pose previous_cmd_pose_;

    geometry_msgs::msg::Pose target_pose; // target quanternion (target pose)
    geometry_msgs::msg::Pose cartesian_pose_command; // cartesian pose command which is published to other ROS topics
    double t = 0.0;
        
  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;
        cartesian_pose_command = initial_cartesian_pose_;
        target_pose = initial_cartesian_pose_;

        rotating(target_pose, initial_cartesian_pose_, 'y', 10.0);

        std::cout << "target pose calculated by function rotating: " << std::endl;
        std::cout << "target_pose.orientation.x " << target_pose.orientation.x << std::endl;
        std::cout << "target_pose.orientation.y " << target_pose.orientation.y << std::endl;
        std::cout << "target_pose.orientation.z " << target_pose.orientation.z << std::endl;
        std::cout << "target_pose.orientation.w " << target_pose.orientation.w << std::endl;

        /* set "initial joints" of KUKA iiwa robot as "2.175727, 0.928842, -0.975961, -1.3158, 0.3883, 1.2622, 0.01274"(the unit is rad),
        and the following quanternion corresponds to the target pose of the end effector, expressed in the "Base" Frame of the robot.
        In addition, the target position of the end effector is as same as the initial position, which can be calculated from "initial joints".
        */

        // make the initial pose matrix of end effector rotate its x-axis for 5 degrees
        /*
        target_quanternion.orientation.w = 0.1350; 
        target_quanternion.orientation.x = -0.5842;
        target_quanternion.orientation.y = 0.7808;
        target_quanternion.orientation.z = -0.1756;
        */

        // make the initial pose matrix of end effector rotate its y-axis for 5 degrees
        /*
        target_quanternion.orientation.w = 0.0749; 
        target_quanternion.orientation.x = -0.5828;
        target_quanternion.orientation.y = 0.7917;
        target_quanternion.orientation.z = -0.1670; 
        */

        // make the initial pose matrix of end effector rotate its z-axis for 5 degrees
        /*
        target_pose.orientation.w = 0.1154; 
        target_pose.orientation.x = -0.5546;
        target_pose.orientation.y = 0.8127;
        target_pose.orientation.z = -0.1365; 
        */

        /*
        std::cout << "initial_pose:" << std::endl;
        std::cout << "position x: " << initial_cartesian_pose_.position.x;
        std::cout << " y: " << initial_cartesian_pose_.position.y;
        std::cout << " z: " << initial_cartesian_pose_.position.z;
        std::cout << std::endl;
        std::cout << "quternion w: " << initial_cartesian_pose_.orientation.w;
        std::cout << " x: " << initial_cartesian_pose_.orientation.x;
        std::cout << " y: " << initial_cartesian_pose_.orientation.y;
        std::cout << " z: " << initial_cartesian_pose_.orientation.z << std::endl;
        */

        is_init_ = true;
      }
      else
      {        
        if(t < 1)
        {
          /* Interpolation of quanternion, which follows the equation "q = [(1-t)*q_1 + t*q_2]/|(1-t)*q_1 + t*q_2|, 
          where q_1 is the initial quanternion corresponding to initial pose, and q_2 is the target quanternion corresponding to target pose.
          */
          cartesian_pose_command.orientation.w = (1-t)*initial_cartesian_pose_.orientation.w + t*target_pose.orientation.w;
          cartesian_pose_command.orientation.x = (1-t)*initial_cartesian_pose_.orientation.x + t*target_pose.orientation.x;
          cartesian_pose_command.orientation.y = (1-t)*initial_cartesian_pose_.orientation.y + t*target_pose.orientation.y;
          cartesian_pose_command.orientation.z = (1-t)*initial_cartesian_pose_.orientation.z + t*target_pose.orientation.z;
          double norm =  std::pow(cartesian_pose_command.orientation.w, 2) + std::pow(cartesian_pose_command.orientation.x, 2) + std::pow(cartesian_pose_command.orientation.y, 2) + std::pow(cartesian_pose_command.orientation.z, 2);
          norm = sqrt(norm);
          cartesian_pose_command.orientation.w = cartesian_pose_command.orientation.w / norm;
          cartesian_pose_command.orientation.x = cartesian_pose_command.orientation.x / norm;
          cartesian_pose_command.orientation.y = cartesian_pose_command.orientation.y / norm;
          cartesian_pose_command.orientation.z = cartesian_pose_command.orientation.z / norm;
          t = t + 0.001*2; // here, the time step is 0.001*2, so the total steps are 1.0/(0.001*2)=500. The control frequency is 100 Hz, so execution time is 5 seconds
          //std::cout << "Cartesian Pose Command: " << std::endl;
          //std::cout << cartesian_pose_command.orientation.w << " " << cartesian_pose_command.orientation.x << " " << cartesian_pose_command.orientation.y << " " << cartesian_pose_command.orientation.z << std::endl;
          cartesian_pose_publisher_->publish(cartesian_pose_command);
        }

        //cartesian_pose_publisher_->publish(cartesian_pose_command);

        previous_cmd_pose_ = cartesian_pose_command;
      }

      return;
    }

  public:
    /**
     * @function: rotate the current end effector pose to the desired end effector pose
     * @param target_pose used to pass the target pose
     * @param current_cartesian_pose current end effector pose, expressed as a quanternion
     * @param axis specify which axis we make the end effector rotate around, we can choose 'x', 'y', 'z'  
     * @param rotation_angle how much the angle we need to rotate, whose unit is [degree], not [rad]     
    */
    void rotating(geometry_msgs::msg::Pose& target_pose, geometry_msgs::msg::Pose current_cartesian_pose, char axis, double rotation_angle_deg)
    {
      // extract the current quaternion from the variable "current_cartesian_pose"
      tf2::Quaternion q_current(
        current_cartesian_pose.orientation.x,
        current_cartesian_pose.orientation.y,
        current_cartesian_pose.orientation.z,
        current_cartesian_pose.orientation.w
      );

      // convert 'degree' to 'rad'
      double rotation_angle_rad = rotation_angle_deg * M_PI / 180.0;

      // create the quaternion which rotates around z axis
      tf2::Quaternion q_rotation;
      if(axis == 'x')
      {
        q_rotation.setRPY(rotation_angle_rad, 0, 0);
      }
      else if(axis == 'y')
      {
        q_rotation.setRPY(0, rotation_angle_rad, 0);
      }
      else if(axis == 'z')
      {
        q_rotation.setRPY(0, 0, rotation_angle_rad);
      }
      else
      {
        std::cout << "wrong input, please input 'x', 'y' or 'z' for specifying the axis" << std::endl;
      }

      // make the current pose of end effector rotate its z axis for 5 degrees
      tf2::Quaternion q_target = q_current * q_rotation; 

      target_pose.orientation.x = q_target.x();
      target_pose.orientation.y = q_target.y();
      target_pose.orientation.z = q_target.z();
      target_pose.orientation.w = q_target.w();

      return;
    }

  public:
	  CartesianPosePublisherNode():Node("cartesian_pose_publisher_node")
	  {
      is_init_ = false;
      amplitude_ = 0.05;
      frequency_ = 0.5;
      sampling_time_ = 0.01;
      phase_ = 0.0;

      // the following ros topics for gazebo simulation 
	    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;
}

Another suggestion is, when you are not sure whether the position and pose is a singular one, please run it in Gazebo or other simulation environment first instead of real robot and print the inverse kinematics solution in "pose control node". Comparing it with the current joint position, it is wrong when these two values differ a lot. Hope it is helpful and please contact me if any other problems. Thank you.

[TalentLeshen-DLUT]

I checked all answers, but I didn't find any information related to velocity control (maybe I missed something). Could you please give me a demo of how to achieve the velocity control of the end joint of the robotic arm?

[mhubii]

hi @TalentLeshen-DLUT. Currently the quickest means of controlling the twist is to run

ros2 launch lbr_bringup hardware.launch.py \
    ctrl:=twist_controller

this creates a topic under /lbr/command/twist that will allow you to control the end-effector velocity. You can find sample configurations here:

lbr_fri_ros2_stack/lbr_description/ros2_control/lbr_controllers.yaml

Line 112 in b615b9c

/**/twist_controller:

[TalentLeshen-DLUT]

@mhubii Maybe I'm too stupid! I tried running twist_controller and sent linear_msgs and angular_msgs to /lbr/command/twist_controller, which you'll see in the code below. I found that the movements of the robotic arms were not the same as I expected, they were about 10 times different (10 times different in the y direction, with a small error in the xz plane).

#include "rclcpp/rclcpp.hpp"
#include "geometry_msgs/msg/twist.hpp"
#include <cmath>

class SpiralMotionPublisher : public rclcpp::Node
{
public:
    SpiralMotionPublisher()
        : Node("spiral_motion_publisher"), 
        r_(0.012),           // Spiral radius (m)
        v_xz_(0.004),        // Linear velocity in the plane (m/s)
        h_(-0.02),          // Descending distance per revolution (m)
        time_step_(0.01),    // Time step (s)
        t_(0.0),             // Time counter (s)
        H_(0.1)             // Total descending distance (m)
    {
        // Calculate parameters
        double circumference = 2 * M_PI * r_;         // Circumference of the circle (m)
        T_ = circumference / v_xz_;                  // Time per revolution (s)
        omega_ = 2 * M_PI / T_;                       // Angular velocity (rad/s)
        vy_ = h_ / T_;                                // Vertical velocity in y direction (m/s)
        max_time_ = H_ * T_ / std::abs(h_);           // Maximum runtime (s)

        // Create publisher
        publisher_ = this->create_publisher<geometry_msgs::msg::Twist>("/lbr/command/twist", 10);

        // Create timer, callback is executed every 10 ms
        timer_ = this->create_wall_timer(
            std::chrono::milliseconds(static_cast<int>(time_step_ * 1000)),
            std::bind(&SpiralMotionPublisher::publishTwist, this));
    }

private:
    void publishTwist()
    {
        // Check if the maximum runtime is exceeded
        if (t_ >= max_time_)
        {
            RCLCPP_INFO(this->get_logger(), "Reached maximum runtime. Shutting down.");
            rclcpp::shutdown(); // Terminate program
            return;
        }

        // Create Twist message
        auto twist_msg = geometry_msgs::msg::Twist();

        // Calculate spiral linear velocity components
        twist_msg.linear.x = -r_ * omega_ * std::sin(omega_ * t_);  // Linear velocity in x direction
        twist_msg.linear.z = r_ * omega_ * std::cos(omega_ * t_);   // Linear velocity in z direction
        twist_msg.linear.y = vy_;                                  // Linear velocity in y direction

        twist_msg.angular.x = 0.0;  // Acceleration in x direction
        twist_msg.angular.z = 0.0;  // Acceleration in z direction
        twist_msg.angular.y = 0.0;  // Acceleration in y direction

        // Publish message
        publisher_->publish(twist_msg);

        // Update time
        t_ += time_step_;
    }

    rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr publisher_;  // Publisher
    rclcpp::TimerBase::SharedPtr timer_;  // Timer

    // Spiral parameters
    double r_;         // Spiral radius
    double v_xz_;      // Linear velocity in the plane
    double h_;         // Descending distance per revolution
    double vy_;        // Vertical velocity in y direction
    double T_;         // Time per revolution
    double omega_;     // Angular velocity
    double time_step_; // Time step
    double t_;         // Current time
    double H_;         // Total descending distance
    double max_time_;  // Maximum runtime
};

int main(int argc, char *argv[])
{
    // Initialize ROS2
    rclcpp::init(argc, argv);

    // Create node
    auto node = std::make_shared<SpiralMotionPublisher>();

    // Run node
    rclcpp::spin(node);

    // Shutdown ROS2
    rclcpp::shutdown();
    return 0;
}

Next, I tried to make the arm go in a straight line. You can see the code below. I wanted the arm to move 100mm in the y direction, but in fact it only moved about 10mm.

#include "rclcpp/rclcpp.hpp"
#include "geometry_msgs/msg/twist.hpp"

class ConstantMotionPublisher : public rclcpp::Node
{
public:
    ConstantMotionPublisher()
        : Node("constant_motion_publisher"), 
        time_step_(0.01),    // Time step (s)
        t_(0.0),             // Current time
        max_time_(10.0)      // Maximum runtime (10 seconds)
    {
        // Create publisher
        publisher_ = this->create_publisher<geometry_msgs::msg::Twist>("/lbr/command/twist", 10);

        // Create timer, callback is executed every 10 ms
        timer_ = this->create_wall_timer(
            std::chrono::milliseconds(static_cast<int>(time_step_ * 1000)),
            std::bind(&ConstantMotionPublisher::publishTwist, this));
    }

private:
    void publishTwist()
    {
        // Check if maximum runtime is exceeded
        if (t_ >= max_time_)
        {
            RCLCPP_INFO(this->get_logger(), "Reached maximum runtime. Shutting down.");
            rclcpp::shutdown(); // Terminate the program
            return;
        }

        // Create Twist message
        auto twist_msg = geometry_msgs::msg::Twist();

        // Set velocity
        twist_msg.linear.x = 0.0;  // Linear velocity in x direction (0 m/s)
        twist_msg.linear.y = 0.01; // Linear velocity in y direction (10 mm/s or 0.01 m/s)
        twist_msg.linear.z = 0.0;  // Linear velocity in z direction (0 m/s)

        twist_msg.angular.x = 0.0;  // Acceleration in x direction
        twist_msg.angular.y = 0.0;  // Acceleration in y direction
        twist_msg.angular.z = 0.0;  // Acceleration in z direction

        // Publish message
        publisher_->publish(twist_msg);

        // Update time
        t_ += time_step_;
    }

    rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr publisher_;  // Publisher
    rclcpp::TimerBase::SharedPtr timer_;  // Timer

    double time_step_; // Time step
    double t_;         // Current time
    double max_time_;  // Maximum runtime
};

int main(int argc, char *argv[])
{
    // Initialize ROS2
    rclcpp::init(argc, argv);

    // Create node
    auto node = std::make_shared<ConstantMotionPublisher>();

    // Run node
    rclcpp::spin(node);

    // Shutdown ROS2
    rclcpp::shutdown();
    return 0;
}

I guess this error has something to do with the fact that I didn't set the acceleration, because when I set the acceleration of y to 1, the arm started to move sideways at a fairly fast speed, but this movement was obviously incorrect. : (

[mhubii]

everything is fine on your end @TalentLeshen-DLUT (please bear in mind twist_msg.angular is not an acceleration but the angular velocity) . This is because there is an exponential smooth on the command:

lbr_fri_ros2_stack/lbr_fri_ros2/src/interfaces/position_command.cpp

Line 47 in b615b9c

joint_position_filter_.compute(command_target_.joint_position, command_.joint_position);

Configurable here:

lbr_fri_ros2_stack/lbr_description/ros2_control/lbr_system_config.yaml

Line 13 in b615b9c

joint_position_tau: 0.04

You can set joint_position_tau to a lower value but might experience some noise, so please be careful here. 0.04 means that after 0.04 seconds, 63% of the target is reached (https://en.wikipedia.org/wiki/Exponential_smoothing#Time_constant). This is a known issue of this stack. I hope this makes sense to you.

[TalentLeshen-DLUT]

@mhubii Based on my tests, joint position tau does not appear to be a control parameter for the smoothing command. We guess this is a receiving parameter on a frequency. Because when we increase it, the robotic arm can't receive all the information, and when we decrease it will vibrate. So I adjusted this parameter to 0.01, which is consistent with the message frequency. I'm trying to adjust the Cartesian gain in the twist controller.

lbr_fri_ros2_stack/lbr_description/ros2_control/lbr_controllers.yaml

Line 123 in b615b9c

cartesian_gains: [1.0, 1.0, 1.0, 1.0, 1.0, 1.0] # cartesian gains

I changed its value to (1.6, 1.35, 3.5, 1.0, 1.0, 1.0), and the accuracy of the robot arm was improved to some extent. At present, in the y and z directions, its accuracy has reached about 0.02mm, but in the x direction, its accuracy is about 1.1mm. I want to improve the accuracy of the x direction, but I am at my wits' end. Can you give me some suggestions?

[TalentLeshen-DLUT]

Also, I ran the ros2 topic echo /lbr/joint states and it shows the following:

- lbr_A2 
- lbr_A3 
- lbr_A4 
- lbr_A6 
- lbr_A1 
- lbr_A5 
- lbr_A7

I'm not sure if the order of the joints will make a difference to the result.

[mhubii]

Can you give me some suggestions?

you could add some form of PID. I.e., specify a target trajectory, then compute a twist using twist = gain * (pose_target - pose).

I'm not sure if the order of the joints will make a difference to the result.

Yes, so this is the default joint_state_broadcaster of ros2_control. This might be changed by adding joint names to the configuration file

lbr_fri_ros2_stack/lbr_description/ros2_control/lbr_controllers.yaml

Line 8 in b615b9c

joint_state_broadcaster: