[Bug]: set_ee_cartesian_trajectory function not working properly

[urospopovic1996]

What happened?

I have started working on wx250s robot again after some time (previous activity was last summer), so I installed Ubuntu 22.04 on new machine and followed ROS 2 Standard Software Setup (https://docs.trossenrobotics.com/interbotix_xsarms_docs/ros_interface/ros2/software_setup.html).
But, it seems that there are some issues related to set_ee_cartesian_trajectory function now. Robot is doing movement until half of the trajectory and then suddenly stops, and after some time completes the whole trajectory. I tried with different values for function parameters moving_time, wp_moving_time, wp_accel_time and wp_period, in order to increase/decrease number of waypoints that needs to be calculated, but every time robot stops at the half of the trajectory, and then completes the whole movement after some time.
Examples that I tried are some of my own scripts, but I also tried with bartender.py script and the robot is not behaving like in simulation.

This was not the issue back in the summer, so this is what I tried:

1. Uninstall ROS 2
2. Modify xsarm_amd64_install.sh script to use some older commits for interbotix_ros_core, interbotix_ros_manipulators and interbotix_ros_toolboxes
3. Install ROS 2 using the modified xsarm_amd64_install.sh script

After doing these steps, the issue is no longer present and set_ee_cartesian_trajectory function is working properly. So I assume that there were some changes in these repositories in the meantime that are causing the issue.

Changes that are made in the modified install script:
Instead of lines 314-316, these lines are added:

git clone -b "$ROS_DISTRO_TO_INSTALL" https://github.com/Interbotix/interbotix_ros_core.git
cd interbotix_ros_core
git checkout e77575dae762c312f7b2410db9402209fdf41609
cd ..
git clone -b "$ROS_DISTRO_TO_INSTALL" https://github.com/Interbotix/interbotix_ros_manipulators.git
cd interbotix_ros_manipulators
git checkout cd6679d4f09dd7e8d47fd985d377341446cc72df
cd ..
git clone -b "$ROS_DISTRO_TO_INSTALL" https://github.com/Interbotix/interbotix_ros_toolboxes.git
cd interbotix_ros_toolboxes
git checkout b0451935d6a8647ab2910daec521bdbbfb5873f7
cd ..

Robot Model

wx250s

Operating System

Ubuntu 22.04

ROS Distro

ROS 2 Humble

Steps To Reproduce

In one terminal, run the following command:

1. ros2 launch interbotix_xsarm_control xsarm_control.launch.py robot_model:=wx250s xs_driver_logging_level:=DEBUG

By setting logging level to DEBUG, I was able to see what commands are sent to robot motors and that is how I found out that the robot stops moving exactly at half of the trajectory.

In second terminal, run the following command:

1. python3 bartender.py

Relevant log output

No response

Additional Info

No response

[lukeschmitt-tr]

The Interbotix Python-ROS 2 API backend has undergone some changes recently that should solve this issue. Please update all repos to their latest commits, rebuild your workspace, and check out the updated bartender.py script for usage information.

Let us know if you run into any issues or have any questions.

[caicaixia]

Hello, I’m also facing the exactly same problem as above. Today morning I reinstalled everything again from scratch for the Humble version. The problem which is “**Robot is doing movement until half of the trajectory and then suddenly stops, and after some time completes the whole trajectory**” still remains. The set_ee_cartesian_trajectory is totally unpredictable, and it seems like the trajectory callback function has some problem:

[1726028197.524827521] [interbotix_xs_sdk.xs_sdk]: Trajectory rejected since joints are still moving.

Can you try to test it (ee_cartesian_trajectory.py from the examples) from your side and help figure it out? Thank you very much.

[caicaixia]

After doing these steps, the issue is no longer present and set_ee_cartesian_trajectory function is working properly. So I assume that there were some changes in these repositories in the meantime that are causing the issue.

Changes that are made in the modified install script: Instead of lines 314-316, these lines are added:

git clone -b "$ROS_DISTRO_TO_INSTALL" https://github.com/Interbotix/interbotix_ros_core.git
cd interbotix_ros_core
git checkout e77575dae762c312f7b2410db9402209fdf41609
cd ..
git clone -b "$ROS_DISTRO_TO_INSTALL" https://github.com/Interbotix/interbotix_ros_manipulators.git
cd interbotix_ros_manipulators
git checkout cd6679d4f09dd7e8d47fd985d377341446cc72df
cd ..
git clone -b "$ROS_DISTRO_TO_INSTALL" https://github.com/Interbotix/interbotix_ros_toolboxes.git
cd interbotix_ros_toolboxes
git checkout b0451935d6a8647ab2910daec521bdbbfb5873f7
cd ..

I tried this solution and reverted the code to an older version. In this version, the set_ee_cartesian_trajectory function works, but the set_ee_pose_components function does not work properly when y < 0. In this case, the arm exhibits unusual wrist movements.