def test_2_move_lin_few_waypoints(self): # move into home position resp = self.client.move_joints(0, pi / 100, 0, -pi / 2, 0, -pi / 2, 0) self.assert_last_srv_call_success(resp) goal_pose = Pose() goal_pose.position = Point(.3, .41, .62) goal_pose.orientation = orientation_from_rpy(-pi / 2, 0, 0) resp = self.client.move_ptp(goal_pose) # only change the orientation a little, no linear motion -> should fail goal_pose.orientation = orientation_from_rpy(-pi / 1.9, 0, 0) success = self.client.move_lin(goal_pose) self.assert_last_srv_call_failed(success, RLLErrorCode.TOO_FEW_WAYPOINTS) # only change the translation a little, no linear motion -> should fail goal_pose.position = Point(.305, .41, .62) success = self.client.move_lin(goal_pose) self.assert_last_srv_call_failed(success, RLLErrorCode.TOO_FEW_WAYPOINTS) # only change the position, orientation from before -> should succeed goal_pose.position = Point(.19, .41, .63) goal_pose.orientation = orientation_from_rpy(-pi / 2, 0, 0) success = self.client.move_lin(goal_pose) self.assert_last_srv_call_success(success)
def test_6_move_ptp_lin_sequence(self): # similar to the hello world for the Playground project resp = self.client.move_joints(0.0, pi / 4, 0.0, -pi / 4, 0.0, -pi / 2, 0.0) self.assert_last_srv_call_success(resp) goal_pose = Pose() goal_pose.position = Point(.5, .2, .7) goal_pose.orientation.z = 1 self.assert_move_ptp_success(goal_pose) goal_pose.orientation = orientation_from_rpy(0, pi / 2, 0) self.assert_move_ptp_success(goal_pose) goal_pose.orientation = orientation_from_rpy(pi / 4, pi / 2, 0) self.assert_move_ptp_success(goal_pose) resp = self.client.move_joints(0.0, pi / 4, 0.0, -pi / 4, 0.0, -pi / 2, 0.0) self.assert_last_srv_call_success(resp) goal_pose.position = Point(0.5, -0.6, 0.3) goal_pose.orientation = orientation_from_rpy(0, pi / 2, 0) self.assert_move_ptp_success(goal_pose) goal_pose.position.z = .7 self.assert_move_lin_success(goal_pose) goal_pose.position.y = -0.2 self.assert_move_lin_success(goal_pose) goal_pose.position.y = -0.6 goal_pose.position.z = .3 self.assert_move_lin_success(goal_pose)
def test_1_move_ptp(self): goal_pose = Pose() goal_pose.position = Point(.4, .4, .5) goal_pose.orientation = orientation_from_rpy(pi / 2, -pi / 4, pi) resp = self.client.move_ptp(goal_pose) self.assert_last_srv_call_success(resp)
def test_3_move_lin_sequence(self): # similar to Tower of Hanoi movements count = 2 # ensuring same global configuration for IK resp = self.client.move_joints(0, pi / 100, 0, -pi / 2, 0, pi / 2, 0) self.assert_last_srv_call_success(resp) goal_pose = Pose() goal_pose.position = Point(.3, -0.2, .2) goal_pose.orientation = orientation_from_rpy(0, pi, pi / 2) resp = self.client.move_lin(goal_pose) self.assert_last_srv_call_success(resp) for i in range(count): rospy.loginfo("Run %d/%d", i + 1, count) goal_pose.position.z = .005 self.assert_move_lin_success(goal_pose) goal_pose.position.z = .1 self.assert_move_lin_success(goal_pose) goal_pose.position.y = .3 self.assert_move_lin_success(goal_pose) goal_pose.position.z = .005 self.assert_move_lin_success(goal_pose) goal_pose.position.z = .3 self.assert_move_lin_success(goal_pose) goal_pose.position.y = -0.3 self.assert_move_lin_success(goal_pose)
def test_7_move_lin_ptp_armangle_config(self): self.client.move_joints(0, pi / 100, 0, -pi / 2, 0, pi / 2, 0) _, arm_angle, config = self.client.get_current_pose(detailed=True) self.assertEqual(config, 2, "wrong config determined") goal_pose = Pose() goal_pose.position = Point(.6, -0.17, .7215) goal_pose.orientation = Quaternion(.0, .7071068, .0, .7071068) goal_arm_angle = 0.0 resp = self.client.move_ptp_armangle(goal_pose, goal_arm_angle) self.assert_last_srv_call_success(resp) _, arm_angle, config = self.client.get_current_pose(detailed=True) self.assertEqual(config, 2, "wrong config for test case") self.assert_joint_values_close(arm_angle, goal_arm_angle) goal_pose.position = Point(.3, .0, .3) goal_pose.orientation = orientation_from_rpy(0, pi, 0) self.assert_move_ptp_success(goal_pose) goal_arm_angle = pi / 2 resp = self.client.move_lin_armangle(goal_pose, goal_arm_angle, True) self.assert_last_srv_call_success(resp) _, arm_angle, config = self.client.get_current_pose(detailed=True) self.assert_joint_values_close(arm_angle, goal_arm_angle) goal_arm_angle = -pi / 2 resp = self.client.move_lin_armangle(goal_pose, goal_arm_angle, False) self.assert_last_srv_call_success(resp) goal_arm_angle = pi / 2 resp = self.client.move_lin_armangle(goal_pose, goal_arm_angle, True) self.assert_last_srv_call_success(resp) goal_arm_angle = 0.0 resp = self.client.move_lin_armangle(goal_pose, goal_arm_angle, False) self.assert_last_srv_call_success(resp) goal_pose.position.y = .4 goal_arm_angle = pi / 3 resp = self.client.move_lin_armangle(goal_pose, goal_arm_angle, True) self.assert_last_srv_call_success(resp) goal_pose.position.x = .0 goal_pose.orientation = orientation_from_rpy(0, pi, pi / 2) goal_arm_angle = -pi / 2 resp = self.client.move_lin_armangle(goal_pose, goal_arm_angle, False) self.assert_last_srv_call_success(resp)
def execute(client): # type: (RLLDefaultMoveClient) -> bool # demonstrates how to use the available services: # # 1. moveJoints(a1, a2, ..., a7) vs move_joints(joint_values) # 2. move_ptp(pose) # 3. move_lin(pose) # 4. generated_pose = move_random() # 5. pose = get_current_pose() # 6. joint_values = get_current_joint_values() resp = client.move_joints(0.0, 0.0, 0.0, -pi / 2, 0.0, 0.0, 0.0) # returns True/False to indicate success, throws of critical failure if not resp: rospy.logerr("move_joints service call failed") joint_values = [pi / 2, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] client.move_joints(joint_values) goal_pose = Pose() goal_pose.position = Point(.4, .4, .5) goal_pose.orientation = orientation_from_rpy(pi / 2, -pi / 4, pi) # move ptp to the specified pose client.move_ptp(goal_pose) goal_pose.position.x = 0.2 goal_pose.position.y = .5 # linear movement to the new pose client.move_lin(goal_pose) # move to random pose, returns Pose/None to indicate success resp = client.move_random() if resp: # response contains the randomly generated pose rospy.loginfo("move_random moved to: %s", resp) # get current pose, should match the previous random pose pose = client.get_current_pose() rospy.loginfo("current end effector pose: %s", pose) # set the joint values again joint_values2 = [pi / 2, 0.2, 0, 0, -pi / 4, .24, 0] client.move_joints(joint_values2) # retrieve the previously set joint values joint_values = client.get_current_joint_values() match = compare_joint_values(joint_values, joint_values2) rospy.loginfo("Set and queried joint values match: %s", "yes" if match else "no") return True
def repeat_movement(self, count=10, reset=True, cause_failure=False): last_joint_values = None for i in range(count): rospy.loginfo("Run %d/%d", i + 1, count) if reset: # reset joints to known start position resp = self.client.move_joints(0, pi / 100, 0, -pi / 2, 0, -pi / 2, 0) self.assert_last_srv_call_success(resp) goal_pose = Pose() goal_pose.position = Point(.3, .41, .63) goal_pose.orientation = orientation_from_rpy(-pi / 2, 0, 0) # move into position resp = self.client.move_ptp(goal_pose) self.assert_last_srv_call_success(resp) if cause_failure: # only change the orientation a little, # no linear motion -> should fail goal_pose.orientation = orientation_from_rpy(-pi / 1.9, 0, 0) success = self.client.move_lin(goal_pose) self.assert_last_srv_call_failed( success, RLLErrorCode.TOO_FEW_WAYPOINTS) # change the position, orientatin stays -> should succeed goal_pose.position = Point(.2, .41, .63) goal_pose.orientation = orientation_from_rpy(-pi / 2, 0, 0) success = self.client.move_lin(goal_pose) self.assert_last_srv_call_success(success) joint_values = self.client.get_current_joint_values() if last_joint_values is not None: # check if moveit chose the same pose every time # this is not a test, just out of curiosity if not compare_joint_values(joint_values, last_joint_values): rospy.loginfo("Chose different joint values!") last_joint_values = joint_values
def test_4_move_lin_sequence_2(self): # similar to maze movements from the Path Planning project # ensuring same global configuration for IK resp = self.client.move_joints(0, pi / 100, 0, -pi / 2, 0, pi / 2, 0) self.assert_last_srv_call_success(resp) goal_pose = Pose() goal_pose.position = Point(-0.4, -0.5, .3) goal_pose.orientation = orientation_from_rpy(0, pi, 0) self.assert_move_ptp_success(goal_pose) goal_pose.position.z = .005 self.assert_move_lin_success(goal_pose) goal_pose.position.x = -0.3 goal_pose.position.y = -0.5 goal_pose.orientation = orientation_from_rpy(0, pi, -pi / 2) self.assert_move_lin_success(goal_pose) goal_pose.position.x = .4 goal_pose.orientation = orientation_from_rpy(0, pi, 0) self.assert_move_lin_success(goal_pose) goal_pose.position.x = .3 goal_pose.position.y = .6 goal_pose.orientation = orientation_from_rpy(0, pi, pi / 2) self.assert_move_lin_success(goal_pose) goal_pose.position.x = -0.4 goal_pose.orientation = orientation_from_rpy(0, pi, 0) self.assert_move_lin_success(goal_pose) goal_pose.position.x = .4 goal_pose.position.y = .3 goal_pose.orientation = orientation_from_rpy(0, pi, -pi / 2) self.assert_move_lin_success(goal_pose) goal_pose.position.x = .2 goal_pose.position.y = -0.4 goal_pose.orientation = orientation_from_rpy(0, pi, 0) self.assert_move_lin_success(goal_pose) goal_pose.position.z = .4 self.assert_move_lin_success(goal_pose)
def test_5_move_armangle(self): self.client.move_joints(0, pi / 100, 0, -pi / 2, 0, pi / 2, 0) goal_pose = Pose() goal_pose.position = Point(-0.4, -0.5, .3) goal_pose.orientation = orientation_from_rpy(0, pi, 0) goal_arm_angle = pi / 2 resp = self.client.move_ptp_armangle(goal_pose, goal_arm_angle) self.assert_last_srv_call_failed(resp, RLLErrorCode.MOVEIT_PLANNING_FAILED) self.assert_move_ptp_success(goal_pose) resp = self.client.move_lin_armangle(goal_pose, goal_arm_angle, True) self.assert_last_srv_call_failed( resp, RLLErrorCode.ONLY_PARTIAL_PATH_PLANNED) goal_arm_angle = 3 * pi / 2 resp = self.client.move_lin_armangle(goal_pose, goal_arm_angle, True) self.assert_last_srv_call_failed(resp, RLLErrorCode.INVALID_INPUT) resp = self.client.move_ptp_armangle(goal_pose, goal_arm_angle) self.assert_last_srv_call_failed(resp, RLLErrorCode.INVALID_INPUT)
def execute(client): # type: (RLLDefaultMoveClient) -> bool approach_pose1 = grip_pose_at(1, .2) approach_pose2 = grip_pose_at(2, .2) approach_pose3 = grip_pose_at(3, .2) grip_pose_cylinder1 = INITIAL_GRIP_POSE_CYLINDER grip_pose_cylinder3 = grip_pose_at(3, CYLINDER_HEIGHT / 2) grip_pose_box1 = INITIAL_GRIP_POSE_BOX grip_pose_box2 = grip_pose_at(2, BOX_HEIGHT / 2.0) # self.validate_pick_place(grip_pose1, True, "box1") # self.validate_pick_place(grip_pose1, False, "box1") # TODO(mark): this should fail! the cylinder is NOT at this position # TODO(mark): add more error codes for failed constraints # self.validate_pick_place(grip_pose1, True, "cylinder1") # self.validate_pick_place(grip_pose1, False, "cylinder1") rospy.loginfo("Moving into position to pick and place box1") # Note: move to position with joints to avoid possible PTP issues # resp = self.move_ptp(approach_pose3) # self.move_joints(joint_values_at_z15(1)) client.move_ptp( Pose(approach_pose3.position, orientation_from_rpy(0, pi, 0))) client.pick_place_here(grip_pose_box1, True, "box1") client.move_ptp(approach_pose2) client.pick_place_here(grip_pose_box2, False, "box1") client.move_ptp(approach_pose1) client.pick_place_here(grip_pose_cylinder1, True, "cylinder1") client.move_ptp(approach_pose2) client.pick_place(approach_pose3, grip_pose_cylinder3, approach_pose3, False, "cylinder1") return True
def test_4_move_lin_collision(self): # ensuring same global configuration for IK resp = self.client.move_joints(0, -pi / 100, 0, -pi / 2, 0, pi / 2, 0) self.assert_last_srv_call_success(resp) goal_pose = Pose() goal_pose.position = Point(-0.15, .4, .2) goal_pose.orientation = orientation_from_rpy(0, pi, 0) self.assert_move_ptp_success(goal_pose) goal_pose.position.z = .1 self.assert_move_lin_success(goal_pose) goal_pose.position.z = 0.0 resp = self.client.move_lin(goal_pose) self.assert_last_srv_call_failed(resp, RLLErrorCode.NO_IK_SOLUTION_FOUND) goal_pose.position.y = .55 goal_pose.position.z = .1 self.assert_move_lin_success(goal_pose) goal_pose.position.y = 0.7 resp = self.client.move_lin(goal_pose) self.assert_last_srv_call_failed(resp, RLLErrorCode.GOAL_IN_COLLISION)
def move_to_home_position(self, move_ptp=False): if not move_ptp: return self.move_joints(0, 0, 0, -pi / 2, 0, pi / 2, 0) return self.move_ptp(Pose(Point(0.20, 0.00, 0.39), orientation_from_rpy(3.14, 0.00, 3.14)))
import rlcompleter # noqa: F401, pylint: disable=unused-import import readline import termios from math import pi import readchar import rospy from geometry_msgs.msg import Pose, Point, sys from rll_move_client.client import RLLDefaultMoveClient from rll_move_client.formatting import override_formatting_for_ros_types from rll_move_client.util import direction_from_quaternion, rotate_quaternion from rll_move_client.util import orientation_from_rpy from rll_tools.run import run_project_in_background HISTORY = os.path.join(".rll_client_history") DOWN = orientation_from_rpy(0, pi, 0) try: readline.read_history_file(HISTORY) except Exception: # noqa: E722, pylint: disable=broad-except pass # default history len is -1 (infinite), which may grow unruly readline.set_history_length(2000) atexit.register(readline.write_history_file, HISTORY) readline.parse_and_bind('tab: complete') # readline.set_completer_delims(' \t\n;') CAN_READ_CHAR = False try:
_ALL_DROP_VALUES = [_DROP_LOC1_VALUES, _DROP_LOC2_VALUES, _DROP_LOC3_VALUES] DROP_AREA_1_RADIUS = .025 POS_BOX = Point(_DROP_LOC3[0], _DROP_LOC3[1], BOX_HEIGHT / 2) POS_CYLINDER = Point(_DROP_LOC1[0], _DROP_LOC1[1], CYLINDER_HEIGHT / 2) def drop_pos(location, z_pos): assert 0 < location < 4 if location == 1: return Point(_DROP_LOC1[0], _DROP_LOC1[1], z_pos) elif location == 2: return Point(_DROP_LOC2[0], _DROP_LOC2[1], z_pos) return Point(_DROP_LOC3[0], _DROP_LOC3[1], z_pos) def joint_values_at_z15(location): assert 0 < location < 4 return _ALL_DROP_VALUES[location - 1] def grip_pose_at(location, z_pos=.05): return Pose(drop_pos(location, z_pos), DOWN) INITIAL_GRIP_POSE_CYLINDER = grip_pose_at(1, CYLINDER_HEIGHT / 2) INITIAL_GRIP_POSE_BOX = Pose(POS_BOX, orientation_from_rpy(0, math.pi, math.pi * 0.25))
def hello_world(move_client): print("Hello world") # avoid using print() for logging rospy.loginfo("Hello ROS") # better use rospy.loginfo(), logerror()... # move to a random pose, this service call requires no arguments move_client.move_random() # The robot should now be moving (in RViz)! The delays in this code # are only for illustrative purposes and can be removed time.sleep(2) # move all seven joints into their zero position by calling the move_joints # service and passing the joint values as arguments rospy.loginfo("calling move_joints with all joint values = 0") move_client.move_joints(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0) time.sleep(2) # rotate the fourth joint by 90 degrees (pi/2 since we work with radians) rospy.loginfo("calling move_joints with joint_4 = pi/2") resp = move_client.move_joints(0.0, 0.0, 0.0, pi / 2, 0.0, 0.0, 0.0) # previously we neglected to check the response of the service call. # You should always check the result of a service call. If a critical # failure occurs during the service call an exception will be raised. # If you do not check the result of service call you might miss a non # critical failure, because i.e. you passed an invalid pose if not resp: rospy.logwarn("move_joints service call failed (as expected)") # ups, moving the fourth joint by 90 degrees didn't work, we bumped into # the workspace boundaries # Let's try to move joint 2, 4 and 6 so that we end up in an upright # position of the end effector close to the front of the workspace. rospy.loginfo("calling move_joints to move into an upright position " "close to the front of the workspace") resp = move_client.move_joints(0.0, pi / 4, 0.0, -pi / 4, 0.0, -pi / 2, 0.0) if not resp: rospy.logerr("move_joints service call failed (unexpectedly)!") time.sleep(2) # moving by specifying joint angle values is not the most intuitive way # it's easier to specify the pose of the end effector we'd like to reach goal_pose = Pose() goal_pose.position = Point(.5, .2, .7) goal_pose.orientation.z = 1 # rotate 180 degrees around z (see below) # the move_ptp service call requires a Pose argument resp = move_client.move_ptp(goal_pose) # not all poses can be reached, remember to check the result if not resp: rospy.logerr("move_ptp service call failed") time.sleep(2) # The orientation of a pose is stored as a quaternion and usually you # don't specify them manually. It's easier to e.g. use euler or RPY angles # HINT: display the coordinate systems in RViz to visualize orientations. # In RViz the XYZ axes are color coded in RGB: X=red, Y=green, Z=blue # the end effector is pointing along the blue z-axis # rotate the end effector 90 degrees around the (stationary) y axis goal_pose.orientation = orientation_from_rpy(0, pi / 2, 0) rospy.loginfo("move_ptp to same position but different orientation") move_client.move_ptp(goal_pose) # (error check omitted) time.sleep(1) # rotate 90deg around the y-axis and 45deg around the new (relative) x-axis goal_pose.orientation = orientation_from_rpy(pi / 4, pi / 2, 0) rospy.loginfo("move_ptp to same position but different orientation") move_client.move_ptp(goal_pose) # (error check omitted) time.sleep(2) # Next up: move the end effector on a triangular path # while maintaining the same orientation rospy.loginfo("Next: move the end effector on a triangular path") # orient the z-axis "forward" (along the base x-axis) goal_pose.orientation = orientation_from_rpy(0, pi / 2, 0) # move to the starting position still in a ptp fashion goal_pose.position = Point(0.5, -0.6, 0.3) rospy.loginfo("move_ptp to the starting point of the triangle") move_client.move_ptp(goal_pose) # (error check omitted) time.sleep(1) # move up, its a right angled triangle goal_pose.position.z = .7 # this time we move on a linear trajectory to the specified pose rospy.loginfo("move_lin to the tip of the triangle") move_client.move_lin(goal_pose) # (error check omitted) time.sleep(1) # next point is the upper right point of the triangle goal_pose.position.y = -0.15 rospy.loginfo("move_lin to the upper right point of the triangle") move_client.move_lin(goal_pose) # (error check omitted) # close the triangle by moving back diagonally to the start position goal_pose.position.y = -0.6 goal_pose.position.z = .3 rospy.loginfo("move_lin to the start to close the triangle shape") move_client.move_lin(goal_pose) # (error check omitted) time.sleep(1) # Note: move_lin is not always successful, even if move_ptp succeeds. # This is because moving on a linear trajectory is more constraining # Example: move to a positive y-position will fail with move_lin goal_pose.position.y = 0.3 rospy.loginfo("try to move_lin to: ") resp = move_client.move_lin(goal_pose) if not resp: rospy.logerr("move_lin service call failed (as expected)") # calling move_ptp with the exact same goal pose succeeds rospy.loginfo("try to move_ptp: ") resp = move_client.move_ptp(goal_pose) if not resp: rospy.logerr("move_ptp service call failed (unexpectedly)!") time.sleep(2) # the response sometimes holds more information than only a boolean status # move_random() returns the random pose the end effector moved to rospy.loginfo("move_random to a new random position") resp = move_client.move_random() # (error check omitted) # we can obtain the chosen random pose from the response rospy.loginfo("move_random moved to: %s", resp) return True
def test_0_orientation(self): ori = orientation_from_rpy(0, 0, 0) self.assertTrue(np.allclose(quaternion_to_array(ori), [0, 0, 0, 1]))