def move_to_joints(self, r_arm, l_arm):
'''Makes the arms move to indicated joint poses'''
time_to_r_pose = Arms._get_time_to_pose(r_arm, 0)
time_to_l_pose = Arms._get_time_to_pose(l_arm, 1)
# If both arms are moving adjust velocities and find most moving arm
is_r_moving = (time_to_r_pose != None)
is_l_moving = (time_to_l_pose != None)
if (not is_r_moving):
Response.look_at_point(l_arm.ee_pose.position)
elif (not is_l_moving):
Response.look_at_point(r_arm.ee_pose.position)
else:
if (time_to_r_pose > time_to_l_pose):
time_to_l_pose = time_to_r_pose
Response.look_at_point(r_arm.ee_pose.position)
else:
time_to_r_pose = time_to_l_pose
Response.look_at_point(l_arm.ee_pose.position)
# Move arms to target
if (is_r_moving):
Arms.arms[0].move_to_joints(r_arm.joint_pose, time_to_r_pose)
if (is_l_moving):
Arms.arms[1].move_to_joints(l_arm.joint_pose, time_to_l_pose)
# Wait until both arms complete the trajectory
while ((Arms.arms[0].is_executing() or Arms.arms[1].is_executing())
and not self.preempt):
time.sleep(0.01)
rospy.loginfo('Arms reached target.')
# Verify that both arms succeeded
if ((not Arms.arms[0].is_successful() and is_r_moving)
or (not Arms.arms[1].is_successful() and is_l_moving)):
rospy.logwarn('Aborting because arms failed to move to pose.')
return False
else:
return True
def move_to_joints(self, r_arm, l_arm):
'''Makes the arms move to indicated joint poses'''
time_to_r_pose = Arms._get_time_to_pose(r_arm, 0)
time_to_l_pose = Arms._get_time_to_pose(l_arm, 1)
# If both arms are moving adjust velocities and find most moving arm
is_r_moving = (time_to_r_pose != None)
is_l_moving = (time_to_l_pose != None)
if (not is_r_moving):
Response.look_at_point(l_arm.ee_pose.position)
elif (not is_l_moving):
Response.look_at_point(r_arm.ee_pose.position)
else:
if (time_to_r_pose > time_to_l_pose):
time_to_l_pose = time_to_r_pose
Response.look_at_point(r_arm.ee_pose.position)
else:
time_to_r_pose = time_to_l_pose
Response.look_at_point(l_arm.ee_pose.position)
# Move arms to target
if (is_r_moving):
Arms.arms[0].move_to_joints(r_arm.joint_pose, time_to_r_pose)
if (is_l_moving):
Arms.arms[1].move_to_joints(l_arm.joint_pose, time_to_l_pose)
# Wait until both arms complete the trajectory
while((Arms.arms[0].is_executing() or
Arms.arms[1].is_executing()) and not self.preempt):
time.sleep(0.01)
rospy.loginfo('Arms reached target.')
# Verify that both arms succeeded
if ((not Arms.arms[0].is_successful() and is_r_moving) or
(not Arms.arms[1].is_successful() and is_l_moving)):
rospy.logwarn('Aborting because arms failed to move to pose.')
return False
else:
return True
