dry run working

README.md

@@ -4,7 +4,7 @@ Take a standard gcode file and print it using only 1 arm.
 
 Includes the following important components:
 - connecting with single arm through ros
-- calibration: todo
+- calibration: done
 - connecting with extruder through ros: todo
 - printing the whole thing: todo
 
@@ -45,10 +45,10 @@ launch ur5_e_moveit_config ur5_e_moveit_planning_execution.launch
 roslaunch ur5_e_moveit_config moveit_rviz.launch config:=true
 ```
 
-3. Run the controller file.
+3. Run the main code.
 
 ```bash
-ipython -i controller.py
+ipython -i main.py
 ```
 
 #### How to Run in Simulation
@@ -57,6 +57,11 @@ ipython -i controller.py
 
 ```bash
 # Launch simulated ur5e with moveit and visualisation 
-# TODO: change topic names of simulated to be same as real
 launch ur5_e_moveit_config ur5_e_moveit_planning_execution.launch
 ```
+
+2. Run the main code.
+
+```bash
+ipython -i main.py
+```

main.py

@@ -22,13 +22,8 @@ if __name__ == "__main__":
     # Calibrate system
     con.calibrate(CalibrationType.centre)
 
-    # convert (a) contour to arm trajectory
-    traj = con.contour2traj(contours[0])
-
-    # 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)
+    # Plan and execute (planning is slow because ik calculation rosservice used)
+    for i in range(10): #only do first 10 contours
+        t = con.contour2traj(contours[i])
+        con.move_to_next_contour(t)
+        con.exec_traj(t)