def create_move_arm_goal(self): goal = MoveArmGoal() if self.whicharm == "r": goal.motion_plan_request.group_name = "right_arm" else: goal.motion_plan_request.group_name = "left_arm" goal.motion_plan_request.num_planning_attempts = 2 goal.motion_plan_request.allowed_planning_time = rospy.Duration(5.0) goal.motion_plan_request.planner_id = "" goal.planner_service_name = "ompl_planning/plan_kinematic_path" return goal
def move_right_arm(self, position, orientation, frame_id, waiting_time): goal = MoveArmGoal() goal.motion_plan_request.group_name = "right_arm" goal.motion_plan_request.num_planning_attempts = 10 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 / 10.); position_constraint = PositionConstraint() position_constraint.header.frame_id = frame_id 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 = 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 = "r_wrist_roll_link" # 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) goal.disable_collision_monitoring = True # rospy.loginfo("Goal: " + str(goal)) state = self.move_right_arm_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 = ['joint1', 'joint2', 'joint3', 'joint4', 'joint5', 'joint6'] motion_plan_request = MotionPlanRequest() motion_plan_request.goal_constraints.joint_constraints = [JointConstraint() for i in range(len(joint_names))] motion_plan_request.group_name = "irp6p" motion_plan_request.num_planning_attempts = 1 motion_plan_request.allowed_planning_time = rospy.Duration(5.) motion_plan_request.planner_id = "" planner_service_name = "ompl_planning/plan_kinematic_path" 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 = 0.0 motion_plan_request.goal_constraints.joint_constraints[i].tolerance_above = 0.08 motion_plan_request.goal_constraints.joint_constraints[i].tolerance_below = 0.08 motion_plan_request.goal_constraints.joint_constraints[0].position = -0.0 motion_plan_request.goal_constraints.joint_constraints[1].position = -1.57 motion_plan_request.goal_constraints.joint_constraints[3].position = 0.0 motion_plan_request.goal_constraints.joint_constraints[4].position = 4.71 motion_plan_request.goal_constraints.joint_constraints[5].position = -2.8 #1.57 goal = MoveArmGoal() goal.planner_service_name = "ompl_planning/plan_kinematic_path" goal.motion_plan_request = motion_plan_request 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
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(10): 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) motion_plan_request = motion_planning_msgs.msg.MotionPlanRequest() motion_plan_request.group_name = "irp6p" motion_plan_request.num_planning_attempts = 1 motion_plan_request.allowed_planning_time = rospy.Duration(30.) motion_plan_request.planner_id = "" planner_service_name = "ompl_planning/plan_kinematic_path" motion_plan_request.goal_constraints.position_constraints = [motion_planning_msgs.msg.PositionConstraint() for _ in range(1)] motion_plan_request.goal_constraints.position_constraints[0].header.stamp = rospy.Time.now() motion_plan_request.goal_constraints.position_constraints[0].header.frame_id = "base_link" motion_plan_request.goal_constraints.position_constraints[0].link_name = "link6" motion_plan_request.goal_constraints.position_constraints[0].position.x = .85 motion_plan_request.goal_constraints.position_constraints[0].position.y = 0.35 motion_plan_request.goal_constraints.position_constraints[0].position.z = 0.90 motion_plan_request.goal_constraints.position_constraints[0].constraint_region_shape.type = Shape.BOX motion_plan_request.goal_constraints.position_constraints[0].constraint_region_shape.dimensions = [float(.02) for _ in range(3)] motion_plan_request.goal_constraints.position_constraints[0].constraint_region_orientation.w = 1.0 motion_plan_request.goal_constraints.position_constraints[0].weight = 1.0 motion_plan_request.goal_constraints.orientation_constraints = [motion_planning_msgs.msg.OrientationConstraint() for _ in range(1)] motion_plan_request.goal_constraints.orientation_constraints[0].header.stamp = rospy.Time.now() motion_plan_request.goal_constraints.orientation_constraints[0].header.frame_id = "base_link" motion_plan_request.goal_constraints.orientation_constraints[0].link_name = "link6" vals = [float() for _ in range(4)] vals = [-1.0, -.0, 0.0, .0] mag = math.sqrt(vals[0]*vals[0]+vals[1]*vals[1]+vals[2]*vals[2]+vals[3]*vals[3]) vals[0] /= mag vals[1] /= mag vals[2] /= mag vals[3] /= mag motion_plan_request.goal_constraints.orientation_constraints[0].orientation.x = vals[0] motion_plan_request.goal_constraints.orientation_constraints[0].orientation.y = vals[1] motion_plan_request.goal_constraints.orientation_constraints[0].orientation.z = vals[2] motion_plan_request.goal_constraints.orientation_constraints[0].orientation.w = vals[3] motion_plan_request.goal_constraints.orientation_constraints[0].absolute_roll_tolerance = 0.04 motion_plan_request.goal_constraints.orientation_constraints[0].absolute_pitch_tolerance = 0.04 motion_plan_request.goal_constraints.orientation_constraints[0].absolute_yaw_tolerance = 0.04 motion_plan_request.goal_constraints.orientation_constraints[0].weight = 1.0 goal = MoveArmGoal() goal.planner_service_name = "ompl_planning/plan_kinematic_path" goal.motion_plan_request = motion_plan_request 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 #----------------------------------------------------------------------------- motion_plan_request.goal_constraints.position_constraints[0].header.stamp = rospy.Time.now() motion_plan_request.goal_constraints.position_constraints[0].header.frame_id = "base_link" motion_plan_request.goal_constraints.position_constraints[0].link_name = "link6" motion_plan_request.goal_constraints.position_constraints[0].position.x = .85 motion_plan_request.goal_constraints.position_constraints[0].position.y = -0.35 motion_plan_request.goal_constraints.position_constraints[0].position.z = 0.90 # motion_plan_request.path_constraints.orientation_constraints = [motion_planning_msgs.msg.OrientationConstraint() for _ in range(1)] # motion_plan_request.path_constraints.orientation_constraints[0].header.stamp = rospy.Time.now() # motion_plan_request.path_constraints.orientation_constraints[0].header.frame_id = "base_link" # motion_plan_request.path_constraints.orientation_constraints[0].link_name = "link6" # motion_plan_request.path_constraints.orientation_constraints[0].orientation.x = -1.0 # motion_plan_request.path_constraints.orientation_constraints[0].orientation.y = 0.0 # motion_plan_request.path_constraints.orientation_constraints[0].orientation.z = 0.0 # motion_plan_request.path_constraints.orientation_constraints[0].orientation.w = 0.0 # motion_plan_request.path_constraints.orientation_constraints[0].absolute_roll_tolerance = 1.5 # motion_plan_request.path_constraints.orientation_constraints[0].absolute_pitch_tolerance = 1.5 # motion_plan_request.path_constraints.orientation_constraints[0].absolute_yaw_tolerance = 6.04 # motion_plan_request.path_constraints.orientation_constraints[0].type = motion_planning_msgs.msg.OrientationConstraint.HEADER_FRAME # motion_plan_request.path_constraints.orientation_constraints[0].weight = 1.0 goal = MoveArmGoal() goal.planner_service_name = "ompl_planning/plan_kinematic_path" goal.motion_plan_request = motion_plan_request 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
def testMotionExecutionBuffer(self): global padd_name global extra_buffer # too much trouble to read for now allow_padd = 0.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.0) 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 = 0.22 goal.motion_plan_request.goal_constraints.joint_constraints[1].position = 0.51 goal.motion_plan_request.goal_constraints.joint_constraints[2].position = 0.05 goal.motion_plan_request.goal_constraints.joint_constraints[3].position = -0.41 goal.motion_plan_request.goal_constraints.joint_constraints[4].position = -0.06 goal.motion_plan_request.goal_constraints.joint_constraints[5].position = -0.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)
from move_arm_msgs.msg import MoveArmAction from motion_planning_msgs.msg import PositionConstraint from motion_planning_msgs.msg import OrientationConstraint from geometric_shapes_msgs.msg import Shape from actionlib_msgs.msg import GoalStatus if __name__ == '__main__': rospy.init_node('arm_cartesian_goal_sender') move_arm = actionlib.SimpleActionClient('move_right_arm', MoveArmAction) move_arm.wait_for_server() rospy.logout('Connected to server') goalA = MoveArmGoal() goalA.motion_plan_request.group_name = 'right_arm' 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.) pc = PositionConstraint() pc.header.stamp = rospy.Time.now() pc.header.frame_id = 'torso_lift_link' pc.link_name = 'r_wrist_roll_link' pc.position.x = 0.75 pc.position.y = -0.188 pc.position.z = 0
from move_arm_msgs.msg import MoveArmGoal from motion_planning_msgs.msg import JointConstraint from actionlib_msgs.msg import GoalStatus if __name__ == '__main__': import hrl_lib.transforms as tr rospy.init_node('move_arm_joint_goal_test') move_arm = actionlib.SimpleActionClient('move_right_arm', MoveArmAction) move_arm.wait_for_server() rospy.loginfo('Connected to server') goalB = MoveArmGoal() 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' ] goalB.motion_plan_request.group_name = 'right_arm' goalB.motion_plan_request.num_planning_attempts = 1 goalB.motion_plan_request.allowed_planning_time = rospy.Duration(5.0) goalB.motion_plan_request.planner_id = '' goalB.planner_service_name = 'ompl_planning/plan_kinematic_path' import roslib
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 = [ 'right_arm_0_joint', 'right_arm_1_joint', 'right_arm_2_joint', 'right_arm_3_joint', 'right_arm_4_joint', 'right_arm_5_joint', 'right_arm_6_joint' ] motion_plan_request = MotionPlanRequest() motion_plan_request.goal_constraints.joint_constraints = [ JointConstraint() for i in range(len(joint_names)) ] motion_plan_request.group_name = "right_arm" motion_plan_request.num_planning_attempts = 1 motion_plan_request.allowed_planning_time = rospy.Duration(5.) motion_plan_request.planner_id = "" planner_service_name = "ompl_planning/plan_kinematic_path" 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 = 0.0 motion_plan_request.goal_constraints.joint_constraints[ i].tolerance_above = 0.08 motion_plan_request.goal_constraints.joint_constraints[ i].tolerance_below = 0.08 motion_plan_request.goal_constraints.joint_constraints[ 0].position = 1.5 motion_plan_request.goal_constraints.joint_constraints[ 1].position = -1.8 motion_plan_request.goal_constraints.joint_constraints[3].position = .5 goal = MoveArmGoal() goal.planner_service_name = "ompl_planning/plan_kinematic_path" goal.motion_plan_request = motion_plan_request 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 motion_plan_request.goal_constraints.joint_constraints[ 0].position = 0.0 motion_plan_request.goal_constraints.joint_constraints[ 1].position = 0.0 motion_plan_request.goal_constraints.joint_constraints[ 3].position = 0.0 goal = MoveArmGoal() goal.planner_service_name = "ompl_planning/plan_kinematic_path" goal.motion_plan_request = motion_plan_request 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
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 = mapping_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)
def testMotionExecutionBuffer(self): global padd_name global extra_buffer #too much trouble to read for now allow_padd = .05 #rospy.get_param(padd_name) motion_plan_request = MotionPlanRequest() motion_plan_request.group_name = "right_arm" motion_plan_request.num_planning_attempts = 1 motion_plan_request.allowed_planning_time = rospy.Duration(5.) motion_plan_request.planner_id = "" planner_service_name = "ompl_planning/plan_kinematic_path" motion_plan_request.goal_constraints.position_constraints = [ PositionConstraint() for _ in range(1) ] motion_plan_request.goal_constraints.position_constraints[ 0].header.stamp = rospy.Time.now() motion_plan_request.goal_constraints.position_constraints[ 0].header.frame_id = "base_link" motion_plan_request.goal_constraints.position_constraints[ 0].link_name = "right_arm_hand_link" motion_plan_request.goal_constraints.position_constraints[ 0].position.x = .65 motion_plan_request.goal_constraints.position_constraints[ 0].position.y = -0.55 motion_plan_request.goal_constraints.position_constraints[ 0].position.z = .9 motion_plan_request.goal_constraints.position_constraints[ 0].constraint_region_shape.type = Shape.BOX motion_plan_request.goal_constraints.position_constraints[ 0].constraint_region_shape.dimensions = [ float(.02) for _ in range(3) ] motion_plan_request.goal_constraints.position_constraints[ 0].constraint_region_orientation.w = 1.0 motion_plan_request.goal_constraints.position_constraints[ 0].weight = 1.0 motion_plan_request.goal_constraints.orientation_constraints = [ OrientationConstraint() for _ in range(1) ] motion_plan_request.goal_constraints.orientation_constraints[ 0].header.stamp = rospy.Time.now() motion_plan_request.goal_constraints.orientation_constraints[ 0].header.frame_id = "base_link" motion_plan_request.goal_constraints.orientation_constraints[ 0].link_name = "right_arm_hand_link" vals = [float() for _ in range(4)] vals = [-.895249, -.187638, 0.092325, .393443] mag = math.sqrt(vals[0] * vals[0] + vals[1] * vals[1] + vals[2] * vals[2] + vals[3] * vals[3]) vals[0] /= mag vals[1] /= mag vals[2] /= mag vals[3] /= mag motion_plan_request.goal_constraints.orientation_constraints[ 0].orientation.x = vals[0] motion_plan_request.goal_constraints.orientation_constraints[ 0].orientation.y = vals[1] motion_plan_request.goal_constraints.orientation_constraints[ 0].orientation.z = vals[2] motion_plan_request.goal_constraints.orientation_constraints[ 0].orientation.w = vals[3] motion_plan_request.goal_constraints.orientation_constraints[ 0].absolute_roll_tolerance = 0.04 motion_plan_request.goal_constraints.orientation_constraints[ 0].absolute_pitch_tolerance = 0.04 motion_plan_request.goal_constraints.orientation_constraints[ 0].absolute_yaw_tolerance = 0.04 motion_plan_request.goal_constraints.orientation_constraints[ 0].weight = 1.0 goal = MoveArmGoal() goal.planner_service_name = "ompl_planning/plan_kinematic_path" goal.motion_plan_request = motion_plan_request 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
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 = mapping_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)