With Stretch Body v0.5.3, we have introduced a safety braking mechanism during velocity control for all the joints. When a joint travels too near and towards the hardstops, the braking mechanism automatically dampens the issued joint velocities and brakes smoothly without hitting the hardstops. This will ensure the safety of the hardware during an open-loop velocity control of the joints. This safety feature is implemented for the lift, arm, and all the dynamixel joints (head, end-of-arm).
Usage
This feature is implemented through the robot sentry. This safety feature will function only when the set_velocity() methods are issued from the Robot class object.
from stretch_body.robot import Robot
import time
robot = Robot()
robot.startup()
while True:
robot.lift.set_velocity(0.15)
robot.arm.set_velocity(0.4)
robot.head.get_joint('head_pan').set_velocity(6)
robot.end_of_arm.get_joint('wrist_yaw').set_velocity(3)
robot.push_command()
time.sleep(0.01)
Executing the above snippet, you should be able to see that all the joints moved at high velocities would brake smoothly at the hardstops.
Default Settings
By default, this feature is turned on for all the joints (not applicable for base). You can turn it off/on by overwriting the default settings in your stretch_user_params.yaml.
>>$ stretch_params.py | grep set_safe_velocity
stretch_body.robot_params.nominal_params param.arm.set_safe_velocity 1
stretch_body.robot_params.nominal_params param.head_pan.set_safe_velocity 1
stretch_body.robot_params.nominal_params param.head_tilt.set_safe_velocity 1
stretch_body.robot_params.nominal_params param.lift.set_safe_velocity 1
stretch_body.robot_params.nominal_params param.stretch_gripper.set_safe_velocity 1
stretch_body.robot_params.nominal_params param.wrist_yaw.set_safe_velocity 1
stretch_body.robot_params.nominal_system_params param.wrist_pitch.set_safe_velocity 1
stretch_body.robot_params.nominal_system_params param.wrist_roll.set_safe_velocity 1
Additionally, you can increase or decrease the parameter vel_brakezone_factor, allowing you to over-dampen or under-damp the braking mechanism near the hardtops. This value is already set to optimal values, ensuring safe braking even while maximum velocities are applied. Therefore, this param should only be increased if you notice the joints hitting the hardstops during velocity control.
The vel_brakezone_factor should be a positive value in the range 0 to 10.
>>$ stretch_params.py | grep vel_brakezone_factor
stretch_body.robot_params.nominal_params param.arm.motion.vel_brakezone_factor 0.2
stretch_body.robot_params.nominal_params param.head_pan.motion.vel_brakezone_factor 1.5
stretch_body.robot_params.nominal_params param.head_tilt.motion.vel_brakezone_factor 1.2
stretch_body.robot_params.nominal_params param.lift.motion.vel_brakezone_factor 0.03
stretch_body.robot_params.nominal_params param.stretch_gripper.motion.vel_brakezone_factor 1
stretch_body.robot_params.nominal_params param.wrist_yaw.motion.vel_brakezone_factor 1
stretch_body.robot_params.nominal_system_params param.wrist_pitch.motion.vel_brakezone_factor 1
stretch_body.robot_params.nominal_system_params param.wrist_roll.motion.vel_brakezone_factor 1
Upgrading/Installing
You must upgrade your Stretch Body and Stretch Tool share if using a dex wrist.
pip3 install -U hello-robot-stretch-body
pip3 install -U hello-robot-stretch-tool-share