Velocity control for dxl joints + bugfixes

[hello-binit]

This PR introduces the set_velocity() API to the Dynamixel joints, including the head pan/tilt and wrist yaw/pitch/roll/gripper. This is the same velocity control API used by the other joints/mobile base. Additionally, this PR fixes a bug where interrupting a low-level communication with the Dxls (e.g. through a SIGINT) would cause the dxl to become inaccessible until the Linux process was killed.

An example script using the new API:

import stretch_body.robot

r = stretch_body.robot.Robot()
r.startup()
assert(r.is_calibrated()) # the robot must be homed

# move the wrist_yaw counter clockwise at 0.1 rad/s
r.end_of_arm.get_joint('wrist_yaw').set_velocity(0.1)

How to test

1. Install this branch locally using the instructions here.
2. Run the example script above and verify the wrist yaw joint rotates at 0.1 rad/s

[hello-fazil]

The DynamixelXL430.enable_vel() is an EEPROM write operation, which delays the set_velocity() method execution when called within a control loop which makes the motion jittery and inaccurate.
Verified this behavior with test_dxl_vel.py which tests velocity control accuracy using higher-level Dynamixel.HelloXL430.set_velocity() compared to using the lower-level DynamixelXL430.set_vel() which provides smoother/accurate motion within a control loop.

Suggested change

	self.motor.disable_torque()
	self.motor.enable_vel()
	self.motor.enable_torque()

We could suggest users enable velocity mode using a dedicated method and avoid calling enable_vel() method on every set_velocity() command.

[hello-fazil]

@hello-binit Could you confirm if the set_velocity() is not intended to be used in a control loop?
Else I will revert the following commit.

[hello-binit]

Set_velocity most likely would be used in a >10hz control loop, so you're correct in taking the EEPROM calls out of the method. Instead of creating a separate method to change into velocity mode, I think the set_velocity method should check the operating mode and change into velocity mode only when needed. This would make it so only motor.get_operating_mode() gets called at the control rate. If calling get_operating_mode() still causes jitter, I think we can cache the current control mode in the DynamixelXL430Hello class. Wherever the operating mode is changed, a variable is also updated. Then move_to/by can check the variable to see if the joint is in position/multiturn mode before sending a command, and set_velocity can check if the joint is in velocity mode before sending a command.

[hello-fazil]

The motor.get_operating_mode()(EEPROM read operation) doesn't seem to affect the motion when called every time and performs same as accessing the lower level method. The velocity mode is enabled here only after checking the operating mode now. Thanks for the suggestions.

[hello-fazil]

Stretch ROS drivers errors out when dynamixel groups are accessed with ValueError: signal only works in main thread. This is because Python signals could only be used from the main thread, not external threads, i.e., simple action server. (reference).

[hello-binit]

Let's wrap the call to signal.signal in a try/catch where the ValueError is ignored. A KeyboardInterrupt wouldn't reach a python thread anyways, so there's no risk of the packet_handler becoming interrupted at the wrong time.

[hello-fazil]

Thanks for the suggestions. Appropriate exception handling is implemented and fixes the issues.

[hello-fazil]

Good to merge.