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_rotation_random_angles(self):
"""
Tests that the 'modify_pose_rotation' function returns the same pose,
but with a rotation offset and a limited rotation for a series of random
angles.
"""
# test angles: different rotations around the Z axis
# 35 degrees
angle_1 = geometry_msgs.msg.PoseStamped()
angle_1.pose.orientation.x = 0.0
angle_1.pose.orientation.y = 0.0
angle_1.pose.orientation.z = 0.30071
angle_1.pose.orientation.w = 0.95372
# 120 degrees
angle_2 = geometry_msgs.msg.PoseStamped()
angle_2.pose.orientation.x = 0.0
angle_2.pose.orientation.y = 0.0
angle_2.pose.orientation.z = math.cos(math.pi / 6)
angle_2.pose.orientation.w = 0.5
# 250 degrees
angle_3 = geometry_msgs.msg.PoseStamped()
angle_3.pose.orientation.x = 0.0
angle_3.pose.orientation.y = 0.0
angle_3.pose.orientation.z = 0.81915
angle_3.pose.orientation.w = -0.57358
# 320 degrees
angle_4 = geometry_msgs.msg.PoseStamped()
angle_4.pose.orientation.x = 0.0
angle_4.pose.orientation.y = 0.0
angle_4.pose.orientation.z = 0.34202
angle_4.pose.orientation.w = -0.93969
actual_first = pregrasp_planner_utils.modify_pose_rotation(
angle_1, offset=170, rotation_range=[0, 180]
)
actual_second = pregrasp_planner_utils.modify_pose_rotation(
angle_2, offset=170, rotation_range=[0, 180]
)
actual_third = pregrasp_planner_utils.modify_pose_rotation(
angle_3, offset=170, rotation_range=[0, 180]
)
actual_fourth = pregrasp_planner_utils.modify_pose_rotation(
angle_4, offset=170, rotation_range=[0, 180]
)
# for these test angles: [35.0, 120.0, 250.0, 320.0],
# the desired angles should be: [205.0, 290.0, 240.0, 310.0]
# 205 degrees
desired_1 = geometry_msgs.msg.PoseStamped()
desired_1.pose.orientation.x = 0.0
desired_1.pose.orientation.y = 0.0
desired_1.pose.orientation.z = 0.9763
desired_1.pose.orientation.w = -0.21644
# 290 degrees
desired_2 = geometry_msgs.msg.PoseStamped()
desired_2.pose.orientation.x = 0.0
desired_2.pose.orientation.y = 0.0
desired_2.pose.orientation.z = 0.57358
desired_2.pose.orientation.w = -0.81915
# 240 degrees
desired_3 = geometry_msgs.msg.PoseStamped()
desired_3.pose.orientation.x = 0.0
desired_3.pose.orientation.y = 0.0
desired_3.pose.orientation.z = math.cos(math.pi / 6.0)
desired_3.pose.orientation.w = -0.5
# 310 degrees
desired_4 = geometry_msgs.msg.PoseStamped()
desired_4.pose.orientation.x = 0.0
desired_4.pose.orientation.y = 0.0
desired_4.pose.orientation.z = 0.42262
desired_4.pose.orientation.w = -0.90631
self.assertAlmostEqual(
actual_first.pose.orientation.x, desired_1.pose.orientation.x,
places=5
)
self.assertAlmostEqual(
actual_first.pose.orientation.y, desired_1.pose.orientation.y,
places=5
)
self.assertAlmostEqual(
actual_first.pose.orientation.z, desired_1.pose.orientation.z,
places=5
)
self.assertAlmostEqual(
actual_first.pose.orientation.w, desired_1.pose.orientation.w,
places=5
)
self.assertAlmostEqual(
actual_second.pose.orientation.x, desired_2.pose.orientation.x,
places=5
)
self.assertAlmostEqual(
actual_second.pose.orientation.y, desired_2.pose.orientation.y,
places=5
)
self.assertAlmostEqual(
actual_second.pose.orientation.z, desired_2.pose.orientation.z,
places=5
)
self.assertAlmostEqual(
actual_second.pose.orientation.w, desired_2.pose.orientation.w,
places=5
)
self.assertAlmostEqual(
actual_third.pose.orientation.x, desired_3.pose.orientation.x,
places=5
)
self.assertAlmostEqual(
actual_third.pose.orientation.y, desired_3.pose.orientation.y,
places=5
)
self.assertAlmostEqual(
actual_third.pose.orientation.z, desired_3.pose.orientation.z,
places=5
)
self.assertAlmostEqual(
actual_third.pose.orientation.w, desired_3.pose.orientation.w,
places=5
)
self.assertAlmostEqual(
actual_fourth.pose.orientation.x, desired_4.pose.orientation.x,
places=5
)
self.assertAlmostEqual(
actual_fourth.pose.orientation.y, desired_4.pose.orientation.y,
places=5
)
self.assertAlmostEqual(
actual_fourth.pose.orientation.z, desired_4.pose.orientation.z,
places=5
)
self.assertAlmostEqual(
actual_fourth.pose.orientation.w, desired_4.pose.orientation.w,
places=5
)
def test_modify_pose_rotation_complete(self):
"""
Tests that the 'modify_pose_rotation' function returns the same pose,
but with a rotation offset and a limited rotation.
"""
# test angles: different rotations around the Z axis
zero_degrees = geometry_msgs.msg.PoseStamped()
zero_degrees.pose.orientation.x = 0.0
zero_degrees.pose.orientation.y = 0.0
zero_degrees.pose.orientation.z = 0.0
zero_degrees.pose.orientation.w = 1.0
ninety_degrees = geometry_msgs.msg.PoseStamped()
ninety_degrees.pose.orientation.x = 0.0
ninety_degrees.pose.orientation.y = 0.0
ninety_degrees.pose.orientation.z = math.cos(math.pi / 4)
ninety_degrees.pose.orientation.w = math.cos(math.pi / 4)
one_eighty_degrees = geometry_msgs.msg.PoseStamped()
one_eighty_degrees.pose.orientation.x = 0.0
one_eighty_degrees.pose.orientation.y = 0.0
one_eighty_degrees.pose.orientation.z = 1.0
one_eighty_degrees.pose.orientation.w = 0.0
two_seventy_degrees = geometry_msgs.msg.PoseStamped()
two_seventy_degrees.pose.orientation.x = 0.0
two_seventy_degrees.pose.orientation.y = 0.0
two_seventy_degrees.pose.orientation.z = math.cos(math.pi / 4)
two_seventy_degrees.pose.orientation.w = -math.cos(math.pi / 4)
actual_zero = pregrasp_planner_utils.modify_pose_rotation(
zero_degrees, offset=90, rotation_range=[0, 180]
)
actual_ninety = pregrasp_planner_utils.modify_pose_rotation(
ninety_degrees, offset=90, rotation_range=[0, 180]
)
actual_one_eighty = pregrasp_planner_utils.modify_pose_rotation(
one_eighty_degrees, offset=90, rotation_range=[0, 180]
)
actual_two_seventy = pregrasp_planner_utils.modify_pose_rotation(
two_seventy_degrees, offset=90, rotation_range=[0, 180]
)
self.assertAlmostEqual(
actual_zero.pose.orientation.x, ninety_degrees.pose.orientation.x,
places=5
)
self.assertAlmostEqual(
actual_zero.pose.orientation.y, ninety_degrees.pose.orientation.y,
places=5
)
self.assertAlmostEqual(
actual_zero.pose.orientation.z, ninety_degrees.pose.orientation.z,
places=5
)
self.assertAlmostEqual(
actual_zero.pose.orientation.w, ninety_degrees.pose.orientation.w,
places=5
)
self.assertAlmostEqual(
actual_ninety.pose.orientation.x, one_eighty_degrees.pose.orientation.x,
places=5
)
self.assertAlmostEqual(
actual_ninety.pose.orientation.y, one_eighty_degrees.pose.orientation.y,
places=5
)
self.assertAlmostEqual(
actual_ninety.pose.orientation.z, one_eighty_degrees.pose.orientation.z,
places=5
)
self.assertAlmostEqual(
actual_ninety.pose.orientation.w, one_eighty_degrees.pose.orientation.w,
places=5
)
self.assertAlmostEqual(
actual_one_eighty.pose.orientation.x, two_seventy_degrees.pose.orientation.x,
places=5
)
self.assertAlmostEqual(
actual_one_eighty.pose.orientation.y, two_seventy_degrees.pose.orientation.y,
places=5
)
self.assertAlmostEqual(
actual_one_eighty.pose.orientation.z, two_seventy_degrees.pose.orientation.z,
places=5
)
self.assertAlmostEqual(
actual_one_eighty.pose.orientation.w, two_seventy_degrees.pose.orientation.w,
places=5
)
self.assertAlmostEqual(
actual_two_seventy.pose.orientation.x, one_eighty_degrees.pose.orientation.x,
places=5
)
self.assertAlmostEqual(
actual_two_seventy.pose.orientation.y, one_eighty_degrees.pose.orientation.y,
places=5
)
self.assertAlmostEqual(
actual_two_seventy.pose.orientation.z, one_eighty_degrees.pose.orientation.z,
places=5
)
self.assertAlmostEqual(
actual_two_seventy.pose.orientation.w, one_eighty_degrees.pose.orientation.w,
places=5
)
