# Implementing contact detection for dynamixels

**URL:** <https://forum.hello-robot.com/t/implementing-contact-detection-for-dynamixels/868>\
**Category:** Share\
**Created:** [December 12, 2023, 9:41pm UTC](https://forum.hello-robot.com/t/implementing-contact-detection-for-dynamixels/868 "2023-12-12T21:41:12Z")\
**Posts on this page:** 3\
**Page:** 1

<div class="post-metadata">

**Author:** ![jesusrdz](https://yyz2.discourse-cdn.com/flex030/user_avatar/forum.hello-robot.com/jesusrdz/32/580_2.png) [@jesusrdz](https://forum.hello-robot.com/u/jesusrdz)\
**Post date:** [December 12, 2023, 9:41pm UTC](https://forum.hello-robot.com/t/implementing-contact-detection-for-dynamixels/868/1 "2023-12-12T21:41:12Z")

</div>

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](https://github.com/hello-robot/stretch_body/pull/203) 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](https://docs.hello-robot.com/0.2/stretch-tutorials/stretch_body/tutorial_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](https://github.com/hello-robot/stretch_body/blob/master/body/stretch_body/dynamixel_hello_XL430.py) and minimal changes in the **wrist\_yaw** (if you want to learn more about the [dynamixel servos](https://docs.hello-robot.com/0.2/stretch-tutorials/stretch_body/tutorial_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:

```auto
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:

```auto
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!

```auto
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.

```auto
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.

```auto
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.

```auto
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.

```auto
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](https://github.com/hello-robot/stretch_body/blob/master/body/stretch_body/wrist_yaw.py), [pitch](https://github.com/hello-robot/stretch_tool_share/blob/master/python/stretch_tool_share/stretch_dex_wrist/wrist_pitch.py) and [roll](https://github.com/hello-robot/stretch_tool_share/blob/master/python/stretch_tool_share/stretch_dex_wrist/wrist_roll.py) 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:

```python
    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:

```auto
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!**

![video_hr_3](https://canada1.discourse-cdn.com/flex030/uploads/hello_robot2/original/1X/c897845d4178098d9c65c3bc809678ef3ff09c1c.gif)

Best,  
Jesus Rodriguez

---

<div class="post-metadata">

**Author:** ![Vipul\_Patel](https://yyz2.discourse-cdn.com/flex030/user_avatar/forum.hello-robot.com/vipul_patel/32/928_2.png) [@Vipul\_Patel](https://forum.hello-robot.com/u/Vipul_Patel)\
**Post date:** [January 28, 2025, 6:44pm UTC](https://forum.hello-robot.com/t/implementing-contact-detection-for-dynamixels/868/2 "2025-01-28T18:44:08Z")

</div>

This is a really useful experiment, has it been added to stretch core?

---

<div class="post-metadata">

**Author:** ![bshah](https://yyz2.discourse-cdn.com/flex030/user_avatar/forum.hello-robot.com/bshah/32/55_2.png) [@bshah](https://forum.hello-robot.com/u/bshah)\
**Post date:** [January 28, 2025, 8:37pm UTC](https://forum.hello-robot.com/t/implementing-contact-detection-for-dynamixels/868/3 "2025-01-28T20:37:47Z")

</div>

No, this code isn’t mainlined in [Stretch Body](https://github.com/hello-robot/stretch_body). IIRC, the challenge is filtering out false positives. @jesusrdz’s filter depends on servo effort, which is impacted by confounding variables, e.g. system voltage, payload weight, age of the motor. You can normalize for these variables through system ID, incorporate other sources of data into the filter (e.g. visual inspection), and/or add simplifying assumptions. For your application, if you can add assumptions (e.g. the robot won’t be holding any payload), you’ll find it easier to design a robust contact detection filter and @jesusrdz’s tutorial is a great start to an implementation.