slow down trajectory execution between contours since moveit plans too fast (#18)

src/KeyboardController.py

@@ -26,6 +26,7 @@ from moveit_msgs.msg import DisplayTrajectory
 
 ## Our libraries
 from Controller import Controller,getR_ET
+from utils import speed_multiplier
 
 
 # used for fast-calibration
@@ -591,7 +592,7 @@ class KeyboardController(Controller):
                 if priors:
                     # calculate IK for next point and move there
                     mat = np.identity(4)
-                    p[3] += 0.01 # go 1cm above bed for safety
+                    p[2] += 0.01 # go 1cm above bed for safety
                     mat[:3,3] = p
                     mat = world2armT.dot(mat).dot(_vert_or).dot(tip2armT)
                     q = Rotation.from_dcm(mat[:3,:3]).as_quat()
@@ -608,6 +609,7 @@ class KeyboardController(Controller):
                             0.01, # ee_step
                             0)
 
+                    speed_multiplier(plan.joint_trajectory, 4) # 4x slowers
                     # display path on screen and confirm execution
                     try:
                         print("Displaying Trajectory to estimated point.")

src/utils/planning_tools.py

@@ -79,5 +79,9 @@ def speed_multiplier(jtraj, multiplier):
     """
     used for slowing down contour execution for testing
     """
-    jtraj.time = [t*multiplier for t in jtraj.time]
+    if isinstance(jtraj, JTrajectory):
+        jtraj.time = [t*multiplier for t in jtraj.time]
+    elif isinstance(jtraj, JointTrajectory):
+        for p in jtraj.points:
+            p.time_from_start = p.time_from_start*multiplier
     return jtraj