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 )