Commit 5968e1ba authored by Jayant Khatkar's avatar Jayant Khatkar
Browse files

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!
Please register or to comment