When launching the stretch_driver in trajectory mode: ros2 launch stretch_core stretch_driver.launch.py mode:=trajectory, then using ros action client to send trajectory to /stretch_controller/follow_joint_trajectory. The lift will “bump” up a little, but then the arm and lift both did not move at all for the entire trajectory duration while driver announced traj succeeded. During the trajectory, I can hear extra humming noise from the robot (I assume its motor being activated) but no motion is visible. On the other hand, If I launch the driver with position mode, the arm and lift both moved. I have also tried to also send the same command to joint_wrist_yaw and I can see the wrist yaw motor moved.
After some debugging, I have track down the issue to around the stretch_body.prismatic_joint.PrismaticJoint.update_trajectory function. Basically it should be incrementing the next_segment_id, however, the self.motor.status['waypoint_traj']['state'] is stuck on waiting_on_sync for both arm and lift the whole time. Thus not going into the logic and advancing the trajectory.
I initially run into this problem when working on a moveit port. However I have also confirmed without moveit, the same problem persists. I did the additional tests with this script draw_circle.py · GitHub (need to remove the rclpy.init() in hello_misc helper). With reboot and updated stretch_body pip package to latest.
This is a big blocker for my project as I need to do trajectory control for Stretch. Any help or input would be great. I can also do more testing and debugging if anyone have idea on what would cause the motors to be stuck in the waiting_on_sync mode.
draw_circle_SE3.py is a clone of the draw_circle example, simple_ros_traj.py simply sends a ROS trajectoy action that move lift and arm up-down for a tiny bit. Both of which uses the stretch ros driver ros2 launch stretch_core stretch_driver.launch.py mode:=trajectory broadcast_odom_tf:=True.
The third script Traj-minimal.py is a minimal script that move the lift without using ROS. It directly use the python API with a Robot object, but doing similar call like robot.lift.trajectory.add, robot.follow_trajectory(), and robot.lift.update_trajectory().
When running the first two script, I have kept running into the same issue, where motor is not moving and self.motor.status['waypoint_traj']['state'] is stuck on waiting_on_sync.
On the other hand, when running the last script Traj-minimal.py, it can always move the lift even right after previous Ros driver move have failed.
This seems to imply the issue would be at how the ROS robot driver sends the command over to the robot.
Hi @leograyc, when you run Traj-minimal.py, do you see the trajectory execute smoothly? Would you try running stretch_trajectory_jog.py --lift , execute the trajectory, and film a quick video of the robot’s motion? It brings up Matplotlib, so you’ll need a display plugged into the robot.
The Traj-minimal.py some time runs decently smooth, however sometime will trigger a dump event and drop. I assume this will happen because I didn’t have a smooth trapezoid trajectory, just a constant velocity traj from beginning. I’ve included a video below, where the arm think it’s bumpped when coming down.
When running the stretch_trajectory_jog.py --lift the robot was notable to complete the trajectory and failed half way.
Before both run, the robot has been left on for a couple days, and I just call the homing sequence right before running these scripts.
I have powered off the robot, let it sit for 1 minute, and re-tested these.
This time Traj-minimal.py runs fine, but stretch_trajectory_jog.py --lift still failed to go through.
Hi @leograyc, we were able to identify the bug that was preventing the motors from tracking these trajectories correctly. The fix is released as a firmware update. Would you update your firmware and see if the trajectories in Traj-minimal.py and stretch_trajectory_jog.py execute fully.
So After updating the ros package, I found the issue still exists when trying to command the robot with ros action in trajectory mode. The issue will be cleared after a homing sequence. However it still pop up quite often.
And when I launch the driver, I sometime gets this error
▶ ros2 launch stretch_core stretch_driver.launch.py mode:=trajectory broadcast_odom_tf:=True
[INFO] [launch]: All log files can be found below /home/leogray/.ros/log/2024-08-15-17-47-49-556740-stretch-se3-3047-53755
[INFO] [launch]: Default logging verbosity is set to INFO
[INFO] [stretch_driver-3]: process started with pid [53761]
[INFO] [joint_state_publisher-1]: process started with pid [53757]
[INFO] [robot_state_publisher-2]: process started with pid [53759]
[stretch_driver-3] [INFO] [1723769271.094701470] [stretch_driver]: For use with S T R E T C H (TM) RESEARCH EDITION from Hello Robot Inc.
[stretch_driver-3] [INFO] [1723769271.094962489] [stretch_driver]: stretch_driver started
[stretch_driver-3] Key mapped to End-Of-Arm Tool: eoa_wrist_dw3_tool_sg3
[stretch_driver-3] Caught Exception in Thread: SystemMonitorThread
[stretch_driver-3] Traceback (most recent call last):
[stretch_driver-3] File "/usr/lib/python3.10/threading.py", line 1016, in _bootstrap_inner
[stretch_driver-3] self.run()
[stretch_driver-3] File "/home/leogray/.local/lib/python3.10/site-packages/stretch_body/robot.py", line 166, in run
[stretch_driver-3] self.step()
[stretch_driver-3] File "/home/leogray/.local/lib/python3.10/site-packages/stretch_body/robot.py", line 158, in step
[stretch_driver-3] self.robot._update_trajectory_non_dynamixel()
[stretch_driver-3] File "/home/leogray/.local/lib/python3.10/site-packages/stretch_body/robot.py", line 678, in _update_trajectory_non_dynamixel
[stretch_driver-3] self.arm.update_trajectory()
[stretch_driver-3] File "/home/leogray/.local/lib/python3.10/site-packages/stretch_body/prismatic_joint.py", line 531, in update_trajectory
[stretch_driver-3] if self.traj_guarded_event!=self.motor.status['guarded_event']:
[stretch_driver-3] AttributeError: 'Arm' object has no attribute 'traj_guarded_event'
However, the driver didn’t crash after this, but I do get the issue of arm not moving when this show up.