Add stop_trajectory to dynamixels

hello-robot · Oct 6, 2021 · 49ab7e8 · 49ab7e8
1 parent 9c13817
commit 49ab7e8
Showing 1 changed file with 5 additions and 0 deletions.
diff --git a/body/stretch_body/dynamixel_hello_XL430.py b/body/stretch_body/dynamixel_hello_XL430.py
@@ -549,6 +549,11 @@ def update_trajectory(self):
             self.move_to(self.trajectory[-1].position, self._waypoint_vel, self._waypoint_accel)
             self._waypoint_ts, self._waypoint_vel, self._waypoint_accel = None, None, None
 
+    def stop_trajectory(self):
+        """Stop waypoint trajectory immediately and resets hardware
+        """
+        self._waypoint_ts, self._waypoint_vel, self._waypoint_accel = None, None, None
+
 # ##########################################
     """
     Servo calibration works by: