def test_modify_pose_no_changes(self):
"""
Tests that the 'modify_pose' function returns the same pose, since
the object is not standing.
"""
height_threshold = 1.0
pose_in = geometry_msgs.msg.PoseStamped()
# the height (position in Z) will be compared against the height threshold
pose_in.pose.position.x = 0.4
pose_in.pose.position.y = 0.0
pose_in.pose.position.z = 0.2
# laying orientation
pose_in.pose.orientation.x = 0.0
pose_in.pose.orientation.y = 0.0
pose_in.pose.orientation.z = 0.0
pose_in.pose.orientation.w = 1.0
expected_pose = copy.deepcopy(pose_in)
result, standing = pregrasp_planner_utils.modify_pose(pose_in, height_threshold)
self.assertEqual(result, expected_pose)
self.assertEqual(standing, False)
def publish_component_outputs(self):
"""
Publishes the component's outputs.
"""
modified_pose, object_is_upwards = pregrasp_planner_utils.modify_pose(
self.pose_in,
self.height_tolerance,
angular_tolerance=self.angular_tolerance)
sampling_parameters = mcr_manipulation_msgs.msg.SphericalSamplerParameters(
)
sampling_parameters.radial_distance.minimum = self.min_distance_to_object
sampling_parameters.radial_distance.maximum = self.max_distance_to_object
sampling_parameters.azimuth.minimum = math.radians(self.min_azimuth)
sampling_parameters.azimuth.maximum = math.radians(self.max_azimuth)
sampling_parameters.zenith.minimum = math.radians(self.min_zenith)
sampling_parameters.zenith.maximum = math.radians(self.max_zenith)
sampling_parameters.yaw.minimum = math.radians(self.min_roll)
sampling_parameters.yaw.maximum = math.radians(self.max_roll)
if object_is_upwards:
print "publish inmediatlyyyyyyyyy"
self.pose_out.publish(modified_pose)
self.sampling_parameters.publish(sampling_parameters)
self.grasp_type.publish('side_grasp')
self.event_out.publish('e_success')
else:
# the object is laying down, thus the rotation will be restricted
# to only 180 degrees (e.g. top grasp)
rotated_pose = pregrasp_planner_utils.modify_pose_rotation(
modified_pose,
offset=self.rotation_offset,
reference_axis=self.reference_axis,
rotation_range=self.rotation_range)
if rotated_pose:
self.pose_out.publish(rotated_pose)
self.sampling_parameters.publish(sampling_parameters)
self.grasp_type.publish('top_grasp')
self.event_out.publish('e_success')
else:
self.event_out.publish('e_failure')
def running_state(self):
"""
Executes the RUNNING state of the state machine.
:return: The updated state.
:rtype: str
"""
if self.event == "e_stop":
status = "e_stopped"
self.event_out.publish(status)
self.reset_component_data()
return "INIT"
# 1. transform pose to target frame
# 2. check if pose is for laying or standing object and modify accordingly
# 3. generate pose samples around pose
# 4. find a reachable pose from the generated samples (also get joint config for reachable pose)
# 5. if joint waypoint list is required publish list of waypoints
transformed_pose = self.pose_transformer.get_transformed_pose(
self.pose_in, self.target_frame)
if not transformed_pose:
rospy.logerr("Unable to transform pose to {0}".format(
self.target_frame))
status = "e_failure"
self.event_out.publish(status)
self.reset_component_data()
return "INIT"
if self.ignore_orientation:
input_pose = geometry_msgs.msg.PoseStamped()
input_pose.header = transformed_pose.header
input_pose.pose.position = transformed_pose.pose.position
solution = self.orientation_independent_ik.get_reachable_pose_and_joint_msg_from_point(
input_pose.pose.position.x, input_pose.pose.position.y,
input_pose.pose.position.z, input_pose.header.frame_id)
if solution is None:
rospy.logerr("Could not find IK solution")
status = 'e_failure'
self.event_out.publish(status)
self.reset_component_data()
return 'INIT'
reachable_pose, joint_msg = solution
rospy.loginfo('Found solution')
self.selected_pose.publish(reachable_pose)
self.joint_configuration.publish(joint_msg)
self.event_out.publish("e_success")
self.reset_component_data()
return 'INIT'
modified_pose, object_is_upwards = pregrasp_planner_utils.modify_pose(
transformed_pose,
self.height_tolerance,
angular_tolerance=self.angular_tolerance,
)
if not object_is_upwards:
rotated_pose = pregrasp_planner_utils.modify_pose_rotation(
modified_pose,
offset=self.rotation_offset,
reference_axis=self.reference_axis,
rotation_range=self.rotation_range,
)
modified_pose = rotated_pose
grasp_type = "top_grasp"
else:
grasp_type = "side_grasp"
self.grasp_type.publish(grasp_type)
pose_samples = self.pose_generator.calculate_poses_list(modified_pose)
self.pose_samples_pub.publish(pose_samples)
(
reachable_pose,
brics_joint_config,
joint_config,
) = self.reachability_pose_selector.get_reachable_pose_and_configuration(
pose_samples, self.linear_offset)
if not reachable_pose:
rospy.logerr("Could not find IK solution")
status = "e_failure"
self.event_out.publish(status)
self.reset_component_data()
return "INIT"
self.selected_pose.publish(reachable_pose)
self.joint_configuration.publish(brics_joint_config)
# if we want to reach a pre-pregrasp pose (specified by joint offsets)
# before reaching the final pose, generate a waypoint list
if abs(max(self.joint_offset,
key=abs)) > 0 and self.generate_pregrasp_waypoint:
if grasp_type == "side_grasp":
pregrasp_waypoint = self.joint_config_shifter.shift_joint_configuration(
joint_config, self.joint_offset_side_grasp)
elif grasp_type == "top_grasp":
pregrasp_waypoint = self.joint_config_shifter.shift_joint_configuration(
joint_config, self.joint_offset)
pregrasp_msg = std_msgs.msg.Float64MultiArray()
pregrasp_msg.data = pregrasp_waypoint
grasp_msg = std_msgs.msg.Float64MultiArray()
grasp_msg.data = joint_config
joint_waypoints = mcr_manipulation_msgs.msg.JointSpaceWayPointsList(
)
joint_waypoints.list_of_joint_values_lists.append(pregrasp_msg)
joint_waypoints.list_of_joint_values_lists.append(grasp_msg)
self.joint_waypoint_list_pub.publish(joint_waypoints)
self.event_out.publish("e_success")
self.reset_component_data()
return "INIT"
def test_modify_pose_short_object(self):
"""
Tests that the 'modify_pose' function returns a modified version of the pose,
since the object's height does not exceed the height_threshold. Namely,
the modified version should have a 90 degree rotation around the Y axis
with respect to the pose.
"""
height_threshold = 0.15
pose_in = geometry_msgs.msg.PoseStamped()
# the height (position in Z) will be compared against the height threshold
pose_in.pose.position.x = 0.2
pose_in.pose.position.y = 0.0
pose_in.pose.position.z = 0.125
# standing orientation
pose_in.pose.orientation.x = 0.0
pose_in.pose.orientation.y = -math.sqrt(2) / 2
pose_in.pose.orientation.z = 0.0
pose_in.pose.orientation.w = math.sqrt(2) / 2
result, standing = pregrasp_planner_utils.modify_pose(pose_in, height_threshold)
expected_pose = copy.deepcopy(pose_in)
expected_pose.pose.orientation.x = 0.0
expected_pose.pose.orientation.y = -1.0
expected_pose.pose.orientation.z = 0.0
expected_pose.pose.orientation.w = 0.0
self.assertEqual(standing, False)
self.assertAlmostEqual(
result.pose.orientation.x, expected_pose.pose.orientation.x, places=4
)
self.assertAlmostEqual(
result.pose.orientation.y, expected_pose.pose.orientation.y, places=4
)
self.assertAlmostEqual(
result.pose.orientation.z, expected_pose.pose.orientation.z, places=4
)
self.assertAlmostEqual(
result.pose.orientation.w, expected_pose.pose.orientation.w, places=4
)
# standing orientation + 45 degree rotation around the X axis
pose_in.pose.orientation.x = 0.2706
pose_in.pose.orientation.y = -0.65328
pose_in.pose.orientation.z = 0.2706
pose_in.pose.orientation.w = 0.65328
result, standing = pregrasp_planner_utils.modify_pose(pose_in, height_threshold)
# laying horizontally + 45 degree rotation around the X axis
expected_pose.pose.orientation.x = 0.0
expected_pose.pose.orientation.y = -0.9238795
expected_pose.pose.orientation.z = 0.3826834
expected_pose.pose.orientation.w = 0.0
self.assertEqual(standing, False)
self.assertAlmostEqual(
result.pose.orientation.x, expected_pose.pose.orientation.x, places=4
)
self.assertAlmostEqual(
result.pose.orientation.y, expected_pose.pose.orientation.y, places=4
)
self.assertAlmostEqual(
result.pose.orientation.z, expected_pose.pose.orientation.z, places=4
)
self.assertAlmostEqual(
result.pose.orientation.w, expected_pose.pose.orientation.w, places=4
)
# standing orientation + 90 degree rotation around the X axis
pose_in.pose.orientation.x = 0.5
pose_in.pose.orientation.y = -0.5
pose_in.pose.orientation.z = 0.5
pose_in.pose.orientation.w = 0.5
result, standing = pregrasp_planner_utils.modify_pose(pose_in, height_threshold)
# laying horizontally + 90 degree rotation around the X axis
expected_pose.pose.orientation.x = 0.0
expected_pose.pose.orientation.y = -math.sqrt(2) / 2
expected_pose.pose.orientation.z = math.sqrt(2) / 2
expected_pose.pose.orientation.w = 0.0
self.assertEqual(standing, False)
self.assertAlmostEqual(
result.pose.orientation.x, expected_pose.pose.orientation.x, places=4
)
self.assertAlmostEqual(
result.pose.orientation.y, expected_pose.pose.orientation.y, places=4
)
self.assertAlmostEqual(
result.pose.orientation.z, expected_pose.pose.orientation.z, places=4
)
self.assertAlmostEqual(
result.pose.orientation.w, expected_pose.pose.orientation.w, places=4
)
# standing orientation + 180 degree rotation around the X axis
pose_in.pose.orientation.x = math.sqrt(2) / 2
pose_in.pose.orientation.y = 0.0
pose_in.pose.orientation.z = math.sqrt(2) / 2
pose_in.pose.orientation.w = 0.0
result, standing = pregrasp_planner_utils.modify_pose(pose_in, height_threshold)
# laying horizontally + 180 degree rotation around the X axis
expected_pose.pose.orientation.x = 0.0
expected_pose.pose.orientation.y = 0.0
expected_pose.pose.orientation.z = -1.0
expected_pose.pose.orientation.w = 0.0
self.assertEqual(standing, False)
self.assertAlmostEqual(
result.pose.orientation.x, expected_pose.pose.orientation.x, places=4
)
self.assertAlmostEqual(
result.pose.orientation.y, expected_pose.pose.orientation.y, places=4
)
self.assertAlmostEqual(
result.pose.orientation.z, expected_pose.pose.orientation.z, places=4
)
self.assertAlmostEqual(
result.pose.orientation.w, expected_pose.pose.orientation.w, places=4
)
def test_modify_pose_tall_object(self):
"""
Tests that the 'modify_pose' function returns a modified version of the pose,
given an object that is standing and its height not exceed the
height_threshold. Namely, the modified version should retain the orientation
(i.e. standing), but with no rotation around the axis pointing upwards
(X axis is assumed in this case).
"""
height_threshold = 0.15
pose_in = geometry_msgs.msg.PoseStamped()
# the height (position in Z) will be compared against the height threshold
pose_in.pose.position.x = 0.2
pose_in.pose.position.y = 0.0
pose_in.pose.position.z = 0.16
# standing orientation
pose_in.pose.orientation.x = 0.0
pose_in.pose.orientation.y = math.sqrt(2) / 2
pose_in.pose.orientation.z = 0.0
pose_in.pose.orientation.w = -math.sqrt(2) / 2
result, standing = pregrasp_planner_utils.modify_pose(pose_in, height_threshold)
expected_pose = copy.deepcopy(pose_in)
# standing orientation + 0 degree rotation around the X axis
expected_pose.pose.orientation.x = 0.0
expected_pose.pose.orientation.y = math.sqrt(2) / 2
expected_pose.pose.orientation.z = 0.0
expected_pose.pose.orientation.w = -math.sqrt(2) / 2
self.assertEqual(result, expected_pose)
self.assertEqual(standing, True)
# standing orientation + 45 degree rotation around the X axis
pose_in.pose.orientation.x = -0.2706
pose_in.pose.orientation.y = 0.65328
pose_in.pose.orientation.z = -0.2706
pose_in.pose.orientation.w = -0.65328
result, standing = pregrasp_planner_utils.modify_pose(pose_in, height_threshold)
# expected_pose: standing orientation + 0 degree rotation around the X axis
self.assertEqual(result, expected_pose)
self.assertEqual(standing, True)
# standing orientation + 90 degree rotation around the X axis
pose_in.pose.orientation.x = -0.5
pose_in.pose.orientation.y = 0.5
pose_in.pose.orientation.z = -0.5
pose_in.pose.orientation.w = -0.5
result, standing = pregrasp_planner_utils.modify_pose(pose_in, height_threshold)
# expected_pose: standing orientation + 0 degree rotation around the X axis
self.assertEqual(result, expected_pose)
self.assertEqual(standing, True)
# standing orientation + 180 degree rotation around the X axis
pose_in.pose.orientation.x = -math.sqrt(2) / 2
pose_in.pose.orientation.y = 0.0
pose_in.pose.orientation.z = -math.sqrt(2) / 2
pose_in.pose.orientation.w = -0.0
result, standing = pregrasp_planner_utils.modify_pose(pose_in, height_threshold)
# expected_pose: standing orientation + 0 degree rotation around the X axis
self.assertEqual(result, expected_pose)
self.assertEqual(standing, True)
