Commit 56dea719 authored by Jayant Khatkar's avatar Jayant Khatkar
Browse files

fix trajectory failed error (fix #17)

parent e6261aba
...@@ -141,22 +141,31 @@ class Controller(object): ...@@ -141,22 +141,31 @@ class Controller(object):
""" """
plan = RobotTrajectory() plan = RobotTrajectory()
plan.joint_trajectory = JTraj2ROS(jtraj) 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: if contour is not None:
print("THIS NEXT TRAJ REQUIRES EXTRUSION") print("THIS NEXT TRAJ REQUIRES EXTRUSION")
self.preprint_load() self.preprint_load()
self.extrude(contour.Ext[-1], jtraj.time[-1]) 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.traj_complete = False
self.group.execute(plan, wait=False) self.group.execute(plan, wait=False)
if wait: if wait:
s = time.time() s = time.time()
t = jtraj.time[-1] t = jtraj.time[-1]
while not self.traj_complete: while not self.traj_complete or time.time() - s < t - 1:
if time.time() - s > t + 3: if time.time() - s > t + 1:
Exception("traj_complete failed") Exception("traj_complete failed")
break break
self.traj_complete = False self.traj_complete = False
...@@ -323,12 +332,11 @@ class Controller(object): ...@@ -323,12 +332,11 @@ class Controller(object):
if len(msg.status_list)>0: if len(msg.status_list)>0:
self.traj_complete = (msg.status_list[-1].status==3 or self.traj_complete = (msg.status_list[-1].status==3 or
msg.status_list[-1].status==2) msg.status_list[-1].status==2)
if msg.status_list[-1].status >= 4: if msg.status_list[-1].status >= 4 and msg.status_list[-1].status!=6:
print("Trajectory has failed to execute, please check ROS MoveIt execution planner to see why") print("Unexpected Trajectory Status")
print("STATUS: {}".format(msg.status_list[-1].status)) print("STATUS: {}".format(msg.status_list[-1].status))
print("For info on status meaning, go to:\n" + \ print("For info on status meaning, go to:\n" + \
"http://docs.ros.org/en/melodic/api/actionlib_msgs/html/msg/GoalStatus.html") "http://docs.ros.org/en/melodic/api/actionlib_msgs/html/msg/GoalStatus.html")
sys.exit(0)
def getR_ET(): def getR_ET():
......
...@@ -592,8 +592,8 @@ class KeyboardController(Controller): ...@@ -592,8 +592,8 @@ class KeyboardController(Controller):
if __name__=="__main__": if __name__=="__main__":
#con = KeyboardController(disableExtruder=True) con = KeyboardController(disableExtruder=True)
con = KeyboardController() #con = KeyboardController()
print('Collecting configrations...') print('Collecting configrations...')
configuations = con.prompt_configurations() configuations = con.prompt_configurations()
......
...@@ -25,7 +25,7 @@ def execute_plan(plan, ...@@ -25,7 +25,7 @@ def execute_plan(plan,
skip_first=1, skip_first=1,
contour_speed=0.2, contour_speed=0.2,
move_speed=1, move_speed=1,
first_n=None): n_trajs=None):
""" """
Executes a plan on MoveIt Executes a plan on MoveIt
...@@ -37,9 +37,10 @@ def execute_plan(plan, ...@@ -37,9 +37,10 @@ def execute_plan(plan,
# execute trajectories in plan # execute trajectories in plan
for i, t in enumerate(plan.trajs[0]): for i, t in enumerate(plan.trajs[0]):
print("---")
if i == first_n: if i == n_trajs + skip_first:
print("Finishing prematurely, completed {} trajectories".format(i)) print("Finishing prematurely, completed {} trajectories".format(i - skip_first))
break break
if i < skip_first: if i < skip_first:
...@@ -51,13 +52,15 @@ def execute_plan(plan, ...@@ -51,13 +52,15 @@ def execute_plan(plan,
if a== 'n': if a== 'n':
break 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: 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)) while not controller.exec_ctraj(speed_multiplier(t, 1/contour_speed), contour=contours[t.contour]):
controller.exec_ctraj(speed_multiplier(t, 1/contour_speed), contour=contours[t.contour]) pass
else: else:
print("Printing {}th trajectory in the plan, which is contour {}".format(i, t.contour)) #print("Moving arm without extrusion")
print("Moving arm without extrusion") while not controller.exec_ctraj(speed_multiplier(t, 1/move_speed)):
controller.exec_ctraj(speed_multiplier(t, 1/move_speed)) pass
return return
...@@ -67,12 +70,12 @@ if __name__ == '__main__': ...@@ -67,12 +70,12 @@ if __name__ == '__main__':
plan2, flexirex_contours = load_plan('flexirex', contours=True) plan2, flexirex_contours = load_plan('flexirex', contours=True)
# Printing # Printing
#con = Controller() con = Controller()
#execute_plan(plan2, con, contours=flexirex_contours, confirm=True) execute_plan(plan2, con, contours=flexirex_contours, confirm=False, n_trajs=50, skip_first=200)
# no printing # no printing
con = Controller(disable_extruder=True) #con = Controller(disable_extruder=True)
execute_plan(plan2, con, confirm=False, move_speed=5) #execute_plan(plan2, con, confirm=False, n_trajs=200, move_speed=1)
# go to origin and stay (to check calibration) # go to origin and stay (to check calibration)
#plan05 = copy.deepcopy(plan1) #plan05 = copy.deepcopy(plan1)
......
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