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

travelling between trajectories

parent a6c34938
......@@ -317,6 +317,36 @@ class controller:
return t
def move_to_next_contour(self, contour_traj):
"""
Move from one contour to the next
Steps are:
1. Rise 1cm from the current position
2. move in a straight line to 1cm above next contour
3. Lower 1cm
"""
# Get current pose
wp0 = self.group.get_current_pose().pose
# Shift is 1cm up
wp1 = deepcopy(wp0)
wp1.position.z += 0.01
# Get first pose of contour_traj
wp3 = self.fk(contour_traj.points[0].positions).pose_stamped[0].pose
# Get 1cm above that
wp2 = deepcopy(wp3)
wp2.position.z += 0.01
# Create cartesian plan path between the points
(plan, frac) = self.group.compute_cartesian_path([wp0, wp1, wp2, wp3], 0.01, 0.0)
# Execute plan
self.group.execute(plan)
if __name__=="__main__":
con = controller()
......@@ -28,3 +28,7 @@ if __name__ == "__main__":
# Execute trajectory
con.move_to(traj.points[0].positions)
con.exec_traj(traj)
# move to next trajectory
t2 = con.contour2traj(contours[98])
con.move_to_next_contour(t2)
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