def pose_constraint_to_position_orientation_constraints(pose_constraint):
position_constraint = PositionConstraint()
orientation_constraint = OrientationConstraint()
position_constraint.header = pose_constraint.header
position_constraint.link_name = pose_constraint.link_name
position_constraint.position = pose_constraint.pose.position
position_constraint.constraint_region_shape.type = geometric_shapes_msgs.msg.Shape.BOX
position_constraint.constraint_region_shape.dimensions.append(2*pose_constraint.absolute_position_tolerance.x)
position_constraint.constraint_region_shape.dimensions.append(2*pose_constraint.absolute_position_tolerance.y)
position_constraint.constraint_region_shape.dimensions.append(2*pose_constraint.absolute_position_tolerance.z)
position_constraint.constraint_region_orientation.x = 0.0
position_constraint.constraint_region_orientation.y = 0.0
position_constraint.constraint_region_orientation.z = 0.0
position_constraint.constraint_region_orientation.w = 1.0
position_constraint.weight = 1.0
orientation_constraint.header = pose_constraint.header
orientation_constraint.link_name = pose_constraint.link_name
orientation_constraint.orientation = pose_constraint.pose.orientation
orientation_constraint.type = pose_constraint.orientation_constraint_type
orientation_constraint.absolute_roll_tolerance = pose_constraint.absolute_roll_tolerance
orientation_constraint.absolute_pitch_tolerance = pose_constraint.absolute_pitch_tolerance
orientation_constraint.absolute_yaw_tolerance = pose_constraint.absolute_yaw_tolerance
orientation_constraint.weight = 1.0
return position_constraint, orientation_constraint
def pose_constraint_to_position_orientation_constraints(pose_constraint):
position_constraint = PositionConstraint()
orientation_constraint = OrientationConstraint()
position_constraint.header = pose_constraint.header
position_constraint.link_name = pose_constraint.link_name
position_constraint.position = pose_constraint.pose.position
position_constraint.constraint_region_shape.type = geometric_shapes_msgs.msg.Shape.BOX
position_constraint.constraint_region_shape.dimensions.append(
2 * pose_constraint.absolute_position_tolerance.x)
position_constraint.constraint_region_shape.dimensions.append(
2 * pose_constraint.absolute_position_tolerance.y)
position_constraint.constraint_region_shape.dimensions.append(
2 * pose_constraint.absolute_position_tolerance.z)
position_constraint.constraint_region_orientation.x = 0.0
position_constraint.constraint_region_orientation.y = 0.0
position_constraint.constraint_region_orientation.z = 0.0
position_constraint.constraint_region_orientation.w = 1.0
position_constraint.weight = 1.0
orientation_constraint.header = pose_constraint.header
orientation_constraint.link_name = pose_constraint.link_name
orientation_constraint.orientation = pose_constraint.pose.orientation
orientation_constraint.type = pose_constraint.orientation_constraint_type
orientation_constraint.absolute_roll_tolerance = pose_constraint.absolute_roll_tolerance
orientation_constraint.absolute_pitch_tolerance = pose_constraint.absolute_pitch_tolerance
orientation_constraint.absolute_yaw_tolerance = pose_constraint.absolute_yaw_tolerance
orientation_constraint.weight = 1.0
return position_constraint, orientation_constraint
