Greetings Stretch community!
My name is Jesus Rodriguez and I’m currently an intern here at Hello Robot. I wanted to share with you something that I’ve been working on these past weeks. Please note that this is only an experiment, so use this at your own risk.
As you can see in the title, I’ve been working on how to implement contact detection in the dynamixels for the dex wrist (just in case, don’t try this on the camera as it is more fragile than the wrist and you can break it!). In addition, FYI, I used the recently added gamepad_teleop that’s been recently released, this is currently in beta.
To give everyone a brief insight on this topic, there is already contact detection for the prismatic joint class (there are also other contact models, check them in our docs), but there’s no contact detection in the wrist that is part from the dynamixel class, that’s why I made this tutorial.
Changes in the dynamixels
Every change that we make is going to be inside the dynamixel_hello_XL430.py and minimal changes in the wrist_yaw (if you want to learn more about the dynamixel servos), if you have the dex wrist just like me then you’ll also need to make changes in the wrist_pitch and wrist_roll. For now, let’s just go to the dynamixel_hello_XL430 and search the set_velocity method:
def set_velocity(self,v_des,a_des=None):
if self.was_runstopped:
return
if not self.watchdog_enabled:
self.motor.disable_watchdog()
self.disable_torque()
self.motor.enable_watchdog()
self.watchdog_enabled = True
self.enable_torque()
v = min(self.params['motion']['max']['vel'],abs(v_des))
v_des = -1*v if v_des<0 else v
nretry = 2
if not self.hw_valid:
return
if self.params['req_calibration'] and not self.is_calibrated:
self.logger.warning('Dynamixel not calibrated: %s' % self.name)
print('Dynamixel not calibrated:', self.name)
return
success = False
if self.motor.get_operating_mode()!=1:
self.enable_velocity_ctrl()
for i in range(nretry):
try:
if self.params['set_safe_velocity'] and self.in_vel_brake_zone: # only when sentry is active
self._step_vel_braking(v_des, a_des)
else:
self.set_motion_params(a_des=a_des)
t_des = self.world_rad_to_ticks_per_sec(v_des)
self.motor.set_vel(t_des)
self._prev_set_vel_ts = time.time()
success = True
break
except(termios.error, DynamixelCommError, IndexError):
self.logger.warning('Dynamixel communication error during move_to_vel on %s: ' % self.name)
self.comm_errors.add_error(rx=True, gsr=False)
if self.bubble_up_comm_exception:
raise DynamixelCommError
This is the normal set_velocity API that everyone has, we are going to make changes in here, check how we are going to modify it:
def set_velocity(self,v_des,a_des=None, contact_thresh_pos=8, contact_thresh_neg = -8):
#print(f"Effort[{self.name}] : {self.status['effort']}")
v = min(self.params['motion']['max']['vel'],abs(v_des))
v_des = -1*v if v_des<0 else v
nretry = 2
if not self.hw_valid:
return
if self.params['req_calibration'] and not self.is_calibrated:
self.logger.warning('Dynamixel not calibrated: %s' % self.name)
print('Dynamixel not calibrated:', self.name)
return
if self.in_collision_stop['pos'] and v_des>0:
self.logger.warning('In collision. Motion disabled in direction %s for %s. Not executing set_velocity'%('pos',self.name))
v_des=0
elif self.in_collision_stop['neg'] and v_des<0:
self.logger.warning('In collision. Motion disabled in direction %s for %s. Not executing set_velocity'%('neg',self.name))
v_des=0
success = False
if self.motor.get_operating_mode()!=1:
self.enable_velocity_ctrl()
for i in range(nretry):
try:
if self.status['effort'] > contact_thresh_pos or self.status['effort'] < contact_thresh_neg:
self.set_motion_params(a_des=self.params['motion']['slow']['accel'])
self._prev_set_vel_ts = time.time()
print(f"Contact detected at {self.name} motor")
current_position = self.status['pos']
kp = -1.5
self.motor.set_vel(self.status['effort']*kp)
self.move_to(current_position)
elif self.params['set_safe_velocity'] and self.in_vel_brake_zone: # only when sentry is active
self._step_vel_braking(v_des, a_des)
else:
self.set_motion_params(a_des=a_des)
t_des = self.world_rad_to_ticks_per_sec(v_des)
self.motor.set_vel(t_des)
self._prev_set_vel_ts = time.time()
success = True
break
except(termios.error, DynamixelCommError, IndexError):
self.logger.warning('Dynamixel communication error during move_to_vel on %s: ' % self.name)
self.comm_errors.add_error(rx=True, gsr=False)
if self.bubble_up_comm_exception:
raise DynamixelCommError
Let’s break the code down!
def set_velocity(self,v_des,a_des=None, contact_thresh_pos=8, contact_thresh_neg = -8):
To define the contact thresholds, we need to add them inside the set_velocity, you can define the thresholds that you want to try.
for i in range(nretry):
try:
if self.status['effort'] > contact_thresh_pos or self.status['effort'] < contact_thresh_neg:
self.set_motion_params(a_des=self.params['motion']['slow']['accel'])
self._prev_set_vel_ts = time.time()
print(f"Contact detected at {self.name} motor")
current_position = self.status['pos']
kp = -1.5
self.motor.set_vel(self.status['effort']*kp)
self.move_to(current_position)
elif self.params['set_safe_velocity'] and self.in_vel_brake_zone: # only when sentry is active
self._step_vel_braking(v_des, a_des)
Going to the for loop, you will need to rearrange the if statements and create yours– we want ours to be a priority so the contact detection is going to be the first one. As you can see, the self.params now is an elif and our if statement is going to be our contact detection.
if self.status['effort'] > contact_thresh_pos or self.status['effort'] < contact_thresh_neg:
self.set_motion_params(a_des=self.params['motion']['slow']['accel'])
self._prev_set_vel_ts = time.time()
print(f"Contact detected at {self.name} motor")
current_position = self.status['pos']
We want to sense if the effort is above 8 and below -8 for our dex wrist to stop the movement, (this is something that you can change according to your preferences). Then we want to change the acceleration from the set_motion_params to slow, because we want to make the movement as smooth as possible– when it bumps it decreases the acceleration slowly instead of decreasing at max acceleration and having a jerky movement with oscillations.
We also want to know when a contact is detected for security purposes, but be aware that just the movement of the wrist can cause contact with itself, however, it doesn’t cause that much effort. That’s the reason why we set a threshold of -8 and 8 in this tutorial and not -2 and 2 for example.
print(f"Effort[{self.name}] : {self.status['effort']}")
This print statement is only if you want to print in the terminal the effort from the wrist movements to adjust the thresholds yourself, we will write it at the beginning of the method. It is commented in the code.
current_position = self.status['pos']
kp = -1.5
self.motor.set_vel(self.status['effort']*kp)
self.move_to(current_position)
Now for the important part, we want to get our current position, the best way to do this is to write self.status with the ‘pos’ flag, but this will only work if we are importing the robot class. If we are not importing the robot class, we need to call the pull status every time before the set velocity. If we use self.get_pos it’s going to work as well, but it’s redundant and may cause a race condition. We write our P controller with the value that you think it’s better, in my case I decided for -1.5 and multiply the effort with it, this will set a new velocity when it bumps into an obstacle and will adjust the velocity depending in the amount of effort it detected and finally we are going to move_to the same position that we got earlier. The reason behind this is because it’s a safe measure, if the velocity doesn’t adjust correctly on time we will move to the current position and this will stop the movement “completely”, it also prevents overloading from the dynamixels making the wrist to keep moving.
And that’s it, there’s your contact detection for the dynamixels, you can make a better controller instead of the P controller that I made, this tutorial is just for you to try it out and make something better!
Changes in the wrist codes
Moving forward to the wrist changes, if you have a dex wrist, you’ll need to make these changes in the yaw, pitch and roll codes. If you have a standard wrist, then you’ll only need to make changes in the yaw code (the process is the same so I’ll do it in the wrist_yaw). First go to the step_sentry method and then comment every line except for the DynamixelHelloXL430.step_sentry just like this:
def step_sentry(self, robot):
DynamixelHelloXL430.step_sentry(self, robot)
# if self.hw_valid and self.robot_params['robot_sentry']['wrist_yaw_overload']:
# if self.status['stall_overload']:
# if self.in_vel_mode:
# self.enable_pos()
# if self.status['effort']>0:
# self.move_by(self.params['stall_backoff'])
# self.logger.debug('Backoff at stall overload')
# else:
# self.move_by(-1*self.params['stall_backoff'])
# self.logger.debug('Backoff at stall overload')
As I said, if you have a standard wrist, those are all the changes that you need to make, if you have a dex wrist then make the same changes for the pitch and roll. With this, you’ve finished making your own contact detection! So now save everything and run in the terminal:
stretch_gamepad_teleop.py
And that’s the end of this post, I have high hopes that everyone can make something really useful with this, just keep in mind that we are using the new gamepad_teleop and that if we lift something heavy then we are going to have some problems, mostly if you have a dex wrist and want to move the object by making a pitch upward movement, so remember that this is not resolving everything it’s just an experiment!
Best,
Jesus Rodriguez