def move_arm_to(self, goal_in):
(reachable, ik_goal, duration) = self.full_ik_check(goal_in)
rospy.sleep(duration)
rospy.loginfo("Composing move_" + self.arm + "_arm goal")
goal_out = MoveArmGoal()
goal_out.motion_plan_request.group_name = self.arm + "_arm"
goal_out.motion_plan_request.num_planning_attempts = 10
goal_out.motion_plan_request.planner_id = ""
goal_out.planner_service_name = "ompl_planning/plan_kinematic_path"
goal_out.motion_plan_request.allowed_planning_time = rospy.Duration(
1.0)
pos = PositionConstraint()
pos.header.frame_id = goal_in.header.frame_id
pos.link_name = self.arm[0] + "_wrist_roll_link"
pos.position = goal_in.pose.position
pos.constraint_region_shape.type = 0
pos.constraint_region_shape.dimensions = [0.01]
pos.constraint_region_orientation = Quaternion(0, 0, 0, 1)
pos.weight = 1
goal_out.motion_plan_request.goal_constraints.position_constraints.append(
pos)
ort = OrientationConstraint()
ort.header.frame_id = goal_in.header.frame_id
ort.link_name = self.arm[0] + "_wrist_roll_link"
ort.orientation = goal_in.pose.orientation
ort.absolute_roll_tolerance = 0.02
ort.absolute_pitch_tolerance = 0.02
ort.absolute_yaw_tolerance = 0.02
ort.weight = 0.5
goal_out.motion_plan_request.goal_constraints.orientation_constraints.append(
ort)
goal_out.accept_partial_plans = True
goal_out.accept_invalid_goals = True
goal_out.disable_collision_monitoring = True
rospy.loginfo("Sending composed move_" + self.arm + "_arm goal")
finished_within_time = False
self.move_arm_client.send_goal(goal_out)
finished_within_time = self.move_arm_client.wait_for_result(
rospy.Duration(60))
if not (finished_within_time):
self.move_arm_client.cancel_goal()
self.log_out.publish(data="Timed out achieving " + self.arm +
" arm goal pose")
rospy.loginfo("Timed out achieving right arm goal pose")
return False
else:
state = self.move_arm_client.get_state()
result = self.move_arm_client.get_result()
if (state == 3): #3 == SUCCEEDED
rospy.loginfo("Action Finished: %s" % state)
self.log_out.publish(
data="Move " + self.arm.capitalize() +
" Arm with Motion Planning: %s" %
self.move_arm_error_dict[result.error_code.val])
return True
else:
if result.error_code.val == 1:
rospy.loginfo("Move_" + self.arm + "_arm_action failed: \
Unable to plan a path to goal")
self.log_out.publish(data="Move " + self.arm.capitalize() +
" Arm with Motion Planning Failed: \
Unable to plan a path to the goal")
elif result.error_code.val == -31:
(torso_succeeded,
pos) = self.check_torso(self.form_ik_request(goal_in))
if torso_succeeded:
rospy.loginfo("IK Failed in Current Position. \
Adjusting Height and Retrying")
self.log_out.publish(data="IK Failed in Current \
Position. Adjusting \
Height and Retrying")
self.move_arm_to(pos)
else:
rospy.loginfo("Move_" + self.arm +
"_arm action failed: %s" % state)
rospy.loginfo("Failure Result: %s" % result)
self.log_out.publish(
data="Move " + self.arm.capitalize() +
" Arm with Motion Planning \
and Torso Check Failed: %s"
% self.move_arm_error_dict[result.error_code.val])
else:
rospy.loginfo("Move_" + self.arm +
"_arm action failed: %s" % state)
rospy.loginfo("Failure Result: %s" % result)
self.log_out.publish(
data="Move " + self.arm.capitalize() +
" Arm with Motion Planning \
Failed: %s" %
self.move_arm_error_dict[result.error_code.val])
return False
def move_arm_to(self, goal_in):
(reachable, ik_goal, duration) = self.full_ik_check(goal_in)
rospy.sleep(duration)
rospy.loginfo("Composing move_"+self.arm+"_arm goal")
goal_out = MoveArmGoal()
goal_out.motion_plan_request.group_name = self.arm+"_arm"
goal_out.motion_plan_request.num_planning_attempts = 10
goal_out.motion_plan_request.planner_id = ""
goal_out.planner_service_name = "ompl_planning/plan_kinematic_path"
goal_out.motion_plan_request.allowed_planning_time = rospy.Duration(1.0)
pos = PositionConstraint()
pos.header.frame_id = goal_in.header.frame_id
pos.link_name = self.arm[0]+"_wrist_roll_link"
pos.position = goal_in.pose.position
pos.constraint_region_shape.type = 0
pos.constraint_region_shape.dimensions=[0.01]
pos.constraint_region_orientation = Quaternion(0,0,0,1)
pos.weight = 1
goal_out.motion_plan_request.goal_constraints.position_constraints.append(pos)
ort = OrientationConstraint()
ort.header.frame_id=goal_in.header.frame_id
ort.link_name = self.arm[0]+"_wrist_roll_link"
ort.orientation = goal_in.pose.orientation
ort.absolute_roll_tolerance = 0.02
ort.absolute_pitch_tolerance = 0.02
ort.absolute_yaw_tolerance = 0.02
ort.weight = 0.5
goal_out.motion_plan_request.goal_constraints.orientation_constraints.append(ort)
goal_out.accept_partial_plans = True
goal_out.accept_invalid_goals = True
goal_out.disable_collision_monitoring = True
rospy.loginfo("Sending composed move_"+self.arm+"_arm goal")
finished_within_time = False
self.move_arm_client.send_goal(goal_out)
finished_within_time = self.move_arm_client.wait_for_result(
rospy.Duration(60))
if not (finished_within_time):
self.move_arm_client.cancel_goal()
self.log_out.publish(data="Timed out achieving "+self.arm+
" arm goal pose")
rospy.loginfo("Timed out achieving right arm goal pose")
return False
else:
state = self.move_arm_client.get_state()
result = self.move_arm_client.get_result()
if (state == 3): #3 == SUCCEEDED
rospy.loginfo("Action Finished: %s" %state)
self.log_out.publish(data="Move "+self.arm.capitalize()+
" Arm with Motion Planning: %s"
%self.move_arm_error_dict[
result.error_code.val])
return True
else:
if result.error_code.val == 1:
rospy.loginfo("Move_"+self.arm+"_arm_action failed: \
Unable to plan a path to goal")
self.log_out.publish(data="Move "+self.arm.capitalize()+
" Arm with Motion Planning Failed: \
Unable to plan a path to the goal")
elif result.error_code.val == -31:
(torso_succeeded, pos) = self.check_torso(
self.form_ik_request(goal_in))
if torso_succeeded:
rospy.loginfo("IK Failed in Current Position. \
Adjusting Height and Retrying")
self.log_out.publish(data="IK Failed in Current \
Position. Adjusting \
Height and Retrying")
self.move_arm_to(pos)
else:
rospy.loginfo("Move_"+self.arm+"_arm action failed: %s"
%state)
rospy.loginfo("Failure Result: %s" %result)
self.log_out.publish(data="Move "+self.arm.capitalize()+
" Arm with Motion Planning \
and Torso Check Failed: %s"
%self.move_arm_error_dict[
result.error_code.val])
else:
rospy.loginfo("Move_"+self.arm+"_arm action failed: %s" %state)
rospy.loginfo("Failure Result: %s" %result)
self.log_out.publish(data="Move "+self.arm.capitalize()+
" Arm with Motion Planning \
Failed: %s"
%self.move_arm_error_dict[
result.error_code.val])
return False
