Servoing doesn't follow twist command correctly

[PietroValerio]

Description

I'm trying to do servoing, using the c++ api, all i've tried to do is to setting a target twist defined in the planning frame, in my case the robot frame, to move the end effector on the z-axis. As you can see in the video below this does not happen. I've taken the tutorial file used on franka emika panda and modified for working with ur10e.

visualization.mp4

ROS Distro

Jazzy

OS and version

Ubuntu 24.04

Source or binary build?

Source

If binary, which release version?

No response

If source, which branch?

main commit abb440d

Which RMW are you using?

None

Steps to Reproduce

You need to have ros2 driver for ur robot provided by universal robots
Here is my modified .cpp file for servoing using the cpp api, i changed the type of command that the servo node outputs, because the /forward_position_controller/commands accepts only std_msgs::msg::Float64MultiArray

#include <chrono>
#include <moveit_servo/servo.hpp>
#include <moveit_servo/utils/common.hpp>
#include <rclcpp/rclcpp.hpp>
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
#include <tf2_ros/transform_listener.h>
#include <moveit/utils/logger.hpp>

using namespace moveit_servo;

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

  const rclcpp::Node::SharedPtr demo_node = std::make_shared<rclcpp::Node>("moveit_servo_demo");
  moveit::setNodeLoggerName(demo_node->get_name());

 
  const std::string param_namespace = "moveit_servo";
  const std::shared_ptr<const servo::ParamListener> servo_param_listener =
      std::make_shared<const servo::ParamListener>(demo_node, param_namespace);
  const servo::Params servo_params = servo_param_listener->get_params();

//THIS LINE WAS MODIFIED
  rclcpp::Publisher<std_msgs::msg::Float64MultiArray>::SharedPtr trajectory_outgoing_cmd_pub =
      demo_node->create_publisher<std_msgs::msg::Float64MultiArray>(servo_params.command_out_topic,
                                                                         rclcpp::SystemDefaultsQoS());

 
  const planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor =
      createPlanningSceneMonitor(demo_node, servo_params);
  Servo servo = Servo(demo_node, servo_param_listener, planning_scene_monitor);

  std::this_thread::sleep_for(std::chrono::seconds(10));


  auto robot_state = planning_scene_monitor->getStateMonitor()->getCurrentState();
  const moveit::core::JointModelGroup* joint_model_group =
      robot_state->getJointModelGroup(servo_params.move_group_name);

 
  servo.setCommandType(CommandType::TWIST);


  TwistCommand target_twist{ "ur10e_base_link", { 0.0, 0.0, 0.05, 0.0, 0.0, 0.0 } };


  rclcpp::WallRate rate(1.0 / servo_params.publish_period);

  std::chrono::seconds timeout_duration(4);
  std::chrono::seconds time_elapsed(0);
  const auto start_time = std::chrono::steady_clock::now();

  KinematicState joint_state = servo.getCurrentRobotState( /* wait for updated state */);

  RCLCPP_INFO_STREAM(demo_node->get_logger(), servo.getStatusMessage());
  while (rclcpp::ok())
  {
    KinematicState joint_state = servo.getNextJointState(robot_state, target_twist);
    const StatusCode status = servo.getStatus();

    const auto current_time = std::chrono::steady_clock::now();
    time_elapsed = std::chrono::duration_cast<std::chrono::seconds>(current_time - start_time);
    if (time_elapsed > timeout_duration)
    {
      RCLCPP_INFO_STREAM(demo_node->get_logger(), "Timed out");
      break;
    }
    else if (status != StatusCode::INVALID)
    {
       // THIS LINE WAS CHANGED
        const auto msg = composeMultiArrayMessage(servo_params, joint_state);
        trajectory_outgoing_cmd_pub->publish(msg);
    }
    rate.sleep();
  }

  RCLCPP_INFO(demo_node->get_logger(), "Exiting demo.");
  rclcpp::shutdown();
}

Also I leave the .yaml file for configuring the servo node

###############################################
# Modify all parameters related to servoing here
###############################################
use_gazebo: false # Whether the robot is started in a Gazebo simulation environment

## Properties of incoming commands
command_in_type: "speed_units" # "unitless"> in the range [-1:1], as if from joystick. "speed_units"> cmds are in m/s and rad/s
scale:
  # Scale parameters are only used if command_in_type=="unitless"
  linear: 0.6 # Max linear velocity. Meters per publish_period. Unit is [m/s]. Only used for Cartesian commands.
  rotational: 0.3 # Max angular velocity. Rads per publish_period. Unit is [rad/s]. Only used for Cartesian commands.
  # Max joint angular/linear velocity. Rads or Meters per publish period. Only used for joint commands on joint_command_in_topic.
  joint: 0.01
# This is a fudge factor to account for any latency in the system, e.g. network latency or poor low-level
# controller performance. It essentially increases the timestep when calculating the target pose, to move the target
# pose farther away. [seconds]
system_latency_compensation: 0.04

## Properties of outgoing commands
publish_period: 0.004 # 1/Nominal publish rate [seconds]
low_latency_mode: false # Set this to true to publish as soon as an incoming Twist command is received (publish_period is ignored)

# What type of topic does your robot driver expect?
# Currently supported are std_msgs/Float64MultiArray or trajectory_msgs/JointTrajectory
command_out_type: std_msgs/Float64MultiArray

# What to publish? Can save some bandwidth as most robots only require positions or velocities
publish_joint_positions: true
publish_joint_velocities: false
publish_joint_accelerations: false

## Plugins for smoothing outgoing commands
smoothing_filter_plugin_name: "online_signal_smoothing::ButterworthFilterPlugin"

# If is_primary_planning_scene_monitor is set to true, the Servo server's PlanningScene advertises the /get_planning_scene service,
# which other nodes can use as a source for information about the planning environment.
# NOTE: If a different node in your system is responsible for the "primary" planning scene instance (e.g. the MoveGroup node),
# then is_primary_planning_scene_monitor needs to be set to false.
is_primary_planning_scene_monitor: false

## Incoming Joint State properties
low_pass_filter_coeff: 10.0 # Larger --> trust the filtered data more, trust the measurements less.

## MoveIt properties
move_group_name: ur_arm # Often 'manipulator' or 'arm'
planning_frame: ur10e_base_link # The MoveIt planning frame. Often 'base_link' or 'world'

## Other frames
ee_frame: ur10e_tool0 # The name of the end effector link, used to return the EE pose
robot_link_command_frame: ur10e_tool0 # commands must be given in the frame of a robot link. Usually either the base or end effector

## Stopping behaviour
incoming_command_timeout: 0.1 # Stop servoing if X seconds elapse without a new command
# If 0, republish commands forever even if the robot is stationary. Otherwise, specify num. to publish.
# Important because ROS may drop some messages and we need the robot to halt reliably.
num_outgoing_halt_msgs_to_publish: 4

## Configure handling of singularities and joint limits
lower_singularity_threshold: 100.0 # Start decelerating when the condition number hits this (close to singularity)
hard_stop_singularity_threshold: 200.0 # Stop when the condition number hits this

# added as a buffer to joint limits [radians]. If moving quickly, make this larger.
joint_limit_margins: [0.1, 0.1, 0.1, 0.1, 0.1, 0.1]

## Topic names
cartesian_command_in_topic: ~/delta_twist_cmds # Topic for incoming Cartesian twist commands
joint_command_in_topic: ~/delta_joint_cmds # Topic for incoming joint angle commands
joint_topic: /joint_states
status_topic: ~/status # Publish status to this topic
command_out_topic: /forward_position_controller/commands # Publish outgoing commands here

## Collision checking for the entire robot body
check_collisions: true # Check collisions?
collision_check_rate: 5.0 # [Hz] Collision-checking can easily bog down a CPU if done too often.
# Two collision check algorithms are available: "threshold_distance" begins slowing down when
# nearer than a specified distance. Good if you want to tune collision thresholds manually.
# "stop_distance" stops if a collision is nearer than the worst-case stopping distance and the
# distance is decreasing. Requires joint acceleration limits
collision_check_type: threshold_distance
# Parameters for "threshold_distance"-type collision checking
self_collision_proximity_threshold: 0.01 # Start decelerating when a self-collision is this far [m]
scene_collision_proximity_threshold: 0.02 # Start decelerating when a scene collision is this far [m]
# Parameters for "stop_distance"-type collision checking
collision_distance_safety_factor: 1000.0 # Must be >= 1. A large safety factor is recommended to account for latency
min_allowable_collision_distance: 0.01 # Stop if a collision is closer than this [m]

Expected behavior

End effector should move only on the z-axis

Actual behavior

The robot doesn't make the correct movement

Backtrace or Console output

No response

[sea-bass]

(EDIT: I thought I had made a mistake, but checked again and it seems fine. So I don't know what's going on here)

Also generally, your servo configuration file seems to have a lot of deprecated parameters in it. See here for a list of what you actually need:

moveit2/moveit_ros/moveit_servo/config/panda_simulated_config.yaml

Line 1 in cd26fc0

###############################################

List of parameters in your file that no longer exist:

• use_gazebo
• system_latency_compensation / low_latency_mode
  • (see instead max_expected_latency)
• planning_frame / ee_frame / robot_link_command_frame
  • (now inferred from the actual group name used, see apply_twist_commands_about_ee_frame to control whether to use base or tip frame)
• num_outgoing_halt_msgs_to_publish
• collision_check_type / collision_distance_safety_factor

[sea-bass]

Those upper/lower singularity threshold values also look pretty high, but I doubt that's the main issue here...

[sea-bass]

This is a reference UR5 servo configuration which may also help?
https://github.com/PickNikRobotics/moveit_studio_ur_ws/blob/main/src/moveit_pro_ur_configs/picknik_ur_base_config/config/moveit/ur_servo.yaml

[PietroValerio]

I'll close the issue, modifying the .yaml file resolved the issue. Also this was a problem of ur controllers so not completely related to moveit, thanks for the help!