Dynamixel trajectories

body/stretch_body/dynamixel_XL430.py

@@ -509,7 +509,13 @@ def get_pos(self):
         self.handle_comm_result('XL430_ADDR_PRESENT_POSITION', dxl_comm_result, dxl_error)
         return xn
 
-
+    def get_moving_status(self):
+        if not self.hw_valid:
+            return
+        with self.pt_lock:
+            p, dxl_comm_result, dxl_error = self.packet_handler.read1ByteTxRx(self.port_handler, self.dxl_id, XL430_ADDR_MOVING_STATUS)
+        self.handle_comm_result('XL430_ADDR_MOVING_STATUS', dxl_comm_result, dxl_error)
+        return p
 
     def get_load(self):
         if not self.hw_valid:

body/stretch_body/dynamixel_X_chain.py

@@ -23,6 +23,7 @@ def __init__(self, usb, name):
         Device.__init__(self, name)
         self.usb = usb
         self.pt_lock = threading.RLock()
+        self.thread_rate_hz = 15.0
 
         try:
             prh.LATENCY_TIMER = self.params['dxl_latency_timer']
@@ -87,14 +88,32 @@ def startup(self, threaded=False):
                 return False
         return True
 
+    def _thread_loop(self):
+        self.pull_status()
+        self.update_trajectory()
+
     def stop(self):
         Device.stop(self)
         if not self.hw_valid:
             return
-        for mk in self.motors.keys():
-            self.motors[mk].stop()
+        for motor in self.motors:
+            self.motors[motor]._waypoint_ts = None
+            self.motors[motor]._waypoint_vel = None
+            self.motors[motor]._waypoint_accel = None
+            self.motors[motor].stop()
         self.hw_valid = False
 
+    def follow_trajectory(self, v_r=None, a_r=None, req_calibration=False, move_to_start_point=True):
+        for motor in self.motors:
+            self.motors[motor].follow_trajectory(v_r, a_r, req_calibration, move_to_start_point)
+
+    def update_trajectory(self):
+        for motor in self.motors:
+            self.motors[motor].update_trajectory()
+
+    def stop_trajectory(self):
+        for motor in self.motors:
+            self.motors[motor].stop_trajectory()
 
     def pull_status(self):
         if not self.hw_valid:

body/stretch_body/dynamixel_hello_XL430.py

@@ -1,6 +1,7 @@
 from __future__ import print_function
 from stretch_body.dynamixel_XL430 import *
 from stretch_body.device import Device
+from stretch_body.trajectories import RevoluteTrajectory
 import time
 from stretch_body.hello_utils import *
 import termios
@@ -58,6 +59,11 @@ def __init__(self, name, chain=None):
         self.status={'timestamp_pc':0,'comm_errors':0,'pos':0,'vel':0,'effort':0,'temp':0,'shutdown':0, 'hardware_error':0,
                      'input_voltage_error':0,'overheating_error':0,'motor_encoder_error':0,'electrical_shock_error':0,'overload_error':0,
                      'stalled':0,'stall_overload':0,'pos_ticks':0,'vel_ticks':0,'effort_ticks':0}
+        self.thread_rate_hz = 15.0
+        self.trajectory = RevoluteTrajectory()
+        self._waypoint_ts = None
+        self._waypoint_vel = self.params['motion']['trajectory_max']['vel_r']
+        self._waypoint_accel = self.params['motion']['trajectory_max']['accel_r']
 
         #Share bus resource amongst many XL430s
         self.motor = DynamixelXL430(dxl_id=self.params['id'],
@@ -165,9 +171,13 @@ def startup(self, threaded=False):
             print('DynamixelHelloXL430 Ping failed...', self.name)
             return False
 
+    def _thread_loop(self):
+        self.pull_status()
+        self.update_trajectory()
 
     def stop(self):
         Device.stop(self)
+        self._waypoint_ts, self._waypoint_vel, self._waypoint_accel = None, None, None
         if self.hw_valid:
             self.disable_torque()
             self.hw_valid = False
@@ -289,7 +299,20 @@ def mark_zero(self):
         self.params['zero_t']=x
         self.write_device_params(self.name, self.params)
 
+    def wait_until_at_setpoint(self,timeout=15.0):
+        """Polls for moving status to wait until at commanded position goal
 
+        Returns
+        -------
+        bool
+            True if success, False if timeout
+        """
+        ts = time.time()
+        while time.time() - ts < timeout:
+            if self.motor.get_moving_status() & 1 == 1:
+                return True
+            time.sleep(0.1)
+        return False
 
     def pretty_print(self):
         if not self.hw_valid:
@@ -322,11 +345,11 @@ def pretty_print(self):
         #self.motor.pretty_print()
 
     def step_sentry(self, robot):
-
         if self.hw_valid and self.robot_params['robot_sentry']['dynamixel_stop_on_runstop'] and self.params['enable_runstop']:
             is_runstopped = robot.pimu.status['runstop_event']
             if is_runstopped is not self.was_runstopped:
                 if is_runstopped:
+                    self.stop_trajectory()
                     self.disable_torque()
                 else:
                     self.enable_torque()
@@ -449,6 +472,90 @@ def set_pwm(self,x):
         except (termios.error, DynamixelCommError):
             self.comm_errors.add_error(rx=False, gsr=False)
 
+    # ######### Waypoint Trajectory Interface ##############################
+
+    def follow_trajectory(self, v_r=None, a_r=None, req_calibration=True, move_to_start_point=True):
+        """Starts executing a waypoint trajectory
+
+        `self.trajectory` must be populated with a valid trajectory before calling
+        this method.
+
+        Parameters
+        ----------
+        v_r : float
+            velocity limit for trajectory in radians per second
+        a_r : float
+            acceleration limit for trajectory in radians per second squared
+        req_calibration : bool
+            whether to allow motion prior to homing
+        move_to_start_point : bool
+            whether to move to the trajectory's start to avoid a jump, this
+            time to move doesn't count against the trajectory's timeline
+        """
+        # check if joint valid, homed, and previous trajectory not executing
+        if not self.hw_valid:
+            self.logger.warning('Dynamixel connection to hardware not valid')
+            return
+        if req_calibration:
+            if not self.is_calibrated:
+                self.logger.warning('Dynamixel not homed')
+                return
+        if self._waypoint_ts is not None:
+            self.logger.error('Dynamixel waypoint trajectory already active')
+            return
+
+        # check if trajectory valid
+        vel_limit = v_r if v_r is not None else self.params['motion']['trajectory_max']['vel_r']
+        acc_limit = a_r if a_r is not None else self.params['motion']['trajectory_max']['accel_r']
+        valid, reason = self.trajectory.is_valid(vel_limit, acc_limit)
+        if not valid:
+            self.logger.warning('Dynamixel trajectory not valid: {0}'.format(reason))
+            return
+
+        # set defaults
+        self._waypoint_vel = min(abs(v_r), self.params['motion']['trajectory_max']['vel_r']) \
+            if v_r is not None else self.params['motion']['trajectory_max']['vel_r']
+        self._waypoint_accel = min(abs(a_r), self.params['motion']['trajectory_max']['accel_r']) \
+            if a_r is not None else self.params['motion']['trajectory_max']['accel_r']
+
+        # move to start point
+        if move_to_start_point:
+            self.move_to(self.trajectory[0].position)
+            if not self.wait_until_at_setpoint():
+                self.logger.warning('Dynamixel unable to reach starting point')
+                return
+
+        # start trajectory
+        self._waypoint_ts = time.time()
+        p0, _, _ = self.trajectory.evaluate_at(time.time() - self._waypoint_ts)
+        self.move_to(p0, self._waypoint_vel, self._waypoint_accel)
+
+    def update_trajectory(self):
+        """Updates hardware with the next position goal of `self.trajectory`
+
+        This method must be called frequently to enable complete trajectory execution
+        and preemption of future segments. If used with `stretch_body.robot.Robot` or
+        with `self.startup(threaded=True)`, a background thread is launched for this.
+        Otherwise, the user must handle calling this method.
+        """
+        # check if joint valid, previous trajectory not executing, and not runstopped
+        if not self.hw_valid or self._waypoint_ts is None:
+            return
+        if self.was_runstopped:
+            return
+
+        if (time.time() - self._waypoint_ts) < self.trajectory[-1].time:
+            p1, _, _ = self.trajectory.evaluate_at(time.time() - self._waypoint_ts)
+            self.move_to(p1, self._waypoint_vel, self._waypoint_accel)
+        else:
+            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:

body/stretch_body/end_of_arm.py

@@ -19,6 +19,8 @@ def __init__(self, name='end_of_arm'):
             dynamixel_device = getattr(importlib.import_module(module_name), class_name)(chain=self)
             self.add_motor(dynamixel_device)
 
+    def startup(self, threaded=True):
+        return DynamixelXChain.startup(self, threaded=threaded)
 
     def get_joint(self, joint_name):
         """Retrieves joint by name.

body/stretch_body/head.py

@@ -17,6 +17,9 @@ def __init__(self):
                       'wheels': [deg_to_rad(0), deg_to_rad(-90)], 'left': [deg_to_rad(90), deg_to_rad(0)],
                       'up': [deg_to_rad(0), deg_to_rad(30)]}
 
+    def startup(self, threaded=True):
+        return DynamixelXChain.startup(self, threaded=threaded)
+
     def get_joint(self, joint_name):
         """Retrieves joint by name.
 

body/stretch_body/robot_params.py

@@ -75,22 +75,46 @@
     "head_pan": {
         "retry_on_comm_failure": 1,
         "baud": 57600,
-        "enable_runstop": 1
+        "enable_runstop": 1,
+        "motion": {
+            "trajectory_max": {
+                "vel_r": 4.0,
+                "accel_r": 8.0
+            }
+        }
     },
     "head_tilt": {
         "retry_on_comm_failure": 1,
         "baud": 57600,
-        "enable_runstop": 1
+        "enable_runstop": 1,
+        "motion": {
+            "trajectory_max": {
+                "vel_r": 4.0,
+                "accel_r": 8.0
+            }
+        }
     },
     "wrist_yaw": {
         "retry_on_comm_failure": 1,
         "baud": 57600,
-        "enable_runstop": 1
+        "enable_runstop": 1,
+        "motion": {
+            "trajectory_max": {
+                "vel_r": 4.0,
+                "accel_r": 8.0
+            }
+        }
     },
     "stretch_gripper": {
         "retry_on_comm_failure": 1,
         "baud": 57600,
-        "enable_runstop": 1
+        "enable_runstop": 1,
+        "motion": {
+            "trajectory_max": {
+                "vel_r": 50.0,
+                "accel_r": 100.0
+            }
+        }
     },
     "base": {
         "sentry_max_velocity": {

body/stretch_body/stretch_gripper.py

@@ -18,6 +18,9 @@ def __init__(self, chain=None):
                       'open': self.pct_max_open,
                       'close': -100}
 
+    def startup(self, threaded=True):
+        return DynamixelHelloXL430.startup(self, threaded=threaded)
+
     def home(self,move_to_zero=True):
         DynamixelHelloXL430.home(self,single_stop=True,move_to_zero=move_to_zero,delay_at_stop=3.0)
 

body/stretch_body/wrist_yaw.py

@@ -14,6 +14,9 @@ def __init__(self, chain=None):
                       'forward': deg_to_rad(0.0),
                       'stow': deg_to_rad(180.0)}
 
+    def startup(self, threaded=True):
+        return DynamixelHelloXL430.startup(self, threaded=threaded)
+
     def home(self):
         """
         Home to hardstops

body/test/test_end_of_arm.py

@@ -18,7 +18,7 @@ def test_homing(self):
         class_name = e.robot_params[tool_name]['py_class_name']
         e = getattr(importlib.import_module(module_name), class_name)()
         self.assertTrue('stretch_gripper' in e.joints)
-        self.assertTrue(e.startup())
+        self.assertTrue(e.startup(threaded=False))
 
         e.home()
         e.pull_status()

body/test/test_end_of_arm_tools.py

@@ -30,8 +30,8 @@ def test_catch_startup_exception(self):
         if b==115200:
             e = stretch_body.end_of_arm_tools.ToolNone()
             e.params['baud']=57600
-            self.assertFalse(e.startup())
+            self.assertFalse(e.startup(threaded=False))
         else:
             e = stretch_body.end_of_arm_tools.ToolNone()
             e.params['baud'] = 115200
-            self.assertFalse(e.startup())
+            self.assertFalse(e.startup(threaded=False))

body/test/test_head.py

@@ -5,6 +5,9 @@
 import unittest
 import stretch_body.head
 
+import time
+
+
 class TestHead(unittest.TestCase):
 
     def test_homing(self):
@@ -27,4 +30,56 @@ def test_get_joints(self):
         m=h.get_joint('head_pan')
         self.assertEqual('head_pan',m.name)
         m = h.get_joint('foo')
-        self.assertEqual(m,None)
+        self.assertEqual(m,None)
+
+    def test_waypoint_trajectory(self):
+        h = stretch_body.head.Head()
+        self.assertTrue(h.startup())
+
+        h.get_joint('head_tilt').trajectory.add(0, 0)
+        h.get_joint('head_tilt').trajectory.add(3, -1.0)
+        h.get_joint('head_tilt').trajectory.add(6, 0)
+        h.get_joint('head_pan').trajectory.add(0, 0.1)
+        h.get_joint('head_pan').trajectory.add(3, -0.9)
+        h.get_joint('head_pan').trajectory.add(6, 0.1)
+        h.logger.info('Executing tilt {0}'.format(h.get_joint('head_tilt').trajectory.__repr_segments__()))
+        h.logger.info('Executing pan {0}'.format(h.get_joint('head_pan').trajectory.__repr_segments__()))
+        self.assertTrue(h.get_joint('head_tilt').trajectory.is_valid(4, 8))
+        h.follow_trajectory()
+        time.sleep(7)
+        self.assertAlmostEqual(h.get_joint('head_tilt').status['pos'], 0.0, places=1)
+        self.assertAlmostEqual(h.get_joint('head_pan').status['pos'], 0.1, places=1)
+
+        h.get_joint('head_tilt').trajectory.clear()
+        h.get_joint('head_pan').trajectory.clear()
+        h.get_joint('head_tilt').trajectory.add(0, 0, 0)
+        h.get_joint('head_tilt').trajectory.add(3, -1.0, 0)
+        h.get_joint('head_tilt').trajectory.add(6, 0, 0)
+        h.get_joint('head_pan').trajectory.add(0, 0.1, 0)
+        h.get_joint('head_pan').trajectory.add(3, -0.9, 0)
+        h.get_joint('head_pan').trajectory.add(6, 0.1, 0)
+        h.logger.info('Executing tilt {0}'.format(h.get_joint('head_tilt').trajectory.__repr_segments__()))
+        h.logger.info('Executing pan {0}'.format(h.get_joint('head_pan').trajectory.__repr_segments__()))
+        self.assertTrue(h.get_joint('head_tilt').trajectory.is_valid(4, 8))
+        h.follow_trajectory()
+        time.sleep(7)
+        self.assertAlmostEqual(h.get_joint('head_tilt').status['pos'], 0.0, places=1)
+        self.assertAlmostEqual(h.get_joint('head_pan').status['pos'], 0.1, places=1)
+
+        h.get_joint('head_tilt').trajectory.clear()
+        h.get_joint('head_pan').trajectory.clear()
+        h.get_joint('head_tilt').trajectory.add(0, 0, 0, 0)
+        h.get_joint('head_tilt').trajectory.add(3, -1.0, 0, 0)
+        h.get_joint('head_tilt').trajectory.add(6, 0, 0, 0)
+        h.get_joint('head_pan').trajectory.add(0, 0.1, 0, 0)
+        h.get_joint('head_pan').trajectory.add(3, -0.9, 0, 0)
+        h.get_joint('head_pan').trajectory.add(6, 0.1, 0, 0)
+        h.logger.info('Executing tilt {0}'.format(h.get_joint('head_tilt').trajectory.__repr_segments__()))
+        h.logger.info('Executing pan {0}'.format(h.get_joint('head_pan').trajectory.__repr_segments__()))
+        self.assertTrue(h.get_joint('head_tilt').trajectory.is_valid(4, 8))
+        h.follow_trajectory()
+        time.sleep(7)
+        self.assertAlmostEqual(h.get_joint('head_tilt').status['pos'], 0.0, places=1)
+        self.assertAlmostEqual(h.get_joint('head_pan').status['pos'], 0.1, places=1)
+
+        h.stop()