fix trajectory failed error (fix #17)

src/Controller.py

@@ -141,22 +141,31 @@ class Controller(object):
         """
         plan = RobotTrajectory()
         plan.joint_trajectory = JTraj2ROS(jtraj)
-#        plan.joint_trajectory.points = plan.joint_trajectory.points[1:]
+
+        # NOTE: This can only be done safely becuase assuming that the current
+        # joint position is already very close to where we want it to be
+        cj = self.group.get_current_joint_values()
+        sj = plan.joint_trajectory.points[0].positions
+        if any([0.01 < abs(cj[i]-sj[i]) for i in range(6)]):
+            print("Potentially would've failed here, Retrying")
+            #time.sleep(0.1)
+            self.group.go(plan.joint_trajectory.points[0].positions, wait=True)
+            return False
 
         if contour is not None:
             print("THIS NEXT TRAJ REQUIRES EXTRUSION")
             self.preprint_load()
             self.extrude(contour.Ext[-1], jtraj.time[-1])
 
-        rospy.loginfo('Sending trajectory to UR5e')
+        #rospy.loginfo('Sending trajectory to UR5e')
         self.traj_complete = False
         self.group.execute(plan, wait=False)
 
         if wait:
             s = time.time()
             t = jtraj.time[-1]
-            while not self.traj_complete:
-                if time.time() - s > t + 3:
+            while not self.traj_complete or time.time() - s < t - 1:
+                if time.time() - s > t + 1:
                     Exception("traj_complete failed")
                     break
             self.traj_complete = False
@@ -323,12 +332,11 @@ class Controller(object):
         if len(msg.status_list)>0:
             self.traj_complete = (msg.status_list[-1].status==3 or
                 msg.status_list[-1].status==2)
-            if msg.status_list[-1].status >= 4:
-                print("Trajectory has failed to execute, please check ROS MoveIt execution planner to see why")
+            if msg.status_list[-1].status >= 4 and msg.status_list[-1].status!=6:
+                print("Unexpected Trajectory Status")
                 print("STATUS: {}".format(msg.status_list[-1].status))
                 print("For info on status meaning, go to:\n" + \
                     "http://docs.ros.org/en/melodic/api/actionlib_msgs/html/msg/GoalStatus.html")
-                sys.exit(0)
 
 
 def getR_ET():

src/KeyboardController.py

@@ -592,8 +592,8 @@ class KeyboardController(Controller):
 
 
 if __name__=="__main__":
-    #con = KeyboardController(disableExtruder=True)
-    con = KeyboardController()
+    con = KeyboardController(disableExtruder=True)
+    #con = KeyboardController()
 
     print('Collecting configrations...')
     configuations = con.prompt_configurations()

src/main.py

@@ -25,7 +25,7 @@ def execute_plan(plan,
         skip_first=1,
         contour_speed=0.2,
         move_speed=1,
-        first_n=None):
+        n_trajs=None):
     """
     Executes a plan on MoveIt
 
@@ -37,9 +37,10 @@ def execute_plan(plan,
 
     # execute trajectories in plan
     for i, t in enumerate(plan.trajs[0]):
+        print("---")
 
-        if i == first_n:
-            print("Finishing prematurely, completed {} trajectories".format(i))
+        if i == n_trajs + skip_first:
+            print("Finishing prematurely, completed {} trajectories".format(i - skip_first))
             break
 
         if i < skip_first:
@@ -51,13 +52,15 @@ def execute_plan(plan,
             if a== 'n':
                 break
 
+        print("Printing {}th trajectory in the plan, which is contour {}".format(i, t.contour))
+
         if t.contour is not None and contours is not None:
-            print("Printing {}th trajectory in the plan, which is contour {}".format(i, t.contour))
-            controller.exec_ctraj(speed_multiplier(t, 1/contour_speed), contour=contours[t.contour])
+            while not controller.exec_ctraj(speed_multiplier(t, 1/contour_speed), contour=contours[t.contour]):
+                pass
         else:
-            print("Printing {}th trajectory in the plan, which is contour {}".format(i, t.contour))
-            print("Moving arm without extrusion")
-            controller.exec_ctraj(speed_multiplier(t, 1/move_speed))
+            #print("Moving arm without extrusion")
+            while not controller.exec_ctraj(speed_multiplier(t, 1/move_speed)):
+                pass
     return
 
 
@@ -67,12 +70,12 @@ if __name__ == '__main__':
     plan2, flexirex_contours = load_plan('flexirex', contours=True)
 
     # Printing
-    #con = Controller()
-    #execute_plan(plan2, con, contours=flexirex_contours, confirm=True)
+    con = Controller()
+    execute_plan(plan2, con, contours=flexirex_contours, confirm=False, n_trajs=50, skip_first=200)
 
     # no printing
-    con = Controller(disable_extruder=True)
-    execute_plan(plan2, con, confirm=False, move_speed=5)
+    #con = Controller(disable_extruder=True)
+    #execute_plan(plan2, con, confirm=False, n_trajs=200, move_speed=1)
 
     # go to origin and stay (to check calibration)
     #plan05 = copy.deepcopy(plan1)