Fix bug in linear_trajectory_to method. (#41)

* Fix bug in linear_trajectory_to method.

* Add test for linear_trajectory_to method.

Author: shivamvats

autolab_core/rigid_transformations.py

@@ -377,13 +377,10 @@ def linear_trajectory_to(self, target_tf, traj_len):
         """
         if traj_len < 0:
             raise ValueError("Traj len must at least 0")
-        delta_t = 1.0 / (traj_len + 1)
-        t = 0.0
+        ts = np.linspace(0.0, 1.0, traj_len)
         traj = []
-        while t < 1.0:
+        for t in ts:
             traj.append(self.interpolate_with(target_tf, t))
-            t += delta_t
-        traj.append(target_tf)
         return traj
 
     def apply(self, points):

tests/test_rigid_transform.py

@@ -272,6 +272,18 @@ def test_similarity_transformation(self):
         v_b2 = R_a_b.dot(v_a.data)
         self.assertTrue(np.allclose(v_b.data, v_b2))
 
+    def test_linear_trajectory(self):
+        R_a = RigidTransform.random_rotation()
+        t_a = RigidTransform.random_translation()
+        R_b = RigidTransform.random_rotation()
+        t_b = RigidTransform.random_translation()
+        T_a = RigidTransform(R_a, t_a, "w", "a")
+        T_b = RigidTransform(R_b, t_b, "w", "b")
+
+        for i in range(10):
+            traj = T_a.linear_trajectory_to(T_b, i)
+            self.assertEqual(len(traj), i, "Trajectory has incorrect length")
+
 
 if __name__ == "__main__":
     unittest.main()