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 assert_joint_values_close(self, joints1, joints2):
self.assertTrue(
compare_joint_values(joints1, joints2),
"Mismatching joint values: %s vs %s" % (joints1, joints2),
)