Commit d075f6d6 authored by Jayant Khatkar's avatar Jayant Khatkar
Browse files

cleaner movement between contours (implement #51)

parent d3bb5941
......@@ -114,12 +114,44 @@ class Plan:
return self.jointsAtTime(self.cumul_time[arm][-1], arm)
def plan_to(self, joints, arm):
def plan_to(self, joints, arm, from_contour=True, to_contour=True, vert_time = 0.5):
"""
Create a trajectory from the last position in plan to new joint position
"""
j1 = self.getLastJoint(arm)
j2 = joints
j_s = self.getLastJoint(arm)
j_f = joints
if all([i == j for i,j in zip(j_s, self.cc.home)]):
from_contour = False
if all([i == j for i,j in zip(j_f, self.cc.home)]):
to_contour = False
if from_contour:
t_mat = self.cc.fk(j_s, arm)
pos = t_mat[:3,3]
pos[2] += 0.005
j1_1 = self.cc.ik_pos(pos,arm)
diffs = np.linalg.norm(j1_1-j_s, axis=1)
if min(diffs) < 0.025:
j1 = j1_1[np.argmin(diffs)]
else:
j1 = j_s
else:
j1 = j_s
if to_contour:
t_mat = self.cc.fk(j_f, arm)
pos = t_mat[:3,3]
pos[2] += 0.005
j1_1 = self.cc.ik_pos(pos,arm)
diffs = np.linalg.norm(j1_1-j_f, axis=1)
if min(diffs) < 0.025:
j2 = j1_1[np.argmin(diffs)]
else:
j2 = j_f
else:
j2 = j_f
t_start = self.cumul_time[arm][-1]
# TODO time swept should be estimated by distance between j1 and j2
......@@ -128,7 +160,17 @@ class Plan:
joint_list = planTrajectory(arm, j1, j2, self, t_start, t_end)
if joint_list is None:
return None
return JTrajectory(joint_list, np.arange(0,len(joint_list)/4, 0.25), arm)
if from_contour:
traj = JTrajectory([j_s] + joint_list, [0] + list(vert_time+np.arange(0,len(joint_list)/4, 0.25)), arm)
else:
traj = JTrajectory(joint_list, list(np.arange(0,len(joint_list)/4, 0.25)), arm)
if to_contour:
traj.positions.append(j_f)
traj.time.append(traj.time[-1]+vert_time)
return traj
def watch(self, t=10, start=0, end=1):
......
......@@ -313,7 +313,7 @@ def planTrajectory(robot,
plan,
t_start,
t_end,
col_checks=20,
col_checks=10,
dist_fn = "euclidean"):
"""
Generate a trajectory between contours
......
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