Commit 8f58c677 authored by Jayant Khatkar's avatar Jayant Khatkar

go slightly above the calibration point for safety (#18)

parent 417cad0c
......@@ -591,6 +591,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
mat[:3,3] = p
mat = world2armT.dot(mat).dot(_vert_or).dot(tip2armT)
q = Rotation.from_dcm(mat[:3,:3]).as_quat()
......
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