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]))
