Add arm/lift waypoint traj unit tests

body/test/test_arm.py

@@ -112,4 +112,40 @@ def test_move_arm_with_soft_limits(self):
 
         a.stop()
 
+    def test_waypoint_trajectory(self):
+        a = stretch_body.arm.Arm()
+        a.motor.disable_sync_mode()
+        self.assertTrue(a.startup(threaded=True))
+        if not a.motor.status['pos_calibrated']:
+            self.fail('test requires arm to be homed')
+
+        a.trajectory.add(0, 0.1)
+        a.trajectory.add(3, 0.2)
+        a.trajectory.add(6, 0.15)
+        a.logger.debug('Executing {0}'.format(a.trajectory.__repr_segments__()))
+        self.assertTrue(a.trajectory.is_valid(0.1, 0.15))
+        a.follow_trajectory()
+        time.sleep(6)
+        self.assertAlmostEqual(a.status['pos'], 0.15, places=2)
+
+        a.trajectory.clear()
+        a.trajectory.add(0, 0.1, 0.0)
+        a.trajectory.add(3, 0.2, 0.0)
+        a.trajectory.add(6, 0.15, 0.0)
+        a.logger.debug('Executing {0}'.format(a.trajectory.__repr_segments__()))
+        self.assertTrue(a.trajectory.is_valid(0.1, 0.15))
+        a.follow_trajectory()
+        time.sleep(6)
+        self.assertAlmostEqual(a.status['pos'], 0.15, places=2)
+
+        a.trajectory.clear()
+        a.trajectory.add(0, 0.1, 0.0, 0.0)
+        a.trajectory.add(3, 0.2, 0.0, 0.0)
+        a.trajectory.add(6, 0.15, 0.0, 0.0)
+        a.logger.debug('Executing {0}'.format(a.trajectory.__repr_segments__()))
+        self.assertTrue(a.trajectory.is_valid(0.1, 0.15))
+        a.follow_trajectory()
+        time.sleep(6)
+        self.assertAlmostEqual(a.status['pos'], 0.15, places=2)
 
+        a.stop()

body/test/test_lift.py

@@ -100,3 +100,40 @@ def test_move_lift_with_soft_limits(self):
 
         l.stop()
 
+    def test_waypoint_trajectory(self):
+        l = stretch_body.lift.Lift()
+        l.motor.disable_sync_mode()
+        self.assertTrue(l.startup(threaded=True))
+        if not l.motor.status['pos_calibrated']:
+            self.fail('test requires arm to be homed')
+
+        l.trajectory.add(0, 0.2)
+        l.trajectory.add(3, 0.3)
+        l.trajectory.add(6, 0.25)
+        l.logger.debug('Executing {0}'.format(l.trajectory.__repr_segments__()))
+        self.assertTrue(l.trajectory.is_valid(0.1, 0.15))
+        l.follow_trajectory()
+        time.sleep(6)
+        self.assertAlmostEqual(l.status['pos'], 0.15, places=2)
+
+        l.trajectory.clear()
+        l.trajectory.add(0, 0.2, 0.0)
+        l.trajectory.add(3, 0.3, 0.0)
+        l.trajectory.add(6, 0.25, 0.0)
+        l.logger.debug('Executing {0}'.format(l.trajectory.__repr_segments__()))
+        self.assertTrue(l.trajectory.is_valid(0.1, 0.15))
+        l.follow_trajectory()
+        time.sleep(6)
+        self.assertAlmostEqual(l.status['pos'], 0.15, places=2)
+
+        l.trajectory.clear()
+        l.trajectory.add(0, 0.2, 0.0, 0.0)
+        l.trajectory.add(3, 0.3, 0.0, 0.0)
+        l.trajectory.add(6, 0.25, 0.0, 0.0)
+        l.logger.debug('Executing {0}'.format(l.trajectory.__repr_segments__()))
+        self.assertTrue(l.trajectory.is_valid(0.1, 0.15))
+        l.follow_trajectory()
+        time.sleep(6)
+        self.assertAlmostEqual(l.status['pos'], 0.15, places=2)
+
+        l.stop()