Real Arm is shaking when using Moveit Servo

[lorepieri8]

I've adapted the real time servoing tutorial to work with Xarm6. All good in simulation, both using the ros2_control_plugin fake_components/GenericSystem (as done in the Panda tutorial) and the gazebo_ros2_control/GazeboSystem. I'm using a PS3 joystick to control joints and cartesian movements. If I use xarm_control/XArmHW with Gazebo it's also fine, but when working with the real arm, the arm starts shaking and it hardly moves.

To control the real arm I'm using:

ros2 launch xarm_moveit_config xarm6_moveit_realmove.launch.py robot_ip:=myip

and

ros2 launch moveit2_tutorials servo_teleop.launch.py

What is causing the shaking? Do I have to tune any PID values? Or perhaps should I set a lower frequency for the controllers?

[lorepieri8]

This is the config file for the servoing:

###############################################
# 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: "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:  0.2  # Max linear velocity. Meters per publish_period. Unit is [m/s]. Only used for Cartesian commands.
  rotational:  0.4 # 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.25

## Properties of outgoing commands
publish_period: 0.034  # 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: trajectory_msgs/JointTrajectory

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

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

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

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

## Stopping behaviour
incoming_command_timeout:  0.2  # 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:  17.0  # Start decelerating when the condition number hits this (close to singularity)
hard_stop_singularity_threshold: 30.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: /xarm6_traj_controller/joint_trajectory # Publish outgoing commands here

## Collision checking for the entire robot body
check_collisions: false # 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.0025 # Start decelerating when a self-collision is this far [m]
scene_collision_proximity_threshold: 0.0005 # Start decelerating when a scene collision is this far [m]
# Parameters for "stop_distance"-type collision checking
collision_distance_safety_factor: 10.0 # Must be >= 1. A large safety factor is recommended to account for latency
min_allowable_collision_distance: 0.0025 # Stop if a collision is closer than this [m]

[penglongxiang]

Hi @lorepieri8 Thanks for your feedback. To be honest we have not tested with Moveit Servo yet.

I guess you can try controlling with higher frequency (normally we use 100Hz in Moveit application) and make sure the network connection with xArm controller is stable and low-latency (by cable connection and no switches in-between).

Another test may be controlling with velocity interface, by bringing up xArm with ros2 launch xarm_moveit_config xarm6_moveit_realmove.launch.py robot_ip:=your_ip velocity_control:=true, which may have a smoother execution response.

Please be sure to test with a low operation velocity and always keep the emergency button reachable. We will arrange the compatibility test with Moveit Servo accordingly.

[lorepieri8]

Thanks for the quick response @penglongxiang .

I tried pushing the update_rate to 100 Hz in https://github.com/xArm-Developer/xarm_ros2/blob/master/xarm_controller/config/xarm6_controllers.yaml but the arm still has the same vibration issue. I'm connected via ethernet cable directly to the control box.

Regarding the second option (velocity_control:=true), the real arm doesn't move at all. The simulation is still fine and works without issues.

[lorepieri8]

I'm using firmware version 1.6.1, perhaps that's why velocity control is not working?

EDIT: No, the problem is still there after update to 1.7.2.

[penglongxiang]

Hi @lorepieri8 , thank you for reporting these issues, we suggest not to use "moveit_servo" with xArm until we fully test and support this new function, otherwise the behavior can be unpredictable and dangerous. We will notify you once the support is ready.

[penglongxiang]

@lorepieri8 we have tested moveit servo function these days and found out specifying report_type:=dev when launching xarm_driver can solve the shaking issue, please also make sure the PC and xArm controller is connected by cable and no switches in between. In this way, the feedback rate is 100Hz rather than default 5Hz.

However, please DO NOT USE VELOCITY MODE with Moveit Servo, the command generated this way is not stable, we are still trying to figure out the reason. Position control (by default) with report_type:=dev can already achieve the goal for servoing now.

[lorepieri8]

I've tried as you suggested:

ros2 launch xarm_moveit_config xarm6_moveit_realmove.launch.py robot_ip:=myip report_type:=dev

but the shaking is still there. Which topic should I monitor to debug this? If I do

ros2 topic hz /xarm6_traj_controller/state

I do get about 100 hz.

[penglongxiang]

Hi @lorepieri8 have you made any changes in xarm_controller/config/xarm6_controllers.yaml? Could you also send us the controller log after reproducing this shaking phenomenon? Log can be downloaded through xArm studio: Settings->System->Log->Download. You may send it to [email protected]

[lorepieri8]

No edits to the yaml. I've sent the logs, thanks for your support.

[penglongxiang]

Hi @lorepieri8 , it looks like two controlling sources are conflicting, one is from move_group and the other is from joystick. Could you share the modified servo_teleop.launch.py? As well as the output of ros2 node list and round-trip communication time indicated by ping your_xarm_ip?

[lorepieri8]

Hi @penglongxiang , I think we are on the right track, there are many duplicates.

https://gist.github.com/lorepieri8/9eaebf8f6355752dfdd26227bcdae072

WARNING: Be aware that are nodes in the graph that share an exact name, this can have unintended side effects.
/controller_manager
/controller_manager
/controller_to_servo_node
/interactive_marker_display_94326090918640
/joint_state_controller
/joint_state_controller
/joint_state_publisher
/joy_node
/launch_ros_100697
/move_group
/move_group_private
/moveit_servo_demo_container
/moveit_simple_controller_manager
/robot_state_publisher
/robot_state_publisher
/rviz2
/rviz2
/rviz2
/rviz2_private
/rviz2_private
/servo_server
/servo_server_private
/static_tf2_broadcaster
/static_transform_publisher
/transform_listener_impl_555791e6fa00
/transform_listener_impl_5566221bc040
/transform_listener_impl_55ca00fbcb40
/transform_listener_impl_55ca0104f020
/transform_listener_impl_7f12fc0017a0
/transform_listener_impl_7f6604001110
/xarm6_traj_controller
/xarm_driver
/xarm_hw
/xarm_hw
/xarm_ros_client
/xarm_ros_client

ping myip
PING myip (myip) 56(84) bytes of data.
64 bytes from myip: icmp_seq=1 ttl=64 time=0.183 ms
64 bytes from myip: icmp_seq=2 ttl=64 time=0.188 ms
64 bytes from myip: icmp_seq=3 ttl=64 time=0.178 ms
64 bytes from myip: icmp_seq=4 ttl=64 time=0.179 ms
64 bytes from myip: icmp_seq=5 ttl=64 time=0.194 ms
64 bytes from myip: icmp_seq=6 ttl=64 time=0.234 ms
64 bytes from myip: icmp_seq=7 ttl=64 time=0.150 ms
64 bytes from myip: icmp_seq=8 ttl=64 time=0.202 ms
64 bytes from myip: icmp_seq=9 ttl=64 time=0.182 ms
64 bytes from myip: icmp_seq=10 ttl=64 time=0.186 ms
64 bytes from myip: icmp_seq=11 ttl=64 time=0.141 ms
64 bytes from myip: icmp_seq=12 ttl=64 time=0.186 ms
64 bytes from myip: icmp_seq=13 ttl=64 time=0.249 ms
^C
--- myip ping statistics ---
13 packets transmitted, 13 received, 0% packet loss, time 12295ms
rtt min/avg/max/mdev = 0.141/0.188/0.249/0.027 ms

[penglongxiang]

Hi @lorepieri8 , it is still not recommended to run xarm_moveit_config application along with other controlling nodes. I think the above issue is due to duplicated launch of several nodes, the commands or states are not consistent but eventually all goes to the same hardware.

Still, after some modification, we managed to run servo_teleop.launch.py with moveit_config, here is the sample script teleop.launch.py:

import os
import yaml
from launch import LaunchDescription
from launch_ros.actions import Node
from ament_index_python.packages import get_package_share_directory
from launch_ros.actions import ComposableNodeContainer
from launch_ros.descriptions import ComposableNode
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
from launch.launch_description_sources import load_python_launch_file_as_module
from launch_ros.substitutions import FindPackageShare
from launch.actions import OpaqueFunction, IncludeLaunchDescription
import xacro

def load_yaml(package_name, file_path):
    package_path = get_package_share_directory(package_name)
    absolute_file_path = os.path.join(package_path, file_path)

    try:
        with open(absolute_file_path, "r") as file:
            return yaml.safe_load(file)
    except EnvironmentError:  # parent of IOError, OSError *and* WindowsError where available
        return None

def launch_setup(context, *args, **kwargs):
    # *** CUSTOMIZE HERE! Your Moveit Servo Configuration file
    servo_yaml = load_yaml('xarm_moveit_servo', "config/xarm_moveit_servo_config.yaml")
    servo_params = {"moveit_servo": servo_yaml}

    dof = LaunchConfiguration('dof', default=6)
    hw_ns = LaunchConfiguration('hw_ns', default='xarm')
    moveit_config_package_name = 'xarm_moveit_config'
    # robot_description_parameters
    mod = load_python_launch_file_as_module(os.path.join(get_package_share_directory(moveit_config_package_name), 'launch', 'lib', 'xarm_moveit_config_lib.py'))
    get_xarm_robot_description_parameters = getattr(mod, 'get_xarm_robot_description_parameters')
    robot_description_parameters = get_xarm_robot_description_parameters(
        xacro_urdf_file=PathJoinSubstitution([FindPackageShare('xarm_description'), 'urdf', 'xarm_device.urdf.xacro']),
        xacro_srdf_file=PathJoinSubstitution([FindPackageShare('xarm_moveit_config'), 'srdf', 'xarm.srdf.xacro']),
        urdf_arguments={
            'dof': dof,   
            'hw_ns': hw_ns,
        },
        srdf_arguments={
            'dof': dof,
        },
        arguments={
            'xarm_type': 'xarm{}'.format(dof.perform(context)),
        }
    )

    # Launch as much as possible in components
    container = ComposableNodeContainer(
        name="xarm_moveit_servo_container",
        namespace="/",
        package="rclcpp_components",
        executable="component_container",
        composable_node_descriptions=[
            ComposableNode(
                package="moveit_servo",
                plugin="moveit_servo::ServoServer",
                name="servo_server",
                parameters=[
                    servo_params,
                    robot_description_parameters
                ],
                extra_arguments=[{"use_intra_process_comms": True}],
            ),

            # *** CUSTOMIZE HERE! Node to map JoyStick signal to robot command. (In the Constructor, it is better 
            # to add a delay before subscriber, incase robot initial position is not yet updated to latest) 
            ComposableNode(
                package='xarm_moveit_servo',
                plugin='xarm_moveit_servo::JoyToServoPub',
                name='joy_to_servo_node',
                parameters=[
                    servo_params,
                    {'dof': dof, 'ros_queue_size': 10},
                ],
                extra_arguments=[{'use_intra_process_comms': True}],
            ),
            ComposableNode(
                package="joy",
                plugin="joy::Joy",
                name="joy_node",
                extra_arguments=[{"use_intra_process_comms": True}],
            ),
        ],
        output="screen",
    )

    return [container]
    

def generate_launch_description():
    return LaunchDescription([
        OpaqueFunction(function=launch_setup)
    ])

Please modify according to the "CUSTOMIZE HERE" instructions in the code, and test on your side. In this way, there may be no conflict when controlling with joystick when moveit_config application is still running.

(1) pay attention to the "add a delay" suggestion, there may be a chance the joint states are not properly updated at 'joy_to_servo_node' startup and joints may go directly to all-zero position. (2) when switching from joystick control back to moveit planning, you need to update current state as start state or the planner may not be aware of the change. You may also need to exit this teleop program before switching.
(3) proper execution sequence:
ros2 launch xarm_moveit_config xarm6_moveit_realmove.launch.py robot_ip:=your-xArm-IP report_type:=dev
After above launch is fully ready, in another terminal, run:
ros2 launch your-package-name teleop.launch.py dof:=6

We may add a demo package for moveit_servo control with joystick later, but it is supposed to run alone, not together with moveit-config.

[MeticulousFishFrier]

@lorepieri8 @penglongxiang Have either of you tried to run the real arm with MoveIt_Servo using the modifications above? I'm trying to use MoveIt_Servo in ROS Melodic and encountered the same arm shaking problem. I'm thinking of switching to ROS2 if the solution @penglongxiang provided allows smooth movements on a real xarm with Moveit_Servo.

[penglongxiang]

Hi @MeticulousFishFrier, if you consider switching to ROS2, we already updated a demo package for moveit_servo control. Refer here.

[MeticulousFishFrier]

Hi @penglongxiang, I just tested the moveit_servo on a real XArm7 using an XBOX controller. The arm responds to the input commands well, but it still shakes for every movement. I presume this is because every time a new command is sent to the arm by ROS, its joint velocities restart at 0 and accelerate. Is this due to a limitation in the firmware or could a smooth trajectory be achieved with moveit_servo?

[penglongxiang]

Hi @MeticulousFishFrier what is the firmware version of your xArm7? Were you testing with the command ros2 launch xarm_moveit_servo xarm_moveit_servo_realmove.launch.py robot_ip:=your_controller_ip dof:=7 joystick_type:=1 with default settings? We will test accordingly, thanks.

[MeticulousFishFrier]

@penglongxiang, I have the latest firmware on my xArm7 (1.8.3). The command used was the one you mentioned. Please test with the command ros2 launch xarm_moveit_servo xarm_moveit_servo_realmove.launch.py robot_ip:=your_controller_ip dof:=7 joystick_type:=1. On a side note, the arm does seem to move smoother with the velocity control being turned on. However, this causes sporadic behavior like exceeding joint limits probably because the code was written for position control.

[penglongxiang]

Hi @MeticulousFishFrier please try not to use velocity control first, we are still evaluating the velocity interpolation by moveit, sometimes the velocity command is abnormal and oscillating. After our test, the performance under position control seems OK if the network connection is direct and stable.

Please try updating ros-control, ros-controllers and moveit to the latest version, then change the update_rate in xarm_controller/config/xarm7_controllers.yaml to be 100 and see if the execution can be smoother.

Sometimes if moveit_servo believes the robot is reaching singularity, the command can be slowed down in the middle or even stalled. If this can not solve your problem, maybe a video showing the shaking is better for further diagnose.

[MeticulousFishFrier]

I changed the update_rate to be 100 and the execution did become smoother. Although I'm not sure at what the ranges of the frequencies can be, higher frequencies made the execution even smoother! Thank you so much for the fix!! Do you know how this update_rate param is being used such that the execution becomes smoother when it is increased? I was also wondering if there is an equivalent to changing update_rate in xarm_controller/config/xarm7_controllers.yaml in ROS Melodic. I currently have the same arm shaking issue in ROS1 when using moveit_servo. I'd love to solve the problem and submit a pull request to XArm's ROS1 repo with a working moveit_servo solution.

[penglongxiang]

We use "ros2_control_node" provided by "ros2_control" package to implement the controller logic, you can see from its code:

https://github.com/ros-controls/ros2_control/blob/1dc970b3a56913a7ac978b89ae000eeb33550a29/controller_manager/src/ros2_control_node.cpp#L57

which uses the "update_rate" of loaded controller configuration for the actual control frequency, it is possible the higher the rate, the smoother the trajectory will be, however the requirement on network quality / reliability also increases.

For ROS1, it is a different but similar logic, you may change the "update_rate" variable in xarm_controller/src/xarm_control_node.cpp for a higher frequency:

https://github.com/xArm-Developer/xarm_ros/blob/3f010a0d7840f67097739c85270b2e1db3f124b6/xarm_controller/src/xarm_control_node.cpp#L50

Please note the maximun frequency xArm series can respond to is 250Hz.

[MeticulousFishFrier]

I changed the update_rate variable in xarm_controller/src/xarm_control_node.cpp in ROS1 but the arm is still jittering. Is there any reason why the same variable change isn't changing the behavior?

[MeticulousFishFrier]

I changed the update_rate variable in xarm_controller/src/xarm_control_node.cpp in ROS1 but the arm is still jittering. Is there any reason why the same variable change isn't changing the behavior?

I found the issue. The topic /xarm/xarm_states is publishing at 5 hz in ROS1 but it is publishing at 100 hz in ROS2. This causes the bottleneck in the publishing rate to the arm in ROS1. Is there any way I can increase the publishing rate of /xarm/xarm_states in ROS1?

[penglongxiang]

In ROS1, you can specify report_type:=dev when launching the application, which can increase the state publish rate to 100Hz.

[MeticulousFishFrier]

Thanks for your help @penglongxiang . We were able to achieve super smooth motions in ROS1 by adding our own JointGroupVelocityControl controller and velocity_control:=true in my bringup file. I'll be submitting a pull request to xarm_ros with this change in the coming weeks.

[Akumar201]

@MeticulousFishFrier @lorepieri8 I hope you are doing well. I am trying to run moveit servoing, I am following the steps

1. roslaunch realMove_exec.launch launch_file
2. roslaunch pose_tracking_example.launch launch_file

My config file looks like

`
###############################################
# 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.4  # Max linear velocity. Meters per publish_period. Unit is [m/s]. Only used for Cartesian commands.
  rotational:  0.8 # 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.5

## Properties of outgoing commands
publish_period: 0.034  # 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: trajectory_msgs/JointTrajectory

# 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

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

## MoveIt properties
move_group_name:  xarm6   # rewrite by robot dof
planning_frame: link_base  # The MoveIt planning frame. Often 'base_link' or 'world'

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

## Stopping behaviour
incoming_command_timeout:  0.2  # 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:  17.0  # Start decelerating when the condition number hits this (close to singularity)
hard_stop_singularity_threshold: 30.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: servo_server/delta_twist_cmds  # Topic for incoming Cartesian twist commands
joint_command_in_topic: servo_server/delta_joint_cmds # Topic for incoming joint angle commands
joint_topic: /joint_states
status_topic: servo_server/status # Publish status to this topic
command_out_topic: /xarm/xarm6_traj_controller/follow_joint_trajectory # rewrite by robot dof

## 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]

`

I am not sure why my arm is not moving, do we have to change the controller, can you please guide me through this how you moved in ros1 ? I am kind of struck at it since very long time. I think I am doing something wrong with the controller as I am not changing the controller right now. Thank You

[peterdavidfagan]

The lite6 is also shaking quite a bit for me when I run MoveIt Servo :(.

[peterdavidfagan]

@MeticulousFishFrier was your code merged?

[Akumar201]

@peterdavidfagan you are using ros1 or ros2?

[Akumar201]

@MeticulousFishFrier Hi, can you please share the JointGroupVelocityControl controller file?

[peterdavidfagan]

@peterdavidfagan you are using ros1 or ros2?

Hi @Akumar201 I have been using ROS 2. I am revisiting this now, I still observe shaking. Did you get to resolve this issue in ROS 2?

@penglongxiang do you have any advice for this, I am planning on deploying learnt policies on the robot arm and solving this issue would really help.

ROS Workspace https://github.com/peterdavidfagan/lite6_ws

Reproduce

• Build ROS workspace
• Run ros2 launch lite6_moveit_demos servo.launch.py
• Run ros2 run lite6_moveit_demos collect_data.py

Qualitative Behaviour video

Note: initial motion is using regular motion planning, after this you can see the camera shaking in the background as the robot moves from one target to the next using moveit_servo.