def test_move_arm(position, orientation, frame): rospy.loginfo("Moving the arm to x: %f y: %f z: %f r: %f p: %f y: %f", position[0], position[1], position[2], orientation[0], orientation[1], orientation[2]); client = actionlib.SimpleActionClient("move_right_arm", MoveArmAction) client.wait_for_server() goal = MoveArmGoal() goal.motion_plan_request.group_name = "right_arm" goal.motion_plan_request.num_planning_attempts = 3 goal.motion_plan_request.planner_id = "" goal.planner_service_name = "ompl_planning/plan_kinematic_path" goal.motion_plan_request.allowed_planning_time = rospy.Duration(15.) position_constraint = PositionConstraint() position_constraint.header.frame_id = frame position_constraint.link_name = "r_wrist_roll_link" position_constraint.position.x = position[0] position_constraint.position.y = position[1] position_constraint.position.z = position[2] position_constraint.constraint_region_shape.type = position_constraint.constraint_region_shape.BOX tolerance = 0.04 position_constraint.constraint_region_shape.dimensions = [tolerance, tolerance, tolerance] position_constraint.constraint_region_orientation.x = 0. position_constraint.constraint_region_orientation.y = 0. position_constraint.constraint_region_orientation.z = 0. position_constraint.constraint_region_orientation.w = 1. position_constraint.weight = 1.0 orientation_constraint = OrientationConstraint() orientation_constraint.header.frame_id = frame orientation_constraint.link_name = "r_wrist_roll_link" orientation_constraint.orientation = quaternion_to_msg(tf.transformations.quaternion_from_euler(orientation[0], orientation[1], orientation[2])) orientation_constraint.absolute_roll_tolerance = 0.04 orientation_constraint.absolute_pitch_tolerance = 0.04 orientation_constraint.absolute_yaw_tolerance = 0.04 orientation_constraint.weight = 1.0 goal.motion_plan_request.goal_constraints.position_constraints.append(position_constraint) goal.motion_plan_request.goal_constraints.orientation_constraints.append(orientation_constraint) goal.disable_collision_monitoring = True rospy.loginfo("Calling the move arm client") state = client.send_goal_and_wait(goal, rospy.Duration(120.)) if state == actionlib.GoalStatus.SUCCEEDED: rospy.loginfo("Succeeded") else: rospy.loginfo(state)
def testMotionExecutionBuffer(self): global padd_name global extra_buffer #too much trouble to read for now allow_padd = .05 #rospy.get_param(padd_name) joint_names = [ '%s_%s' % ('r', j) for j in [ 'shoulder_pan_joint', 'shoulder_lift_joint', 'upper_arm_roll_joint', 'elbow_flex_joint', 'forearm_roll_joint', 'wrist_flex_joint', 'wrist_roll_joint' ] ] goal = MoveArmGoal() goal.motion_plan_request.goal_constraints.joint_constraints = [ JointConstraint() for i in range(len(joint_names)) ] goal.motion_plan_request.group_name = "right_arm" goal.motion_plan_request.num_planning_attempts = 1 goal.motion_plan_request.allowed_planning_time = rospy.Duration(5.) goal.motion_plan_request.planner_id = "" goal.planner_service_name = "chomp_planner_longrange/plan_path" goal.disable_collision_monitoring = True r = rospy.Rate(10) goal.motion_plan_request.goal_constraints.joint_constraints = [ JointConstraint() for i in range(len(joint_names)) ] for z in range(6): for i in range(len(joint_names)): goal.motion_plan_request.goal_constraints.joint_constraints[ i].joint_name = joint_names[i] goal.motion_plan_request.goal_constraints.joint_constraints[ i].position = 0.0 goal.motion_plan_request.goal_constraints.joint_constraints[ i].tolerance_above = 0.08 goal.motion_plan_request.goal_constraints.joint_constraints[ i].tolerance_below = 0.08 if (z % 2 == 0): goal.motion_plan_request.goal_constraints.joint_constraints[ 0].position = -1.0 goal.motion_plan_request.goal_constraints.joint_constraints[ 3].position = -0.2 goal.motion_plan_request.goal_constraints.joint_constraints[ 5].position = -0.2 else: goal.motion_plan_request.goal_constraints.joint_constraints[ 0].position = .22 goal.motion_plan_request.goal_constraints.joint_constraints[ 1].position = .51 goal.motion_plan_request.goal_constraints.joint_constraints[ 2].position = .05 goal.motion_plan_request.goal_constraints.joint_constraints[ 3].position = -0.41 goal.motion_plan_request.goal_constraints.joint_constraints[ 4].position = -.06 goal.motion_plan_request.goal_constraints.joint_constraints[ 5].position = -.1 goal.motion_plan_request.goal_constraints.joint_constraints[ 6].position = 0.0 for x in range(3): self.move_arm_action_client.send_goal(goal) while True: cur_state = self.move_arm_action_client.get_state() if (cur_state != actionlib_msgs.msg.GoalStatus.ACTIVE and cur_state != actionlib_msgs.msg.GoalStatus.PENDING): break r.sleep() end_state = self.move_arm_action_client.get_state() if (end_state == actionlib_msgs.msg.GoalStatus.SUCCEEDED): break final_state = self.move_arm_action_client.get_state() self.assertEqual(final_state, actionlib_msgs.msg.GoalStatus.SUCCEEDED)
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
def __move_arm(self, arm, position, orientation, frame_id, waiting_time, ordered_collision_operations = None, allowed_contacts = None): goal = MoveArmGoal() goal.motion_plan_request.group_name = arm goal.motion_plan_request.num_planning_attempts = 2 goal.motion_plan_request.planner_id = "" goal.planner_service_name = "ompl_planning/plan_kinematic_path" goal.motion_plan_request.allowed_planning_time = rospy.Duration(waiting_time/2.) position_constraint = PositionConstraint() position_constraint.header.frame_id = frame_id if arm == "right_arm": link_name = "r_wrist_roll_link" client = self.move_right_arm_client elif arm == "left_arm": link_name = "l_wrist_roll_link" client = self.move_left_arm_client else: rospy.logerr("Unknown arm: %s"%arm) return False position_constraint.link_name = link_name position_constraint.position.x = position[0] position_constraint.position.y = position[1] position_constraint.position.z = position[2] position_constraint.constraint_region_shape.type = position_constraint.constraint_region_shape.BOX tolerance = 2 * 0.02 position_constraint.constraint_region_shape.dimensions = [tolerance, tolerance, tolerance] position_constraint.constraint_region_orientation.x = 0. position_constraint.constraint_region_orientation.y = 0. position_constraint.constraint_region_orientation.z = 0. position_constraint.constraint_region_orientation.w = 1. position_constraint.weight = 1.0 orientation_constraint = OrientationConstraint() orientation_constraint.header.frame_id = frame_id orientation_constraint.link_name = link_name # orientation_constraint.type = dunno! orientation_constraint.orientation.x = orientation[0] orientation_constraint.orientation.y = orientation[1] orientation_constraint.orientation.z = orientation[2] orientation_constraint.orientation.w = orientation[3] orientation_constraint.absolute_roll_tolerance = 0.04 orientation_constraint.absolute_pitch_tolerance = 0.04 orientation_constraint.absolute_yaw_tolerance = 0.04 orientation_constraint.weight = 1.0 goal.motion_plan_request.goal_constraints.position_constraints.append(position_constraint) goal.motion_plan_request.goal_constraints.orientation_constraints.append(orientation_constraint) if ordered_collision_operations is not None: rospy.loginfo("Adding ordered collisions") goal.operations = ordered_collision_operations if allowed_contacts is not None: rospy.loginfo("Adding allowed_contacts") goal.planning_scene_diff.allowed_contacts = allowed_contacts goal.disable_collision_monitoring = False state = client.send_goal_and_wait(goal, rospy.Duration(waiting_time)) if state == actionlib.GoalStatus.SUCCEEDED: return True else: return False
def testMotionExecutionBuffer(self): global padd_name global extra_buffer #too much trouble to read for now allow_padd = .05#rospy.get_param(padd_name) joint_names = ['%s_%s' % ('r', j) for j in ['shoulder_pan_joint', 'shoulder_lift_joint', 'upper_arm_roll_joint', 'elbow_flex_joint', 'forearm_roll_joint', 'wrist_flex_joint', 'wrist_roll_joint']] goal = MoveArmGoal() goal.motion_plan_request.goal_constraints.joint_constraints = [JointConstraint() for i in range(len(joint_names))] goal.motion_plan_request.group_name = "right_arm" goal.motion_plan_request.num_planning_attempts = 1 goal.motion_plan_request.allowed_planning_time = rospy.Duration(5.) goal.motion_plan_request.planner_id = "" goal.planner_service_name = "chomp_planner_longrange/plan_path" goal.disable_collision_monitoring = True; r = rospy.Rate(10) goal.motion_plan_request.goal_constraints.joint_constraints = [JointConstraint() for i in range(len(joint_names))] for z in range(6): for i in range(len(joint_names)): goal.motion_plan_request.goal_constraints.joint_constraints[i].joint_name = joint_names[i] goal.motion_plan_request.goal_constraints.joint_constraints[i].position = 0.0 goal.motion_plan_request.goal_constraints.joint_constraints[i].tolerance_above = 0.08 goal.motion_plan_request.goal_constraints.joint_constraints[i].tolerance_below = 0.08 if(z%2==0): goal.motion_plan_request.goal_constraints.joint_constraints[0].position = -1.0 goal.motion_plan_request.goal_constraints.joint_constraints[3].position = -0.2 goal.motion_plan_request.goal_constraints.joint_constraints[5].position = -0.2 else: goal.motion_plan_request.goal_constraints.joint_constraints[0].position = .22 goal.motion_plan_request.goal_constraints.joint_constraints[1].position = .51 goal.motion_plan_request.goal_constraints.joint_constraints[2].position = .05 goal.motion_plan_request.goal_constraints.joint_constraints[3].position = -0.41 goal.motion_plan_request.goal_constraints.joint_constraints[4].position = -.06 goal.motion_plan_request.goal_constraints.joint_constraints[5].position = -.1 goal.motion_plan_request.goal_constraints.joint_constraints[6].position = 0.0 for x in range(3): self.move_arm_action_client.send_goal(goal) while True: cur_state = self.move_arm_action_client.get_state() if(cur_state != actionlib_msgs.msg.GoalStatus.ACTIVE and cur_state != actionlib_msgs.msg.GoalStatus.PENDING): break r.sleep() end_state = self.move_arm_action_client.get_state() if(end_state == actionlib_msgs.msg.GoalStatus.SUCCEEDED): break final_state = self.move_arm_action_client.get_state() self.assertEqual(final_state, actionlib_msgs.msg.GoalStatus.SUCCEEDED)
def __move_arm(self, arm, position, orientation, frame_id, waiting_time, ordered_collision_operations=None, allowed_contacts=None): goal = MoveArmGoal() goal.motion_plan_request.group_name = arm goal.motion_plan_request.num_planning_attempts = 2 goal.motion_plan_request.planner_id = "" goal.planner_service_name = "ompl_planning/plan_kinematic_path" goal.motion_plan_request.allowed_planning_time = rospy.Duration( waiting_time / 2.) position_constraint = PositionConstraint() position_constraint.header.frame_id = frame_id if arm == "right_arm": link_name = "r_wrist_roll_link" client = self.move_right_arm_client elif arm == "left_arm": link_name = "l_wrist_roll_link" client = self.move_left_arm_client else: rospy.logerr("Unknown arm: %s" % arm) return False position_constraint.link_name = link_name position_constraint.position.x = position[0] position_constraint.position.y = position[1] position_constraint.position.z = position[2] position_constraint.constraint_region_shape.type = position_constraint.constraint_region_shape.BOX tolerance = 2 * 0.02 position_constraint.constraint_region_shape.dimensions = [ tolerance, tolerance, tolerance ] position_constraint.constraint_region_orientation.x = 0. position_constraint.constraint_region_orientation.y = 0. position_constraint.constraint_region_orientation.z = 0. position_constraint.constraint_region_orientation.w = 1. position_constraint.weight = 1.0 orientation_constraint = OrientationConstraint() orientation_constraint.header.frame_id = frame_id orientation_constraint.link_name = link_name # orientation_constraint.type = dunno! orientation_constraint.orientation.x = orientation[0] orientation_constraint.orientation.y = orientation[1] orientation_constraint.orientation.z = orientation[2] orientation_constraint.orientation.w = orientation[3] orientation_constraint.absolute_roll_tolerance = 0.04 orientation_constraint.absolute_pitch_tolerance = 0.04 orientation_constraint.absolute_yaw_tolerance = 0.04 orientation_constraint.weight = 1.0 goal.motion_plan_request.goal_constraints.position_constraints.append( position_constraint) goal.motion_plan_request.goal_constraints.orientation_constraints.append( orientation_constraint) if ordered_collision_operations is not None: rospy.loginfo("Adding ordered collisions") goal.operations = ordered_collision_operations if allowed_contacts is not None: rospy.loginfo("Adding allowed_contacts") goal.planning_scene_diff.allowed_contacts = allowed_contacts goal.disable_collision_monitoring = False state = client.send_goal_and_wait(goal, rospy.Duration(waiting_time)) if state == actionlib.GoalStatus.SUCCEEDED: return True else: return False