Document with mathematical notation the geometric model used by Blender

[traversaro]

In https://github.com/robotology/blender-robotics-utils/blob/master/script/urdfToBlender.py we have a URDF --> Blender rig converter that is working fine on some URDF model, but creating problems on some other models. However, debugging the problem is tricky as we do not have a clear mathematical definition and/or understanding of how the Blender Armature system ( https://docs.blender.org/manual/en/latest/animation/armatures/structure.html#chains-of-bones ) works. In this issue we will document this with mathematical notation, so it will simplify future mantainance of the software.

Unless noted otherwise, the notation used for expressing homegenous transforms is the one defined in https://research.tue.nl/en/publications/multibody-dynamics-notation-version-2, and as LaTeX formulas will be used in the issue, please install an appropriate plugin such as https://chrome.google.com/webstore/detail/mathjax-plugin-for-github/ioemnmodlmafdkllaclgeombjnmnbima?hl=en for Chrome or https://addons.mozilla.org/en-US/firefox/addon/green-pi/ for Firefox to visualize them in the browser.

cc @Nicogene

[traversaro]

Recap URDF Formalism

This is based on http://wiki.ros.org/urdf/XML/model and some well established URDF implementation such as https://github.com/ros/kdl_parser or https://github.com/ignitionrobotics/sdformat/blob/sdformat11_11.2.2/src/parser_urdf.cc .

Links

In URDF, each link is associated with a frame rigidly connected to it. Each link is defined with a <link> XML tag: <link>...</link> . In the rest of the recap, we will indicate links names with capital letters such as $P$, $C$, $L$ or $B$, even if in practice tipically underscore separated names such as l_hand are used.

Fixed Joints

The relative interconnection between two links, and the relative position between two interconnected links is defined by the joint element. The most simple case of joint is the fixed joint, for example:

 <joint name="JF" type="fixed">
    <origin xyz="0 0 1" rpy="0 0 3.1416"/>
    <parent link="P"/>
    <child link="C"/>
 </joint>

Relation between Origin Element and Homogeneous Transform

In this case, the relative position between the frame rigidly connected to the links ${}^E H_L$ is constant, and can be obtained directly from the origin element <origin xyz="0 0 1" rpy="0 0 3.1416"/> . In particular, the conversion between the two 3D vector for translation and rotation of the origin element and the 4x4 homogenous matrix is:

$$ {}^P H_C(x, y, z, r, p, y) = \begin{bmatrix} R(r, p, y) & xyz \\\ 0_{3 \times 1} & 1 \end{bmatrix} \in \mathbb{R}^{4 \times 4} $$

where
$$
xyz =
\begin{bmatrix}
x \\
y \\
z
\end{bmatrix}
\in \mathbb{R}^{3 \times 1}
$$
and
$$
R(r, p, y) =
\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}
\in \mathbb{R}^{3 \times 3}
$$

Unfortunatly this information is not documented in the URDF format specification, but an accurate documentation can be found in SDF <pose> documentation, that has a similar semantics: http://sdformat.org/tutorials?tut=specify_pose&ver=1.4&cat=specification& . The iDynTree's Rotation::RPY method is implementing exactly this mapping, so you can use it whenever you need to parse URDF files. The exact same convention is used to express Euler Angles in the OPC UA standard OPC 10000-5 / OPC 10001-11 Annex G, see https://user-images.githubusercontent.com/1857049/130938396-83429310-d514-4c09-b0c6-7f77ad051854.PNG .

Revolute Joints

When representing robots, the most commont type of joint is the revolute joint, that constrains the relative rotation between two links to be a rotation around a given axis. This is tipically represented by an element such:

 <joint name="J" type="revolute">
    <origin xyz="0 0 1" rpy="0 0 3.1416"/>
    <parent link="P"/>
    <child link="C"/>
    <axis xyz="1.0 0.0 0.0"/>
    [...]
 </joint>

Other elements are required (tipically limits and friction information) but for the sake of this discussion we can disregard them. For the fixed joints, we saw earlier that the homogenous transform ${}^E H_L$ is completly defined by the information contained in the origin tag. In the case of revolute joints, ${}^E H_L$ depends on both the origin element data, and on two additional piece of info:

• The direction of the axis around which the joint is rotating, defined by the axis element, that we indicated with $a \in \mathbb{R}^3$, with $|a| = 1$.
• The so-called joint position, that is a scalar $q_{J} \in \mathbb{R}$ that concisely describes the relative position between ${}^E H_L$, considering the joint constraint

In particular, the formula that relates this info is:
$$
{}^P H_C(xyz, rpy, a, q_J) =
{}^P H_{C_{0}} (xyz, rpy) {}^{C_{0}} H_C(a, q_J)
$$

The transform can be decomposed in two transform that combined give the complete ${}^P H_C$ transform:

• The first one is ${}^P H_{C_{0}} (xyz, rpy)$, that is exactly the value of ${}^P H_C$ when $q_J = 0$, and can be computed from the origin tag exactly as in the case of the fixed joint
• The second one is ${}^{C_{0}} H_C(a, q_J)$, that describes the effect of the joint position, and is defined as:
  $$
  {}^{C_{0}} H_C (a, q_J)
  =
  \begin{bmatrix}
  \mathbf{I} + \sin(q_J) k \times + (1-\cos(q_J)) (k \times)^2 & 0_{3 \times 1} \\
  0_{1 \times 3} & 1
  \end{bmatrix}
  $$

Where the rotation part is obtained via the https://en.wikipedia.org/wiki/Rodrigues%27_rotation_formula . Note that the translation part of this transform is always $0_{3 \times 1}$ this simplifies this computation, but is the reason why URDF has the annoying constraint that the child link frame must be located on the joint axis in the case the parent and the child link are connected by a revolute joint.

Forward Kinematics

To compute the forward kinematics between the base link $B$ and a link $L$ in a URDF file, once you know how to compute the transform between neighbor links, you can use this information to compute also the transform ${}^B H_L$ . In particular, if we indicate with $\lambda_B (L)$ the parent of $L$ considering $B$, and we defined as $\lambda_B^2 (L) = \lambda_B(\lambda_B(L)$ its grandparerent, then ${}^B H_L$ can be computed as:
$$
{}^B H_L = {}^B H_{\lambda^{d-1}_B(L)} {}^{\lambda^{d-1}_B(L)} H_{\lambda^{d-2}_B(L)} \dots {}^{\lambda_B(L)} H_L
$$
where $d$ is the distance in joints between $B$ and $L$, i.e. $d \in \mathbb{N} \ s.t. \ \lambda^{d}_B(L) = B$ (usually called the depth of L considering $B$ as the base).

Obvioustly, for computing each joint transform it is necessary to have all the necessary information, in particular for revolute joints the joint position, that tipically can change at runtime, differently from the rest of the parameters. This is usually called forward kinematics, and is the algorithm executed by tools such as ROS's robot-state-publisher or idyntree-yarp-tools' yarprobotstatepublisher. In their case, the joint positions are read from the /joint_states topic, while all other information are read from the URDF file.

[traversaro]

cc @xEnVrE

[fbottarel]

@traversaro just stumbled upon this while chatting with @xEnVrE.

I cannot render your math the way you have formatted it in this issue. E.g.

$$ xyz = \begin{bmatrix} x \\ y \\ z \end{bmatrix} \in \mathbb{R}^{3 \times 1} $$

does not work in my Chrome+xdoc setup, while

$$xyz = \begin{bmatrix} x \\\ y \\\ z \end{bmatrix} \in \mathbb{R}^{3 \times 1}$$

does. Maybe you also happen to have MathJax installed, and that's actually doing the math rendering for you instead of xdoc?

[traversaro]

Indeed I am rendering with MathJax, but in my previous test the two where quite compatible, let me check with Chrome.

[fbottarel]

xdoc documentation states

### LaTeX
Use inline and display math like
````markdown
Display math:
```math
e^{i\pi} + 1 = 0
```
and line math $`a^2 + b^2 = c^2`$.
````

I noticed because I am not using MathJax anymore, and your examples were not rendering. 👀

[traversaro]

Not sure if any xdoc user has any input on this? @pattacini

[pattacini]

That's normal as xdoc makes use of a completely different notation – which I deem superior – so it's unable to interpret the double-dollar escape sequence.

Nevertheless, I can see everything rendered correctly as I've installed both xdoc and MathJax 3 Plugin for Github Chrome plugins.

[traversaro]

That's normal as xdoc makes use of a completely different notation – which I deem superior – so it's unable to interpret the double-dollar escape sequence.

Nevertheless, I can see everything rendered correctly as I've installed both xdoc and MathJax 3 Plugin for Github Chrome plugins.

Ack, I think I misunderstood the xdoc features then. I updated the links to avoid mentioning xdoc .

[pattacini]

@diegoferigo even purple-pi is now discontinued in favor of xdoc: see https://github.com/nschloe/purple-pi/blob/main/README.md?plain=1#L1.

[traversaro]

@fbottarel feel free to let me know if using green-pi/purple-pi or mathjax is working fine for you or if you still have problems.

[traversaro]

In today's meeting, we sorted out most of the doubts, and I will provide a recap soon. The only doubt was the semantics of the roll w.r.t. to how the X and Z direction of the bone are oriented, but w.r.t. to that the bpy.types.Bone.MatrixFromAxisRoll method (see https://docs.blender.org/api/current/bpy.types.Bone.html?highlight=bone#bpy.types.Bone.MatrixFromAxisRoll) should provide all the information. By digging in the code, the underlying implementation of that method seems to be https://github.com/blender/blender/blob/594f47ecd2d5367ca936cf6fc6ec8168c2b360d0/source/blender/blenkernel/intern/armature.c#L2155 .

[traversaro]

In today's meeting, we sorted out most of the doubts, and I will provide a recap soon. The only doubt was the semantics of the roll w.r.t. to how the X and Z direction of the bone are oriented, but w.r.t. to that the bpy.types.Bone.MatrixFromAxisRoll method (see https://docs.blender.org/api/current/bpy.types.Bone.html?highlight=bone#bpy.types.Bone.MatrixFromAxisRoll) should provide all the information. By digging in the code, the underlying implementation of that method seems to be https://github.com/blender/blender/blob/594f47ecd2d5367ca936cf6fc6ec8168c2b360d0/source/blender/blenkernel/intern/armature.c#L2155 .

Related SO questions:

• https://blender.stackexchange.com/questions/165742/using-python-how-do-you-get-the-roll-of-a-pose-bone
• https://blender.stackexchange.com/questions/222968/how-is-the-bone-matrix-axis-orientation-roll-defined

[traversaro]

Recap Blender Bone Formalism

This is based on https://docs.blender.org/manual/en/latest/animation/armatures/introduction.html and the meeting that we had with @Nicogene and @xEnVrE .

In Blender, the basic unit of the kinematic structure is the bone, that is a rough equivalent of a link in URDF.

Each bone rest position is defined by three parameters:

• ${}^W h \in \mathbb{R}^3$ : the head, a point in the 3D space expressed in the WORLD reference frame
• ${}^W t \in \mathbb{R}^3$ : the tail, a point in the 3D space expressed in the WORLD reference frame.
• $r \in \mathbb{R}$: the roll

To each bone it correspond a frame, whose origin is centered in the head, whose Y direction is specified by the vector ${}^W t - {}^W h$ (and this is the reason why the head and tail cannot be coincident) and the roll specify in some way the X and Z direction in the plane ortoghonal to Y (with the logic implemented in https://github.com/blender/blender/blob/594f47ecd2d5367ca936cf6fc6ec8168c2b360d0/source/blender/blenkernel/intern/armature.c#L2155, that however I did not investigated).

Something that is work stressing is that the position of the tail does not play any role in defining the position of any child of the bone. The rest position of any bone is always expessed with head, tail and roll w.r.t. to the world frame of simulation, and the tail position of its parent is not related at all to it.

So, let's say that that we have two bones, $P$ (parent) and $C$ (child). Their rest positions in the world frame $W$ are defined as:
$$
{}^W H_{P_0} = H({}^W h_P, {}^W t_P, r_P)
$$

$$ {}^W H_{C_0} = H({}^W h_C, {}^W t_C, r_C) $$

The rotational motions that can be constrainted for a child bone w.r.t. to its parent bone are the main one of the axis aligned with the child bone: so if we want to permit a rotational motion around an arbitrary axis, we need to make sure that one direction of the bone frame is aligned with the axis. The converter in blender-robotics-utils ensures that by always choosing the head and tail of the bone to be aligned with the axis specified in the URDF. So assuming that you have a model that only allows a rotation around the Y direction (as the one generated by the converter), the relative transformation between P and C is given by:
$$
{}^P H_{C} = {}^{P_0} H_{C_0} AxisAngle( {}^{C_0} R_W normalize({}^W t_C-{}^W h_C), q_J)
$$

where $ {}^{P_0} H_{C_0}$ is computed as:
$$
{}^{P_0} H_{C_0} = {}^W H_{P_0}^{-1} {}^W H_{C_0}
$$

[traversaro]

I personally understood much more after our discussion. It would be nice to introduce some of this formalism in the docs, but if we want to do this we just need to get it from here, so I think we can close the issue now.

[Nicogene]

It would be nice to introduce some of this formalism in the docs

I completely agree with that, we should find a way to publish it without requiring addons to be installed in the browser if possible