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.