work towards executing contour execution dry

controller.py

@@ -41,10 +41,11 @@ class Calibration:
         assert(isinstance(calibration_type, CalibrationType))
 
         self.cb_t = calibration_type
-        self.cb_parameters = []
+        self.cb_joints = []
+        self.cb_pose = []
         self.complete = False
 
-    def push_parameter(self, param):
+    def push_parameter(self, joints, pose):
         """
         Save the joint angles to reach particular point in grid
         param should be a list of floats (joint angles) (not asserted)
@@ -217,7 +218,10 @@ class controller:
             self._internal_move_endpoint(0, 0, -0.01)
         elif key == keyboard.Key.enter:
             # save current pose into calibaration
-            return self._temp_calib.push_parameter(self.group.get_current_joint_values())
+            return self._temp_calib.push_parameter(
+                self.group.get_current_joint_values(),
+                self.group.get_current_pose()
+                )
 
 
     def _internal_on_release(self, key):
@@ -247,6 +251,18 @@ class controller:
             self.calibration = self._temp_calib
 
 
+    def contour2traj(self, contour):
+        """
+        Convert a contour to a arm trajectory:
+            1. shift points to calibration
+            2. convert positions to poses
+            3. get IK for each pose
+            4. save points and timestamp in trajectory
+        """
+
+        pass
+
+
 if __name__=="__main__":
 
     ########################################

main.py

 import time
 import numpy as np
 import gcode2contour as gc
+from controller import controller, Calibration, CalibrationType
 
 import rospy
 from trajectory_msgs.msg import (
@@ -9,49 +10,18 @@ from trajectory_msgs.msg import (
 )
 
 
-def contour2traj(c):
-    """
-    Converts contours to JointTrajectory objects
-
-    Assumes that the contour XYZ locations have already been offset
-    """
-
-    # IK for each point in the trajectory
-        # choose valid starting IK -> closest to current position
-
-    # create Trajectory using IK.
-    traj = JointTrajectory()
-    for name in planner.joint_names[robot]:
-        traj_msg.joint_names.append(name)
-    for x in c.pos:
-        point = JointTrajectoryPoint()
-        point.positions = copy([0,0,0,0,0,0])# TODO change to get the relevant IK
-        point.time_from_start = rospy.Duration(0) # TODO get time out of contour
-    pass
-
-
-def print_contours(contours):
-    """
-    takes a list contours and prints them
-    """
-
-    #TODO current pose - get from 
-    for c in contours:
-        t = contour2traj(c)#, current_pose)
-
-        # Execute Trajectory
-    pass
-
-
 if __name__ == "__main__":
 
     # convert gcode file to contours
     fname = "benchy.gcode"
     contours = gc.decode_gcode(fname)
 
+    # Load controller
+    con = controller()
+
     # Calibrate system
-    # TODO
-    # Apply calibration offset to contours
-    # TODO
+    con.calibrate()
+
+    # convert (a) contour to arm trajectory
+    traj = con.contour2traj(contours[0])
 
-    #print_contours(contours)