| Integrated application resource: Kinematics | ISO 10303-105:2019(E) © ISO |
The subject of the kinematic_state_schema is the description of a kinematic mechanism in a particular state, that is defined by applying actual pair values to each kinematic pair within the kinematic mechanism.
This clause defines the information requirements to which implementations shall conform using the EXPRESS language as defined in ISO 10303-11. The following EXPRESS declaration begins the kinematic_state_schema and identifies the necessary external references.Short names of entities defined in this schema are described in Annex A. Unambiguous identification of this schema is defined in Annex B.
EXPRESS specification:
*)
SCHEMA kinematic_state_schema;
REFERENCE FROM
kinematic_structure_schema;
--
ISO 10303-105
REFERENCE FROM
geometry_schema
--
ISO 10303-42
(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
--
ISO 10303-41
(conversion_based_unit,
global_unit_assigned_context,
length_measure,
plane_angle_measure,
si_prefix,
si_unit,
si_unit_name,
unit);
REFERENCE FROM
representation_schema
--
ISO 10303-43
(functionally_defined_transformation,
item_defined_transformation,
representation,
representation_context,
representation_item,
representation_relationship,
representation_relationship_with_transformation);
(*
NOTE 1 The schemas referenced above are specified in the following parts:
kinematic_structure_schema ISO 10303-105 geometry_schema ISO 10303-42 measure_schema ISO 10303-41 representation_schema ISO 10303-43
NOTE 2 See Annex D for a graphical representation of this schema.
A mechanism_state_representation is the root object to describe a kinematic state. It defines the state for one particular mechanism_representation. There might be zero, one or many mechanism_state_representations for a single mechanism_representation. Motion can be simulated by interpolating through a sequence of kinematic states.
The items of a mechanism_state_representation are pair_values. For each particular sub-subtype of kinematic_pair except for fully_constrained_pair and unconstrained_pair a corresponding subtype of pair_value is defined that provides actual values for all available degrees of freedoms of that pair. These values tight the adjacent links of the pair into a particular relative position to each other.
A particular pair_value can be used by many mechanism_state_representations. This allows implementations to minimise the number of needed pair_values even in situations with many kinematic states and kinematic pairs.
EXPRESS specification:
*)
TYPE
spatial_rotation
=
SELECT
(ypr_rotation,
rotation_about_direction);
END_TYPE;
(*
EXPRESS specification:
*)
TYPE
spherical_pair_select
=
SELECT
(spherical_pair,
spherical_pair_with_pin);
END_TYPE;
(*
NOTE For more detailed information on yaw, pitch, and roll convention see ISO 8855:1991
EXPRESS specification:
*)
TYPE
ypr_enumeration
=
ENUMERATION
OF
(yaw,
pitch,
roll);
END_TYPE;
(*
Enumerated item definitions:
yaw: the angle of rotation about the z-axis;
pitch: the angle of rotation about the y′-axis;
roll: the angle of rotation about the x′-axis.
NOTE An EXPRESS function which returns the rotation matrix corresponding to an input ypr_rotation is given in annex E. This function has been provided for applications that use this part of ISO 10303, but need the rotational transformation to be expressed in terms of a rotation matrix rather than in terms of a sequence of rotation angles.
EXPRESS specification:
*)
TYPE
ypr_rotation
=
ARRAY[ypr_index(yaw):ypr_index(roll)] OF plane_angle_measure;
END_TYPE;
(*
EXPRESS specification:
*)
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;
(*
Attribute definitions:
applies_to_pair: the cylindrical_pair to which the cylindrical_pair_value applies.
actual_translation: the translation value.
actual_rotation: the angle of rotation.
NOTE If the gear pair is part of a more complex gear mechanism, the gear pair values as defined here may not be the primary design targets. In the case of a planetary gear mechanism, for example, it is more important to know the rotation angles of the planetary gears with respect to an inertial coordinate system than the rotation angles between the planetary gears and the frames which support them. See [1] for a detailed description of this specific example.
For a given configuration of pairs within a mechanism, the more global motion parameters can be derived, in general, from the individual pair parameters and vice versa. This is not possible, however, generically without knowledge of the configuration. Therefore, the specification of functions which provide such derivations is left to an application protocol for which these derivations are required.
EXPRESS specification:
*)
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;
(*
Attribute definitions:
applies_to_pair: the gear_pair to which the gear_pair_value applies.
actual_rotation_1: the value of the pair parameter of the first link.
actual_rotation_2: the value of the pair parameter of the second link.
EXPRESS specification:
*)
ENTITY low_order_kinematic_pair_value
SUBTYPE OF (pair_value);
SELF\pair_value.applies_to_pair : low_order_kinematic_pair;
actual_translation_x : length_measure;
actual_translation_y : length_measure;
actual_translation_z : length_measure;
actual_rotation_x : plane_angle_measure;
actual_rotation_y : plane_angle_measure;
actual_rotation_z : plane_angle_measure;
END_ENTITY;
(*
Attribute definitions:
applies_to_pair: the low_order_kinematic_pair to which the low_order_kinematic_pair_value applies.
actual_translation_x: the translation value in x-direction.
actual_translation_y: the translation value in y-direction.
actual_translation_z: the translation value in z-direction.
actual_rotation_x: the rotation value for the yaw angle.
actual_rotation_y: the rotation value for the pitch angle.
actual_rotation_z: the rotation value for the roll angle.
EXPRESS specification:
*)
ENTITY mechanism_state_representation
SUBTYPE OF (representation);
SELF\representation.items : SET[1:?] OF pair_value;
represented_mechanism : mechanism_representation;
DERIVE
SELF\representation.context_of_items : geometric_representation_context := represented_mechanism.context_of_items;
END_ENTITY;
(*
Attribute definitions:
items: an inherited attribute that shall be of type pair_value. There shall exist at least one pair_value for the mechanism_state_representation.
represented_mechanism: specifies the mechanism_representation for which a particular state is represented.
context_of_items: an inherited attribute that shall be of type geometric_representation_context.
EXPRESS specification:
*)
ENTITY pair_value
ABSTRACT SUPERTYPE
OF (ONEOF (sliding_surface_pair_value,
rolling_surface_pair_value,
revolute_pair_value,
prismatic_pair_value,
screw_pair_value,
cylindrical_pair_value,
spherical_pair_value,
sliding_curve_pair_value,
rolling_curve_pair_value,
gear_pair_value,
rack_and_pinion_pair_value,
universal_pair_value,
planar_pair_value,
unconstrained_pair_value,
point_on_surface_pair_value,
point_on_planar_curve_pair_value,
low_order_kinematic_pair_value))
SUBTYPE OF (geometric_representation_item);
applies_to_pair : kinematic_pair;
END_ENTITY;
(*
Attribute definitions:
applies_to_pair: the kinematic_pair to which the pair_value applies.
EXPRESS specification:
*)
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;
(*
Attribute definitions:
applies_to_pair: the planar_pair to which the planar_pair_value applies.
actual_rotation: the value of the angle of rotation for a planar_pair.
actual_translation_x: the value of translation in x-direction for a planar_pair.
actual_translation_y: the value of translation in y-direction for a planar_pair.
EXPRESS specification:
*)
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;
(*
Attribute definitions:
applies_to_pair: the point_on_planar_curve_pair to which the point_on_planar_curve_pair_value applies.
actual_point_on_curve: the positional value for the point on the curve.
input_orientation: the input specification of the rotational pair parameter values from which the actual_orientation is derived. An input_orientation is either a ypr_rotation or a rotation_about_direction.
actual_orientation: the array of rotational pair parameter values for a point_on_planar_curve_pair according to the ypr notation. This is derived from input_orientation by the function convert_spatial_to_ypr_rotation.
Formal propositions:
WR1: The actual_point_on_curve shall be defined as a point on the pair_curve of the point_on_planar_curve_pair referenced by applies_to_pair.
EXPRESS specification:
*)
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;
(*
Attribute definitions:
applies_to_pair: the point_on_surface_pair to which the point_on_surface_pair_value applies.
actual_point_on_surface: the positional value for the point on the surface.
input_orientation: the input specification of the rotational pair parameter values from which the actual_orientation is derived. An input_orientation is either an ypr_rotation or a rotation_about_direction.
actual_orientation: the set of rotational pair parameter values for a point_on_surface_pair according to the ypr notation. This is derived from input_orientation by the function convert_spatial_to_ypr_rotation.
Formal propositions:
WR1: The actual_point_on_surface shall be defined as a point on the pair_surface of the point_on_surface_pair referenced by applies_to_pair.
EXPRESS specification:
*)
ENTITY prismatic_pair_value
SUBTYPE OF (pair_value);
SELF\pair_value.applies_to_pair : prismatic_pair;
actual_translation : length_measure;
END_ENTITY;
(*
Attribute definitions:
applies_to_pair: the prismatic_pair to which the prismatic_pair_value applies.
actual_translation: the value of the pair parameter.
EXPRESS specification:
*)
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 := 0.0;
END_ENTITY;
(*
Attribute definitions:
applies_to_pair: the rack_and_pinion_pair to which the rack_and_pinion_pair_value applies.
actual_displacement: the value of the pair parameter for the rack.
actual_rotation: the resulting rotation of the pinion.
EXPRESS specification:
*)
ENTITY revolute_pair_value
SUBTYPE OF (pair_value);
SELF\pair_value.applies_to_pair : revolute_pair;
actual_rotation : plane_angle_measure;
END_ENTITY;
(*
Attribute definitions:
applies_to_pair: the revolute_pair to which the revolute_pair_value applies.
actual_rotation: the value of the pair parameter.
EXPRESS specification:
*)
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;
(*
Attribute definitions:
applies_to_pair: the rolling_curve_pair to which the rolling_curve_pair_value applies.
actual_point_on_curve_1: the contact point, defined in the parameter space of the curve on the first link.
Formal propositions:
WR1: The actual_point_on_curve_1 shall be defined as a point on curve_1 of the planar_curve_pair referenced by applies_to_pair.
EXPRESS specification:
*)
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;
(*
Attribute definitions:
applies_to_pair: the rolling_surface_pair to which the rolling_surface_pair_value applies.
actual_point_on_surface: the contact point of the contact surface (surface_1) of the first link.
actual_rotation: the angle required to rotate the xcp-direction of the first link surface around the zc-direction of that surface until it coincides with the xc-direction of the second link surface.
Formal propositions:
WR1: The actual_point_on_surface shall be defined as a point on surface_1 of the surface_pair referenced by applies_to_pair.
EXPRESS specification:
*)
ENTITY rotation_about_direction
SUBTYPE OF (geometric_representation_item);
direction_of_axis : direction;
rotation_angle : plane_angle_measure;
WHERE
WR1: SIZEOF (direction_of_axis.direction_ratios) = 3;
END_ENTITY;
(*
Attribute definitions:
direction_of_axis: the three-dimensional direction of the axis of rotation.
rotation_angle: the angle of rotation about the axis of rotation.
Formal propositions:
WR1: The direction_of_axis shall be a three-dimensional direction.
EXPRESS specification:
*)
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;
(*
Attribute definitions:
applies_to_pair: the screw_pair to which the screw_pair_value applies.
actual_rotation: the rotation angle of the screw_pair.
actual_translation: the resulting translation.
EXPRESS specification:
*)
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;
(*
Attribute definitions:
applies_to_pair: the sliding_curve_pair to which the sliding_curve_pair_value applies.
actual_point_on_curve_1: the contact point, defined in the parameter space of the curve on the first link.
actual_point_on_curve_2: the contact point, defined in the parameter space of the curve on the second link.
Formal propositions:
WR1: The actual_point_on_curve_1 shall be defined as a point on curve_1 of the planar_curve_pair referenced by applies_to_pair.
WR2: The actual_point_on_curve_2 shall be defined as a point on curve_2 of the planar_curve_pair referenced by applies_to_pair.
EXPRESS specification:
*)
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;
(*
Attribute definitions:
applies_to_pair: the sliding_surface_pair to which the sliding_surface_pair_value applies.
actual_point_on_surface_1: the contact point of the contact surface (surface_1) of the first link.
actual_point_on_surface_2: the contact point of the contact surface (surface_2) of the second link.
actual_rotation: the angle required to rotate the xcp-direction of the first link surface around the zc-direction of that surface until it coincides with the xc-direction of the second link surface.
Formal propositions:
WR1: The actual_point_on_surface_1 shall be defined as a point on surface_1 of the surface_pair referenced by applies_to_pair.
WR2: The actual_point_on_surface_2 shall be defined as a point on surface_2 of the surface_pair referenced by applies_to_pair.
EXPRESS specification:
*)
ENTITY spherical_pair_value
SUBTYPE OF (pair_value);
SELF\pair_value.applies_to_pair : spherical_pair_select;
input_orientation : spatial_rotation;
DERIVE
actual_orientation : ypr_rotation := convert_spatial_to_ypr_rotation
(SELF\pair_value.applies_to_pair,
input_orientation);
END_ENTITY;
(*
Attribute definitions:
applies_to_pair: the spherical_pair to which the spherical_pair_value applies.
input_orientation: the input specification of the pair parameter values from which the actual_orientation is derived. An input_orientation is either an ypr_rotation or a rotation_about_direction.
actual_orientation: the set of pair parameter values for a spherical_pair according to the ypr notation. This is derived from input_orientation by the function convert_spatial_to_ypr_rotation.
EXPRESS specification:
*)
ENTITY unconstrained_pair_value
SUBTYPE OF (pair_value);
SELF\pair_value.applies_to_pair : unconstrained_pair;
actual_placement : axis2_placement_3d;
END_ENTITY;
(*
Attribute definitions:
applies_to_pair: the unconstrained_pair to which the unconstrained_pair_value applies.
actual_placement: the placement of the second pair frame with respect to the first pair frame for the unconstrained_pair.
EXPRESS specification:
*)
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;
(*
Attribute definitions:
applies_to_pair: the universal_pair to which the universal_pair_value applies.
first_rotation_angle: the angle of rotation around the first axis.
second_rotation_angle: the angle of rotation around the second axis.
The kss_geometric_representation_item_subtypes constraint specifies a constraint that applies to instances of subtypes of geometric_representation_item.
A kss_geometric_representation_item_subtypes is a constraint that specifies the subtypes rotation_about_direction and su_parameters of geometric_representation_item are mutually exclusive.EXPRESS specification:
*)
SUBTYPE_CONSTRAINT kss_geometric_representation_item_subtypes FOR geometric_representation_item;
ONEOF (rotation_about_direction,
su_parameters);
END_SUBTYPE_CONSTRAINT;
(*
If rotation is already of type ypr_rotation, it is returned immediately. Otherwise, convert_spatial_to_ypr_rotation returns either an ypr_rotation that is equivalent to the rotation_about_direction which has been used as the rotation argument, or an indeterminate value (?), if the unit evaluation has not been successful. The type of the function is ypr_rotation.
EXPRESS specification:
*)
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 NOT('MEASURE_SCHEMA.PLANE_ANGLE_MEASURE' IN TYPEOF(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;
(*
Argument definitions:
pair: the kinematic_pair the context of whose is to be evaluated with regard to the plane_angle_unit.
rotation: the spatial_rotation that is to be converted by the function.
EXPRESS specification:
*)
FUNCTION ypr_index (ypr : ypr_enumeration) : INTEGER;
CASE ypr OF
yaw : RETURN (1);
pitch : RETURN (2);
roll : RETURN (3);
END_CASE;
RETURN (?);
END_FUNCTION;
(*
Argument definitions:
ypr: one axis of rotation as specified by the ypr_enumeration.
*)
END_SCHEMA; -- kinematic_state_schema
(*
© ISO 2019 — All rights reserved