日本ソフトウェア科学会第 33 回大会 (2016 年度) 講演論文集

Formal Foundations for Rigid Body
Transformation
Reynald Aﬀeldt

Cyril Cohen

We are interested in the formal specification of safety properties of robot manipulators. To this end,
we have been developing a formalization of rigid body transformations in the Coq proof-assistant using theMathematical Components library. This paper provides an overview of the current status of our formalization. We start by providing theories for angles and for three-dimensional Euclidean geometry, including
the cross-product. We use these theories to formalize the basic geometry required for robotics. First, weformalize a comprehensive theory of three-dimensional rotations, including exponentials of skew-symmetricmatrices and quaternions. Then, we provide a formalization of rigid body transformations in terms of isometries and of their homogeneous representation. Finally, we formalize the basics of screw motions, including
Chasles’ theorem. Using these ingredients, it should now be possible to tackle the formalization of open
chains for serial robots.

1 Towards a Formal Theory of Robotics
Our ultimate goal is the formal verification ofsafety properties of robots. Robots have alreadymade their way in manufacturing plants in the formof robot manipulators on assembly lines. Even
though industrial robots operate in a controlled environment, their safety is already subject to several standards (e.g., ISO 10218). Safety concerns
will increase furthermore now that robots are considered for life-critical missions such as rescue andhealth care. It is therefore questionable whether
testing can achieve a satisfactory level of safety. For
this reason, formal methods are now being considered to improve the rigor of the safety analysis ofrobots. Most experiments carried so far have beenusing model-checking, see Sect. 10 for experiments
This is an unrefereed paper. Copyrights belong to the
Author(s).
剛体変換のための形式基礎
Reynald Aﬀeldt, 独立行政法人産業技術総合研究所 情報技術研究部門, Information Technology ResearchInstitute, National Institute of Advanced Industrial
Science and Technology.
Cyril Cohen, フ ラ ン ス 国 立 情 報 学 自 動 制 御 研 究 所
MARELLE プロジェクト・チーム, MARELLE projectteam, INRIA Sophia Antipolis.

using proof-assistants.
In this paper, we focus on the formal verificationof the theoretical foundations of robot manipulators. Robot manipulators are an interesting target because they are already pervasive in the industry and because advanced robots such as humanoid robots can be understood as made of several robot manipulators. The theoretical foundations of robot manipulators essentially amount tothree-dimensional geometry. This suggests the useof a proof-assistant to perform formal verification.
Indeed, proof-assistants excel at the symbolic manipulation of formal algebra, in which geometry canbe represented. We use the Coq proof-assistant [3]and the Mathematical Components library. The
latter is a library for formal algebra that was developed to formalize the odd order theorem, an important theorem in group theory [4]. This libraryfeatures in particular a formalization of linear algebra that we use as a starting point.
Let us give a concrete idea of the kind of threedimensional geometry robotics is dealing with. A
robot manipulator consists of (rigid) links connected by joints. It is modeled by the relative
positioning of frames attached to its links. Figure 1 is a simple example of rigid body transformation. It consists of two rigid bodies connected

z

z
z
A

B

y

y

x

x
x

C

y

2 Angles and Trigonometric Functions

φ
ℓ2

ℓ1
simpleFig. 1:A
rigidtion [6] (Examples 2.1 and 2.6)

bodytransforma-

by a joint that can only rotate around a verticalaxis (it is a revolute joint). If we denote by φthe angle of the rotation, then the orientation offrame B w.r.t. A
can be expressedby a rotation[ cos]
φ sin φ 0
matrix A RB = − sin φ cos φ 0 . The coordinate of00

ize rigid body transformations: equivalence withdirect isometries in Sect. 7, homogeneous representations in Sect. 8, and screw motions in Sect. 9. We
comment on related work in Sect. 10 and conclude
in Sect. 11.

1

the origin of the frame B w.r.t. A is A PB = [0; ℓ1 ; 0].
It is customary to represent rigid body transformations using the[ so-called] homogeneous representaA
B 0tion, which is AR(row-vector convention) inPB 1
the case of Fig. 1.
The main contribution of this paper is to provide the various formal definitions and lemmas thatserve as the theoretical foundations of the theoryof robot manipulators. Concretely, we have formalized most aspects of rigid body transformations. Basic geometric elements (points, vectors,etc.) can be defined right away using the linear algebra from the Mathematical Components
library. Yet, we lack basic support for reasoning
about rotations (most notably, angles), in particular in three dimensions (in particular, we need
the cross-product). In fact, there are many theories that are put at work to deal with rotations
in robotics: rotation matrices, exponential coordinates, quaternions, etc. Rigid body transformations form a dedicated theory with various alternative representations such as isometries and screw
motions. Using all the theories above, it becomes
possible to model formally robot manipulators andmore generally open chains for serial robots.
This paper is organized as follows. We startwith background material: angles and trigonometric functions in Sect. 2 and basic elements of threedimensional geometry in Sect. 3. Then, we provide
an exhaustive formalization of rotations: formal
definitions in Sect. 4, exponential coordinates in
Sect. 5, and quaternions in Sect. 6. Last, we formal-

This section provides an overview of a formalization of angles and trigonometric functions that
we have been developing in particular to formalize
rotations (see Sect. 4 for example).
The basic idea to formalize angles and trigonometric functions is to use the complex numbers of
the Mathematical Components library.
2. 1 Angles
Angles are defined using complex numbers of unit
norm. In Mathematical Components, the type ofcomplex numbers is R[i], where R is a real closed
field (rcfType). An angle is defined as a complex
number with norm 1:
Variable R : rcfType.
Record angle := Angle {expi : R[i];_ : `| expi | == 1 }.

In particular, the argument of a complex number
defines an angle:
Definition arg (x : R[i]) : angle :=insubd angle0 (x / `| x |).

(angle0 is the angle corresponding to the complex
number 0; insubd is a Mathematical Components
construct that projects a type into a subtype witha default proof.)
We need to equip above definitions to deal withscaling of angles (e.g., double-angles in Sect. 6. 2)
and half-angles (e.g., to prove etwist_is_onto_SE in
Sect. 9. 2). Specific angles are defined as the arguments of specific complex numbers. For example,
we define π as the argument of −1:
Definition pi := arg (-1).

2. 2 Trigonometric Functions
Table 1 displays the most important trigonometric functions of our formalization. The functions
cos and sin are defined using the real and imaginary parts of the complex numbers that define angles. Other trigonometric functions are formalizedusing their definition in terms of complex numbers.

functioncos(a)
sin(a)
arcsin(x)arccos(x)arctan(x)Table 1: Main trigonometric functions used in this paper
formalization in Coq
pencil-and-paper definition
Re (expi a)
Im (expi a)arg (Num.sqrt (1 -xˆ2) +i* x)
arg (x +i* Num.sqrt (1 -xˆ2))
if x == 0 then 0 else arg ((xˆ-1 +i* 1) *˜ sgz (x))

For example, we use arccos in Sect. 5. 3 to define
the angle-axis representation of a rotation matrix,
arctan in Sect. 6. 2 to define rotations using quaternions, and cotangent in Sect. 9. 2 to prove the surjectivity of the exponential of twists.
We equip this formalization of trigonometricfunctions with various lemmas, in particular thecancellation laws between trigonometric functions
and their inverse, and many trigonometric identities (Pythagorean identity, double-angle and halfangle formulas, etc.).

3 Basics of Three-dimensional Geometry
In this section, we introduce the basic elementsof our formalization of three-dimensional geometry.
3. 1 Vectors
Vectors are provided by the Mathematical Components library in the form of a type denoted by'rV[R]_n whose elements are row-vectors of length nwith components of type R.
We denote by 'e_0, 'e_1, and 'e_2 the vectors of
the canonical basis. Concretely, 'e_k is formalized
by the row-vector [δ0k ; δ1k ; δ2k ], where δ is Kronecker’s.
We denote by u ∗d v the dot-product of the vectors u and v. It is formally defined by the only
component of the 1 × 1 matrix u ∗m vˆT, where ∗m
is matrix multiplication and ˆT is matrix transpose.
The norm of a vector u is defined using the squareroot of the dot-product ⟨u · u⟩:
Definition norm u := Num.sqrt (u ∗d u).

3. 2 The Cross-ProductThe cross-product of two vectors u and v can be
defined using determinants:

u×v =

√
arg( 1 − x2 + xi)√
arg(x + i 1 − x2 )arg(sgn(x)( x1 + i)) (0 if x = 0)1 0 0
u0 u1 u2v0 v1 v2

e_0 +

'

0 1 0
u0 u1 u2v0 v1 v2

+

'

e_1

0 0 1
u0 u1 u2v0 v1 v2e_2'

We use of the determinant \det of the Mathematical Components library to formalize the above
formula:Definition crossmul u v :=\row_(k < 3) \det (col_mx3 'e_k u v).
e_i are the vectors of the canonical basis introduced in the previous section (Sect. 3. 1). col_mx3
is the concatenation of three row-vectors into a 3×3
matrix. Hereafter, we denote crossmul u v by u ∗v v.
We equip this formalization of the cross-productwith the expected standard lemmas. For example, the double cross-product is a technical but
useful lemma about the cross-product and the dotproduct (used for example in the proof of rodriguesPin Sect. 5. 2 and to deal with quaternions in Sect. 6):'

Lemma double_crossmul u v w :u ∗v (v ∗v w) = (u ∗d w) *: v -(u ∗d v) *: w.

3. 3 Frames
As highlighted in the introduction, frames are important to model robot manipulators.
We define a non-oriented frame by three unit vectors i, j, and k that are pairwise orthogonal:
Record t := mk {i : 'rV[R]_3 ; j : 'rV[R]_3 ; k : 'rV[R]_3 ;_ : norm i = 1 ; _ : norm j = 1 ; _ : norm k = 1 ;_ : i ∗d j = 0 ; _ : j ∗d k = 0 ; _ : i ∗d k = 0 }.

The sign of a non-oriented frame is defined asi ∗d (j ∗v k) and can only be 1 or −1. When it is 1,
we talk about a positive (or right-handed) frame.
We will need to build positive frames given one
non-zero vector (for example to specify rotations in
Sect. 4. 1). For this purpose, we provide a module
Base that features in particular two functions j andk such that, given a unit vector i, the vectors i,
j i, and k i form a positive frame. Concretely, thevv − ⟨u · v⟩u⟨u · v⟩u

Fig. 2: The normal component of v w.r.t. ufunction j is defined as follows1 :Definition j i :=if colinear i 'e_0 then 'e_1
else normalize (normalcomp 'e_0 i).

If i is colinear with 'e_0 then the desired frame canbe built using the vectors of the canonical basis.
Otherwise, we compute the normal component of'e_0 w.r.t. i (see Fig. 2) and we normalize to a unit
vector. As for the function k i, it is simply definedby the cross-product of i and j i: this guarantees
that the frame is positive.

4 Rotations and Rotation Matrices
In this section, we formally define threedimensional rotations and show the equivalencewith rotation matrices.
4. 1 Definition of Rotations
A rotation of angle a around a vector u is alinear function f such that f (u) = u (in otherwords, the axis is invariant by rotation), f (j) =cos(a)j + sin(a)k, and f (k) = − sin(a)j + cos(a)k,uwhere ||u||, j, and k form a positive frame.
To build the positive frame from vector u we
use the functions Base.j and Base.k explained in
Sect. 3. 3. The formal specification of a rotation
takes the following form:
CoInductive is_around_axis u a(f : {linear 'rV_3 →'rV_3}) : Prop :=mkIsAroundAxis of
fu=u&
let: j := Base.j u in let: k := Base.k u in
f j = cos a *: j + sin a *: k &let: j := Base.j u in let: k := Base.k u in
f k = -sin a *: j + cos a *: k.



1 The functions colinear, normalize, and normalcomp
are utility functions from our formalization. The
naming should be self-explanatory.

4. 2 Rotations Matrices
A matrix M such that M M T = 1 is orthogonal.
It is a rotation matrix when moreover det(M ) = 1.
Let us denote by 'O[R]_n the set of orthogonal matrices and by 'SO[R]_n the set of rotation matrices.
We show that any rotation in the sense ofSect. 4. 1 can indeed by represented by a rotation
matrix:(* assume u != 0 *)Lemma is_around_axis_SO a f :is_around_axis u a f →lin1_mx f \is 'SO[R]_3.

(lin1_mx f is the matrix corresponding to the linear
application f.) The idea of the proof is to express
lin1_mx f as a product P −1 Rx (a)P where Rx (a) is
a rotation around the canonical vector 'e_0 and P
represents the vectors of the frame normalize u, j u,
k u (see Sect. 3. 3) in the canonical basis.
Conversely, given a rotation matrix, one can findan axis and an angle such that the corresponding
linear application is a rotation:
Lemma SO_is_around_axis M : M \is 'SO[R]_3 →exists u a, norm u = 1 /\is_around_axis u a (mx_lin1 M).

(mx_lin1 turns a matrix into the corresponding linear function.) The proof uses Euler’s theorem:
Lemma euler (M : 'M[R]_3) : M \is 'SO[R]_3 →{x : 'rV[R]_3 | (x != 0) && (x ∗m M == x)}.

We use Euler’s theorem to obtain an eigenvector of
the rotation matrix. We use this vector to build
a positive frame (see Sect. 3. 3) and inspect theresult of the action of the matrix M on the frame
vectors. It happens that the coordinates of the resulting vectors have properties that are fulfilled by
the trigonometric functions cos and sin and thatsatisfy the definition of a rotation.

5 Exponential Coordinates for Rotations
Rotation matrices can be represented using exponentials of skew-symmetric matrices. In general,
the exponential of a matrix is a power series. For
skew-symmetric matrices, there is a closed expression: eφS(w) = 1 + (sin φ)S(w) + (1 − cos φ)S(w)2 ,where S(w) is the skew-symmetric matrix corresponding to vector w. In the following, we showthat rotation matrices can be represented by thisformula.
First, we define the set of skew-symmetric matrices 'so[R]_n, i.e., the matrices M such thatM = −M T . To a vector [wx ; wy ; wz ] corresponds
the (row-vector
convention)
skew-symmetric ma[
]
trix

0
wz −wy−wz
0wx
wy −wx 0. Hereafter, we denote theskew-symmetric matrix corresponding to the vector w by \S( w ).
Now, let us define the following function (wherea is an angle and M is a 3 × 3 matrix):
Definition emx3 a M :=1 + sin a *: M + (1 -cos a) *: M ˆ+ 2.

We are interested in the case where the matrix M
is \S( w ) for some w. We denote such a matrix by`eˆ(a, w) and call a and w the exponential coordinates.
5. 1 Exponential Coordinates of Rotation
Matrices
Any rotation matrix can be represented by its
exponential coordinates.
First, we observe that for a unit vector w, thematrix `eˆ(a, w) is a rotation of angle a around w:
Lemma is_around_axis_eskew a w : norm w = 1 →is_around_axis w a (mx_lin1 `eˆ(a, w)).

Conversely, any rotation matrix can be represented using exponential coordinates:
Lemma eskew_is_onto_SO M : M \is 'SO[R]_3 →exists a w, norm w = 1 /\ M = `eˆ(a, w).

The proof is as follows.
We use the lemma
SO_is_around_axis of Sect. 4. 2 to derive an angle a
and an axis w corresponding to M. We want to showthat M = `eˆ(a, w). The lemma is_around_axis_eskew
above says that `eˆ(a, w) is a rotation of angle aaround w. This rotation and the rotation corresponding to M are the same, so are the matrices.
5. 2 Rodrigues’ Formula
We have shown in the previous section (Sect. 5. 1)that multiplication by a matrix `eˆ(a, w) provides
rotation. Rodrigues’ formula is a classical resultthat provides an alternative expression for such arotation.
Rodrigues’ formula transforms a vector v according to angle a and vector w by means of the dot-

product and of the cross-product:
Definition rodrigues v a w := cos a *: v
+ (1 -cos a) * (v ∗d w) *: w + sin a *: (w ∗v v).

It provably performs the same transformation as
rotation using `eˆ(a, w):
Lemma rodriguesP u a w : norm w = 1 →rodrigues u a w = u ∗m `eˆ(a, w).

The proof is by appealing to the properties of skewsymmetric matrices and of the cross-product (inparticular the double cross-product—see Sect. 3. 2).
5. 3 Angle-Axis Representation
The angle-axis representation of a rotation matrix M is a pair of an angle and a vector computed
directly from M .
The angle of a matrix M is defined by:arccos ((tr(M ) − 1)/2) .
Put formally:
Definition angle_of_rot M := acos ((\tr M -1) / 2%:R).

The (row-vector convention) axial vector of a matrix M is defined by:
w = [M1,2 − M2,1 ; M2,0 − M0,2 ; M0,1 − M1,0 ] .
The rotation axis of a matrix M is defined by1
w, where a is the angle of the matrix M
2 sin(a)as defined above and w is the axial vector. Putformally:
Definition axis_of_rot M := let a := angle_of_rot M in
1 / ((sin a) *+ 2) *: axial_vec M.

We can show that M is a rotation of angle
angle_of_rot M and axis axis_of_rot M (as long as
none of them is zero):
Lemma angle_axis_eskew M : M \is 'SO[R]_3 →axis_of_rot M != 0 →sin (angle_of_rot M) != 0 →let a := aangle (angle_axis_of_rot M) in
let w := aaxis (angle_axis_of_rot M) in
M = `eˆ(a, w).

6 Formalization of Quaternions
Quaternions provide an alternative way to represent rotations. They are concise (4 elements as opposed to the 9 elements of a matrix) and their composition is computationally more eﬃcient. Quaternions are interesting in their own right as the first
non-commutative algebra to be studied and they
are used pervasively in robotics (including com-puter vision).
6. 1 Formal Definition of Quaternions
A quaternion has a scalar part and a vector part:
Record quat := mkQuat {quatl : R ; quatr : 'rV[R]_3 }.

Let us denote the scalar (resp. vector) part of aquaternion a by a`0 (resp. a`1). We denote by aˆ∗q
the conjugate of a; it is defined as follows:
Definition conjq a := mkQuat (a`0) (- a`1).

The norm of a quaternion is defined as follows:
Definition sqrq a := a`0 ˆ+ 2 + norm (a`1) ˆ+ 2.
Definition normq a := Num.sqrt (sqrq a).

In particular, unit quaternions (type uquat) arequaternions with norm 1.
Addition of quaternions is defined componentwise. Multiplication is defined using the dotproduct and the cross-product (Sections 3. 1
and 3. 2):
Definition mulq a b := mkQuat(a`0 * b`0 -a`1 ∗d b`1)
(a`0 *: b`1 + b`0 *: a`1 + a`1 ∗v b`1).

Inverse is defined using the norm and the conjugate:
Definition invq a := (1 / sqrq a) *: (a ˆ∗q ).

Using above operations, we showed that quaternions form a ring (ringType) with scaling (lmodType)and units (unitRingType).
6. 2 Rotation with Quaternions
Let a be a quaternion. It turns out that the application v 7→ ava∗ provides us with rotation of(three-dimensional) vectors v:
Definition quat_rot (a : quat) (v : vector) : quat :=(a : quat) * v%:v * aˆ∗q .

We now make precise the nature of this rotation.
First, we observe that quat_rot is a linear function:
Lemma quat_rot_is_linear q :linear (fun v => (quat_rot q v)`1).

Similarly to complex numbers, one can define thepolar coordinates of a quaternion (see Sect. 2. 2 forthe definition of atan and Sect. 3 for normalize andnorm):
Definition polar_of_quat a :=(normalize a`1, atan (norm a`1 / a`0)).

Let us consider a unit quaternion q with a nonzero scalar part (i.e., it is not “pure”) and polar
coordinates (u, a). We can show that the function v 7→ (u, a)v(u, a)∗ is a rotation of angle 2aaround u:
Lemma quat_rot_is_Rot q :q \is uquat →¬pureq q →let: (u, a) := polar_of_quat q in
u != 0 →is_around_axis u (a *+ 2)(Linear (quat_rot_is_linear q)).

7 Rigid Body Transformation
A rigid body transformation (hereafter, rbt) is
a mapping R3 → R3 that preserves lengths and orientation. The preservation of orientation by a rbt
is defined as the preservation of the cross-product.
In this section, we show that a rbt is actually adirect isometry. We define isometries in Sect. 7. 1
and orientation in Sect. 7. 2.
7. 1 Isometries
An isometry is a mapping Rn → Rn that preserves lengths:(* Module Iso *)Record t := mk {f :> 'rV[R]_n →'rV[R]_n ;P : {mono f : a b / norm (a -b)} }.

We denote the type of isometries by 'Iso[R]_n.
There is a theorem that says that, from any
isometry f, one can extract an orthogonal matrix(ortho_of_iso f, the “orthogonal part” of the isometry) and a translation (trans_of_iso f) such thatf x = x ∗m ortho_of_iso f + trans_of_iso f.
The isometry is direct when the determinant of
the orthogonal matrix (its “sign”) is 1. We denote the type of three-dimensional direct isometries
by 'DIso_3[R]:(* Module DIso *)Record t := mk {f :> 'Iso[R]_3 ;P : iso_sgn f == 1 }.

7. 2 Preservation of Orientation
The action of a rbt f on points induces an action f∗ on vectors. Let v be the vector b − awhere b and a are points. f∗ is defined by f∗ (v) =f (b) − f (a) [6] (Chapter 2, Sect. 1, p. 21). f∗ is thederivative map of f [8] (Chapter III, Sect. 2). To formalize the derivative map, we define tangent vectors. We denote by u `@ p the vector u with pointof application p, and by p.-vec the type of such vectors. f∗ transforms a tangent vector p.-vec into atangent vector (f p).-vec according to the following
definition:
Definition dmap (f : 'Iso[R]_3) p (v : p.-vec) :=let C := ortho_of_iso f in(v ∗m C) `@ f p.

Hereafter, we denote the derivative map of f by f`*.
We use the derivative map to state what it means
for an isometry to preserve orientation. An isometry f preserves orientation when f∗ (u × v) =f∗ (u) × f∗ (v) [6] (Definition 2.1). Let p by the pointof application of u × v. Then, the point of application of f∗ (u) × f∗ (v) is f (p). Put formally:
Definition preserves_orientation f :=∀p (u v : p.-vec),f`* ((u ∗v v) `@ p) = ((f`* u) ∗v (f`* v)) `@ f p.

Using above definitions, we prove formally thata direct isometry preserves orientation:
Lemma diso_preserves_orientation (f : 'DIso_3[R]) :preserves_orientation f.

Since it also preserves lengths by definition, thismeans that a direct isometry is a rbt.
Conversely, an isometry that preserves the crossproduct of two non-colinear vectors is direct:
Lemma preserves_crossmul_is_diso (f : 'Iso[R]_3)
p (u v : p.-vec) : ¬colinear u v →f`* ((u ∗v v) `@ p) = ((f`* u) ∗v (f`* v)) `@ f p →iso_sgn f = 1.

This means that a rbt can be represented by adirect isometry.

8 Rigid Body Transformation using
Homogeneous Representation
In Sect. 7, we showed that a rbt can be represented by a direct isometry. In this section, weshow that a direct isometry can be represented byan element of the special Euclidean group, i.e., apair of a translation and a rotation:(* Module SE *)Record t (R : rcfType) : Type := mk {trans : 'rV[R]_3;
rot : 'M[R]_3 ;rotP : rot \in 'SO[R]_3 }.

Before showing in Sect. 8. 2 that an element of thespecial Euclidean group indeed defines a rbt, i.e.,that it preserves lengths and orientation, we define
in Sect. 8. 1 the homogeneous representation of anelement of the special Euclidean group.
8. 1 Homogeneous Representation
The homogeneous representation of an element of[ ]the special Euclidean group is a 4 × 4 matrix rt 01
where r is a rotation and t is a vector (representing
a translation). We define formally such matrices asfollows:
Definition hom (r : 'M[R]_3) (t : 'rV[R]_3) : 'M[R]_4 :=block_mx r 0 t 1.

We denote by SE3[R] the set of matrices of thisshape such that r is a rotation. We show that SE3[R]is indeed a group by using in particular the following definition of inverse: [][ r 0 ]−1T= −tr∗rT 01 .
t1
We denote by 'hP[R] (resp. 'hV[R]) the set ofhomogeneous points [p0 ; p1 ; p2 ; 1] (resp. vectors[v0 ; v1 ; v2 ; 0]).
The application of an element of the special Euclidean group (object T of type SE.t) to a homogeneous point (resp. vector) is the matrix multiplication by its homogeneous representation:
Coercion mx (T : SE.t) := hom (rot T) (trans T).
Definition hom_ap (T : SE.t) x : 'rV[R]_4 := x ∗m T.

When x is an homogeneous point, the applicationof hom_ap performs a rotation and a translation, but
only a rotation when x is an homogeneous vector.
8. 2 Elements of the Special EuclideanGroup are Rigid Body Transformations
We can now define application of an element of the special Euclidean group to a (threedimensional) point or vector.
The homogeneous point (resp. vector) corresponding to thethree-dimensional row-vector x is to_hpoint x (resp.
to_hvector x). from_h is the projection that cancels

to_hpoint and to_hvector. We use these functions tow

embed points and vectors into their homogeneous
representation and recover points and vectors after
having applied hom_ap:pzDefinition ap_point (T : SE.t) p :=from_h (hom_ap T (to_hpoint p)).
Definition ap_vector (T : SE.t) v :=from_h (hom_ap T (to_hvector v)).

y

Since they preserve lengths, the elements of thespecial Euclidean group can be used to build isometries. We can show moreover that an isometry built
in this way preserves orientation:
Lemma SE_preserves_orientation (T : SE.t R) :preserves_orientation (Iso.mk (SE_preserves_length T)).

We have therefore established that the special
Euclidean group coincides with rbt.

9 Screw Motion
In this section, we formalize the basics of screw
theory, in particular the fundamental Chasles’ theorem. In Sect. 9. 1, we show that there exists ascrew axis for any rbt. The existence of a screw
axis suggests that any rbt can be represented as
the composition of a rotation around an axis followed by a translation along the same axis. In
Sect. 9. 2, we show that such a screw motion canbe represented using exponential coordinates.
9. 1 Existence of a Screw Axis
Let us assume a rbt f whose orthogonal part isa rotation with angle a around the unit vector w:
Variable f : 'DIso_3[R].
Let Q : 'M[R]_3 := ortho_of_iso f.
Variable w : vector.
Hypothesis w1 : norm w = 1.
Variable a : angle R.
Hypothesis Qaxis : is_around_axis w a (mx_lin1 Q).

One can show that the components along w of thedisplacements of any two points are the same:
Lemma displacement_proj q p :displacement f q ∗d w = displacement f p ∗d w.

p0

p′

x
Fig. 3: Illustration of a screw motion

We can show that the application of an elementof the special Euclidean group to a point using
ap_point preserves length between points:
Lemma SE_preserves_length (T : SE.t R) :{mono (ap_point T) : a b / norm (a -b)}.

p′′

φ

As a consequence, we have Chasles’ theorem [2] (Theorem 3.2.2). Let us denote by d0 thecomponent along w of the displacement of the origin. Then, any displacement of norm d0 is parallel
to w:
Lemma MozziChasles p :norm (displacement f p) = d0 →colinear (displacement f p) w.

This theorem shows that there is a set of points
that undergo just a translation: this is the screwaxis.
9. 2 Screw Motion
A screw motion is defined by a line (a point and avector that play the role of the axis), an angle, anda pitch (put together in the type Screw.t). Figure 3
shows the eﬀect of a screw motion (that is not apure translation) on a point p (the axis is (p0 , w),
the angle is φ, the pitch is the ratio of translation
to rotation). The eﬀect of this screw motion canbe formalized as follows:
Definition screw_motion s p :=let: (l, a, h) := (Screw.l s, Screw.a s, Screw.h s) inlet (p0, w) := (line_point l, line_vector l) in
p0 + (p -p0) ∗m `eˆ(a, w) + (h * radian a) *: w.

It turns out that a screw motion can be represented by exponential coordinates, similarly tothe exponential coordinates of rotation matrices
in Sect. 5. 1. This requires to generalize skewsymmetric matrices to twists. A twist \T(v,[ w) is a]0pair of two vectors arranged as a matrix \S(w)v 0(row-vector convention). The exponential of a twist
is defined as the following homogeneous matrix 2 :


2 One can find this definition in the literature[6] (Equation 2.36), [10] (Equation 1.27). The relation with the exponential function is explainedin Appendix A.

[
[

I
φv

0
1

]

eφS(w)
φS(w)(w×v)(1−e)+(φv)(wT w)||w||2

if w = 00
1]
if w ̸= 0.

We denote the exponential of a twist by `e$(a, t)
and formalize it as follows:
Definition hom_twist t a e :=let (v, w) := (\v( t ), \w( t )) in
if w == 0 then hom 1 (a *: v)else hom e ((norm w)ˆ-2 *:((w ∗v v) ∗m (1 -e) + (a *: v) ∗m (wˆT ∗m w))).
Definition etwist a t :=hom_twist t (radian a) (`eˆ(a, \w( t ))).

For illustration, the screw_motion function above(more precisely, its homogeneous representation
hom_screw_motion) corresponds to the following
twist [6] (p. 47):
Lemma hom_screw_motion_etwist s :let: (l, a, h) := (Screw.l s, Screw.a s, Screw.h s) inlet (p0, w) := (line_point l, line_vector l) in
let v := -w ∗v p0 + h *: w in
hom_screw_motion s = `e$(a, \T(v, w)).

The parameters v, w, and a are called the exponential coordinates for the corresponding rbt. Indeed,
any rbt can be represented by exponential coordinates:
Lemma etwist_is_onto_SE f : f \is 'SE3[R] →exists t a, f = `e$(angle_of_radian a, t).

The proof is as follows. Let us assume that f is
[ ]of the form rp 01 . If f is a pure translation, thenit suﬃces to choose the twist \T(normalize p, 0) andthe angle norm p. Otherwise, let a, w be the exponential coordinates of r (obtained through lemma
eskew_is_onto_SO from Sect. 5. 1).
Then it suffices to choose the twist \T(v, w) and the angle a,where v is such that p = (norm w)ˆ-2 *: (v ∗m G) with
G = \S(w) ∗m (1 -r) + radian a *: (wˆT ∗m w). The inverse of the latter matrix is a1 − 12 S(w) +(1( ))− 12 cot a2 S(w)2 [9].
a9. 3 Example of Twist Computation
We can use the (constructive) proof of the lastlemma (etwist_is_onto_SE, Sect. 9. 2) to build thetwist \T(v, w) of the rigid body transformation ofFig. 1.
The coordinate of the frame C w.r.t. the frame A

[A
]
C 0
is AR
where A TC is [−ℓ2 sin φ; ℓ1 + ℓ2 cos φ; 0].
TC 1
The exponential coordinates of the rotation are
the axis 'e_2 and the angle φ (see lemma
is_around_axis_eskew of Sect. 5. 1).
We already know that w is 'e_2.
v is given
by A TC G−1 (where G comes from the proofof etwist_is_onto_SE), that is, after calculation,[(ℓ1 − ℓ2 )/2; (ℓ1 + ℓ2 )/(2(1 − cos φ)); 0].

10 Related WorkIn this section, we focus on related work using
proof-assistants to perform formal verification ofrobotics (we leave aside the more numerous related
work dealing with model-checking).
Walter et al. perform in Isabelle the verificationof a collision avoidance algorithm for a vehicle moving in a plane [11]. More precisely, this algorithm
computes safety zones as supersets of braking areas.
It therefore involves mathematics that we have notbeen dealing with: velocity and computational geometry in a plane (as opposed to three-dimensional
geometry for robot manipulators). An important
aspect of their experiment is that it was used aspart of a certification eﬀort and received positive
reviews in this context.
Farooq et al. propose a significant formalizationof two-link planar manipulators in HOL-Light [5].
They apply their theories to the analysis of a (twodimensional) biped walking robot (4 links, 3 joints).
Our work can be seen as a tentative extension to
three-dimensional geometry.
Anand et al. use Coq to write, verify, and executeprograms to be used on actual robots [1]. For that
purpose, they provide an event-based programming
framework dealing with time. They tackle theproblem of verified robot software in a pragmaticway. In comparison, we are dealing with the complementary task of formalizing formal foundations.
Ma et al. propose a formalization of conformal geometric algebra in HOL-Light [7]. They use theirformalization to reason about rigid body transformations and apply their results to the formalization of a grasping algorithm. In comparison,
we are focusing on standard techniques for robot
specification. Though conformal geometric algebra is not mainstream, its formalization may turnout to be inspiring since it deals with multivectors
whose product generalizes the three-dimensional

Table 2: Overview of our formalization of rigid body transformations
File nameaux.v
angle.v
euclidean3.v
vec_angle.v
frame.v
skew.v
rot.v
quaternion.v
rigid.v
screw.v

Contents
Utility definitions and tactics
Angles and trigonometric functions (Sect. 2)
Dot-product, cross-product, etc. (Sect. 3); rotation matrices (Sect. 4. 2)
Utility definitions such as colinear, normalize, normalcomp (Fig. 2, footnote 1)
Frames (Sect. 3. 3)
Skew-symmetric matrices (used in Sections 5 and 9)
Specification of rotations and exponential coordinates (Sections 4 and 5)
Rotation with quaternions (Sect. 6)
Rigid body transformations (Sections 7 and 8)
Screw motions (Sect. 9)

cross-product.

11 Conclusion and Perspectives
In this paper, we gave an overview of an ongoing eﬀort to formalize the foundations of robotics.
Our formalization is about 8,300 l.o.c. (breakdown
in Table 2). This formalization covers a substantial
part of the basic material one can find in standard
textbooks, e.g., [10] (Section 1.2: Position and Orientation Representation), [6] (Chapter 2: Rigid Body
Motion). Currently, we are working on applying ourwork to the formalization of open chains for serial
robots.
The formalization of the theoretical foundationsof robotics is only one step towards the formal verification of robots. At some point, we will needto extend our work to deal with the dynamics ofrobots by introducing velocity. The correctness ofnumerical computations used in robot software isanother aspect of formal verification of robots. This
is not yet our concern but since Coq also excels at
verification of numerical computations, it should bepossible to build on top of our work a comprehensive formal verification framework for robot manipulators.
Acknowledgments The authors acknowledge
partial support from a Grant-in-Aid for Scientific
Research (B) (project number 15H02687).
References[ 1 ] Anand, A. and Knepper, R. A.:
ROSCoq:Robots Powered by Constructive Reals. Proc. ITP
2015, LNCS, Vol. 9236, pp. 34–50.
[ 2 ] Angeles, J.: Fundamentals of Robotic Mechanical Systems—Theory, Methods, and Algorithms.
Springer, 2014. 4th Edition.
[ 3 ] The Coq Development Team: Reference Man-

l.o.c.
147
826
1269
479
894
621
1235
654
1103
1193

ual. INRIA, 1999–2016. Ver. 8.5pl2. Available at
http://coq.inria.fr.
[ 4 ] Gonthier, G., Asperti, A., Avigad, J., Bertot,
Y., Cohen, C., Garillot, F., Le Roux, S., Mahboubi,
A., O’Connor, R., Ould Biha, S., Pasca, I., Rideau,
L., Solovyev, A., Tassi, E., Théry, L.: A MachineChecked Proof of the Odd Order Theorem. Proc.
ITP 2013, LNCS, Vol. 7998, pp. 163–179.
[ 5 ] Farooq, B., Hasan, O., and Iqbal, S.: Formal
Kinematic Analysis of the Two-Link Planar Manipulator. Proc. ICFEM 2013, LNCS, Vol. 8144,
pp. 347–362.
[ 6 ] Murray, R. M., Li, Z., Sastry, S. S.:AMathematical Introduction to Robotic Manipulation. CRC Press, 1994. First edition.
[ 7 ] Ma, S., Shi, Z., Shao, Z., Guan, Y., Li, L., Li,
Y.: Higher-Order Logic Formalization of Conformal
Geometric Algebra and its Application in Verifying
a Robotic Manipulation Algorithm. Advances inApplied Cliﬀord Algebras. March 2016. In press.
[ 8 ] O’Neill, B.: Elementary Diﬀerential Geometry.
Academic Press, 1966.
[ 9 ] Park, F. C., Lynch, K.:
Introduction to
Robotics: Mechanics, Planning, and Control. Seoul
National University, 2012. Course text.
[10] Siciliano, B., Khatib, O. (Eds.): Springer Handbook of Robotics. Springer, 2008.
[11] Walter, D., Täubig, H., Lüth, C.: Experiencesin Applying Formal Verification in Robotics. Proc.
SAFECOMP 2010, LNCS, Vol. 6351, pp. 347–360.

A The Exponential of a Twist using
Taylor Expansion
In Sect. 9. 2, we define a screw motion in term ofa twist by the expression etwist. In the literature,
this expression is derived from definitions of exponential coordinates in terms of power series. We
reproduce this reasoning here using Taylor expansions.
We first define the Taylor expansion of the exponential function:

Definition ecoef n (M : 'M[R]_n.+1) i :=i`!%:Rˆ-1 *: M ˆ+ i.
Definition emx n (M : 'M[R]_n.+1) k :=\sum_(i < k) ecoef M i.

hom_twist, with the diﬀerence that we replace

We now define the exponential coordinates of arotation as the exponential of a skew-symmetricmatrix emx (a *: \S( w )) k (instead of the definition
using emx3 from Sect. 5).
Similarly, we also define the exponential coordinates of a rbt as the exponential of a twistemx (a *: t) k.
We now justify the definition of etwist in
Sect. 9. 2. Let us define a new function emx_twist
similar to etwist: it uses the same definition

We show that this definition is actually equivalent
to the exponential of a twist [6] (Proposition 2.8):eˆ(a, \w( t )) by emx \S( a *: \w( t ) ) k:`

Definition emx_twist a t k :=hom_twist t a (emx \S( a *: \w( t ) ) k).

Lemma emx_twistE (t : Twist.t R) a k :emx (a *: t) k.+2 = emx_twist a t k.+2.

In other words, instead of computing the exponential as a sum using emx, one may also use themore direct expression expmx_twist. This justifies
the definition of hom_twist (and therefore etwist) in
Sect. 9. 2.