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