Merge pull request #42 from BerkeleyAutomation/fix/linear-traj

Fix bug in linear_trajectory_to method. (#41)

autolab_core/image.py

@@ -1194,7 +1194,7 @@ def find_chessboard(self, sx=6, sy=9):
         gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
 
         # Find the chess board corners
-        ret, corners = cv2.findChessboardCorners(gray, (sx, sy), None)
+        ret, corners = cv2.findChessboardCornersSB(gray, (sx, sy), None)
 
         # If found, add object points, image points (after refining them)
         if ret:

autolab_core/rigid_transformations.py

@@ -253,9 +253,9 @@ def axis_angle(self):
         theta = 2 * np.arccos(qw)
         omega = np.array([1, 0, 0])
         if theta > 0:
-            rx = qx / np.sqrt(1.0 - qw ** 2)
-            ry = qy / np.sqrt(1.0 - qw ** 2)
-            rz = qz / np.sqrt(1.0 - qw ** 2)
+            rx = qx / np.sqrt(1.0 - qw**2)
+            ry = qy / np.sqrt(1.0 - qw**2)
+            rz = qz / np.sqrt(1.0 - qw**2)
             omega = np.array([rx, ry, rz])
         return theta * omega
 
@@ -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):

autolab_core/utils.py

@@ -273,7 +273,7 @@ def cart2sph(x, y, z):
     float
         elevation
     """
-    r = np.sqrt(x ** 2 + y ** 2 + z ** 2)
+    r = np.sqrt(x**2 + y**2 + z**2)
     if x > 0 and y > 0:
         az = np.arctan(y / x)
     elif x > 0 and y < 0:

docs/source/conf.py

@@ -52,9 +52,9 @@
 master_doc = "index"
 
 # General information about the project.
-project = u"autolab_core"
-copyright = u"2016, Jeff Mahler"
-author = u"Jeff Mahler"
+project = "autolab_core"
+copyright = "2016, Jeff Mahler"
+author = "Jeff Mahler"
 
 # The version info for the project you're documenting, acts as replacement for
 # |version| and |release|, also used in various other places throughout the
@@ -229,8 +229,8 @@
     (
         master_doc,
         "autolab_core.tex",
-        u"autolab_core Documentation",
-        u"Jeff Mahler",
+        "autolab_core Documentation",
+        "Jeff Mahler",
         "manual",
     ),
 ]
@@ -261,7 +261,7 @@
 # One entry per manual page. List of tuples
 # (source start file, name, description, authors, manual section).
 man_pages = [
-    (master_doc, "autolab_core", u"autolab_core Documentation", [author], 1)
+    (master_doc, "autolab_core", "autolab_core Documentation", [author], 1)
 ]
 
 # If true, show URL addresses after external links.
@@ -277,7 +277,7 @@
     (
         master_doc,
         "autolab_core",
-        u"autolab_core Documentation",
+        "autolab_core Documentation",
         author,
         "autolab_core",
         "One line description of project.",

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()