Commit 5e85f129 authored by Jayant Khatkar's avatar Jayant Khatkar

contour dry execution

parent 29816f7d
......@@ -238,6 +238,7 @@ class controller:
Press enter to lock in a calibration point
"""
self.face_down()
self._temp_calib = Calibration(cb_type)
with keyboard.Listener(
......@@ -251,6 +252,25 @@ class controller:
self.calibration = self._temp_calib
def face_down(self):
"""
Make the robot point downwards
"""
# Change to be at current position but facign down
p = self.group.get_current_pose()
p.pose.orientation.x = 0.5
p.pose.orientation.y = 0.5
p.pose.orientation.z = -0.5
p.pose.orientation.w = 0.5
self.group.set_pose_target(p)
plan = self.group.go(wait=True)
group.stop() # residual motion
group.clear_pose_targets()
return self.group.get_current_pose()
def contour2traj(self, contour):
"""
Convert a contour to a arm trajectory:
......@@ -260,7 +280,24 @@ class controller:
4. save points and timestamp in trajectory
"""
pass
# Get calibration pose:
p0 = self.calibaration.cb_pose[0]
p_last = deepcopy(p0)
joints = []
for position in contour.pos:
pose = deepcopy(p0)
pose[-1].pose.position.x += position[0]/1e3
pose[-1].pose.position.y += position[1]/1e3
pose[-1].pose.position.z += position[2]/1e3
# Get IK for all the poses
j_s = self.ik(pose, start_angle = p_last)
p_last = j_s.solution.joint_state.position
joints.append(p_last)
# Append to trajectory
if __name__=="__main__":
......
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