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):
"""
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():
......
......@@ -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()
......
......@@ -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)
......
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