def poseConstraintToPositionOrientationConstraints(pose_constraint): position_constraint = PositionConstraint() orientation_constraint = OrientationConstraint() position_constraint.header = pose_constraint.header position_constraint.link_name = pose_constraint.link_name position_constraint.position = pose_constraint.pose.position position_constraint.constraint_region_shape.type = 0 position_constraint.constraint_region_shape.dimensions.append(2*pose_constraint.absolute_position_tolerance.x) position_constraint.constraint_region_shape.dimensions.append(2*pose_constraint.absolute_position_tolerance.y) position_constraint.constraint_region_shape.dimensions.append(2*pose_constraint.absolute_position_tolerance.z) position_constraint.constraint_region_orientation.x = 0.0 position_constraint.constraint_region_orientation.y = 0.0 position_constraint.constraint_region_orientation.z = 0.0 position_constraint.constraint_region_orientation.w = 1.0 position_constraint.weight = 1.0 orientation_constraint.header = pose_constraint.header orientation_constraint.link_name = pose_constraint.link_name orientation_constraint.orientation = pose_constraint.pose.orientation orientation_constraint.type = pose_constraint.orientation_constraint_type orientation_constraint.absolute_roll_tolerance = pose_constraint.absolute_roll_tolerance orientation_constraint.absolute_pitch_tolerance = pose_constraint.absolute_pitch_tolerance orientation_constraint.absolute_yaw_tolerance = pose_constraint.absolute_yaw_tolerance orientation_constraint.weight = 1.0 return (position_constraint, orientation_constraint)
def get_interpolated_ik_motion_plan(self, start_pose, goal_pose, start_angles, joint_names, pos_spacing=0.01, rot_spacing=0.1, consistent_angle=math.pi / 9, collision_aware=True, collision_check_resolution=1, steps_before_abort=-1, num_steps=0, ordered_collision_operations=None, frame='base_footprint', start_from_end=0, max_joint_vels=[1.5] * 7, max_joint_accs=[8.0] * 7): res = self.interpolated_ik_params_srv(num_steps, consistent_angle, collision_check_resolution, steps_before_abort, pos_spacing, rot_spacing, collision_aware, start_from_end, max_joint_vels, max_joint_accs) req = GetMotionPlanRequest() req.motion_plan_request.start_state.joint_state.name = joint_names req.motion_plan_request.start_state.joint_state.position = start_angles req.motion_plan_request.start_state.multi_dof_joint_state.poses = [ start_pose.pose ] req.motion_plan_request.start_state.multi_dof_joint_state.child_frame_ids = [ GRIPPER_LINK_FRAME ] req.motion_plan_request.start_state.multi_dof_joint_state.frame_ids = [ start_pose.header.frame_id ] pos_constraint = PositionConstraint() pos_constraint.position = goal_pose.pose.position pos_constraint.header.frame_id = goal_pose.header.frame_id req.motion_plan_request.goal_constraints.position_constraints = [ pos_constraint, ] orient_constraint = OrientationConstraint() orient_constraint.orientation = goal_pose.pose.orientation orient_constraint.header.frame_id = goal_pose.header.frame_id req.motion_plan_request.goal_constraints.orientation_constraints = [ orient_constraint, ] #req.motion_plan_request.link_padding = [LinkPadding(l,0.0) for l in GRIPPER_LINKS] #req.motion_plan_request.link_padding.extend([LinkPadding(l,0.0) for l in ARM_LINKS]) #if ordered_collision_operations is not None: # req.motion_plan_request.ordered_collision_operations = ordered_collision_operations res = self.interpolated_ik_srv(req) return res
def get_interpolated_ik_motion_plan( self, start_pose, goal_pose, start_angles, joint_names, pos_spacing=0.01, rot_spacing=0.1, consistent_angle=math.pi / 9, collision_aware=True, collision_check_resolution=1, steps_before_abort=-1, num_steps=0, ordered_collision_operations=None, frame="base_footprint", start_from_end=0, max_joint_vels=[1.5] * 7, max_joint_accs=[8.0] * 7, ): res = self.interpolated_ik_params_srv( num_steps, consistent_angle, collision_check_resolution, steps_before_abort, pos_spacing, rot_spacing, collision_aware, start_from_end, max_joint_vels, max_joint_accs, ) req = GetMotionPlanRequest() req.motion_plan_request.start_state.joint_state.name = joint_names req.motion_plan_request.start_state.joint_state.position = start_angles req.motion_plan_request.start_state.multi_dof_joint_state.poses = [start_pose.pose] req.motion_plan_request.start_state.multi_dof_joint_state.child_frame_ids = [GRIPPER_LINK_FRAME] req.motion_plan_request.start_state.multi_dof_joint_state.frame_ids = [start_pose.header.frame_id] pos_constraint = PositionConstraint() pos_constraint.position = goal_pose.pose.position pos_constraint.header.frame_id = goal_pose.header.frame_id req.motion_plan_request.goal_constraints.position_constraints = [pos_constraint] orient_constraint = OrientationConstraint() orient_constraint.orientation = goal_pose.pose.orientation orient_constraint.header.frame_id = goal_pose.header.frame_id req.motion_plan_request.goal_constraints.orientation_constraints = [orient_constraint] # req.motion_plan_request.link_padding = [LinkPadding(l,0.0) for l in GRIPPER_LINKS] # req.motion_plan_request.link_padding.extend([LinkPadding(l,0.0) for l in ARM_LINKS]) # if ordered_collision_operations is not None: # req.motion_plan_request.ordered_collision_operations = ordered_collision_operations res = self.interpolated_ik_srv(req) return res
def pose_constraint_to_position_orientation_constraints(pose_constraint): position_constraint = PositionConstraint() orientation_constraint = OrientationConstraint() position_constraint.header = pose_constraint.header position_constraint.link_name = pose_constraint.link_name position_constraint.position = pose_constraint.pose.position position_constraint.constraint_region_shape.type = 0 position_constraint.constraint_region_shape.dimensions.append(2*pose_constraint.absolute_position_tolerance.x) position_constraint.constraint_region_shape.dimensions.append(2*pose_constraint.absolute_position_tolerance.y) position_constraint.constraint_region_shape.dimensions.append(2*pose_constraint.absolute_position_tolerance.z) position_constraint.constraint_region_orientation.x = 0.0 position_constraint.constraint_region_orientation.y = 0.0 position_constraint.constraint_region_orientation.z = 0.0 position_constraint.constraint_region_orientation.w = 1.0 position_constraint.weight = 1.0 orientation_constraint.header = pose_constraint.header orientation_constraint.link_name = pose_constraint.link_name orientation_constraint.orientation = pose_constraint.pose.orientation orientation_constraint.type = pose_constraint.orientation_constraint_type orientation_constraint.absolute_roll_tolerance = pose_constraint.absolute_roll_tolerance orientation_constraint.absolute_pitch_tolerance = pose_constraint.absolute_pitch_tolerance orientation_constraint.absolute_yaw_tolerance = pose_constraint.absolute_yaw_tolerance orientation_constraint.weight = 1.0 return (position_constraint, orientation_constraint)
def _move_to_cartesian(self, coordinates): """ Move the arm to the pose given by (x, y, z, pitch) tuple. The pitch is optional and can be omitted. """ g = MoveArmGoal() pc = PositionConstraint() pc.header.frame_id = "/base_link" pc.header.stamp = rospy.Time.now() pc.position.x = coordinates[0] pc.position.y = coordinates[1] pc.position.z = coordinates[2] g.motion_plan_request.goal_constraints.position_constraints.append(pc) oc = OrientationConstraint() r = 0.0 p = coordinates[3] if len(coordinates) == 4 else self.pitch y = 0.0 (qx, qy, qz, qw) = tf.transformations.quaternion_from_euler(r, p, y) oc.header.frame_id = "/base_link" oc.header.stamp = rospy.Time.now() oc.orientation.x = qx oc.orientation.y = qy oc.orientation.z = qz oc.orientation.w = qw g.motion_plan_request.goal_constraints.orientation_constraints.append( oc) self.move_arm_cart_server.send_goal(g) rospy.loginfo('Sent move arm goal, waiting for result...') self.move_arm_cart_server.wait_for_result() rv = self.move_arm_cart_server.get_result().error_code.val print rv if not rv == 1: raise Exception('Failed to move the arm to the given pose.')
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 addOrientationConstraint(goal): constraint = OrientationConstraint() constraint.header.stamp = rospy.Time.now() constraint.link_name = "r_wrist_roll_link" constraint.header.frame_id = "base_link" constraint.orientation.x = 0.0 constraint.orientation.y = 0.0 constraint.orientation.z = 0.0 constraint.orientation.w = 1.0 constraint.absolute_pitch_tolerance = 0.04 constraint.absolute_roll_tolerance = 0.04 constraint.absolute_yaw_tolerance = 0.04 constraint.weight = 1.0 goal.motion_plan_request.goal_constraints.orientation_constraints.append( constraint)
def _move_to_cartesian(self, coordinates): """ Move the arm to the pose given by (x, y, z, pitch) tuple. The pitch is optional and can be omitted. """ move_arm_to_goal = MoveArmGoal() position_constraint_msg = PositionConstraint() position_constraint_msg.header.frame_id = "/base_link" position_constraint_msg.header.stamp = rospy.Time.now() position_constraint_msg.position.x = coordinates[0] position_constraint_msg.position.y = coordinates[1] position_constraint_msg.position.z = coordinates[2] move_arm_to_goal.motion_plan_request.goal_constraints.position_constraints.append( position_constraint_msg) orientation_constraint_msg = OrientationConstraint() roll_euler = 0.0 pitch_euler = coordinates[3] if len(coordinates) == 4 else self.pitch yaw_euler = 0.0 (quaternion_x, quaternion_y, quaternion_z, quaternion_w) = tf.transformations.quaternion_from_euler( roll_euler, pitch_euler, yaw_euler) orientation_constraint_msg.header.frame_id = "/base_link" orientation_constraint_msg.header.stamp = rospy.Time.now() orientation_constraint_msg.orientation.x = quaternion_x orientation_constraint_msg.orientation.y = quaternion_y orientation_constraint_msg.orientation.z = quaternion_z orientation_constraint_msg.orientation.w = quaternion_w move_arm_to_goal.motion_plan_request.goal_constraints.orientation_constraints.append( orientation_constraint_msg) self.move_arm_cart_server.send_goal(move_arm_to_goal) rospy.loginfo("Sent move arm goal, waiting for result...") self.move_arm_cart_server.wait_for_result() result_value = self.move_arm_cart_server.get_result().error_code.val print result_value if not result_value == 1: raise Exception("Failed to move the arm to the given pose.")
def addOrientationConstraint(goal, ox=0.0, oy=0.0, oz=0.0, ow=0.0, frame="odom_combined"): constraint = OrientationConstraint() constraint.header.stamp = rospy.Time.now() constraint.link_name = "r_wrist_roll_link" constraint.header.frame_id = frame constraint.orientation.x = ox constraint.orientation.y = oy constraint.orientation.z = oz constraint.orientation.w = ow constraint.absolute_pitch_tolerance = 0.04 constraint.absolute_roll_tolerance = 0.04 constraint.absolute_yaw_tolerance = 0.04 constraint.weight = 1.0 goal.motion_plan_request.goal_constraints.orientation_constraints.append(constraint)
def processFeedback(feedback): #s = "Feedback from marker '" + feedback.marker_name #if feedback.event_type == InteractiveMarkerFeedback.BUTTON_CLICK: # rospy.loginfo( s + ": button click" + mp + "." ) if feedback.event_type == InteractiveMarkerFeedback.MENU_SELECT: if (feedback.menu_entry_id == 1): p = feedback.pose print p goalA = arm_navigation_msgs.msg.MoveArmGoal() goalA.motion_plan_request.group_name = "right"; goalA.motion_plan_request.num_planning_attempts = 1; goalA.motion_plan_request.planner_id = ""; goalA.planner_service_name = "ompl_planning/plan_kinematic_path"; goalA.motion_plan_request.allowed_planning_time = rospy.Duration(5.0); desired_pose = PositionConstraint() desired_pose.header.frame_id = "/world"; desired_pose.link_name = "right_arm_7_link"; desired_pose.position = p.position desired_pose.constraint_region_shape.type = Shape.BOX desired_pose.constraint_region_shape.dimensions = [0.02, 0.02, 0.02] desired_pose.constraint_region_orientation.w = 1.0 goalA.motion_plan_request.goal_constraints.position_constraints.append(desired_pose) oc = OrientationConstraint() oc.header.stamp = rospy.Time.now() oc.header.frame_id = "/world"; oc.link_name = "right_arm_7_link"; oc.orientation = p.orientation oc.absolute_roll_tolerance = 0.04 oc.absolute_pitch_tolerance = 0.04 oc.absolute_yaw_tolerance = 0.04 oc.weight = 1. goalA.motion_plan_request.goal_constraints.orientation_constraints.append(oc) client.send_goal(goalA)
def addOrientationConstraint(goal): constraint = OrientationConstraint() constraint.header.stamp = rospy.Time.now() constraint.link_name = "r_wrist_roll_link" constraint.header.frame_id = "base_link" constraint.orientation.x = 0.0 constraint.orientation.y = 0.0 constraint.orientation.z = 0.0 constraint.orientation.w = 1.0 constraint.absolute_pitch_tolerance = 0.04 constraint.absolute_roll_tolerance = 0.04 constraint.absolute_yaw_tolerance = 0.04 constraint.weight = 1.0 goal.motion_plan_request.goal_constraints.orientation_constraints.append(constraint)
def addOrientationConstraint(goal, ox=0.0, oy=0.0, oz=0.0, ow=0.0, frame="odom_combined"): constraint = OrientationConstraint() constraint.header.stamp = rospy.Time.now() constraint.link_name = "r_wrist_roll_link" constraint.header.frame_id = frame constraint.orientation.x = ox constraint.orientation.y = oy constraint.orientation.z = oz constraint.orientation.w = ow constraint.absolute_pitch_tolerance = 0.04 constraint.absolute_roll_tolerance = 0.04 constraint.absolute_yaw_tolerance = 0.04 constraint.weight = 1.0 goal.motion_plan_request.goal_constraints.orientation_constraints.append( constraint)
req = GetMotionPlanRequest() req.motion_plan_request.start_state.joint_state.name = joint_names req.motion_plan_request.start_state.joint_state.position = start_angles req.motion_plan_request.start_state.multi_dof_joint_state.pose = start_pose.pose req.motion_plan_request.start_state.multi_dof_joint_state.child_frame_id = 'L7_wrist_yaw_link' req.motion_plan_request.start_state.multi_dof_joint_state.frame_id = start_pose.header.frame_id pos_constraint = PositionConstraint() pos_constraint.position = goal_pose.pose.position pos_constraint.header.frame_id = goal_pose.header.frame_id req.motion_plan_request.goal_constraints.position_constraints = [ pos_constraint, ] orient_constraint = OrientationConstraint() orient_constraint.orientation = goal_pose.pose.orientation orient_constraint.header.frame_id = goal_pose.header.frame_id req.motion_plan_request.goal_constraints.orientation_constraints = [ orient_constraint, ] if ordered_collision_operations != None: req.motion_plan_request.ordered_collision_operations = ordered_collision_operations try: serv = rospy.ServiceProxy("l_interpolated_ik_motion_plan", GetMotionPlan) res = serv(req) except rospy.ServiceException as e: print "error when calling l_interpolated_ik_motion_plan: %s" % e
def main(): rospy.init_node('move_arm_action_test') rospy.loginfo('Node for testing move_arm action') client = actionlib.SimpleActionClient('/move_arm', MoveArmAction) client.wait_for_server() rospy.loginfo('Server is available, let\'s start') goal = MoveArmGoal() goal.motion_plan_request.group_name = 'arm' goal.motion_plan_request.num_planning_attempts = 5 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.0) # example of valid position # - Translation: [-0.623, -0.460, 1.162] # - Rotation: in Quaternion [0.739, -0.396, -0.533, 0.116] # default position # - Translation: [-0.316, -0.816, 1.593] # - Rotation: in Quaternion [0.380, 0.153, -0.656, 0.634] pos_const = PositionConstraint() pos_const.header.frame_id = 'base_link' pos_const.link_name = 'sdh_palm_link' #pos_const.link_name = 'arm_7_link' # default position #pos_const.position.x = -0.316 #pos_const.position.y = -0.816 #pos_const.position.z = 1.593 pos_const.position.x = -0.623 pos_const.position.y = -0.460 pos_const.position.z = 1.162 pos_const.constraint_region_shape.type = pos_const.constraint_region_shape.BOX pos_const.constraint_region_shape.dimensions.append(2 * 0.02) pos_const.constraint_region_shape.dimensions.append(2 * 0.02) pos_const.constraint_region_shape.dimensions.append(2 * 0.02) pos_const.constraint_region_orientation.x = 0.0 pos_const.constraint_region_orientation.y = 0.0 pos_const.constraint_region_orientation.z = 0.0 pos_const.constraint_region_orientation.w = 1.0 pos_const.weight = 1.0 #pos_const.target_point_offset.x = 0.02 #pos_const.target_point_offset.y = 0.02 #pos_const.target_point_offset.z = 0.02 or_const = OrientationConstraint() or_const.header.frame_id = 'base_link' or_const.link_name = 'sdh_palm_link' #or_const.link_name = 'arm_7_link' #or_const.type = xxx or_const.weight = 1.0 #or_const.orientation # default orientation #or_const.orientation.x = 0.380 #or_const.orientation.y = 0.153 #or_const.orientation.z = -0.656 #or_const.orientation.w = 0.634 or_const.orientation.x = 0.739 or_const.orientation.y = -0.396 or_const.orientation.z = -0.533 or_const.orientation.w = 0.116 or_const.absolute_pitch_tolerance = 0.04 or_const.absolute_roll_tolerance = 0.04 or_const.absolute_yaw_tolerance = 0.04 # -------------------------------------------------- goal.motion_plan_request.goal_constraints.orientation_constraints.append( or_const) goal.motion_plan_request.goal_constraints.position_constraints.append( pos_const) client.send_goal(goal) client.wait_for_result() res = client.get_result() #rospy.loginfo(client.get_state()) print "Result" print res
''' pc = PositionConstraint() pc.header.stamp = rospy.Time.now() pc.header.frame_id = 'base_footprint' pc.link_name = 'r_wrist_roll_link' pc.position.x = .5 pc.position.y = .05 pc.position.z = .9 pc.constraint_region_shape.type = 0 pc.constraint_region_shape.dimensions = [0.02, 0.02, 0.02] pc.constraint_region_orientation.w = 1.0 goalA.motion_plan_request.goal_constraints.position_constraints.append(pc) oc = OrientationConstraint() oc.header.stamp = rospy.Time.now() oc.header.frame_id = 'base_footprint' oc.link_name = 'r_wrist_roll_link' oc.orientation.x = 0. oc.orientation.y = 0. oc.orientation.z = -.3 oc.orientation.w = 1. oc.absolute_roll_tolerance = 0.01 oc.absolute_pitch_tolerance = 0.01 oc.absolute_yaw_tolerance = 0.04 oc.weight = 1. goalA.motion_plan_request.goal_constraints.orientation_constraints.append(
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
except rospy.ServiceException, e: print "error when calling r_interpolated_ik_motion_plan_set_params: %s"%e return 0 req = GetMotionPlanRequest() req.motion_plan_request.start_state.joint_state.name = joint_names req.motion_plan_request.start_state.joint_state.position = start_angles req.motion_plan_request.start_state.multi_dof_joint_state.poses.append(start_pose.pose) req.motion_plan_request.start_state.multi_dof_joint_state.child_frame_ids.append('r_wrist_roll_link') req.motion_plan_request.start_state.multi_dof_joint_state.frame_ids.append(start_pose.header.frame_id) pos_constraint = PositionConstraint() pos_constraint.position = goal_pose.pose.position pos_constraint.header.frame_id = goal_pose.header.frame_id req.motion_plan_request.goal_constraints.position_constraints = [pos_constraint,] orient_constraint = OrientationConstraint() orient_constraint.orientation = goal_pose.pose.orientation orient_constraint.header.frame_id = goal_pose.header.frame_id req.motion_plan_request.goal_constraints.orientation_constraints = [orient_constraint,] #if ordered_collision_operations != None: # req.motion_plan_request.ordered_collision_operations = ordered_collision_operations try: serv = rospy.ServiceProxy("r_interpolated_ik_motion_plan", GetMotionPlan) res = serv(req) except rospy.ServiceException, e: print "error when calling r_interpolated_ik_motion_plan: %s"%e return 0 error_code_dict = {ArmNavigationErrorCodes.SUCCESS:0, ArmNavigationErrorCodes.COLLISION_CONSTRAINTS_VIOLATED:1, \
def main(): rospy.init_node("move_arm_action_test") rospy.loginfo("Node for testing move_arm action") client = actionlib.SimpleActionClient("/move_arm", MoveArmAction) client.wait_for_server() rospy.loginfo("Server is available, let's start") goal = MoveArmGoal() goal.motion_plan_request.group_name = "arm" goal.motion_plan_request.num_planning_attempts = 5 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.0) # example of valid position # - Translation: [-0.623, -0.460, 1.162] # - Rotation: in Quaternion [0.739, -0.396, -0.533, 0.116] # default position # - Translation: [-0.316, -0.816, 1.593] # - Rotation: in Quaternion [0.380, 0.153, -0.656, 0.634] pos_const = PositionConstraint() pos_const.header.frame_id = "base_link" pos_const.link_name = "sdh_palm_link" # pos_const.link_name = 'arm_7_link' # default position # pos_const.position.x = -0.316 # pos_const.position.y = -0.816 # pos_const.position.z = 1.593 pos_const.position.x = -0.623 pos_const.position.y = -0.460 pos_const.position.z = 1.162 pos_const.constraint_region_shape.type = pos_const.constraint_region_shape.BOX pos_const.constraint_region_shape.dimensions.append(2 * 0.02) pos_const.constraint_region_shape.dimensions.append(2 * 0.02) pos_const.constraint_region_shape.dimensions.append(2 * 0.02) pos_const.constraint_region_orientation.x = 0.0 pos_const.constraint_region_orientation.y = 0.0 pos_const.constraint_region_orientation.z = 0.0 pos_const.constraint_region_orientation.w = 1.0 pos_const.weight = 1.0 # pos_const.target_point_offset.x = 0.02 # pos_const.target_point_offset.y = 0.02 # pos_const.target_point_offset.z = 0.02 or_const = OrientationConstraint() or_const.header.frame_id = "base_link" or_const.link_name = "sdh_palm_link" # or_const.link_name = 'arm_7_link' # or_const.type = xxx or_const.weight = 1.0 # or_const.orientation # default orientation # or_const.orientation.x = 0.380 # or_const.orientation.y = 0.153 # or_const.orientation.z = -0.656 # or_const.orientation.w = 0.634 or_const.orientation.x = 0.739 or_const.orientation.y = -0.396 or_const.orientation.z = -0.533 or_const.orientation.w = 0.116 or_const.absolute_pitch_tolerance = 0.04 or_const.absolute_roll_tolerance = 0.04 or_const.absolute_yaw_tolerance = 0.04 # -------------------------------------------------- goal.motion_plan_request.goal_constraints.orientation_constraints.append(or_const) goal.motion_plan_request.goal_constraints.position_constraints.append(pos_const) client.send_goal(goal) client.wait_for_result() res = client.get_result() # rospy.loginfo(client.get_state()) print "Result" print res
pc = PositionConstraint() pc.header.stamp = rospy.Time.now() pc.header.frame_id = 'base_footprint' pc.link_name = 'r_wrist_roll_link' pc.position.x = .5 pc.position.y = .05 pc.position.z = .9 pc.constraint_region_shape.type = 0 pc.constraint_region_shape.dimensions = [0.02, 0.02, 0.02] pc.constraint_region_orientation.w = 1.0 goalA.motion_plan_request.goal_constraints.position_constraints.append(pc) oc = OrientationConstraint() oc.header.stamp = rospy.Time.now() oc.header.frame_id = 'base_footprint' oc.link_name = 'r_wrist_roll_link' oc.orientation.x = 0. oc.orientation.y = 0. oc.orientation.z = -.3 oc.orientation.w = 1. oc.absolute_roll_tolerance = 0.01 oc.absolute_pitch_tolerance = 0.01 oc.absolute_yaw_tolerance = 0.04 oc.weight = 1. goalA.motion_plan_request.goal_constraints.orientation_constraints.append(oc)
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 main(): rospy.init_node('move_arm_action_test') rospy.loginfo('Node for testing move_arm action') client = actionlib.SimpleActionClient('/move_arm',MoveArmAction) client.wait_for_server() rospy.loginfo('Server is available, let\'s start') goal = MoveArmGoal() goal.motion_plan_request.group_name = 'arm' goal.motion_plan_request.num_planning_attempts = 5 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.0) pos_const = PositionConstraint() pos_const.header.frame_id = 'base_footprint' pos_const.link_name = 'arm_wrist_roll_link' # default position # rosrun tf tf_echo base_footprint arm_wrist_roll_link #- Translation: [0.454, 0.000, 0.229] #- Rotation: in Quaternion [0.000, 0.000, 0.000, 1.000] # in RPY [0.000, 0.000, 0.000] pos_const.position.x = 0.315077617598 pos_const.position.y = 0.260552844631 pos_const.position.z = 0.279730321177 pos_const.constraint_region_shape.type = pos_const.constraint_region_shape.BOX pos_const.constraint_region_shape.dimensions.append(2*0.02) pos_const.constraint_region_shape.dimensions.append(2*0.02) pos_const.constraint_region_shape.dimensions.append(2*0.02) pos_const.constraint_region_orientation.x = 0.0 pos_const.constraint_region_orientation.y = 0.0 pos_const.constraint_region_orientation.z = 0.0 pos_const.constraint_region_orientation.w = 1.0 pos_const.weight = 1.0 or_const = OrientationConstraint() or_const.header.frame_id = 'base_footprint' or_const.link_name = 'arm_wrist_roll_link' or_const.weight = 1.0 # default orientation or_const.orientation.x = -0.212992566922 or_const.orientation.y = -0.105167499832 or_const.orientation.z = 0.437894676762 or_const.orientation.w = 0.867076822132 or_const.absolute_pitch_tolerance = 0.04 or_const.absolute_roll_tolerance = 0.04 or_const.absolute_yaw_tolerance = 0.04 # -------------------------------------------------- goal.motion_plan_request.goal_constraints.orientation_constraints.append(or_const) goal.motion_plan_request.goal_constraints.position_constraints.append(pos_const) client.send_goal(goal) client.wait_for_result() res = client.get_result() print "Result" print res
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 __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