Specifying pose


Specifying pose in SDFormat

A fundamental tool for robot modeling is the ability to concisely and intuitively express the relative position and orientation of model components in 3-D.

The SDFormat specification has the <pose/> element which accepts 6 numbers in total:

<pose>x y z roll pitch yaw</pose>

The elements x y z specify the position vector (in meters), and the elements roll pitch yaw are Euler angles (in radians) that specify the orientation, which can be computed by an extrinsic X-Y-Z rotation as shown by the following equation and figure:

$$ R_{rpy} = \begin{bmatrix} \cos(y) & -\sin(y) & 0 \\ \sin(y) & \cos(y) & 0 \\ 0 & 0 & 1 \end{bmatrix} * \begin{bmatrix} \cos(p) & 0 & \sin(p) \\ 0 & 1 & 0 \\ -\sin(p) & 0 & \cos(p) \end{bmatrix} * \begin{bmatrix} 1 & 0 & 0 \\ 0 & \cos(r) & -\sin(r) \\ 0 & \sin(r) & \cos(r) \end{bmatrix} $$

See Also

For a command-line utility to convert between roll-pitch-yaw angles, quaternions, and rotation matrices, please see the quaternion_from_euler and quaternion_to_euler example programs in ignition math.

Software implementations for converting between this Euler angle convention and quaternions can be found in ignition::math::Quaternion C++ class, and the urdf::Rotation C++ class.

Some other implementations: