Servo: Address drift in Gazebo arm joint position control

[sea-bass]

Description

MoveIt Servo can cause drift in joint angles for some configurations if the target arm does not have sufficient filtering/smoothing in place.

The example in this issue is with an Ignition Gazebo simulation used in MoveIt Studio, where the controllers are set up using the standard gz_ros2_control plugin. This arm is now being controlled at 200 Hz using sim time; however, we also see the same issues with other hardware we have tried such as the Kinova Gen3 Lite arm.

https://user-images.githubusercontent.com/4603398/211877454-6a1e2b0c-81e3-4117-9c0a-d44890961836.mp4

Your environment

• ROS Distro: ROS 2 Humble
• OS Version: e.g. Ubuntu 22.04
• Source build of MoveIt 2.5.4
• RMW: CycloneDDS

What we have tried

• Switching the arm to use velocity control, but it was quite unstable. Maybe we configured it wrong?
• Adding a lookahead parameter (as seen in https://github.com/ros-planning/moveit2/pull/1812)

Other info

Here is the servo config in YAML format. Does anything stand out?

use_gazebo: true # Whether the robot is started in a Gazebo simulation environment

## Properties of incoming commands
command_in_type: "unitless" # "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:  2.0  # Max linear velocity. Meters per publish_period. Unit is [m/s]. Only used for Cartesian commands.
  rotational:  3.0 # 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: 2.0

# Disable Servo velocity scaling calculation and drive the arm at 50% max speed.
override_velocity_scaling_factor: 0.5

## Properties of outgoing commands
publish_period: 0.005  # 1/Nominal publish rate [seconds]
low_latency_mode: true  # 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"

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

## MoveIt properties
move_group_name:  manipulator  # Often 'manipulator' or 'arm'
planning_frame: base_link  # The MoveIt planning frame. Often 'base_link' or 'world'
is_primary_planning_scene_monitor: false  # The MoveGroup node maintains the planning scene, so Servo needs to get its world info from there.

## Other frames
ee_frame_name: tool0  # The name of the end effector link, used to return the EE pose
robot_link_command_frame:  base_link  # 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:  30.0  # Start decelerating when the condition number hits this (close to singularity)
hard_stop_singularity_threshold: 50.0 # Stop when the condition number hits this
joint_limit_margin: 0.1 # added as a buffer to joint limits [radians]. If moving quickly, make this larger.

## 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: /robot_controllers/commands # Publish outgoing commands here

## Collision checking for the entire robot body
check_collisions: true # Check collisions?
collision_check_rate: 10.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]

And here is the relevant part of the controller information:

controller_manager:
  ros__parameters:
    update_rate: 200  # Hz
    streaming_controller:
      type: position_controllers/JointGroupPositionController

streaming_controller:
  ros__parameters:
    joints:
      - shoulder_pan_joint
      - shoulder_lift_joint
      - elbow_joint
      - wrist_1_joint
      - wrist_2_joint
      - wrist_3_joint
    command_interfaces:
      - position
    state_interfaces:
      - position

[AndyZe]

I have trouble seeing what is wrong in the video. It looks like it responds well to your input.

[sea-bass]

I have trouble seeing what is wrong in the video. It looks like it responds well to your input.

When we move a single joint, the other joints slowly start drifting instead of staying at their initial values. Seems like every joint angle "sags" due to gravity in both endpoint and single-joint control.

[AndyZe]

I see. Here's my theory on the problem as experienced by prior clients. I've already shared my fix ideas so I won't comment further unless I think of something new.

This is a feedback issue in position control mode where the arm does not track its setpoint to a high degree of accuracy. Usually seen with lower-quality hardware (Kinova, X-arm).

Say that the joint tracking or position feedback is off by 0.1*. On the next iteration of the control loop, Servo takes that position as the new current state of the robot. Thus, the robot moves down 0.1*. On the next iteration, it gets even worse. And so on.

[AndyZe]

How about this as a fix?

• Add a parameter called, say, threshold_change_in_joint_position
• If the change in the joint position is smaller than threshold_change_in_joint_position over one control cycle, ignore the change. Assume the joint is exactly where it was last time.

(OR)

• Keep track of which joints the command actuates with every control cycle
• If a different joint changes position, ignore the change in position

Both options sound potentially dangerous but I think they would solve the problem. They should def be optional.

[sea-bass]

The 2nd option seems safer (read: less unsafe) but will mostly solve the issue for the single-joint control mode and not for the endpoint jogging one, right? Because you may still have small, but nonzero delta commands to certain joints?

I guess we could do something like: if the joint is not actuated or its velocity threshold is less than some value, hold position?

I say velocity threshold because then we support the same for velocity control, plus in position land we can keep the same threshold as we vary the dt?

[AndyZe]

^ that sounds fine to me. Do you want to make the PR or shall I?

[sea-bass]

^ that sounds fine to me. Do you want to make the PR or shall I?

If you have time to do it, we would appreciate the help. @eholum and I would be happy to review and test.

[AndyZe]

PR here: #1861

I ended up not adding the velocity threshold check b/c all calculations inside Servo are done in the position domain. Also, I think it's optional for the RobotState to contain velocities. So I'm not sure how that would work.

Anyway, I think what's in the PR should be adequate. It's based purely on actuated joints. Basically I went with my idea number 2

[ibrahiminfinite]

I have a question about this. If commanded_joint_state is the state robot was supposed to be in at time t as result of command at time t - 1, but is instead at some current_joint_state at time t.

Can the drift issue be solved by computing q_error = commanded_joint_state - current_joint_state and then accounting for this error in the next commanded_joint_state .

eg : Assuming a 2 joint robot , a joint jog command that increases the angle by 0.2 units at every iteration, and the tracking could be off by 0.1 For an iteration that should take joint states from [0.5, 0.5] -> [0.7, 0.5] we have an error [ 0.1, 0.0] giving us [0.6, 0.5] If not corrected, the next command will be [0.8, 0.5], but if we add the error to the command value we would get the proper command of [0.9, 0.5] With this, the velocities would also get computed properly.

Not sure how sensible this is.

[sea-bass]

@ibrahiminfinite If I understand right, I think that's exactly what @AndyZe tried in https://github.com/ros-planning/moveit2/pull/1861 and it didn't quite work out.

We think the issues are in the way Gazebo does control, so doubtful that it's completely a servo problem in this case.

[sea-bass]

One possible lead I completely forgot about was that servo seems to use wall clock for all its calculations... but maybe I'm wrong.

If I'm right, do we think that allowing Servo to use sim time published on the /clock topic would help with Gazebo based servoing?

[AndyZe]

What Ibrahim suggests is a little bit different than what I tried in #1861. It's worth a shot, I think.

[AndyZe]

@sea-bass all of the clocks in Servo are like this: rclcpp::Clock& clock = *node_->get_clock();

My understanding from reading here is that one just needs to set use_sim_time to true for the Servo node, then it would start using the Gazebo clock. No code change needed.

[sea-bass]

What Ibrahim suggests is a little bit different than what I tried in #1861. It's worth a shot, I think.

I see. I guess it would sort of be like changing the proportional "controller" for Servo to a PI or something. If we try it out, I'm happy to test it on the MoveIt Studio Gazebo sim.

We may now be at the point where we can use MoveIt 2's main branch on Studio so we don't have to do too many crazy cherry-picks to test this stuff.

[eholum]

My understanding from reading here is that one just needs to set use_sim_time to true for the Servo node, then it would start using the Gazebo clock. No code change needed.

I think there are a handful of places in Servo that reference the wall clock. I think the wall clock will always use the system system time rather that node time, but that's worth double checking. It's also a simple find and replace to update it.

[Pomorondza]

I have trouble seeing what is wrong in the video. It looks like it responds well to your input.

When we move a single joint, the other joints slowly start drifting instead of staying at their initial values. Seems like every joint angle "sags" due to gravity in both endpoint and single-joint control.

Hi, any updates on this? I am experiencing the same issue with the real UR3e robot. And even when I am sending out an empty JointJog command with MoveIt Servo, the robot starts drifting away.

My environment is ROS2 + MoveIt Foxy on Ubuntu 20.04 machine with RT kernel, and the controller I am using is the same (position_controllers/JointGroupPositionController).

[sguptasarma]

Is this drifting behavior expected on ROS Noetic at present?

[AndyZe]

I don't believe the issue has ever been reported in MoveIt1

[sguptasarma]

Thank you! Still trying to figure out the source of my problem, if it looks related I'll make a new issue.

[github-actions[bot]]

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.

[ibrahiminfinite]

@sea-bass is this something that still needs to be fixed ?

[github-actions[bot]]

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.