I am now using Stretch 3. MoveIt can plan a path for my defined group: joint_arm_l0, joint_arm_l1, joint_arm_l2, joint_arm_l3, joint_lift, joint_wrist_pitch, joint_wrist_roll, and joint_wrist_yaw. The planned results are sent to the service stretch_controller/follow_joint_trajectory. The robot is running in trajectory mode. When it receives the trajectory, only joint_wrist_pitch, joint_wrist_roll, and joint_wrist_yaw can move, while the other joints (joint_arm_l0, joint_arm_l1, joint_arm_l2, joint_arm_l3, and joint_lift) do not move. The feedback from the driver is traj successed. Do you know the possible reason? The following images are my obtained feedback, my trajectory goal, my visible MoveIt planning result, and the planned trajectory saved in a JointTrajectoryPoint format. Are the other joints also available to control using the service stretch_controller/follow_joint_trajectory?
@Yongbo, the /stretch_controller/follow_joint_trajectory action server merges the arm links L0 through L3 before processing the trajectory. The merging function is defined here: stretch_ros2/hello_helpers/src/hello_helpers/hello_misc.py at humble · hello-robot/stretch_ros2 · GitHub
Then, the waypoints are added and executed here: stretch_ros2/stretch_core/stretch_core/joint_trajectory_server.py at 619499a5790f33d4b4da022c002db0063c4aa762 · hello-robot/stretch_ros2 · GitHub
You might try adding print statements along the code path to ensure that the arm link waypoints are being aggregated and executed correctly.