This PR introduces a velocity mode in Stretch Core that enables velocity control of every joint except the gripper, which remains in position control. Mobile base velocity control happens via the cmd_vel topic, and velocity control of the remaining joints happens through the FollowJointTrajectory action server. A move_at_speed() method is added to the HelloNode class to make it easy to use velocity mode.
Limitations:
A bug prevents sending vel cmds to the head tilt joint
Action server returns success immediately
No feedback during action server doing vel ctrl
No backlash state incorporated into joint_states during vel mode (impacts head_pan, head_tilt, and telescoping arm)
Doesn't support Dex Wrist joints pitch and roll
All of the limitations associated with Stretch Body's set_velocity API (including no user changeable range, no smooth vel motion profile, overload errors on Dxl joints occasionally, etc.)
TODO:
[ ] add err check against goal with multiple points
[ ] document new mode and move_at_speed method
[ ] custom_full_goal or thresholds arguments for move_at_speed method
How to test
Install Stretch Body locally and checkout feature/dxl_vel_improvements. In your Catkin workspace, in the stretch_ros repo, change the branch to feature/velocity_control.
Open ipython to use the HelloNode.move_at_speed() method:
In [1]: import hello_helpers.hello_misc as hm
...: temp = hm.HelloNode.quick_create('temp')
[INFO] [1691143231.379349]: /temp started
[INFO] [1691143231.457786]: Node /temp connected to robot services.
In [2]: temp.move_at_speed({'joint_head_pan': 0.05})
In [3]: temp.move_at_speed({'joint_head_pan': 0.0})
Valid joints are joint_head_pan, joint_head_tilt, joint_lift, joint_arm, joint_wrist_yaw.
This PR introduces a velocity mode in Stretch Core that enables velocity control of every joint except the gripper, which remains in position control. Mobile base velocity control happens via the cmd_vel topic, and velocity control of the remaining joints happens through the FollowJointTrajectory action server. A
move_at_speed()method is added to the HelloNode class to make it easy to use velocity mode.Limitations:
TODO:
How to test
Install Stretch Body locally and checkout feature/dxl_vel_improvements. In your Catkin workspace, in the stretch_ros repo, change the branch to feature/velocity_control.
Launch the driver:
Open ipython to use the HelloNode.move_at_speed() method:
Valid joints are joint_head_pan, joint_head_tilt, joint_lift, joint_arm, joint_wrist_yaw.