contour dry execution

controller.py

@@ -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__":