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)