(* Genenerated from: ../../irs/wg12n587.exp *)

SCHEMA kinematic_structure_schema;

REFERENCE FROM geometry_schema
    (axis2_placement_3d,
     cartesian_transformation_operator_3d,
     curve,
     direction,
     geometric_representation_context,
     geometric_representation_item,
     normalise,
     point,
     point_on_curve,
     point_on_surface,
     surface,
     rectangular_trimmed_surface,
     trimmed_curve);

REFERENCE FROM measure_schema
    (conversion_based_unit,
     global_unit_assigned_context,
     length_measure,
     plane_angle_measure,
     si_prefix,
     si_unit,
     si_unit_name,
     unit);

REFERENCE FROM product_property_definition_schema
    (characterized_definition,
     property_definition);

REFERENCE FROM product_property_representation_schema
    (property_definition_representation);

REFERENCE FROM representation_schema
    (functionally_defined_transformation,
     item_defined_transformation,
     representation,
     representation_context,
     representation_item,
     representation_relationship,
     representation_relationship_with_transformation);

REFERENCE FROM support_resource_schema
    (bag_to_set,
     label);

TYPE rigid_placement = SELECT
  (axis2_placement_3d,
   su_parameters);
END_TYPE;

TYPE rotational_range_measure = SELECT
  (plane_angle_measure,
   unlimited_range);
END_TYPE;

TYPE translational_range_measure = SELECT
  (length_measure,
   unlimited_range);
END_TYPE;

TYPE unlimited_range = ENUMERATION OF
  (unlimited);
END_TYPE;

TYPE spatial_rotation = SELECT
  (ypr_rotation,
   rotation_about_direction);
END_TYPE;

TYPE ypr_enumeration = ENUMERATION OF
  (yaw,
   pitch,
   roll);
END_TYPE;

TYPE ypr_rotation = ARRAY [ypr_index(yaw) : ypr_index(roll)]
                    OF plane_angle_measure;
END_TYPE;

TYPE kinematic_frame_background = SELECT
  (point,
   curve,
   surface);
END_TYPE;

ENTITY rotation_about_direction;
  direction_of_axis : direction;
  rotation_angle    : plane_angle_measure;
WHERE
  WR1: SIZEOF (direction_of_axis.direction_ratios) = 3;
END_ENTITY;

ENTITY kinematic_property_definition
  SUBTYPE OF (property_definition);
  ground_definition : characterized_definition;
END_ENTITY;

ENTITY kinematic_property_representation_relation
  SUBTYPE OF (property_definition_representation);
UNIQUE
  UR1: SELF\property_definition_representation.definition;
WHERE
  WR1: 'KINEMATIC_STRUCTURE_SCHEMA.KINEMATIC_PROPERTY_DEFINITION'
         IN TYPEOF (SELF\property_definition_representation.definition);
  WR2: 'KINEMATIC_STRUCTURE_SCHEMA.KINEMATIC_GROUND_REPRESENTATION'
         IN TYPEOF (SELF\property_definition_representation.used_representation);
END_ENTITY;

ENTITY kinematic_ground_representation
  SUBTYPE OF (representation);
INVERSE
  property : kinematic_property_representation_relation FOR used_representation;
WHERE
  WR1: 'GEOMETRY_SCHEMA.GEOMETRIC_REPRESENTATION_CONTEXT' IN
       TYPEOF (SELF\representation.context_of_items);
END_ENTITY;

ENTITY mechanism;
  structure_definition : kinematic_structure;
  base                 : kinematic_link;
  containing_property  : kinematic_property_definition;
WHERE
  WR1: SIZEOF (QUERY (joint <* structure_definition.joints |
               (base :=: joint.first_link) OR
               (base :=: joint.second_link) )) > 0;
END_ENTITY;

ENTITY mechanism_base_placement
  SUBTYPE OF (representation_relationship_with_transformation);
  base_of_mechanism         : mechanism;
  SELF\representation_relationship_with_transformation.
    transformation_operator : cartesian_transformation_operator_3d;
DERIVE
  SELF\representation_relationship.rep_2
                            : kinematic_link_representation
                            := representation_of_link (base_of_mechanism.base);
UNIQUE
  UR1: base_of_mechanism;
WHERE
  WR1: ('KINEMATIC_STRUCTURE_SCHEMA.KINEMATIC_GROUND_REPRESENTATION' IN
         TYPEOF (SELF\representation_relationship.rep_1))
       OR
       ('KINEMATIC_STRUCTURE_SCHEMA.KINEMATIC_LINK_REPRESENTATION' IN
         TYPEOF (SELF\representation_relationship.rep_1));
  WR2: suitably_based_mechanism (SELF, base_of_mechanism);
  WR3: SELF\representation_relationship_with_transformation.
         transformation_operator IN
       SELF\representation_relationship.rep_1.items;
END_ENTITY;

ENTITY initial_state;
  applies_to_mechanism : mechanism;
  pair_values          : SET [1 : ?] OF pair_value;
WHERE
  WR1: SIZEOF (QUERY (joint <* applies_to_mechanism.structure_definition.joints |
               SIZEOF (QUERY (init_val <* pair_values |
                       init_val.applies_to_pair.joint :=: joint)) <> 1)) = 0;
END_ENTITY;

ENTITY kinematic_structure;
  joints : SET [1 : ?] OF kinematic_joint;
END_ENTITY;

ENTITY kinematic_joint ;
  first_link  : kinematic_link;
  second_link : kinematic_link;
INVERSE
  structure   : kinematic_structure FOR joints;
WHERE
  WR1: first_link :<>: second_link;
END_ENTITY;

ENTITY kinematic_link;
WHERE
  WR1: SIZEOF (USEDIN (SELF,
                 'KINEMATIC_STRUCTURE_SCHEMA.KINEMATIC_JOINT.FIRST_LINK') +
               USEDIN (SELF,
                 'KINEMATIC_STRUCTURE_SCHEMA.KINEMATIC_JOINT.SECOND_LINK')) > 0;
  WR2: unique_link_usage (SELF);
END_ENTITY;

ENTITY kinematic_link_representation_relation;
  topological_aspects : kinematic_link;
  geometric_aspects   : kinematic_link_representation;
UNIQUE
  UR1: topological_aspects;
END_ENTITY;

ENTITY kinematic_link_representation
  SUBTYPE OF (representation);
  SELF\representation.context_of_items :  geometric_representation_context;
DERIVE
  link_frame                  :  geometric_representation_context
                              := SELF\representation.context_of_items;
INVERSE
  link_representation_relation :
        kinematic_link_representation_relation FOR geometric_aspects;
WHERE
  WR1: SIZEOF (QUERY (item <* SELF\representation.items |
               NOT (('KINEMATIC_STRUCTURE_SCHEMA.RIGID_PLACEMENT' IN
                      TYPEOF (item))
                    OR
 ('GEOMETRY_SCHEMA.CARTESIAN_TRANSFORMATION_OPERATOR_3D' IN
                      TYPEOF (item))) )) = 0;
END_ENTITY;

ENTITY kinematic_link_representation_association
  SUBTYPE OF (representation_relationship);
  SELF\representation_relationship.rep_1 : kinematic_link_representation;
WHERE
  WR1: SELF\representation_relationship.rep_2.context_of_items :=:
       SELF\representation_relationship.rep_1\representation.context_of_items;
  WR2: SIZEOF (['KINEMATIC_STRUCTURE_SCHEMA.KINEMATIC_GROUND_REPRESENTATION',
                'KINEMATIC_STRUCTURE_SCHEMA.KINEMATIC_LINK_REPRESENTATION'] *
               TYPEOF (SELF\representation_relationship.rep_2)) = 0;
END_ENTITY;

ENTITY kinematic_frame_background_representation
  SUBTYPE OF (representation);
  SELF\representation.items            : SET [1 : ?] OF
                                         kinematic_frame_background;
  SELF\representation.context_of_items : geometric_representation_context;
WHERE
  WR1: SELF\representation.context_of_items\
         geometric_representation_context.coordinate_space_dimension = 3;
END_ENTITY;

ENTITY kinematic_frame_based_transformation
  SUBTYPE OF (geometric_representation_item,
     functionally_defined_transformation);
  transformator : rigid_placement;
WHERE
  WR1: SELF\geometric_representation_item.dim=3;
END_ENTITY;

ENTITY kinematic_frame_background_representation_association
  SUBTYPE OF (representation_relationship_with_transformation);
  SELF\representation_relationship_with_transformation.
    transformation_operator : kinematic_frame_based_transformation;
WHERE
  WR1: 'KINEMATIC_STRUCTURE_SCHEMA.KINEMATIC_LINK_REPRESENTATION' IN
         TYPEOF (SELF\representation_relationship.rep_1);
  WR2: 'KINEMATIC_STRUCTURE_SCHEMA.KINEMATIC_FRAME_BACKGROUND_REPRESENTATION'
         IN TYPEOF (SELF\representation_relationship.rep_2);
  WR3: SELF\representation_relationship_with_transformation.
           transformation_operator\kinematic_frame_based_transformation.
           transformator IN
         SELF\representation_relationship.rep_1.items;
END_ENTITY;

ENTITY su_parameters
  SUBTYPE OF (geometric_representation_item);
  a     : length_measure;
  alpha : plane_angle_measure;
  b     : length_measure;
  beta  : plane_angle_measure;
  c     : length_measure;
  gamma : plane_angle_measure;
WHERE
  WR1: SELF\geometric_representation_item.dim=3;
END_ENTITY;

ENTITY kinematic_pair
  SUBTYPE OF (item_defined_transformation);
  joint : kinematic_joint;
DERIVE
  pair_placement_in_first_link_context
        : rigid_placement
        := SELF\item_defined_transformation.transform_item_1;
  pair_placement_in_second_link_context
        : rigid_placement
        := SELF\item_defined_transformation.transform_item_2;
UNIQUE
  UR1: joint;
WHERE
  WR1: coordinated_pair_link_representation
         (joint.first_link, pair_placement_in_first_link_context);
  WR2: coordinated_pair_link_representation
         (joint.second_link, pair_placement_in_second_link_context);
END_ENTITY;

ENTITY pair_actuator;
  actuated_pair : kinematic_pair;
  name          : label;
UNIQUE
  UR1: actuated_pair;
END_ENTITY;

ENTITY pair_value;
  applies_to_pair : kinematic_pair;
END_ENTITY;

ENTITY simple_pair_range;
  applies_to_pair : kinematic_pair;
END_ENTITY;

ENTITY revolute_pair
  SUBTYPE OF (kinematic_pair);
END_ENTITY;

ENTITY revolute_pair_value
  SUBTYPE OF (pair_value);
  SELF\pair_value.applies_to_pair : revolute_pair;
  actual_rotation                 : plane_angle_measure ;
END_ENTITY;

ENTITY revolute_pair_range
  SUBTYPE OF (simple_pair_range);
  SELF\simple_pair_range.applies_to_pair : revolute_pair;
  lower_limit_actual_rotation            : rotational_range_measure;
  upper_limit_actual_rotation            : rotational_range_measure;
WHERE
  WR1: (('KINEMATIC_STRUCTURE_SCHEMA.UNLIMITED_RANGE' IN
         TYPEOF (lower_limit_actual_rotation))
        OR
        ('KINEMATIC_STRUCTURE_SCHEMA.UNLIMITED_RANGE' IN
         TYPEOF (upper_limit_actual_rotation)))
       XOR
       (lower_limit_actual_rotation < upper_limit_actual_rotation);
END_ENTITY;

ENTITY prismatic_pair
  SUBTYPE OF ( kinematic_pair);
END_ENTITY;

ENTITY prismatic_pair_value
  SUBTYPE OF (pair_value);
  SELF\pair_value.applies_to_pair : prismatic_pair;
  actual_translation              : length_measure;
END_ENTITY;

ENTITY prismatic_pair_range
  SUBTYPE OF (simple_pair_range);
  SELF\simple_pair_range.applies_to_pair : prismatic_pair;
  lower_limit_actual_translation         : translational_range_measure;
  upper_limit_actual_translation         : translational_range_measure;
WHERE
  WR1: (('KINEMATIC_STRUCTURE_SCHEMA.UNLIMITED_RANGE' IN
         TYPEOF (lower_limit_actual_translation))
        OR
        ('KINEMATIC_STRUCTURE_SCHEMA.UNLIMITED_RANGE' IN
         TYPEOF (upper_limit_actual_translation)))
       XOR
       (lower_limit_actual_translation < upper_limit_actual_translation);
END_ENTITY;

ENTITY screw_pair
  SUBTYPE OF (kinematic_pair);
  pitch : length_measure;
END_ENTITY;

ENTITY screw_pair_value
  SUBTYPE OF (pair_value);
  SELF\pair_value.applies_to_pair : screw_pair;
  actual_rotation                 : plane_angle_measure;
DERIVE
  actual_translation              : length_measure
                                  := SELF\pair_value.applies_to_pair\
                                     screw_pair.pitch *
                                     plane_angle_for_pair_in_radian
                                      (SELF\pair_value.applies_to_pair,
                                       actual_rotation) / (2 * PI);
END_ENTITY;

ENTITY screw_pair_range
  SUBTYPE OF (simple_pair_range);
  SELF\simple_pair_range.applies_to_pair : screw_pair;
  lower_limit_actual_rotation            : rotational_range_measure;
  upper_limit_actual_rotation            : rotational_range_measure;
WHERE
  WR1: (('KINEMATIC_STRUCTURE_SCHEMA.UNLIMITED_RANGE' IN
         TYPEOF (lower_limit_actual_rotation))
        OR
        ('KINEMATIC_STRUCTURE_SCHEMA.UNLIMITED_RANGE' IN
         TYPEOF (upper_limit_actual_rotation)))
       XOR
       (lower_limit_actual_rotation < upper_limit_actual_rotation);
END_ENTITY;

ENTITY cylindrical_pair
  SUBTYPE OF (kinematic_pair);
END_ENTITY;

ENTITY cylindrical_pair_value
  SUBTYPE OF (pair_value);
  SELF\pair_value.applies_to_pair : cylindrical_pair;
  actual_translation              : length_measure;
  actual_rotation                 : plane_angle_measure;
END_ENTITY;

ENTITY cylindrical_pair_range
  SUBTYPE OF (simple_pair_range);
  SELF\simple_pair_range.applies_to_pair : cylindrical_pair;
  lower_limit_actual_translation         : translational_range_measure;
  upper_limit_actual_translation         : translational_range_measure;
  lower_limit_actual_rotation            : rotational_range_measure;
  upper_limit_actual_rotation            : rotational_range_measure;
WHERE
  WR1: (('KINEMATIC_STRUCTURE_SCHEMA.UNLIMITED_RANGE' IN
         TYPEOF (lower_limit_actual_translation))
        OR
        ('KINEMATIC_STRUCTURE_SCHEMA.UNLIMITED_RANGE' IN
         TYPEOF (upper_limit_actual_translation)))
       XOR
       (lower_limit_actual_translation < upper_limit_actual_translation);
  WR2: (('KINEMATIC_STRUCTURE_SCHEMA.UNLIMITED_RANGE' IN
         TYPEOF (lower_limit_actual_rotation))
        OR
        ('KINEMATIC_STRUCTURE_SCHEMA.UNLIMITED_RANGE' IN
         TYPEOF (upper_limit_actual_rotation)))
       XOR
       (lower_limit_actual_rotation < upper_limit_actual_rotation);
END_ENTITY;

ENTITY spherical_pair
  SUBTYPE OF (kinematic_pair);
END_ENTITY;

ENTITY spherical_pair_value
  SUBTYPE OF (pair_value);
  SELF\pair_value.applies_to_pair : spherical_pair;
  input_orientation               : spatial_rotation;
DERIVE
  actual_orientation              : ypr_rotation
                                  := convert_spatial_to_ypr_rotation
                                     (SELF\pair_value.applies_to_pair,
                                      input_orientation);
END_ENTITY;

ENTITY spherical_pair_range
  SUBTYPE OF (simple_pair_range);
  SELF\simple_pair_range.applies_to_pair : spherical_pair;
  lower_limit_yaw                        : rotational_range_measure;
  upper_limit_yaw                        : rotational_range_measure;
  lower_limit_pitch                      : rotational_range_measure;
  upper_limit_pitch                      : rotational_range_measure;
  lower_limit_roll                       : rotational_range_measure;
  upper_limit_roll                       : rotational_range_measure;
WHERE
  WR1: (('KINEMATIC_STRUCTURE_SCHEMA.UNLIMITED_RANGE' IN
         TYPEOF (lower_limit_yaw))
        OR
        ('KINEMATIC_STRUCTURE_SCHEMA.UNLIMITED_RANGE' IN
         TYPEOF (upper_limit_yaw)))
       XOR
       (lower_limit_yaw < upper_limit_yaw);
  WR2: (('KINEMATIC_STRUCTURE_SCHEMA.UNLIMITED_RANGE' IN
         TYPEOF (lower_limit_pitch))
        OR
        ('KINEMATIC_STRUCTURE_SCHEMA.UNLIMITED_RANGE' IN
         TYPEOF (upper_limit_pitch)))
       XOR
       (lower_limit_pitch < upper_limit_pitch);
  WR3: (('KINEMATIC_STRUCTURE_SCHEMA.UNLIMITED_RANGE' IN
         TYPEOF (lower_limit_roll))
        OR
        ('KINEMATIC_STRUCTURE_SCHEMA.UNLIMITED_RANGE' IN
         TYPEOF (upper_limit_roll)))
       XOR
       (lower_limit_roll < upper_limit_roll);
END_ENTITY;

ENTITY universal_pair
  SUBTYPE OF (kinematic_pair);
  input_skew_angle : OPTIONAL plane_angle_measure;
DERIVE
  skew_angle       : plane_angle_measure := NVL (input_skew_angle, 0.0);
WHERE
  WR1: COS (plane_angle_for_pair_in_radian (SELF, skew_angle))
       > 0.0;
END_ENTITY;

ENTITY universal_pair_value
  SUBTYPE OF (pair_value);
  SELF\pair_value.applies_to_pair : universal_pair;
  first_rotation_angle            : plane_angle_measure;
  second_rotation_angle           : plane_angle_measure;
END_ENTITY;

ENTITY universal_pair_range
  SUBTYPE OF (simple_pair_range);
  SELF\simple_pair_range.applies_to_pair : universal_pair;
  lower_limit_first_rotation             : rotational_range_measure;
  upper_limit_first_rotation             : rotational_range_measure;
  lower_limit_second_rotation            : rotational_range_measure;
  upper_limit_second_rotation            : rotational_range_measure;
WHERE
  WR1: (('KINEMATIC_STRUCTURE_SCHEMA.UNLIMITED_RANGE' IN
         TYPEOF (lower_limit_first_rotation))
        OR
        ('KINEMATIC_STRUCTURE_SCHEMA.UNLIMITED_RANGE' IN
         TYPEOF (upper_limit_first_rotation)))
       XOR
       (lower_limit_first_rotation < upper_limit_first_rotation);
  WR2: (('KINEMATIC_STRUCTURE_SCHEMA.UNLIMITED_RANGE' IN
         TYPEOF (lower_limit_second_rotation))
        OR
        ('KINEMATIC_STRUCTURE_SCHEMA.UNLIMITED_RANGE' IN
         TYPEOF (upper_limit_second_rotation)))
       XOR
       (lower_limit_second_rotation < upper_limit_second_rotation);
END_ENTITY;

ENTITY planar_pair
  SUBTYPE OF (kinematic_pair);
END_ENTITY;

ENTITY planar_pair_value
  SUBTYPE OF (pair_value);
  SELF\pair_value.applies_to_pair : planar_pair;
  actual_rotation                 : plane_angle_measure;
  actual_translation_x            : length_measure;
  actual_translation_y            : length_measure;
END_ENTITY;

ENTITY planar_pair_range
  SUBTYPE OF (simple_pair_range);
  SELF\simple_pair_range.applies_to_pair : planar_pair;
  lower_limit_actual_rotation            : rotational_range_measure;
  upper_limit_actual_rotation            : rotational_range_measure;
  lower_limit_actual_translation_x       : translational_range_measure;
  upper_limit_actual_translation_x       : translational_range_measure;
  lower_limit_actual_translation_y       : translational_range_measure;
  upper_limit_actual_translation_y       : translational_range_measure;
WHERE
  WR1: (('KINEMATIC_STRUCTURE_SCHEMA.UNLIMITED_RANGE' IN
         TYPEOF (lower_limit_actual_rotation))
        OR
        ('KINEMATIC_STRUCTURE_SCHEMA.UNLIMITED_RANGE' IN
         TYPEOF (upper_limit_actual_rotation)))
       XOR
       (lower_limit_actual_rotation < upper_limit_actual_rotation);
  WR2: (('KINEMATIC_STRUCTURE_SCHEMA.UNLIMITED_RANGE' IN
         TYPEOF (lower_limit_actual_translation_x))
        OR
        ('KINEMATIC_STRUCTURE_SCHEMA.UNLIMITED_RANGE' IN
         TYPEOF (upper_limit_actual_translation_x)))
       XOR
       (lower_limit_actual_translation_x < upper_limit_actual_translation_x);
  WR3: (('KINEMATIC_STRUCTURE_SCHEMA.UNLIMITED_RANGE' IN
         TYPEOF (lower_limit_actual_translation_y))
        OR
        ('KINEMATIC_STRUCTURE_SCHEMA.UNLIMITED_RANGE' IN
         TYPEOF (upper_limit_actual_translation_y)))
       XOR
       (lower_limit_actual_translation_y < upper_limit_actual_translation_y);
END_ENTITY;

ENTITY unconstrained_pair
  SUBTYPE OF (kinematic_pair);
END_ENTITY;

ENTITY unconstrained_pair_value
  SUBTYPE OF (pair_value);
  SELF\pair_value.applies_to_pair : unconstrained_pair;
  actual_placement                : axis2_placement_3d;
END_ENTITY;

ENTITY fully_constrained_pair
  SUBTYPE OF (kinematic_pair);
END_ENTITY;

ENTITY point_on_surface_pair
  SUBTYPE OF (kinematic_pair);
  pair_surface : surface;
WHERE
  WR1: frame_associated_to_background
         (SELF\kinematic_pair.pair_placement_in_first_link_context,
         pair_surface);
END_ENTITY;

ENTITY point_on_surface_pair_value
  SUBTYPE OF (pair_value);
  SELF\pair_value.applies_to_pair : point_on_surface_pair;
  actual_point_on_surface         : point_on_surface;
  input_orientation               : spatial_rotation;
DERIVE
  actual_orientation              : ypr_rotation
                                  := convert_spatial_to_ypr_rotation
                                     (SELF\pair_value.applies_to_pair,
                                      input_orientation);
WHERE
  WR1: SELF\pair_value.applies_to_pair\point_on_surface_pair.pair_surface :=:
       actual_point_on_surface.basis_surface;
END_ENTITY;

ENTITY point_on_surface_pair_range
  SUBTYPE OF (simple_pair_range);
  SELF\simple_pair_range.applies_to_pair : point_on_surface_pair;
  range_on_pair_surface                  : rectangular_trimmed_surface;
  lower_limit_yaw                        : rotational_range_measure;
  upper_limit_yaw                        : rotational_range_measure;
  lower_limit_pitch                      : rotational_range_measure;
  upper_limit_pitch                      : rotational_range_measure;
  lower_limit_roll                       : rotational_range_measure;
  upper_limit_roll                       : rotational_range_measure;
WHERE
  WR1: SELF\simple_pair_range.applies_to_pair\point_on_surface_pair.pair_surface
       :=: range_on_pair_surface.basis_surface;
  WR2: (('KINEMATIC_STRUCTURE_SCHEMA.UNLIMITED_RANGE' IN
         TYPEOF (lower_limit_yaw))
        OR
        ('KINEMATIC_STRUCTURE_SCHEMA.UNLIMITED_RANGE' IN
         TYPEOF (upper_limit_yaw)))
       XOR
       (lower_limit_yaw < upper_limit_yaw);
  WR3: (('KINEMATIC_STRUCTURE_SCHEMA.UNLIMITED_RANGE' IN
         TYPEOF (lower_limit_pitch))
        OR
        ('KINEMATIC_STRUCTURE_SCHEMA.UNLIMITED_RANGE' IN
         TYPEOF (upper_limit_pitch)))
       XOR
       (lower_limit_pitch < upper_limit_pitch);
  WR4: (('KINEMATIC_STRUCTURE_SCHEMA.UNLIMITED_RANGE' IN
         TYPEOF (lower_limit_roll))
        OR
        ('KINEMATIC_STRUCTURE_SCHEMA.UNLIMITED_RANGE' IN
         TYPEOF (upper_limit_roll)))
       XOR
       (lower_limit_roll < upper_limit_roll);
END_ENTITY;

ENTITY surface_pair
  SUBTYPE OF ( kinematic_pair);
  surface_1   : surface;
  surface_2   : surface;
  orientation : BOOLEAN;
WHERE
  WR1: frame_associated_to_background
         (SELF\kinematic_pair.pair_placement_in_first_link_context,
          surface_1);
  WR2: frame_associated_to_background
         (SELF\kinematic_pair.pair_placement_in_second_link_context,
          surface_2);
END_ENTITY;

ENTITY surface_pair_range
  SUBTYPE OF (simple_pair_range);
  SELF\simple_pair_range.applies_to_pair : surface_pair;
  range_on_surface_1                     : rectangular_trimmed_surface;
  range_on_surface_2                     : rectangular_trimmed_surface;
  lower_limit_actual_rotation            : rotational_range_measure;
  upper_limit_actual_rotation            : rotational_range_measure;
WHERE
  WR1: SELF\simple_pair_range.applies_to_pair\surface_pair.surface_1 :=:
       range_on_surface_1.basis_surface;
  WR2: SELF\simple_pair_range.applies_to_pair\surface_pair.surface_2 :=:
       range_on_surface_2.basis_surface;
  WR3: (('KINEMATIC_STRUCTURE_SCHEMA.UNLIMITED_RANGE' IN
         TYPEOF (lower_limit_actual_rotation))
        OR
        ('KINEMATIC_STRUCTURE_SCHEMA.UNLIMITED_RANGE' IN
         TYPEOF (upper_limit_actual_rotation)))
       XOR
       (lower_limit_actual_rotation < upper_limit_actual_rotation);
END_ENTITY;

ENTITY sliding_surface_pair
  SUBTYPE OF (surface_pair);
END_ENTITY;

ENTITY sliding_surface_pair_value
  SUBTYPE OF (pair_value);
  SELF\pair_value.applies_to_pair : sliding_surface_pair;
  actual_point_on_surface_1       : point_on_surface;
  actual_point_on_surface_2       : point_on_surface;
  actual_rotation                 : plane_angle_measure;
WHERE
  WR1: SELF\pair_value.applies_to_pair\surface_pair.surface_1 :=:
       actual_point_on_surface_1.basis_surface;
  WR2: SELF\pair_value.applies_to_pair\surface_pair.surface_2 :=:
       actual_point_on_surface_2.basis_surface;
END_ENTITY;

ENTITY rolling_surface_pair
  SUBTYPE OF (surface_pair);
END_ENTITY;

ENTITY rolling_surface_pair_value
  SUBTYPE OF (pair_value);
  SELF\pair_value.applies_to_pair : rolling_surface_pair;
  actual_point_on_surface         : point_on_surface;
  actual_rotation                 : plane_angle_measure;
WHERE
  WR1: SELF\pair_value.applies_to_pair\surface_pair.surface_1 :=:
       actual_point_on_surface.basis_surface;
END_ENTITY;

ENTITY point_on_planar_curve_pair
  SUBTYPE OF (kinematic_pair);
  pair_curve  : curve;
  orientation : BOOLEAN;
WHERE
  WR1: frame_associated_to_background
         (SELF\kinematic_pair.pair_placement_in_first_link_context,
         pair_curve);
END_ENTITY;

ENTITY point_on_planar_curve_pair_value
  SUBTYPE OF (pair_value);
  SELF\pair_value.applies_to_pair : point_on_planar_curve_pair;
  actual_point_on_curve           : point_on_curve;
  input_orientation               : spatial_rotation;
DERIVE
  actual_orientation              : ypr_rotation
                                  := convert_spatial_to_ypr_rotation
                                     (SELF\pair_value.applies_to_pair,
                                      input_orientation);
WHERE
  WR1: SELF\pair_value.applies_to_pair\point_on_planar_curve_pair.pair_curve
       :=: actual_point_on_curve.basis_curve;
END_ENTITY;

ENTITY point_on_planar_curve_pair_range
  SUBTYPE OF (simple_pair_range);
  SELF\simple_pair_range.applies_to_pair : point_on_planar_curve_pair;
  range_on_pair_curve                    : trimmed_curve;
  lower_limit_yaw                        : rotational_range_measure;
  upper_limit_yaw                        : rotational_range_measure;
  lower_limit_pitch                      : rotational_range_measure;
  upper_limit_pitch                      : rotational_range_measure;
  lower_limit_roll                       : rotational_range_measure;
  upper_limit_roll                       : rotational_range_measure;
WHERE
  WR1: SELF\simple_pair_range.applies_to_pair\
           point_on_planar_curve_pair.pair_curve
       :=: range_on_pair_curve.basis_curve;
  WR2: (('KINEMATIC_STRUCTURE_SCHEMA.UNLIMITED_RANGE' IN
         TYPEOF (lower_limit_yaw))
        OR
        ('KINEMATIC_STRUCTURE_SCHEMA.UNLIMITED_RANGE' IN
         TYPEOF (upper_limit_yaw)))
       XOR
       (lower_limit_yaw < upper_limit_yaw);
  WR3: (('KINEMATIC_STRUCTURE_SCHEMA.UNLIMITED_RANGE' IN
         TYPEOF (lower_limit_pitch))
        OR
        ('KINEMATIC_STRUCTURE_SCHEMA.UNLIMITED_RANGE' IN
         TYPEOF (upper_limit_pitch)))
       XOR
       (lower_limit_pitch < upper_limit_pitch);
  WR4: (('KINEMATIC_STRUCTURE_SCHEMA.UNLIMITED_RANGE' IN
         TYPEOF (lower_limit_roll))
        OR
        ('KINEMATIC_STRUCTURE_SCHEMA.UNLIMITED_RANGE' IN
         TYPEOF (upper_limit_roll)))
       XOR
       (lower_limit_roll < upper_limit_roll);
END_ENTITY;

ENTITY planar_curve_pair
  SUBTYPE OF (kinematic_pair);
  curve_1     : curve;
  curve_2     : curve;
  orientation : BOOLEAN;
WHERE
  WR1: frame_associated_to_background
         (SELF\kinematic_pair.pair_placement_in_first_link_context,
          curve_1);
  WR2: frame_associated_to_background
         (SELF\kinematic_pair.pair_placement_in_second_link_context,
          curve_2);
END_ENTITY;

ENTITY planar_curve_pair_range
  SUBTYPE OF (simple_pair_range);
  SELF\simple_pair_range.applies_to_pair : planar_curve_pair;
  range_on_curve_1                       : trimmed_curve;
  range_on_curve_2                       : trimmed_curve;
WHERE
  WR1: SELF\simple_pair_range.applies_to_pair\planar_curve_pair.curve_1 :=:
       range_on_curve_1.basis_curve;
  WR2: SELF\simple_pair_range.applies_to_pair\planar_curve_pair.curve_2 :=:
       range_on_curve_2.basis_curve;
END_ENTITY;

ENTITY sliding_curve_pair
  SUBTYPE OF (planar_curve_pair);
END_ENTITY;

ENTITY sliding_curve_pair_value
  SUBTYPE OF (pair_value);
  SELF\pair_value.applies_to_pair : sliding_curve_pair;
  actual_point_on_curve_1         : point_on_curve;
  actual_point_on_curve_2         : point_on_curve;
WHERE
  WR1: SELF\pair_value.applies_to_pair\planar_curve_pair.curve_1 :=:
       actual_point_on_curve_1.basis_curve;
  WR2: SELF\pair_value.applies_to_pair\planar_curve_pair.curve_2 :=:
       actual_point_on_curve_2.basis_curve;
END_ENTITY;

ENTITY rolling_curve_pair
  SUBTYPE OF (planar_curve_pair);
END_ENTITY;

ENTITY rolling_curve_pair_value
  SUBTYPE OF (pair_value);
  SELF\pair_value.applies_to_pair : rolling_curve_pair;
  actual_point_on_curve_1         : point_on_curve;
WHERE
  WR1: SELF\pair_value.applies_to_pair\planar_curve_pair.curve_1 :=:
       actual_point_on_curve_1.basis_curve;
END_ENTITY;

ENTITY gear_pair
  SUBTYPE OF (kinematic_pair);
  radius_first_link  : length_measure;
  radius_second_link : length_measure;
  bevel              : plane_angle_measure;
  helical_angle      : plane_angle_measure;
  gear_ratio         : REAL;
END_ENTITY;

ENTITY gear_pair_value
  SUBTYPE OF (pair_value);
  SELF\pair_value.applies_to_pair : gear_pair;
  actual_rotation_1               : plane_angle_measure;
DERIVE
  actual_rotation_2               : plane_angle_measure
                                  := - actual_rotation_1 *
                                     SELF\pair_value.applies_to_pair\
                                     gear_pair.gear_ratio;
END_ENTITY;

ENTITY gear_pair_range
  SUBTYPE OF (simple_pair_range);
  SELF\simple_pair_range.applies_to_pair : gear_pair;
  lower_limit_actual_rotation_1          : rotational_range_measure;
  upper_limit_actual_rotation_1          : rotational_range_measure;
WHERE
  WR1: (('KINEMATIC_STRUCTURE_SCHEMA.UNLIMITED_RANGE' IN
         TYPEOF (lower_limit_actual_rotation_1))
        OR
        ('KINEMATIC_STRUCTURE_SCHEMA.UNLIMITED_RANGE' IN
         TYPEOF (upper_limit_actual_rotation_1)))
       XOR
       (lower_limit_actual_rotation_1 <
        upper_limit_actual_rotation_1);
END_ENTITY;

ENTITY rack_and_pinion_pair
  SUBTYPE OF (kinematic_pair);
  pinion_radius : length_measure;
END_ENTITY;

ENTITY rack_and_pinion_pair_value
  SUBTYPE OF (pair_value);
  SELF\pair_value.applies_to_pair : rack_and_pinion_pair;
  actual_displacement             : length_measure;
DERIVE
  actual_rotation : plane_angle_measure
                  := convert_plane_angle_for_pair_from_radian
                     (SELF\pair_value.applies_to_pair,
                      (- actual_displacement /
                       SELF\pair_value.applies_to_pair\
                       rack_and_pinion_pair.pinion_radius));
END_ENTITY;

ENTITY rack_and_pinion_pair_range
  SUBTYPE OF (simple_pair_range);
  SELF\simple_pair_range.applies_to_pair : rack_and_pinion_pair;
  lower_limit_rack_displacement          : translational_range_measure;
  upper_limit_rack_displacement          : translational_range_measure;
WHERE
  WR1: (('KINEMATIC_STRUCTURE_SCHEMA.UNLIMITED_RANGE' IN
         TYPEOF (lower_limit_rack_displacement))
        OR
        ('KINEMATIC_STRUCTURE_SCHEMA.UNLIMITED_RANGE' IN
         TYPEOF (upper_limit_rack_displacement)))
       XOR
       (lower_limit_rack_displacement < upper_limit_rack_displacement);
END_ENTITY;

ENTITY kinematic_substructure
  SUPERTYPE OF (ONEOF (kinematic_tree_structure,
                       kinematic_network_structure));
  parent_structure : kinematic_structure;
END_ENTITY;

ENTITY kinematic_network_structure
  SUBTYPE OF (kinematic_substructure);
END_ENTITY;

ENTITY kinematic_tree_structure
  SUBTYPE OF (kinematic_substructure);
END_ENTITY;

ENTITY kinematic_loop;
  network : kinematic_network_structure;
WHERE
 WR1 : SIZEOF(USEDIN(SELF,
   'KINEMATIC_STRUCTURE_SCHEMA.JOINT_LOGICAL_RELATIONSHIP.LOOP')) > 0;
 WR2 : SIZEOF (QUERY( relation_1 <* USEDIN(SELF,
    'KINEMATIC_STRUCTURE_SCHEMA.JOINT_LOGICAL_RELATIONSHIP.LOOP') |
         SIZEOF (QUERY (relation_2 <*  (USEDIN(SELF, 
         'KINEMATIC_STRUCTURE_SCHEMA.JOINT_LOGICAL_RELATIONSHIP.LOOP')
              - relation_1) |
              NOT(connected_in_simple_loop (relation_1, relation_2))
         )) > 0 
       )) = 0;
END_ENTITY;

ENTITY joint_logical_relationship;
  loop                             : kinematic_loop;
  previous_joint_logical_structure : oriented_joint;
  next_joint_logical_structure     : oriented_joint;
UNIQUE
  UR1: loop, previous_joint_logical_structure;
  UR2: loop, next_joint_logical_structure;
WHERE
  WR1: previous_joint_logical_structure.exit_link :=:
       next_joint_logical_structure.advent_link;
END_ENTITY;

ENTITY oriented_joint;
  joint       : kinematic_joint;
  orientation : BOOLEAN;
DERIVE
  advent_link : kinematic_link
              := assign_directed_link (joint, orientation);
  exit_link   : kinematic_link
              := assign_directed_link (joint, NOT (orientation));
END_ENTITY;

ENTITY oriented_joint_in_tree
  SUBTYPE OF (oriented_joint);
  parent_structure : kinematic_tree_structure;
END_ENTITY;

ENTITY advent_oriented_joint
  SUBTYPE OF (oriented_joint);
END_ENTITY;

FUNCTION ypr_index (ypr : ypr_enumeration) : INTEGER;
  CASE ypr OF
    yaw    : RETURN (1);
    pitch  : RETURN (2);
    roll   : RETURN (3);
  END_CASE;
  RETURN (?);
END_FUNCTION;

FUNCTION representation_of_link (link : kinematic_link)
    : kinematic_link_representation;
  LOCAL
    link_rep_rel : BAG OF kinematic_link_representation_relation;
  END_LOCAL;

  link_rep_rel := USEDIN (link, 'KINEMATIC_STRUCTURE_SCHEMA.'+
                                'KINEMATIC_LINK_REPRESENTATION_RELATION.'+
                                'TOPOLOGICAL_ASPECTS');
  IF (SIZEOF (link_rep_rel) = 0) THEN
    RETURN (?);
  ELSE
    RETURN (link_rep_rel[1].geometric_aspects);
  END_IF;
END_FUNCTION;

FUNCTION suitably_based_mechanism (mbp  : mechanism_base_placement;
                                   mech : mechanism) : BOOLEAN;
  LOCAL
    kprop  : kinematic_property_definition;
    kgrep  : kinematic_ground_representation;
    klrep  : kinematic_link_representation;
    klnk   : kinematic_link;
    kjnts  : BAG OF kinematic_joint;
    nmechs : BAG OF mechanism;
    nmbps  : BAG OF mechanism_base_placement;
  END_LOCAL;

  kprop := mech.containing_property;

  IF ('KINEMATIC_STRUCTURE_SCHEMA.KINEMATIC_GROUND_REPRESENTATION' IN
      TYPEOF (mbp\representation_relationship.rep_1)) THEN
      kgrep := mbp\representation_relationship.rep_1;

    IF (kgrep.property\property_definition_representation.definition
        :=: kprop) THEN
      RETURN (TRUE);
    ELSE
      RETURN (FALSE);
    END_IF;
  ELSE
    klrep := mbp\representation_relationship.rep_1;
    klnk   := klrep.link_representation_relation.topological_aspects;
    kjnts  := USEDIN (klnk,
            'KINEMATIC_STRUCTURE_SCHEMA.KINEMATIC_JOINT.FIRST_LINK') +
              USEDIN (klnk,
            'KINEMATIC_STRUCTURE_SCHEMA.KINEMATIC_JOINT.SECOND_LINK');
    nmechs := USEDIN (kjnts[1].structure,
         'KINEMATIC_STRUCTURE_SCHEMA.MECHANISM.STRUCTURE_DEFINITION');

    IF (nmechs[1] :=: mech) THEN
      RETURN (FALSE);
    ELSE
      IF (nmechs[1].containing_property :<>: kprop) THEN
        RETURN (FALSE);
      ELSE
        nmbps := USEDIN (nmechs[1], 'KINEMATIC_STRUCTURE_SCHEMA.'+
                        'MECHANISM_BASE_PLACEMENT.BASE_OF_MECHANISM');

        IF (SIZEOF (nmbps) = 0) THEN
          RETURN (FALSE);
        ELSE
          RETURN (suitably_based_mechanism (nmbps[1], mech));
        END_IF;
      END_IF;
    END_IF;
  END_IF;
END_FUNCTION;

FUNCTION unique_link_usage (link : kinematic_link) : BOOLEAN;
  LOCAL
    mechs  : SET OF mechanism;
    joints : SET OF kinematic_joint;
    struct : kinematic_structure;
  END_LOCAL;
  joints := bag_to_set
              (USEDIN (link,
                 'KINEMATIC_STRUCTURE_SCHEMA.KINEMATIC_JOINT.FIRST_LINK') +
               USEDIN (link,
                 'KINEMATIC_STRUCTURE_SCHEMA.KINEMATIC_JOINT.SECOND_LINK'));
  struct := joints[1].structure;
  REPEAT i := 2 TO SIZEOF (joints);
    IF (joints[i].structure :<>: struct) THEN
      RETURN (FALSE);
    END_IF;
  END_REPEAT;
  mechs := bag_to_set
             (USEDIN (struct,
                'KINEMATIC_STRUCTURE_SCHEMA.MECHANISM.STRUCTURE_DEFINITION'));
  IF (SIZEOF (mechs) <> 1) THEN
    RETURN (FALSE);
  END_IF;

  RETURN (TRUE);

END_FUNCTION;

FUNCTION coordinated_pair_link_representation
    (link           : kinematic_link;
     pair_placement : rigid_placement) : BOOLEAN;
  LOCAL
    link_rep : kinematic_link_representation;
  END_LOCAL;

  link_rep := representation_of_link (link);

  IF (link_rep = ?) THEN
    RETURN (FALSE);
  ELSE
    IF NOT (pair_placement IN link_rep\representation.items) THEN
      RETURN (FALSE);
    ELSE
      RETURN (TRUE);
    END_IF;
  END_IF;
END_FUNCTION;

FUNCTION frame_associated_to_background
  (frame      : rigid_placement;
   background : kinematic_frame_background) : BOOLEAN;
  LOCAL
    rep_bag : BAG OF kinematic_frame_background_representation;
    trf_bag : BAG OF kinematic_frame_based_transformation;
    trm_bag : BAG OF kinematic_frame_based_transformation;
    ass_bag : BAG OF
              kinematic_frame_background_representation_association;
    rep     : kinematic_frame_background_representation;
    ass     : kinematic_frame_background_representation_association;
  END_LOCAL;

  rep_bag := QUERY ( bg <* USEDIN (background,
                     'KINEMATIC_STRUCTURE_SCHEMA.' +
                     'REPRESENTATION.ITEMS')|
                     'KINEMATIC_STRUCTURE_SCHEMA.'+
                     'KINEMATIC_FRAME_BACKGROUND_REPRESENTATION' 
                      IN TYPEOF (bg) );

  IF SIZEOF (rep_bag) = 0 THEN
    RETURN (FALSE);
  END_IF;

  trf_bag := USEDIN (frame,
                     'KINEMATIC_STRUCTURE_SCHEMA.' +
                     'KINEMATIC_FRAME_BASED_TRANSFORMATION.' +
                     'TRANSFORMATOR');

  IF SIZEOF (trf_bag) = 0 THEN
    RETURN (FALSE);
  END_IF;

  REPEAT i := 1 TO HIINDEX (rep_bag);
    rep := rep_bag[i];

ass_bag := QUERY ( kfbra <* USEDIN ( rep,
             'KINEMATIC_STRUCTURE_SCHEMA.' +
             'REPRESENTATION_RELATIONSHIP.REP_2') |
             'KINEMATIC_STRUCTURE_SCHEMA.'+
             'KINEMATIC_FRAME_BACKGROUND_REPRESENTATION_ASSOCIATION'
             IN TYPEOF ( kfbra ) );

    IF SIZEOF (ass_bag) > 0 THEN
      REPEAT j:= 1 TO HIINDEX (ass_bag);
        ass := ass_bag[j];

        trm_bag := QUERY (trm <* trf_bag |
          (trm :=:
           ass\representation_relationship_with_transformation.
           transformation_operator));

        IF SIZEOF (trm_bag) > 0 THEN
          RETURN (TRUE);
        END_IF;

      END_REPEAT;
    END_IF;
  END_REPEAT;

  RETURN (FALSE);

END_FUNCTION;

FUNCTION plane_angle_for_pair_in_radian
         (pair  : kinematic_pair;
          angle : plane_angle_measure) : plane_angle_measure;
  LOCAL
    converted_angle : plane_angle_measure := angle;
    link_rep        : kinematic_link_representation
                    := representation_of_link (pair.joint.first_link);
    link_cntxt      : representation_context;
    pa_units        : SET OF unit := [];
    pau             : unit;
  END_LOCAL;

  link_cntxt := link_rep\representation.context_of_items;

  IF NOT ('MEASURE_SCHEMA.GLOBAL_UNIT_ASSIGNED_CONTEXT'
          IN TYPEOF (link_cntxt)) THEN
    RETURN (?);
  END_IF;

  pa_units := QUERY (unit <* link_cntxt\global_unit_assigned_context.units |
                     'MEASURE_SCHEMA.PLANE_ANGLE_UNIT' IN TYPEOF (unit));

  IF SIZEOF (pa_units) <> 1 THEN
    RETURN (?);
  END_IF;

  pau := pa_units[1];

  IF (NOT ('MEASURE_SCHEMA.SI_UNIT' IN TYPEOF (pau)) AND
      NOT ('MEASURE_SCHEMA.CONVERSION_BASED_UNIT' IN TYPEOF (pau))) THEN
    RETURN (?);
  END_IF;

  REPEAT WHILE ('MEASURE_SCHEMA.CONVERSION_BASED_UNIT' IN TYPEOF (pau));
    converted_angle := converted_angle *
                       pau\conversion_based_unit.conversion_factor.
                       value_component;
    pau := pau\conversion_based_unit.conversion_factor.unit_component;

    IF ((NOT ('MEASURE_SCHEMA.SI_UNIT' IN TYPEOF (pau)) AND
         NOT ('MEASURE_SCHEMA.CONVERSION_BASED_UNIT' IN TYPEOF (pau))) OR
        (NOT ('MEASURE_SCHEMA.PLANE_ANGLE_UNIT' IN TYPEOF (pau)))) THEN
      RETURN (?);
    END_IF;
  END_REPEAT;

  IF (pau\si_unit.name <> si_unit_name.radian) THEN
    RETURN (?);
  END_IF;

  CASE pau\si_unit.prefix OF
    si_prefix.exa     : RETURN (1.E18 * converted_angle);
    si_prefix.peta    : RETURN (1.E15 * converted_angle);
    si_prefix.tera    : RETURN (1.E12 * converted_angle);
    si_prefix.giga    : RETURN (1.E9 * converted_angle);
    si_prefix.mega    : RETURN (1.E6 * converted_angle);
    si_prefix.kilo    : RETURN (1.E3 * converted_angle);
    si_prefix.hecto   : RETURN (1.E2 * converted_angle);
    si_prefix.deca    : RETURN (1.E1 * converted_angle);
    si_prefix.deci    : RETURN (1.E-1 * converted_angle);
    si_prefix.centi   : RETURN (1.E-2 * converted_angle);
    si_prefix.milli   : RETURN (1.E-3 * converted_angle);
    si_prefix.micro   : RETURN (1.E-6 * converted_angle);
    si_prefix.nano    : RETURN (1.E-9 * converted_angle);
    si_prefix.pico    : RETURN (1.E-12 * converted_angle);
    si_prefix.femto   : RETURN (1.E-15 * converted_angle);
    si_prefix.atto    : RETURN (1.E-18 * converted_angle);
  OTHERWISE           : RETURN (converted_angle);
  END_CASE;

END_FUNCTION;

FUNCTION convert_plane_angle_for_pair_from_radian
         (pair       : kinematic_pair;
          angle_expr : REAL) : plane_angle_measure;
  LOCAL
    link_rep    : kinematic_link_representation
                := representation_of_link (pair.joint.first_link);
    link_cntxt  : representation_context;
    pa_units    : SET OF unit := [];
    pau         : unit;
    conv_factor : REAL := 1.0;
    result      : plane_angle_measure;
  END_LOCAL;

  link_cntxt := link_rep\representation.context_of_items;

  IF NOT ('MEASURE_SCHEMA.GLOBAL_UNIT_ASSIGNED_CONTEXT'
          IN TYPEOF (link_cntxt)) THEN
    RETURN (?);
  END_IF;

  pa_units := QUERY (unit <* link_cntxt\global_unit_assigned_context.units |
                     'MEASURE_SCHEMA.PLANE_ANGLE_UNIT' IN TYPEOF (unit));

  IF SIZEOF (pa_units) <> 1 THEN
    RETURN (?);
  END_IF;

  pau := pa_units[1];

  IF (NOT ('MEASURE_SCHEMA.SI_UNIT' IN TYPEOF (pau)) AND
      NOT ('MEASURE_SCHEMA.CONVERSION_BASED_UNIT' IN TYPEOF (pau))) THEN
    RETURN (?);
  END_IF;

  REPEAT WHILE ('MEASURE_SCHEMA.CONVERSION_BASED_UNIT' IN TYPEOF (pau));
    conv_factor := conv_factor *
                   pau\conversion_based_unit.conversion_factor.
                   value_component;
    pau := pau\conversion_based_unit.conversion_factor.unit_component;

    IF ((NOT ('MEASURE_SCHEMA.SI_UNIT' IN TYPEOF (pau)) AND
         NOT ('MEASURE_SCHEMA.CONVERSION_BASED_UNIT' IN TYPEOF (pau))) OR
        (NOT ('MEASURE_SCHEMA.PLANE_ANGLE_UNIT' IN TYPEOF (pau)))) THEN
      RETURN (?);
    END_IF;
  END_REPEAT;

  IF (pau\si_unit.name <> si_unit_name.radian) THEN
    RETURN (?);
  END_IF;

  CASE pau\si_unit.prefix OF
    si_prefix.exa     : conv_factor := 1.E18 * conv_factor;
    si_prefix.peta    : conv_factor := 1.E15 * conv_factor;
    si_prefix.tera    : conv_factor := 1.E12 * conv_factor;
    si_prefix.giga    : conv_factor := 1.E9 * conv_factor;
    si_prefix.mega    : conv_factor := 1.E6 * conv_factor;
    si_prefix.kilo    : conv_factor := 1.E3 * conv_factor;
    si_prefix.hecto   : conv_factor := 1.E2 * conv_factor;
    si_prefix.deca    : conv_factor := 1.E1 * conv_factor;
    si_prefix.deci    : conv_factor := 1.E-1 * conv_factor;
    si_prefix.centi   : conv_factor := 1.E-2 * conv_factor;
    si_prefix.milli   : conv_factor := 1.E-3 * conv_factor;
    si_prefix.micro   : conv_factor := 1.E-6 * conv_factor;
    si_prefix.nano    : conv_factor := 1.E-9 * conv_factor;
    si_prefix.pico    : conv_factor := 1.E-12 * conv_factor;
    si_prefix.femto   : conv_factor := 1.E-15 * conv_factor;
    si_prefix.atto    : conv_factor := 1.E-18 * conv_factor;
  END_CASE;

  result := angle_expr / conv_factor;
  RETURN (result);
END_FUNCTION;

FUNCTION convert_spatial_to_ypr_rotation (pair     : kinematic_pair;
                                          rotation : spatial_rotation)
                                                   : ypr_rotation;
  LOCAL
    axis       : direction;
    angle      : plane_angle_measure;   -- rotation angle in application
                                        -- specific units
    conv_angle : plane_angle_measure;   -- rotation angle in radians
    ya, pa, ra : plane_angle_measure;   -- yaw, pitch, and roll angle
    ucf        : REAL;                  -- unit conversion factor
    dx, dy, dz : REAL;                  -- components of direction vector
    s_a, c_a   : REAL;                  -- sine and cosine of rotation angle
    rotmat     : ARRAY [1 : 3] OF
                 ARRAY [1 : 3] OF REAL; -- rotation matrix
    cm1        : REAL;
    s_y, c_y   : REAL;
    s_r, c_r   : REAL;
  END_LOCAL;

  -- If rotation is already a ypr_rotation, return it immediately
  IF 'KINEMATIC_STRUCTURE_SCHEMA.YPR_ROTATION' IN TYPEOF (rotation) THEN
    RETURN (rotation);
  END_IF;

  -- rotation is a rotation_about_direction

  axis  := normalise (rotation\rotation_about_direction.direction_of_axis);
  angle := rotation\rotation_about_direction.rotation_angle;

  -- a zero rotation is converted trivially
  IF (angle = 0.0) THEN
    RETURN ([0.0, 0.0, 0.0]);
  END_IF;

  dx := axis.direction_ratios[1];
  dy := axis.direction_ratios[2];
  dz := axis.direction_ratios[3];

  -- provide angle measured in radian

  conv_angle := plane_angle_for_pair_in_radian (pair, angle);

  IF (conv_angle = ?) THEN
    RETURN (?);
  END_IF;

  ucf := angle / conv_angle;
  s_a := SIN (conv_angle);
  c_a := COS (conv_angle);

  -- axis parallel either to x-axis or to z-axis?
  IF (dy = 0.0) AND (dx * dz = 0.0) THEN
    REPEAT WHILE (conv_angle <= - PI);
      conv_angle := conv_angle + 2.0 * PI;
    END_REPEAT;
    REPEAT WHILE (conv_angle > PI);
      conv_angle := conv_angle - 2.0 * PI;
    END_REPEAT;

    ya := ucf * conv_angle;
    IF (conv_angle <> PI) THEN
      ra := - ya;
    ELSE
      ra := ya;
    END_IF;

    IF (dx <> 0.0) THEN
      -- axis parallel to x-axis - use x-axis as roll axis
      IF (dx > 0.0) THEN
        RETURN ([0.0, 0.0, ya]);
      ELSE
        RETURN ([0.0, 0.0, ra]);
      END_IF;
    ELSE
      -- axis parallel to z-axis - use z-axis as yaw axis
      IF (dz > 0.0) THEN
        RETURN ([ya, 0.0, 0.0]);
      ELSE
        RETURN ([ra, 0.0, 0.0]);
      END_IF;
    END_IF;
  END_IF;

  -- axis parallel to y-axis - use y-axis as pitch axis
  IF ((dy <> 0.0) AND (dx = 0.0) AND (dz = 0.0)) THEN
    IF (c_a >= 0.0) THEN
      ya := 0.0;
      ra := 0.0;
    ELSE
      ya := ucf * PI;
      ra := ya;
    END_IF;

    pa := ucf * ATAN (s_a, ABS (c_a));
    IF (dy < 0.0) THEN
      pa := - pa;
    END_IF;

    RETURN ([ya, pa, ra]);
  END_IF;

  -- axis not parallel to any axis of coordinate system
  -- compute rotation matrix

  cm1 := 1.0 - c_a;

  rotmat := [ [ dx * dx * cm1 + c_a,
                dx * dy * cm1 - dz * s_a,
                dx * dz * cm1 + dy * s_a ],
              [ dx * dy * cm1 + dz * s_a,
                dy * dy * cm1 + c_a,
                dy * dz * cm1 - dx * s_a ],
              [ dx * dz * cm1 - dy * s_a,
                dy * dz * cm1 + dx * s_a,
                dz * dz * cm1 + c_a ] ];

  -- rotmat[1][3] equals SIN (pitch_angle)
  IF (ABS (rotmat[1][3]) = 1.0) THEN
    -- |pa| = PI/2
    BEGIN
      IF (rotmat[1][3] = 1.0) THEN
        pa := 0.5 * PI;
      ELSE
        pa := -0.5 * PI;
      END_IF;

      -- In this case, only the sum or difference of roll and yaw angles
      -- is relevant and can be evaluated from the matrix.
      -- According to IP `rectangular pitch angle' for ypr_rotation,
      -- the roll angle is set to zero.

      ra := 0.0;
      ya := ATAN (rotmat[2][1], rotmat[2][2]);

      -- result of ATAN is in the range [-PI/2, PI/2].
      -- Here all four quadrants are needed.

      IF (rotmat[2][2] < 0.0) THEN
        IF ya <= 0.0 THEN
          ya := ya + PI;
        ELSE
          ya := ya - PI;
        END_IF;
      END_IF;
    END;
  ELSE
    -- COS (pitch_angle) not equal to zero
    BEGIN
      ya := ATAN (- rotmat[1][2], rotmat[1][1]);

      IF (rotmat[1][1] < 0.0) THEN
        IF (ya <= 0.0) THEN
          ya := ya + PI;
        ELSE
          ya := ya - PI;
        END_IF;
      END_IF;

      ra := ATAN (-rotmat[2][3], rotmat[3][3]);

      IF (rotmat[3][3] < 0.0) THEN
        IF (ra <= 0.0) THEN
          ra := ra + PI;
        ELSE
          ra := ra - PI;
        END_IF;
      END_IF;

      s_y := SIN (ya);
      c_y := COS (ya);
      s_r := SIN (ra);
      c_r := COS (ra);

      IF ((ABS (s_y) > ABS (c_y)) AND
          (ABS (s_y) > ABS (s_r)) AND
          (ABS (s_y) > ABS (c_r))) THEN
        cm1 := - rotmat[1][2] / s_y;
      ELSE
        IF ((ABS (c_y) > ABS (s_r)) AND (ABS (c_y) > ABS (c_r))) THEN
          cm1 := rotmat[1][1] / c_y;
        ELSE
          IF (ABS (s_r) > ABS (c_r)) THEN
            cm1 := - rotmat[2][3] / s_r;
          ELSE
            cm1 := rotmat[3][3] / c_r;
          END_IF;
        END_IF;
      END_IF;

      pa := ATAN (rotmat[1][3], cm1);

    END;
  END_IF;

  ya := ya * ucf;
  pa := pa * ucf;
  ra := ra * ucf;

  RETURN ([ya, pa, ra]);

END_FUNCTION;

FUNCTION assign_directed_link
   (joint : kinematic_joint; orientation : BOOLEAN ) : kinematic_link;
   IF (orientation) THEN
      RETURN (joint.first_link);
   ELSE
      RETURN (joint.second_link);
   END_IF;
END_FUNCTION;

FUNCTION connected_in_simple_loop
   (relation_1 : joint_logical_relationship;
    relation_2 : joint_logical_relationship) : BOOLEAN;
  LOCAL
    next_jlr_in_loop_set : SET [1:?] OF joint_logical_relationship;
  END_LOCAL;

  IF ((relation_1.loop:<>: relation_2.loop) OR
       (relation_1 :=: relation_2)) THEN
         RETURN (FALSE);
  ELSE
    IF (relation_1.next_joint_logical_structure :=:
        relation_2.previous_joint_logical_structure) THEN
       RETURN (TRUE);
    ELSE
      next_jlr_in_loop_set :=
          QUERY (relation <* 
           bag_to_set (USEDIN (relation_1.next_joint_logical_structure,
           'KINEMATIC_STRUCTURE_SCHEMA.JOINT_LOGICAL_RELATIONSHIP.' +
           'PREVIOUS_JOINT_LOGICAL_STRUCTURE'))
            | relation.loop :=: relation_1.loop);
      IF (SIZEOF(next_jlr_in_loop_set) <> 1) THEN
         RETURN (FALSE);
      ELSE
         RETURN (connected_in_simple_loop(next_jlr_in_loop_set [1],
                 relation_2));
      END_IF;
    END_IF;
  END_IF;
END_FUNCTION;

END_SCHEMA;         -- end kinematic_structure_schema
