def move_arm_to_grasping_joint_pose(joint_names, joint_positions, allowed_contacts=[], link_padding=[], collision_operations=OrderedCollisionOperations()): move_arm_client = SimpleActionClient('move_left_arm', MoveArmAction) move_arm_client.wait_for_server() rospy.loginfo('connected to move_left_arm action server') goal = MoveArmGoal() goal.planner_service_name = 'ompl_planning/plan_kinematic_path' goal.motion_plan_request.planner_id = '' goal.motion_plan_request.group_name = 'left_arm' goal.motion_plan_request.num_planning_attempts = 1 goal.motion_plan_request.allowed_planning_time = rospy.Duration(5.0) goal.motion_plan_request.goal_constraints.joint_constraints = [JointConstraint(j, p, 0.1, 0.1, 0.0) for (j,p) in zip(joint_names,joint_positions)] goal.motion_plan_request.allowed_contacts = allowed_contacts goal.motion_plan_request.link_padding = link_padding goal.motion_plan_request.ordered_collision_operations = collision_operations move_arm_client.send_goal(goal) finished_within_time = move_arm_client.wait_for_result(rospy.Duration(200.0)) if not finished_within_time: move_arm_client.cancel_goal() rospy.loginfo('timed out trying to achieve a joint goal') else: state = move_arm_client.get_state() if state == GoalStatus.SUCCEEDED: rospy.loginfo('action finished: %s' % str(state)) return True else: rospy.loginfo('action failed: %s' % str(state)) return False
def move_arm_ompl(x, y, z, ox=0.0, oy=0.0, oz=0.0, ow=1.0, arm_service="move_right_arm", shoulder_tolerance=(0.5, 0.3), frame="odom_combined"): client = actionlib.SimpleActionClient(arm_service, MoveArmAction) client.wait_for_server() goal = MoveArmGoal() goal.disable_ik = True goal.planner_service_name = "ompl_planning/plan_kinematic_path" goal.motion_plan_request.group_name = "right_arm" goal.motion_plan_request.num_planning_attempts = 10 goal.motion_plan_request.allowed_planning_time = roslib.rostime.Duration(5) goal.motion_plan_request.planner_id = "" addPositionConstraint(goal, x, y, z, frame) addOrientationConstraint(goal, ox, oy, oz, ow, frame) constraint = JointConstraint() constraint.joint_name = "r_shoulder_lift_joint" constraint.position = -0.05 constraint.tolerance_above = shoulder_tolerance[0] constraint.tolerance_below = shoulder_tolerance[1] constraint.weight = 1.0 goal.motion_plan_request.goal_constraints.joint_constraints.append( constraint) client.send_goal(goal, done_cb=move_arm_completed_cb) print client.wait_for_result()
def move_arm_ompl(x, y, z, ox=0.0, oy=0.0, oz=0.0, ow=1.0, arm_service = "move_right_arm", shoulder_tolerance=(0.5, 0.3), frame="odom_combined"): client = actionlib.SimpleActionClient(arm_service, MoveArmAction) client.wait_for_server() goal = MoveArmGoal() goal.disable_ik = True goal.planner_service_name = "ompl_planning/plan_kinematic_path" goal.motion_plan_request.group_name = "right_arm" goal.motion_plan_request.num_planning_attempts = 10 goal.motion_plan_request.allowed_planning_time = roslib.rostime.Duration(5) goal.motion_plan_request.planner_id = "" addPositionConstraint(goal, x, y, z, frame) addOrientationConstraint(goal, ox, oy, oz, ow, frame) constraint = JointConstraint() constraint.joint_name = "r_shoulder_lift_joint" constraint.position = -0.05 constraint.tolerance_above = shoulder_tolerance[0] constraint.tolerance_below = shoulder_tolerance[1] constraint.weight = 1.0 goal.motion_plan_request.goal_constraints.joint_constraints.append(constraint) client.send_goal(goal, done_cb=move_arm_completed_cb) print client.wait_for_result()
def move_arm_joint_goal(move_arm_client, joint_names, joint_positions, allowed_contacts=[], link_padding=[], collision_operations=OrderedCollisionOperations()): goal = MoveArmGoal() goal.planner_service_name = 'ompl_planning/plan_kinematic_path' goal.motion_plan_request.planner_id = '' goal.motion_plan_request.group_name = ARM_GROUP_NAME goal.motion_plan_request.num_planning_attempts = 3 goal.motion_plan_request.allowed_planning_time = rospy.Duration(5.0) goal.motion_plan_request.goal_constraints.joint_constraints = [ JointConstraint(j, p, 0.1, 0.1, 0.0) for (j, p) in zip(joint_names, joint_positions) ] goal.planning_scene_diff.allowed_contacts = allowed_contacts goal.planning_scene_diff.link_padding = link_padding goal.operations = collision_operations move_arm_client.send_goal(goal) finished_within_time = move_arm_client.wait_for_result( rospy.Duration(200.0)) if not finished_within_time: move_arm_client.cancel_goal() rospy.logerr('timed out trying to achieve joint goal') return False else: state = move_arm_client.get_state() if state == GoalStatus.SUCCEEDED: return True else: rospy.logerr( 'failed to achieve joint goal (returned status code %s)' % str(state)) return False
def getJointConstraints(goal): joint_names = [ "r_shoulder_pan_joint", "r_shoulder_lift_joint", "r_upper_arm_roll_joint", "r_elbow_flex_joint", "r_forearm_roll_joint", "r_wrist_flex_joint", "r_wrist_roll_joint" ] joint_constraints = [] for joint_name in joint_names: constraint = JointConstraint() constraint.joint_name = joint_name constraint.position = 0 constraint.tolerance_above = 0.1 constraint.tolerance_below = 0.1 constraint.weight = 1.0 joint_constraints.append(constraint) joint_constraints[0].position = -2 joint_constraints[3].position = -0.2 joint_constraints[5].position = -0.15 goal.motion_plan_request.goal_constraints.joint_constraints = joint_constraints
def create_motion_plan_request_for_joints(joint_positions): """Setup motion plan request. The length of parameter joint_positions must match the length of global variable JOINT_NAMES. """ motion_plan_request = MotionPlanRequest() motion_plan_request.group_name = "SchunkArm" motion_plan_request.num_planning_attempts = 1 motion_plan_request.allowed_planning_time = rospy.Duration(5.0) motion_plan_request.planner_id = "" motion_plan_request.goal_constraints.joint_constraints = [ JointConstraint() for i in range(len(JOINT_NAMES)) ] for i in range(len(JOINT_NAMES)): motion_plan_request.goal_constraints.joint_constraints[ i].joint_name = JOINT_NAMES[i] motion_plan_request.goal_constraints.joint_constraints[ i].position = joint_positions[i] motion_plan_request.goal_constraints.joint_constraints[ i].tolerance_above = 0.01 motion_plan_request.goal_constraints.joint_constraints[ i].tolerance_below = 0.01 return motion_plan_request
def move_arm_ompl(x, y, z): service = "move_right_arm" client = actionlib.SimpleActionClient(service, MoveArmAction) client.wait_for_server() goal = MoveArmGoal() goal.disable_ik = True goal.planner_service_name = "ompl_planning/plan_kinematic_path" goal.motion_plan_request.group_name = "right_arm" goal.motion_plan_request.num_planning_attempts = 5 goal.motion_plan_request.allowed_planning_time = roslib.rostime.Duration(5) goal.motion_plan_request.planner_id = "" addPositionConstraint(goal, x, y, z) addOrientationConstraint(goal) ''' constraint = JointConstraint() constraint.joint_name = "r_upper_arm_roll_joint" constraint.position = -2 constraint.tolerance_above = 0.1 constraint.tolerance_below = 0.1 constraint.weight = 1.0 goal.motion_plan_request.goal_constraints.joint_constraints.append(constraint) ''' constraint = JointConstraint() constraint.joint_name = "r_shoulder_lift_joint" constraint.position = 0.4 constraint.tolerance_above = 0.2 constraint.tolerance_below = 0.2 constraint.weight = 1.0 goal.motion_plan_request.goal_constraints.joint_constraints.append( constraint) client.send_goal(goal, done_cb=move_arm_completed_cb) client.wait_for_result()
def getJointConstraints(goal): joint_names = ["r_shoulder_pan_joint", "r_shoulder_lift_joint", "r_upper_arm_roll_joint", "r_elbow_flex_joint", "r_forearm_roll_joint", "r_wrist_flex_joint", "r_wrist_roll_joint"] joint_constraints = [] for joint_name in joint_names: constraint = JointConstraint() constraint.joint_name = joint_name constraint.position = 0 constraint.tolerance_above = 0.1 constraint.tolerance_below = 0.1 constraint.weight = 1.0 joint_constraints.append(constraint) joint_constraints[0].position = -2 joint_constraints[3].position = -0.2 joint_constraints[5].position = -0.15 goal.motion_plan_request.goal_constraints.joint_constraints = joint_constraints
def move_arm_ompl(x, y, z): service = "move_right_arm" client = actionlib.SimpleActionClient(service, MoveArmAction) client.wait_for_server() goal = MoveArmGoal() goal.disable_ik = True goal.planner_service_name = "ompl_planning/plan_kinematic_path" goal.motion_plan_request.group_name = "right_arm" goal.motion_plan_request.num_planning_attempts = 5 goal.motion_plan_request.allowed_planning_time = roslib.rostime.Duration(5) goal.motion_plan_request.planner_id = "" addPositionConstraint(goal, x, y, z) addOrientationConstraint(goal) ''' constraint = JointConstraint() constraint.joint_name = "r_upper_arm_roll_joint" constraint.position = -2 constraint.tolerance_above = 0.1 constraint.tolerance_below = 0.1 constraint.weight = 1.0 goal.motion_plan_request.goal_constraints.joint_constraints.append(constraint) ''' constraint = JointConstraint() constraint.joint_name = "r_shoulder_lift_joint" constraint.position = 0.4 constraint.tolerance_above = 0.2 constraint.tolerance_below = 0.2 constraint.weight = 1.0 goal.motion_plan_request.goal_constraints.joint_constraints.append(constraint) client.send_goal(goal, done_cb=move_arm_completed_cb) client.wait_for_result()
def move_arm_joint(self, arm, joint_angles): self.set_planning_scene() rospy.loginfo( "asking move_arm to send the %s arm to go to angles: %s" % (arm, self.pplist(joint_angles))) goal = self.create_move_arm_goal(arm) #add the joint angles as a joint constraint for (joint_name, joint_angle) in zip(self.joint_names[arm], joint_angles): joint_constraint = JointConstraint() joint_constraint.joint_name = joint_name joint_constraint.position = joint_angle joint_constraint.tolerance_below = .1 joint_constraint.tolerance_above = .1 goal.motion_plan_request.goal_constraints.joint_constraints.append( joint_constraint) #send the goal off to move arm result = self.send_move_arm_goal(arm, goal) return result
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 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 = "ompl_planning/plan_kinematic_path" goal.motion_plan_request.goal_constraints.joint_constraints = [ JointConstraint() for i in range(len(joint_names)) ] 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 for z in range(2): min_dist = 1000 if (z % 2 == 0): goal.motion_plan_request.goal_constraints.joint_constraints[ 0].position = -2.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 = 0.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 for x in range(3): self.move_arm_action_client.send_goal(goal) r = rospy.Rate(10) 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 #getting right finger tip link in base_link frame t = self.tf.getLatestCommonTime( "/base_link", "/r_gripper_r_finger_tip_link") finger_point = PointStamped() finger_point.header.frame_id = "/r_gripper_r_finger_tip_link" finger_point.header.stamp = t finger_point.point.x = 0 finger_point.point.y = 0 finger_point.point.z = 0 finger_point_base = self.tf.transformPoint( "base_link", finger_point) distance = math.sqrt( math.pow(finger_point_base.point.x - .6, 2) + math.pow(finger_point_base.point.y + .6, 2)) # pole is .1 in diameter distance -= .1 if distance < min_dist: rospy.loginfo("X: %g Y: %g Dist: %g", finger_point_base.point.x, finger_point_base.point.y, distance) min_dist = distance r.sleep() end_state = self.move_arm_action_client.get_state() if (end_state == actionlib_msgs.msg.GoalStatus.SUCCEEDED): break rospy.loginfo("Min dist %d is %g", z, min_dist) #should be a .5 buffer, allowing .1 buffer self.failIf(min_dist < (allow_padd - extra_buffer)) final_state = self.move_arm_action_client.get_state() self.assertEqual(final_state, actionlib_msgs.msg.GoalStatus.SUCCEEDED)
def testAllowedNotAllowedInitialContact(self): #adding object in collision with base obj2 = CollisionObject() obj2.header.stamp = rospy.Time.now() obj2.header.frame_id = "base_link" obj2.id = "base_object" obj2.operation.operation = arm_navigation_msgs.msg.CollisionObjectOperation.ADD obj2.shapes = [Shape() for _ in range(1)] obj2.shapes[0].type = Shape.BOX obj2.shapes[0].dimensions = [float() for _ in range(3)] obj2.shapes[0].dimensions[0] = .1 obj2.shapes[0].dimensions[1] = .1 obj2.shapes[0].dimensions[2] = .1 obj2.poses = [Pose() for _ in range(1)] obj2.poses[0].position.x = 0 obj2.poses[0].position.y = 0 obj2.poses[0].position.z = 0 obj2.poses[0].orientation.x = 0 obj2.poses[0].orientation.y = 0 obj2.poses[0].orientation.z = 0 obj2.poses[0].orientation.w = 1 self.obj_pub.publish(obj2) rospy.sleep(5.) 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 = "ompl_planning/plan_kinematic_path" goal.motion_plan_request.goal_constraints.joint_constraints = [ JointConstraint() for i in range(len(joint_names)) ] 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 goal.motion_plan_request.goal_constraints.joint_constraints[ 0].position = -2.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 self.move_arm_action_client.send_goal(goal) r = rospy.Rate(10) 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 #should still have succeedeed final_state = self.move_arm_action_client.get_state() self.failIf(final_state != actionlib_msgs.msg.GoalStatus.SUCCEEDED) # but we can still overwrite coll = CollisionOperation() coll.object1 = "base_link" coll.object2 = coll.COLLISION_SET_OBJECTS coll.operation = coll.ENABLE goal.motion_plan_request.ordered_collision_operations.collision_operations.append( coll) goal.motion_plan_request.goal_constraints.joint_constraints[ 0].position = 0.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 self.move_arm_action_client.send_goal(goal) r = rospy.Rate(10) 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 #should still have succeedeed final_state = self.move_arm_action_client.get_state() self.failIf(final_state == actionlib_msgs.msg.GoalStatus.SUCCEEDED)
goal.motion_plan_request.group_name = 'left_arm' goal.motion_plan_request.num_planning_attempts = 1 goal.motion_plan_request.allowed_planning_time = rospy.Duration(5.0) links = ('L7_wrist_roll_link', 'L6_wrist_pitch_link', 'L5_forearm_roll_link', 'L4_elbow_flex_link', 'L3_upperarm_roll_link', 'L2_shoulder_pan_link', 'L1_shoulder_pitch_link', 'L8_left_finger_link ', 'L9_right_finger_link') joints = ('shoulder_pitch_joint', 'shoulder_pan_joint', 'upperarm_roll_joint', 'elbow_flex_joint', 'forearm_roll_joint', 'wrist_pitch_joint', 'wrist_roll_joint') goal.motion_plan_request.goal_constraints.joint_constraints = [ JointConstraint(j, 0.0, 0.1, 0.1, 0.0) for j in joints ] goal.planning_scene_diff.link_padding = [ LinkPadding(l, 0.0) for l in links ] init = (0.0, ) * 7 #pick = (0.08297109,-1.29328384, 0.01069185,-0.14805498, 0.14074285, 1.67430316, -1.56874745) tucked = (-1.9715, -1.7406, 0.0213, -0.1807, -1.8408, 1.0840, 0.1483) tucked1 = (-1.751, -1.820, -0.084, -0.214, -1.815, 1.089, 0.031) tucked2 = (-1.8497, -1.8508, 0.0171, -0.2459, -1.9839, 1.0329, 0.1790) #tucked = (-1.837, -1.823, 0.175, -0.195, -1.861, 1.089, 0.128) sample1 = (0.0216, -1.1143, 0.1235, 0.9809, 2.6989, -1.7704, 0.2139) sample2 = (-0.7155, 0.0364, 1.0405, 0.1331, 0.4470, -0.3615, -0.4405) pick = (0.10373755, -0.25866649, 0.02566044, -0.1677145, -0.32607513, 1.3214246, 1.60355412)