Velocity Control

[hello-binit]

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.

Launch the driver:

roslaunch stretch_core stretch_driver.launch mode:=velocity

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.