Pilz LIN planner deviates from target linear speed when rotation is involved in a move

[gaspatxo]

Description

My goal is to control the linear velocity at which my end effector moves throughout a move sequence. I am using the Pilz LIN planner and its multi-segment capability.

The speed is respected when movements do not include rotation, being the max_trans_vel (in pilz_cartesianl_limits.yaml) multiplied by maxVelocityScalingFactor.

However, when rotation is involved in a move, it deviates from the specified:

In all moves the maxVelocityScalingFactor is the same, 0.1

pilz_cartesianl_limits.yaml:

# Cartesian limits for the Pilz planner
cartesian_limits:
  max_trans_vel: 1.0
  max_trans_acc: 4.25
  max_trans_dec: -5.0
  max_rot_vel: 1.57

I have tested this in the Panda and xarm7 robotic arms and in both cases the behaviour is the same.

Your environment

ROS2 Humble on Ubuntu 22.04 using moveit2 binary humble install with cyclone DDS

Steps to reproduce

1. Install moveit tutorials
2. Activate the /move_group_sequence capability for the panda arm #1281
3. Run my test file

#include <memory>
#include <string>
using std::string;
#include <fstream>
#include <iostream>
#include <math.h>
#include <sstream>
#include <unistd.h>
#include <vector>

#include "moveit/move_group_interface/move_group_interface.h"
#include "moveit/planning_scene_interface/planning_scene_interface.h"
#include "moveit/robot_model/robot_model.h"
#include "moveit/robot_state/conversions.h"
#include "moveit/robot_state/robot_state.h"
#include "moveit/robot_trajectory/robot_trajectory.h"
#include "moveit/trajectory_processing/iterative_time_parameterization.h"

using moveit::planning_interface::MoveGroupInterface;
#include "moveit_msgs/action/move_group_sequence.hpp"
#include "moveit_msgs/msg/motion_sequence_request.hpp"
using moveit_msgs::action::MoveGroupSequence;

#include "rclcpp/rclcpp.hpp"
using namespace rclcpp;
#include "rclcpp_action/rclcpp_action.hpp"
#include "tf2/LinearMath/Quaternion.h"
using tf2::Quaternion;

using geometry_msgs::msg::Pose;

class MotionController : public Node {
public:
  MotionController(NodeOptions node_options)
      : Node("motion_controller", node_options) {
    RCLCPP_INFO(get_logger(), "Initiated %s node", this->get_name());
    move_group_name = "panda_arm";
    max_tcp_speed = 0.4; // same value as in pilz_cartesian_limits.yaml
  }

  void init() {
    RCLCPP_DEBUG(get_logger(), "Creating `move_group object");
    move_group_ = std::make_shared<MoveGroupInterface>(shared_from_this(),
                                                       move_group_name);
    move_group_->setMaxVelocityScalingFactor(0.6); // travel speed
    move_group_->setPlanningTime(10);

    RCLCPP_INFO(get_logger(), "Controlled Link: %s",
                move_group_->getEndEffectorLink().c_str());

    RCLCPP_INFO(get_logger(), "Maximum end effector lin speed: %f [m/s]",
                max_tcp_speed);

    pilz_sequence_client_ = rclcpp_action::create_client<MoveGroupSequence>(
        this, "sequence_move_group");
  }

  void run() {
    move_group_->setPlanningPipelineId("pilz_industrial_motion_planner");
    move_group_->setPlannerId("LIN");
    move_group_->setMaxAccelerationScalingFactor(0.1);
    move_group_->setNumPlanningAttempts(1);
    move_group_->setPlanningTime(10.0);

    auto home_pose = move_group_->getCurrentPose().pose;
    RCLCPP_INFO(get_logger(),
                "current_pose = x%.2f y%.2f z%.2f qx%.2f qy%.2f qz%.2f qw%.2f",
                home_pose.position.x, home_pose.position.y,
                home_pose.position.z, home_pose.orientation.x,
                home_pose.orientation.y, home_pose.orientation.z,
                home_pose.orientation.w);

    std::vector<Pose> waypoints;
    waypoints.push_back(generateRelativePose(0.350, 0.3, -0.2, 0.0, 0.0, 0.0));
    waypoints.push_back(generateRelativePose(-0.10, 0.3, -0.2, 0.0, 0.0, 0.0));
    waypoints.push_back(generateRelativePose(0.350, 0.3, -0.2, 0.1, 0.0, 0.0));
    waypoints.push_back(generateRelativePose(-0.10, 0.3, -0.2, 0.0, 0.0, 0.0));
    waypoints.push_back(generateRelativePose(0.350, 0.3, -0.2, 0.4, 0.0, 0.0));
    waypoints.push_back(generateRelativePose(-0.10, 0.3, -0.2, 0.0, 0.0, 0.0));
    waypoints.push_back(generateRelativePose(0.350, 0.3, -0.2, 0.8, 0.0, 0.0));
    waypoints.push_back(generateRelativePose(-0.10, 0.3, -0.2, 0.0, 0.0, 0.0));
    waypoints.push_back(generateRelativePose(0.350, 0.3, -0.2, 1.6, 0.0, 0.0));
    waypoints.push_back(generateRelativePose(-0.10, 0.3, -0.2, 0.0, 0.0, 0.0));
    waypoints.push_back(home_pose);

    auto sequence_goal = MoveGroupSequence::Goal();
    for (Pose waypoint : waypoints) {
      const double max_velocity_scaling_factor = 0.1;
      move_group_->setMaxVelocityScalingFactor(max_velocity_scaling_factor);
      move_group_->setPoseTarget(waypoint);

      moveit_msgs::msg::MotionSequenceItem motion_sequence_item;
      move_group_->constructMotionPlanRequest(motion_sequence_item.req);
      motion_sequence_item.blend_radius = 0.0;

      sequence_goal.request.items.push_back(motion_sequence_item);
    }
    // add start state only to first waypoint
    moveit::core::robotStateToRobotStateMsg(
        *move_group_->getCurrentState(),
        sequence_goal.request.items[0].req.start_state);

    sequence_goal.planning_options.plan_only = false;
    sendPilzSequenceGoal(sequence_goal); // plan and execute
  }

private:
  string move_group_name;
  std::shared_ptr<MoveGroupInterface> move_group_;
  double max_tcp_speed;
  rclcpp_action::Client<MoveGroupSequence>::SharedPtr pilz_sequence_client_;

  void sendPilzSequenceGoal(MoveGroupSequence::Goal sequence_request) {
    std::chrono::milliseconds pilz_timeout(1000); // 1[s] timeout
    if (!pilz_sequence_client_->wait_for_action_server(pilz_timeout)) {
      RCLCPP_ERROR(get_logger(), "Action server not available after waiting");
      rclcpp::shutdown();
    }

    RCLCPP_INFO(get_logger(), "Sending goal");

    pilz_sequence_client_->async_send_goal(sequence_request);
    sleep(5);
  }

  Pose generateRelativePose(double x, double y, double z, double roll,
                            double pitch, double yaw) {
    Pose pose;

    pose.position.x = x + move_group_->getCurrentPose().pose.position.x;
    pose.position.y = y + move_group_->getCurrentPose().pose.position.y;
    pose.position.z = z + move_group_->getCurrentPose().pose.position.z;

    Quaternion q;
    q.setRPY(roll, pitch + M_PI, yaw + M_PI);
    q.normalize();

    pose.orientation.x = q.x();
    pose.orientation.y = q.y();
    pose.orientation.z = q.z();
    pose.orientation.w = q.w();

    return pose;
  }
};

int main(int argc, char **argv) {
  init(argc, argv);
  auto motion_controller_node = std::make_shared<MotionController>(
      NodeOptions().automatically_declare_parameters_from_overrides(true));

  // needed for getCurrentPose() ->
  // https://robotics.stackexchange.com/questions/103393/how-to-extract-position-of-the-gripper-in-ros2-moveit2/103394#103394
  executors::SingleThreadedExecutor executor;
  executor.add_node(motion_controller_node);

  std::thread spinner = std::thread([&executor]() { executor.spin(); });

  // Call initialize function after creating MotionController instance
  motion_controller_node->init();
  motion_controller_node->run();

  shutdown();
  spinner.join();
  return 0;
}

Note

It could be that this behaviour is not erroneous. However, I have not been able to find much documentation on the pilz planner and I currently reading the source coude to understand where this is coming from. If this is the intended behaviour, it seems to me contradictory to the philosophy of the pilz LIN planner

The pilz LIN section in moveit docs says:

This planner generates a linear Cartesian trajectory between goal and start poses. The planner uses the Cartesian limits to generate a trapezoidal velocity profile in Cartesian space. The translational motion is a linear interpolation between start and goal position vector. The rotational motion is quaternion slerp between start and goal orientation. The translational and rotational motion is synchronized in time.

[pfucile]

This is probably because of the way KDL handles synchronization between translation and rotation, using eqradius.

See path_line.cpp used by pilz planner trajectory_generator.cpp

/**
* Constructs a Line Path
* F_base_start and F_base_end give the begin and end frame wrt the base
* orient gives the method of rotation interpolation
* eqradius : equivalent radius :
*		serves to compare rotations and translations.
*		the "amount of motion"(pos,vel,acc) of the rotation is taken
*      to be the amount motion of a point at distance eqradius from the
*      rotation axis.
*
* Eqradius is introduced because it is unavoidable that you have to compare rotations and translations :
* e.g. : You can have motions that only contain rotation, and motions that only contain translations.
* The motion planning goes as follows :
*  - translation is planned with the given parameters
*  - rotation is planned planned with the parameters calculated with eqradius.
*  - The longest of the previous two remains unchanged,
*    the shortest in duration is scaled to take as long as the longest.
* This guarantees that the geometric path in 6D space remains independent of the motion profile parameters.

[github-actions]

This issue is being labeled as stale because it has been open 45 days with no activity. It will be automatically closed after another 45 days without follow-ups.

[gaspatxo]

I tried using moveit built from source after #3000 , the issue seems to have been reduced a bit, but it is still very much present.

[github-actions]

This issue is being labeled as stale because it has been open 45 days with no activity. It will be automatically closed after another 45 days without follow-ups.

[vivekcdavid]

The issue seems to arise from two factors within Pilz, both related to KDL::Path_line. While testing the further, we found issues with the TCP speed calculations we used for testing purposes. So for all further testing we used the actual time required for the motions rather than the speed.

1. as mentioned by @gaspatxo @pfucile while generating the path using KDL::Path_line a parameter called eqradius is used. The use of Eqradius prevents very fast angular motions but as a side effect the motion deviated from the specified linear speed.

solution : set eqradius to zero if the motion should only consider the specified linear TCP speed

2. Even after setting eqradius to zero we observed a small deviation in the time take for motion, this was pin-pointed to the use of trapezoidal velocity profile.
   solution : By using a rectangular velocity profile this issue seems to be solved.

A modified version of Pilz with the above mentioned changes can be found here

As noted by @gaspatxo this seems to a safety feature but this has not been clearly described in the documentation.

[sea-bass]

Thanks for sharing your findings and solutions! I'd encourage you to put up a PR, nothing that:

1. I don't know why that is the default calculation for eqradius -- probably worth leaving it as a default, but having the ability to override it via parameters seems useful.
2. Rectangular velocity profiles are almost never used in practice because they mean infinite acceleration which is never physically realizable. Similarly, this could be a parameter where you choose the profile you want, defaulting to trapezoidal. An alternative could be if no acceleration limits are specified, this profile is selected?

[vivekcdavid]

@sea-bass Yes I would do that