Commit 5968e1ba by Jayant Khatkar

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

parent 8f58c677
 ... @@ -26,6 +26,7 @@ from moveit_msgs.msg import DisplayTrajectory ... @@ -26,6 +26,7 @@ from moveit_msgs.msg import DisplayTrajectory ## Our libraries ## Our libraries from Controller import Controller,getR_ET from Controller import Controller,getR_ET from utils import speed_multiplier # used for fast-calibration # used for fast-calibration ... @@ -591,7 +592,7 @@ class KeyboardController(Controller): ... @@ -591,7 +592,7 @@ class KeyboardController(Controller): if priors: if priors: # calculate IK for next point and move there # calculate IK for next point and move there mat = np.identity(4) 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[:3,3] = p mat = world2armT.dot(mat).dot(_vert_or).dot(tip2armT) mat = world2armT.dot(mat).dot(_vert_or).dot(tip2armT) q = Rotation.from_dcm(mat[:3,:3]).as_quat() q = Rotation.from_dcm(mat[:3,:3]).as_quat() ... @@ -608,6 +609,7 @@ class KeyboardController(Controller): ... @@ -608,6 +609,7 @@ class KeyboardController(Controller): 0.01, # ee_step 0.01, # ee_step 0) 0) speed_multiplier(plan.joint_trajectory, 4) # 4x slowers # display path on screen and confirm execution # display path on screen and confirm execution try: try: print("Displaying Trajectory to estimated point.") print("Displaying Trajectory to estimated point.") ... ...
 ... @@ -79,5 +79,9 @@ def speed_multiplier(jtraj, multiplier): ... @@ -79,5 +79,9 @@ def speed_multiplier(jtraj, multiplier): """ """ used for slowing down contour execution for testing 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 return jtraj
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!