lite6 [Firmware V 2.4] for ros2 humble - Stuttering (Shaking) by using moveit servo with xBox controller

[lu4k87]

hi after the firmware update from v 2.0.1 to v 2.4 i have a stuttering(shaking) movement when executing moveit servo.
i use the xbox controller to enter the direction.
what do i have to do to make moveit servo for ros2 humble (i have successfully installed it as described here).

i have seen that in git: xArm-Developer/xArm_ros
(not xArm_ros2) the files were updated 2 days ago.

what is the best way to update the files? (config, urdf, srdf, ...)
I use:
ros2 launch xarm_moveit_servo lite6_moveit_servo_realmove.launch.py robot_ip:=192.168.1.xxx joystick_type:=1

thanks for help

[lu4k87]

btw... when iam using:

ros2 launch xarm_moveit_servo lite6_moveit_servo_fake.launch.py joystick_type:=1

I got a: MotionPlanning Status Warn: Requesting initial scene failed.
is there any way to downgrade the firmware to V 2.0.1?

[penglongxiang]

Hi @lu4k87, after firmware version 2.4.0, the feedback position will be from actual encoders rather than received command, so it may require higher control rate for ros-control, please try editing this file and change the parameter update_rate to be 125 to 200 (Hz). In this way the control can be smoother. Please have a try and tell us if this works for you.

Regarding to "MotionPlanning Status Warn: Requesting initial scene failed" warning in your last post, since this comes from fake/simulation scenario, it has nothing to do with our firmware version. Could you please give us the full output from the terminal in this case?

[lu4k87]

Thanks for the fast reply, will give u Feedback soon

[lu4k87]

it is necessary to build(colcom build) or/and source my ws after change the update rate?

sorry, iam a beginner

[lu4k87]

i changed the update_rate to 250 and the movement is much better...
[INFO] [launch]: All log files can be found below /home/labadmin/.ros/log/2024-09-12-17-43-23-806051-labadmin-MacPro-25459
[INFO] [launch]: Default logging verbosity is set to INFO
[INFO] [rviz2-1]: process started with pid [25479]
[INFO] [ros2_control_node-2]: process started with pid [25481]
[INFO] [component_container-3]: process started with pid [25483]
[INFO] [spawner-4]: process started with pid [25485]
[INFO] [spawner-5]: process started with pid [25487]
[ros2_control_node-2] [WARN] [1726155807.306227024] [controller_manager]: [Deprecated] Passing the robot description parameter directly to the control_manager node is deprecated. Use '~/robot_description' topic from 'robot_state_publisher' instead.
[ros2_control_node-2] [INFO] [1726155807.306582889] [resource_manager]: Loading hardware 'uf_robot_hardware/UFRobotSystemHardware'
[ros2_control_node-2] [INFO] [1726155807.319379839] [resource_manager]: Initialize hardware 'uf_robot_hardware/UFRobotSystemHardware'
[ros2_control_node-2] [INFO] [1726155807.355656811] [UFACTORY.RobotHW]: [192.168.1.175] namespace: /
[ros2_control_node-2] [INFO] [1726155807.355699273] [UFACTORY.RobotHW]: [192.168.1.175] robot_type: lite, hw_ns: ufactory, prefix: , report_type: dev
[ros2_control_node-2] [INFO] [1726155807.356393154] [UFACTORY.RobotHW]: [192.168.1.175] dof: 6, velocity_control: 0, add_gripper: 0, baud_checkset: 1, default_gripper_baud: 2000000
[ros2_control_node-2] [INFO] [1726155807.356715456] [ufactory_driver]: robot_ip=192.168.1.175, report_type=dev, dof=6
[ros2_control_node-2] [INFO] [1726155807.356815238] [ufactory_driver]: baud_checkset: 1, default_gripper_baud: 2000000
[ros2_control_node-2] SDK_VERSION: 1.13.6
[ros2_control_node-2] Tcp control connection successful
[ros2_control_node-2] change protocol identifier to 2
[component_container-3] [INFO] [1726155807.403313454] [xarm_moveit_servo_container]: Load Library: /home/labadmin/ros2_humble/install/robot_state_publisher/lib/librobot_state_publisher_node.so
[component_container-3] [INFO] [1726155807.410412916] [xarm_moveit_servo_container]: Found class: rclcpp_components::NodeFactoryTemplate<robot_state_publisher::RobotStatePublisher>
[component_container-3] [INFO] [1726155807.410492923] [xarm_moveit_servo_container]: Instantiate class: rclcpp_components::NodeFactoryTemplate<robot_state_publisher::RobotStatePublisher>
[component_container-3] [INFO] [1726155807.435045283] [robot_state_publisher]: got segment link1
[component_container-3] [INFO] [1726155807.435122090] [robot_state_publisher]: got segment link2
[component_container-3] [INFO] [1726155807.435139109] [robot_state_publisher]: got segment link3
[component_container-3] [INFO] [1726155807.435151857] [robot_state_publisher]: got segment link4
[component_container-3] [INFO] [1726155807.435164367] [robot_state_publisher]: got segment link5
[component_container-3] [INFO] [1726155807.435176304] [robot_state_publisher]: got segment link6
[component_container-3] [INFO] [1726155807.435188465] [robot_state_publisher]: got segment link_base
[component_container-3] [INFO] [1726155807.435200859] [robot_state_publisher]: got segment link_eef
[component_container-3] [INFO] [1726155807.435213501] [robot_state_publisher]: got segment link_tcp
[component_container-3] [INFO] [1726155807.435225455] [robot_state_publisher]: got segment uflite_vacuum_gripper_link
[component_container-3] [INFO] [1726155807.435238014] [robot_state_publisher]: got segment world
[INFO] [launch_ros.actions.load_composable_nodes]: Loaded node '/robot_state_publisher' in container '/xarm_moveit_servo_container'
[component_container-3] [INFO] [1726155807.444943781] [xarm_moveit_servo_container]: Load Library: /home/labadmin/ros2_humble/install/tf2_ros/lib/libstatic_transform_broadcaster_node.so
[component_container-3] [INFO] [1726155807.445499720] [xarm_moveit_servo_container]: Found class: rclcpp_components::NodeFactoryTemplate<tf2_ros::StaticTransformBroadcasterNode>
[component_container-3] [INFO] [1726155807.445527521] [xarm_moveit_servo_container]: Instantiate class: rclcpp_components::NodeFactoryTemplate<tf2_ros::StaticTransformBroadcasterNode>
[INFO] [launch_ros.actions.load_composable_nodes]: Loaded node '/static_tf2_broadcaster' in container '/xarm_moveit_servo_container'
[component_container-3] [INFO] [1726155807.463922031] [xarm_moveit_servo_container]: Load Library: /opt/ros/humble/lib/libservo_node.so
[component_container-3] [INFO] [1726155807.498972187] [xarm_moveit_servo_container]: Found class: rclcpp_components::NodeFactoryTemplate<moveit_servo::ServoNode>
[component_container-3] [INFO] [1726155807.499035897] [xarm_moveit_servo_container]: Instantiate class: rclcpp_components::NodeFactoryTemplate<moveit_servo::ServoNode>
[component_container-3] [WARN] [1726155807.509020826] [moveit_servo.servo_node]: Intra-process communication is disabled, consider enabling it by adding:
[component_container-3] extra_arguments=[{'use_intra_process_comms' : True}]
[component_container-3] to the Servo composable node in the launch file
[component_container-3] [INFO] [1726155807.528351834] [moveit_rdf_loader.rdf_loader]: Loaded robot model in 0.0105282 seconds
[component_container-3] [INFO] [1726155807.528411603] [moveit_robot_model.robot_model]: Loading robot model 'UF_ROBOT'...
[component_container-3] [INFO] [1726155807.528434437] [moveit_robot_model.robot_model]: No root/virtual joint specified in SRDF. Assuming fixed joint
[ros2_control_node-2] ROBOT_IP: 192.168.1.175, VERSION: v2.4.0, PROTOCOL: V1, DETAIL: 6,9,LI1006,DL1000,v2.4.0, TYPE1300: [0, 0]
[ros2_control_node-2] Tcp Report Rich connection successful
[ros2_control_node-2] change protocol identifier to 3
[ros2_control_node-2] Tcp Report develop connection successful
[ros2_control_node-2] report_data_size: 574, size_is_not_confirm: 0
[component_container-3] [INFO] [1726155807.677719319] [moveit_ros.current_state_monitor]: Listening to joint states on topic '/joint_states'
[ros2_control_node-2] report_data_size: 135, size_is_not_confirm: 0
[ros2_control_node-2] [INFO] [1726155807.677881499] [ufactory_driver]: [TCP STATUS] CONTROL: 1, REPORT: 1
[component_container-3] [INFO] [1726155807.683907518] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Listening to '/attached_collision_object' for attached collision objects
[component_container-3] [INFO] [1726155807.683953925] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Starting planning scene monitor
[component_container-3] [INFO] [1726155807.686109775] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Listening to '/planning_scene'
[component_container-3] [INFO] [1726155807.686646171] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Publishing maintained planning scene on '/servo_server/publish_planning_scene'
[ros2_control_node-2] [INFO] [1726155807.699548363] [ufactory_driver]: gripper_speed: 2000, gripper_max_pos: 850, gripper_frequency : 10, gripper_threshold: 3, gripper_threshold_times: 10
[ros2_control_node-2] [INFO] [1726155807.711794044] [UFACTORY.RobotHW]: [192.168.1.175] System Sucessfully configured!
[ros2_control_node-2] [INFO] [1726155807.711868151] [resource_manager]: Successful initialization of hardware 'uf_robot_hardware/UFRobotSystemHardware'
[ros2_control_node-2] [INFO] [1726155807.712142753] [resource_manager]: 'configure' hardware 'uf_robot_hardware/UFRobotSystemHardware'
[ros2_control_node-2] [INFO] [1726155807.712173108] [resource_manager]: Successful 'configure' of hardware 'uf_robot_hardware/UFRobotSystemHardware'
[ros2_control_node-2] [INFO] [1726155807.712192064] [resource_manager]: 'activate' hardware 'uf_robot_hardware/UFRobotSystemHardware'
[INFO] [launch_ros.actions.load_composable_nodes]: Loaded node '/servo_server' in container '/xarm_moveit_servo_container'
[component_container-3] [INFO] [1726155807.720959047] [xarm_moveit_servo_container]: Load Library: /home/labadmin/ros2_humble/install/xarm_moveit_servo/lib/libxarm_joystick_input.so
[component_container-3] [INFO] [1726155807.722543981] [xarm_moveit_servo_container]: Found class: rclcpp_components::NodeFactoryTemplate<xarm_moveit_servo::JoyToServoPub>
[component_container-3] [INFO] [1726155807.722580867] [xarm_moveit_servo_container]: Instantiate class: rclcpp_components::NodeFactoryTemplate<xarm_moveit_servo::JoyToServoPub>
[ros2_control_node-2] [set_state], xArm is ready to move
[component_container-3] [INFO] [1726155807.733460511] [joy_to_servo_node]: Found parameter - dof: 6
[component_container-3] [INFO] [1726155807.734125710] [joy_to_servo_node]: Found parameter - ros_queue_size: 10
[component_container-3] [INFO] [1726155807.734789397] [joy_to_servo_node]: Found parameter - joystick_type: 1
[ros2_control_node-2] [INFO] [1726155807.734895945] [UFACTORY.RobotHW]: [192.168.1.175] System Sucessfully started!
[ros2_control_node-2] [INFO] [1726155807.734945790] [resource_manager]: Successful 'activate' of hardware 'uf_robot_hardware/UFRobotSystemHardware'
[component_container-3] [INFO] [1726155807.735475799] [joy_to_servo_node]: Found parameter - joy_topic: /joy
[component_container-3] [INFO] [1726155807.736186242] [joy_to_servo_node]: Found parameter - moveit_servo.cartesian_command_in_topic: ~/delta_twist_cmds
[component_container-3] [INFO] [1726155807.736924359] [joy_to_servo_node]: Found parameter - moveit_servo.joint_command_in_topic: ~/delta_joint_cmds
[component_container-3] [INFO] [1726155807.737643676] [joy_to_servo_node]: Found parameter - moveit_servo.robot_link_command_frame: link_base
[component_container-3] [INFO] [1726155807.738247948] [joy_to_servo_node]: Found parameter - moveit_servo.ee_frame_name: link_eef
[component_container-3] [INFO] [1726155807.738836381] [joy_to_servo_node]: Found parameter - moveit_servo.planning_frame: link_base
[ros2_control_node-2] [INFO] [1726155807.742687894] [controller_manager]: update rate is 200 Hz
[ros2_control_node-2] [INFO] [1726155807.743024148] [controller_manager]: Successful set up FIFO RT scheduling policy with priority 50.
[ros2_control_node-2] The mode may be incorrect, just as a reminder, mode: 1 (0)
[INFO] [launch_ros.actions.load_composable_nodes]: Loaded node '/joy_to_servo_node' in container '/xarm_moveit_servo_container'
[ros2_control_node-2] [INFO] [1726155807.886459769] [controller_manager]: Loading controller 'lite6_traj_controller'
[rviz2-1] [INFO] [1726155807.891597817] [rviz2]: Stereo is NOT SUPPORTED
[rviz2-1] [INFO] [1726155807.892115284] [rviz2]: OpenGl version: 4.5 (GLSL 4.5)
[ros2_control_node-2] [WARN] [1726155807.917575561] [lite6_traj_controller]: [Deprecated]: "allow_nonzero_velocity_at_trajectory_end" is set to true. The default behavior will change to false.
[ros2_control_node-2] [INFO] [1726155807.922209903] [controller_manager]: Loading controller 'joint_state_broadcaster'
[rviz2-1] [INFO] [1726155807.924691581] [rviz2]: Stereo is NOT SUPPORTED
[spawner-5] [INFO] [1726155807.953545145] [spawner_lite6_traj_controller]: Loaded lite6_traj_controller
[ros2_control_node-2] [INFO] [1726155807.955309925] [controller_manager]: Configuring controller 'lite6_traj_controller'
[ros2_control_node-2] [INFO] [1726155807.955996827] [lite6_traj_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter.
[ros2_control_node-2] [INFO] [1726155807.956037430] [lite6_traj_controller]: Command interfaces are [position velocity] and state interfaces are [position velocity].
[ros2_control_node-2] [INFO] [1726155807.956083405] [lite6_traj_controller]: Using 'splines' interpolation method.
[ros2_control_node-2] [INFO] [1726155807.957137762] [lite6_traj_controller]: Controller state will be published at 25.00 Hz.
[ros2_control_node-2] [INFO] [1726155807.958794222] [lite6_traj_controller]: Action status changes will be monitored at 10.00 Hz.
[spawner-5] [INFO] [1726155807.980285584] [spawner_lite6_traj_controller]: Configured and activated lite6_traj_controller
[spawner-4] [INFO] [1726155807.982985234] [spawner_joint_state_broadcaster]: Loaded joint_state_broadcaster
[ros2_control_node-2] [INFO] [1726155807.985194241] [controller_manager]: Configuring controller 'joint_state_broadcaster'
[ros2_control_node-2] [INFO] [1726155807.985399718] [joint_state_broadcaster]: 'joints' or 'interfaces' parameter is empty. All available state interfaces will be published
[spawner-4] [INFO] [1726155808.000792899] [spawner_joint_state_broadcaster]: Configured and activated joint_state_broadcaster
[component_container-3] [INFO] [1726155808.051606396] [xarm_moveit_servo_container]: Load Library: /opt/ros/humble/lib/libjoy.so
[component_container-3] [INFO] [1726155808.068668271] [xarm_moveit_servo_container]: Found class: rclcpp_components::NodeFactoryTemplatejoy::Joy
[component_container-3] [INFO] [1726155808.068727950] [xarm_moveit_servo_container]: Instantiate class: rclcpp_components::NodeFactoryTemplatejoy::Joy
[INFO] [spawner-5]: process has finished cleanly [pid 25487]
[INFO] [spawner-4]: process has finished cleanly [pid 25485]
[rviz2-1] Warning: class_loader.impl: SEVERE WARNING!!! A namespace collision has occurred with plugin factory for class rviz_default_plugins::displays::InteractiveMarkerDisplay. New factory will OVERWRITE existing one. This situation occurs when libraries containing plugins are directly linked against an executable (the one running right now generating this message). Please separate plugins out into their own library or just don't link against the library and use either class_loader::ClassLoader/MultiLibraryClassLoader to open.
[rviz2-1] at line 253 in /opt/ros/humble/include/class_loader/class_loader/class_loader_core.hpp
[INFO] [launch_ros.actions.load_composable_nodes]: Loaded node '/joy_node' in container '/xarm_moveit_servo_container'
[component_container-3] [INFO] [1726155809.634916606] [joy_node]: Opened joystick: Xbox One Elite 2 Controller. deadzone: 0.050000
[rviz2-1] [ERROR] [1726155811.217366668] [moveit_ros_visualization.motion_planning_frame]: Action server: /recognize_objects not available
[rviz2-1] [INFO] [1726155811.260771917] [moveit_ros_visualization.motion_planning_frame]: MoveGroup namespace changed: / -> . Reloading params.
[rviz2-1] [ERROR] [1726155812.442206007] [rviz2]: The link is static or has unrealistic inertia, so the equivalent inertia box will not be shown.
[rviz2-1]
[rviz2-1] [INFO] [1726155812.573722745] [moveit_rdf_loader.rdf_loader]: Loaded robot model in 0.0162654 seconds
[rviz2-1] [INFO] [1726155812.573845654] [moveit_robot_model.robot_model]: Loading robot model 'UF_ROBOT'...
[rviz2-1] [INFO] [1726155812.573911304] [moveit_robot_model.robot_model]: No root/virtual joint specified in SRDF. Assuming fixed joint
[rviz2-1] [INFO] [1726155812.793245401] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Starting planning scene monitor
[rviz2-1] [INFO] [1726155812.793920417] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Listening to '/monitored_planning_scene'
[rviz2-1] [ERROR] [1726155812.797592898] [rviz2]: The link is static or has unrealistic inertia, so the equivalent inertia box will not be shown.
[rviz2-1]
[rviz2-1] [ERROR] [1726155812.804151883] [rviz2]: The link is static or has unrealistic inertia, so the equivalent inertia box will not be shown.
[rviz2-1]
[rviz2-1] [INFO] [1726155812.816873511] [interactive_marker_display_94369123597264]: Connected on namespace: /rviz_moveit_motion_planning_display/robot_interaction_interactive_marker_topic
[rviz2-1] [ERROR] [1726155812.821129630] [rviz2]: The link is static or has unrealistic inertia, so the equivalent inertia box will not be shown.
[rviz2-1]
[rviz2-1] [ERROR] [1726155812.827036367] [rviz2]: The link is static or has unrealistic inertia, so the equivalent inertia box will not be shown.
[rviz2-1]
[rviz2-1] [INFO] [1726155812.833093090] [moveit_ros_visualization.motion_planning_frame]: group lite6
[rviz2-1] [INFO] [1726155812.833130113] [moveit_ros_visualization.motion_planning_frame]: Constructing new MoveGroup connection for group 'lite6' in namespace ''
[rviz2-1] [INFO] [1726155812.840067329] [interactive_marker_display_94369123597264]: Sending request for interactive markers
[rviz2-1] [INFO] [1726155812.859461781] [interactive_marker_display_94369123597264]: Service response received for initialization
[component_container-3] [WARN] [1726155822.769881583] [moveit_servo.utilities]: Moving away from a singularity, decelerating
[component_container-3] [WARN] [1726155825.898093956] [moveit_servo.utilities]: Moving closer to a singularity, decelerating
[component_container-3] [WARN] [1726155828.923996514] [moveit_servo.utilities]: Moving closer to a singularity, decelerating

[penglongxiang]

@lu4k87 you should rebuild the package after parameter changing in ROS2.

May I confirm the output you posted is from which execution command? Is it from real robot control or fake simulation?

[lu4k87]

the output comes after the cmd:

ros2 launch xarm_moveit_servo lite6_moveit_servo_realmove.launch.py robot_ip:=192.168.1.xxx joystick_type:=1

i tested with xbox one elite 2 controller. and it works, after i changed the update_rate!
Unfortunately, I didn't set up the setup, but someone else who I can no longer ask.