Commit 5968e1ba authored by Jayant Khatkar's avatar 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
## 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.")
......
......@@ -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
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