{
"cells": [
{
"cell_type": "markdown",
"metadata": {
"colab_type": "text",
"id": "view-in-github",
"tags": [
"no-tex"
]
},
"source": [
""
]
},
{
"cell_type": "code",
"execution_count": 2,
"metadata": {
"id": "JoW4C_OkOMhe",
"tags": [
"remove-cell"
]
},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"Note: you may need to restart the kernel to use updated packages.\n"
]
}
],
"source": [
"%pip install -q -U gtbook\n"
]
},
{
"cell_type": "code",
"execution_count": 3,
"metadata": {
"id": "10-snNDwOSuC",
"tags": [
"remove-cell"
]
},
"outputs": [],
"source": [
"import math\n",
"import gtsam\n"
]
},
{
"attachments": {},
"cell_type": "markdown",
"metadata": {},
"source": [
"# Moving in Three Dimensions\n",
"\n",
"> Motion in 3D has 6 degrees of freedom.\n",
"\n",
""
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"In Section 6.1 we introduced the space of 2D rigid transformations $SE(2)$, and showed how $3 \\times 3$ matrices can be used to simultaneously represent \n",
"rotation and translation in two dimensions.\n",
"At that time, we promised that this could be easily generalized to the case of rotation and\n",
"translation in three dimensions. In this section, we deliver on that promise, and introduce two new spaces:\n",
"the 3D rotations $SO(3)$ and 3D rigid transformations $SE(3)$.\n",
"\n",
"In addition to representing the *pose* of a drone in 3D we are also interested in\n",
"representing velocity.\n",
"We will discuss both linear and angular velocity, i.e., the rate of change in the orientation of one coordinate frame with respect to another.\n",
"\n",
"The discussion below holds for generic coordinate frames, but because this is the drone chapter, we introduce two frames here that make things a bit more tangible:\n",
"\n",
"- the **navigation frame** $N$, which you can think of as attached to the Earth;\n",
"- the **body frame** $B$, which you can think of as rigidly attached to the drone.\n",
"\n",
"In the next section the different conventions for these coordinate frames are discussed in more detail. Below we simply use the subscripts \"n\" and \"b\" in the definitions and examples, irrespective of which convention is used."
]
},
{
"cell_type": "markdown",
"id": "41fe8146",
"metadata": {},
"source": [
"## Rotations in 3D aka SO(3)\n",
"\n",
"In Section 6.1, we constructed a rotation matrix $R^0_1 \\in SO(2)$\n",
"by projecting the axes of Frame 1 onto Frame 0.\n",
"The extension to 3D is straightforward.\n",
"Using the navigation and body coordinate frames defined above, let us project the axes $(x_b, y_b, z_b)$ of the body frame $B$ onto the axes $(x_n,y_n, z_n)$ of the navigation frame $N$. \n",
"Of course, in the 3D case each frame is equipped with an additional $z$-axis:\n",
"\n",
"$$\n",
"R^{n}_{b}\n",
"=\\begin{bmatrix}\n",
"x_n \\cdot x_b & x_n \\cdot y_b & x_n \\cdot z_b \\\\\n",
"y_n \\cdot x_b & y_n \\cdot y_b & y_n \\cdot z_b \\\\\n",
"z_n \\cdot x_b & z_n \\cdot y_b & z_n \\cdot z_b \\\\\n",
"\\end{bmatrix}\n",
"$$\n",
"\n",
"The expression for *rotational* coordinate transformations is now exactly the\n",
"same as in Chapter 6.\n",
"Given a point $p$ with coordinates expressed relative to the body\n",
"frame $B$, we compute its coordinates $p^n$ in the navigation frame $N$ as follows:\n",
"\n",
"$$\n",
"p^n = R^n_b p^b\n",
"$$\n",
"\n",
"Extending the semantics from 2D rotations in $SO(2)$ to 3D rotations represented by $SO(3)$,\n",
"the columns\n",
"of $R_{b}^{n}$ represent the axes of frame $B$ in the $N$ coordinate\n",
"frame: \n",
"\n",
"$$\n",
"R_{b}^{n}=\\left[\\begin{array}{ccc}\n",
"\\hat{x}_{b}^{n} & \\hat{y}_{b}^{n} & \\hat{z}_{b}^{n}\\end{array}\\right]\n",
"$$"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"The 3D rotations together with composition constitute the **special\n",
"orthogonal group** $SO(3)$, hence the abbreviation \"SO\". It is made up of all $3\\times3$ orthonormal\n",
"matrices with determinant $+1$. This group has matrix multiplication as its\n",
"composition operation, \n",
"but unlike in two dimensions, *3D rotations do not commute*, i.e., in general $R_{2}R_{1}\\neq R_{1}R_{2}$.\n",
"The group thus obtained it is called \"special\" because of the +1 determinant. Orthonormal matrices either have a -1 or +1 determinant, but those with -1 are *reflections*, which we specifically exclude here."
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"It is often useful to refer to rotations about one of the coordinate axes.\n",
"For example, when a drone rotates around the vertical axis, this will typically be around the Z-axis, i.e., the yaw axis.\n",
"The basic rotation matrices around X, Y, and Z-axes respectively are given by\n",
"\n",
"$$\n",
"R_{x,\\phi}\n",
"=\\begin{bmatrix}\n",
"1 & 0 & 0 \\\\\n",
"0 & \\cos \\phi & - \\sin \\phi \\\\\n",
"0 & \\sin \\phi & \\cos \\phi \n",
"\\end{bmatrix}\n",
"~~~\n",
"R_{y,\\theta}\n",
"=\\begin{bmatrix}\n",
"\\cos \\theta & 0 & \\sin \\theta \\\\\n",
"0 & 1 & 0 \\\\\n",
"-\\sin \\theta & 0 & \\cos \\theta\n",
"\\end{bmatrix}\n",
"~~~\n",
"R_{z,\\psi}\n",
"=\\begin{bmatrix}\n",
"\\cos \\psi &-\\sin \\psi & 0 \\\\\n",
"\\sin \\psi &\\cos \\psi & 0 \\\\\n",
"0 & 0 & 1\n",
"\\end{bmatrix}\n",
"$$\n",
"\n",
"Note, in particular, the form of $R_{z,\\psi}$, the rotation about the $z$-axis.\n",
"The upper-left $2\\times 2$ block of this matrix is exactly a rotation matrix $R \\in SO(2)$\n",
"as introduced in Section 6.1.\n",
"This can be understood by realizing that rotation in the $xy$ plane is actually\n",
"a rotation about an implicit $z$- axis that points \"out of the page.\""
]
},
{
"cell_type": "markdown",
"id": "eede1a83",
"metadata": {},
"source": [
"## 3D Rigid transforms aka SE(3)\n",
"\n",
"In a similar way we can extend the group of 2D rigid transforms $SE(2)$ to the case\n",
"of *rotation and translation* in three dimensions.\n",
"Suppose a point $p$ is rigidly attached to frame $B$, and\n",
"we wish to determine its coordinates with respect to the navigation frame $N$.\n",
"If the coordinates of $p$ with respect to frame $B$ are given by $p^b$,\n",
"then we can express it in frame $N$ as follows:\n",
"\n",
"$$\n",
"p^{n}=R_{b}^{n}p^{b}+t_{b}^{n}\n",
"$$ \n",
"\n",
"Above $R^n_b \\in SO(3)$ is the rotation matrix that specifies the orientation\n",
"of frame $B$ w.r.t. frame $N$,\n",
"and $t^n_b \\in \\mathbb{R}^3$ gives the location of the origin\n",
"of frame $B$ specified in the coordinates of frame $N$.\n",
"\n",
"We denote this transform by\n",
"$T_{b}^{n}\\doteq\\left(R_{b}^{n},\\,t_{b}^{n}\\right)$. \n",
"These transforms $T_{b}^{n}$ are elements of\n",
"the **special Euclidean group** of order 3, $SE(3)$.\n",
"Similarly to the previous chapter we can construct\n",
"matrices in $SE(3)$ by embedding the rotation and translation into a $4\\times4$ invertible matrix defined as\n",
"\n",
"$$\n",
"T_{b}^{n}=\\begin{bmatrix}\n",
"R_{b}^{n} & t_{b}^{n}\\\\\n",
"0 & 1\n",
"\\end{bmatrix}.\n",
"$$\n",
"\n",
"Again, as in the previous chapter, we can implement the group operation as simple matrix multiplication.\n",
"And, in analogy with the 2D case, if we use the homogeneous coordinates\n",
"for a point $p$, we define\n",
"a 3D homogeneous transformation as\n",
"\n",
"$$\n",
"\\begin{bmatrix}\n",
"R_{b}^{n} & t_{b}^{n}\\\\\n",
"0 & 1\n",
"\\end{bmatrix}\n",
"\\begin{bmatrix}\n",
"p^{b}\\\\\n",
"1\n",
"\\end{bmatrix}\n",
"=\\begin{bmatrix}\n",
"R_{b}^{n}p^{b}+t_{b}^{n}\\\\\n",
"1\n",
"\\end{bmatrix}\n",
"$$\n",
"\n",
"Note that both ways of defining and applying 3D rigid transformations are equivalent: sometimes it is more convenient to use one over the other. It should be apparent from the context as to which flavor we use."
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## The Group $SE(3)$\n",
"\n",
"> $SE(3)$ is a non-commutative group.\n",
"\n",
"Unsurprisingly, $SE(3)$ is also a group, as the following properties all hold:\n",
"\n",
"1. **Closure**: For all transforms $T, T' \\in SE(3)$, their product is also in $SE(3)$, i.e., $T T' \\in SE(3)$.\n",
"2. **Identity**: The $4\\times 4$ identity matrix $I$ is included in the group, and for\n",
"all $T \\in SE(3)$ we have $T I = I T = T$.\n",
"3. **Inverse**: For every $T \\in SE(3)$ there exists $T^{-1} \\in SE(3)$ such that $T^{-1}T = T T^{-1} = I$.\n",
"4. **Associativity**: For all $T_1, T_2, T_3 \\in SE(3)$, $(T_1 T_2) T_3 = T_1 (T_2 T_3)$.\n",
"\n",
"The inverse $T^{-1}$ is given by:\n",
"\n",
"$$\n",
"T^{-1} = \\begin{bmatrix}R & d\\\\0_3 & 1\\end{bmatrix}^{-1}\n",
" = \\begin{bmatrix}R^T & -R^T d\\\\0_3 & 1\\end{bmatrix}\n",
"$$\n",
"\n",
"\n",
"However, 3D rigid transforms do *not* commute. Hence, $SE(3)$ is a **noncommutative group**. In fact, so is the group $SO(3)$: 3D rotations only commute when they represent rotations along the same axis. A special case is the set of 2D rotations from the last chapter, which are all around the imagined z-axis: this explains why rotations in 2D commute, but rotations in 3D do not, in general.\n"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## $SE(3)$ in GTSAM\n",
"\n",
"> `Pose3` is a noncommutative group which acts on `Point3`.\n",
"\n",
"Again, all of this is built into GTSAM, where both 3D poses and 3D rigid transforms are represented by the type `Pose3`, with rotation matrices represented by `Rot3`:"
]
},
{
"cell_type": "code",
"execution_count": 4,
"metadata": {},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"3D Pose R: [\n",
"\t-0.720878, -0.693062, 0;\n",
"\t0.693062, -0.720878, 0;\n",
"\t0, 0, 1\n",
"]\n",
"t: 10 20 30\n",
"\n",
"corresponding to the transformation matrix:\n",
"\n",
"[[-0.72087779 -0.6930622 0. 10. ]\n",
" [ 0.6930622 -0.72087779 0. 20. ]\n",
" [ 0. 0. 1. 30. ]\n",
" [ 0. 0. 0. 1. ]]\n"
]
}
],
"source": [
"R = gtsam.Rot3.Yaw(math.degrees(20)) # rotation around Z-axis by 20 degrees\n",
"T = gtsam.Pose3(R, [10,20,30])\n",
"print(f\"3D Pose {T}\\ncorresponding to the transformation matrix:\\n\\n{T.matrix()}\")"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"We can verify the group properties for 3D transforms in code, as we do below, noting that the last assertion for commutativity yields `False`, as $SE(3)$ is a *noncommutative* group:"
]
},
{
"cell_type": "code",
"execution_count": 5,
"metadata": {},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"Closure: True\n",
"Identity: True\n",
"Inverse: True\n",
"Associativity: True\n",
"Commutativity: False\n"
]
}
],
"source": [
"print(\"Closure:\", isinstance(T * T, gtsam.Pose3)) # closure\n",
"I = gtsam.Pose3.Identity()\n",
"print(\"Identity:\", T.equals(T * I, 1e-9)) # identity\n",
"print(\"Inverse:\", (T * T.inverse()).equals(I, 1e-9)) # inverse\n",
"T1, T2, T3 = T, gtsam.Pose3(gtsam.Rot3.Roll(0.1), [1,2,3]), gtsam.Pose3(gtsam.Rot3.Roll(0.2), [1,2,3])\n",
"print(\"Associativity:\", ((T1 * T2)* T3).equals(T1 * (T2 * T3), 1e-9)) # associative\n",
"print(\"Commutativity:\", (T1 * T2).equals(T2 * T1, 1e-9)) # NOT commutative"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"Finally, 3D transforms can *act* on 3D points, which we can do using matrix multiplication, or using the `Pose3.transformFrom` method:"
]
},
{
"cell_type": "code",
"execution_count": 6,
"metadata": {},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"P0 = [ 5.78599563 18.50261322 33. 1. ]\n",
"P0 = [ 5.78599563 18.50261322 33. ]\n"
]
}
],
"source": [
"P1 = gtsam.Point3(2, 4, 3)\n",
"print(f\"P0 = {T.matrix() @ [2, 4, 3, 1]}\") # need to make P0 homogeneous\n",
"print(f\"P0 = {T.transformFrom(P1)}\")"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"You can see from the code snippet above that both approaches yield the same numerical values for the first three coordinates."
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## Velocity\n",
"\n",
">Velocity comes in two flavors: linear and angular.\n",
"\n",
"The **linear velocity** $v$ of the origin of a moving frame is easy to understand.\n",
"This velocity is obtained merely by computing the derivative of the translation $t$ of the frame's origin:\n",
"\n",
"$$\n",
"v \\doteq \\frac{d}{dt}t\n",
"$$\n",
"\n",
"We omit coordinate frame indices here to not clutter the notation.\n",
"\n",
"The derivative $\\dot{R}$ of orientation is more interesting. \n",
"We begin by introducing the family of $3 \\times 3$ skew symmetric matrices, denoted by $\\mathfrak{so}(3)$. \n",
"The relationship between $\\mathfrak{so}(3)$ and $SO(3)$ will become apparent shortly.\n",
"A matrix $S$ is said to be a **skew symmetric matrix** if\n",
"\n",
"$$\n",
"S = - S^T\n",
"$$\n",
"\n",
"If we examine this equation for each element of $S$, we obtain nine equations, \n",
"\n",
"$$ s_{i,j} = - s_{j,i}, ~~~~ i,j = 1,2,3 $$\n",
"\n",
"When $i=j$, we have $s_{ii} = -s_{ii}$, which is satisfied only if $s_{ii} = 0$.\n",
"Thus, the diagonal elements of a skew symmetric matrix are all zeros.\n",
"The remaining equations constrain the off-diagonal entries, such that\n",
"every $S \\in so(3)$ can be written as\n",
"\n",
"$$\n",
"\\hat{s} = S =\n",
"\\begin{bmatrix}\n",
"0 & -s_z &s_y \\\\\n",
"s_z & 0 & -s_x \\\\\n",
"-s_y & s_x & 0\n",
"\\end{bmatrix}\n",
"$$\n",
"\n",
"in which the **hat operator** $\\hat{\\cdot}$, maps a vector $s = (s_x, s_y, s_z)$ to\n",
"a skew symmetric matrix as above."
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"Skew symmetric matrices play a key role in representing the angular rate of change of coordinate frames.\n",
"Let $R(t)$ be a time-varying rotation matrix.\n",
"Because $R(t) \\in SO(3)$, at each time $t$ we have $R^T(t) R(t) = I$.\n",
"If we differentiate both sides of this equation, we obtain\n",
"\n",
"$$\n",
"\\begin{aligned}\n",
"\\frac{d}{dt} \\left[ R^T(t) R(t) \\right] &= \\frac{d}{dt} I \\\\\n",
" \\dot{R}^T R + R^T \\dot{R} &= 0\n",
"\\end{aligned}\n",
"$$\n",
"\n",
"In other words, $\\dot{R}^T R = -R^T \\dot{R}$ and hence the matrix $\\dot{R}^T R$ *is a skew symmetric matrix*\n",
"\n",
"$$\\hat{\\omega} = \\dot{R}^T R$$ \n",
"\n",
"with $\\omega$ some three-vector. Recalling that $R^{-1} = R^T$,\n",
"we can solve this equation for $\\dot{R}$ to obtain:\n",
"\n",
"$$\\dot{R} = \\hat{\\omega} R .$$\n",
"\n",
"The above gives an expression for the derivative of a rotation matrix $\\dot{R}$ that\n",
"will be used below in our formulation of drone dynamics.\n",
"The vector $\\omega$ is called the **angular velocity**,\n",
"and is the same angular velocity that you may have\n",
"learned in an introductory physics course.\n",
"\n",
"Note that the angular velocity vector $\\omega$ is a free vector, and therefore,\n",
"we can define its coordinates with respect to the coordinate frame of our choice.\n",
"To understand this, it is worth reconsidering the above in the context of\n",
"a specific rotation matrix, say $R^n_b$ that specifies the orientation of \n",
"body frame with respect to the navigation frame.\n",
"In this case, making explicit the relevant coordinate frames, we would write\n",
"\n",
"$$\\dot{R}^n_b = \\hat{\\omega}^n_{n,b} R^n_b .$$\n",
"\n",
"Here, the notation $\\hat{\\omega}^n_{n,b}$ can be understood to mean that\n",
"the angular velocity $\\omega$ determines the relative rate of angular rotation\n",
"of the body frame with respect to the navigation frame (the \"$n,b$\" in the subscript), and that\n",
"the coordinates of the angular velocity vector are given with respect to the navigation\n",
"frame (the \"$n$\" in superscript).\n",
"Most often, the subscript can be inferred from context, and will be omitted.\n",
"If, for example, we are concerned with the rotation of the quadrotor with respect\n",
"to the navigation frame, the two coordinate frames will be clear from the context,\n",
"and we can reduce clutter by omitting the subscript.\n",
"In contrast, we will typically keep the superscript to make clear the reference\n",
"frame in which the angular velocity is defined.\n",
"This will be particularly important, for example, when we express control inputs relative to the\n",
"quadrotor's body frame, with a control goal of stabilizing its orientation\n",
"relative to the fixed navigation coordinate frame.\n"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"\n",
"Equipped with this notation, it is easy to understand how to transform angular velocity vectors from one frame to another.\n",
"If we are given $\\omega^n_{n,b}$ (i.e., the angular velocity is expressed with respect to the navigation frame),\n",
"and we instead wish to specify the angular velocity with respect to the body frame, we merely perform the coordinate\n",
"transformation\n",
"\n",
"$$\n",
"\\omega^b_{n,b} = R^b_n \\omega^n_{n,b}\n",
"$$\n",
"\n",
"or, when the context is understood, more compactly, as\n",
"\n",
"$$\n",
"\\omega^b = R^b_n \\omega^n\n",
"$$\n",
"\n",
"This transformation will be useful when expressing a desired angular velocity\n",
"with respect to the body frame of a quadrotor."
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## Summary\n",
"\n",
"Now that we know how to talk about 3D rotations, 3D rigid transformations, *and* their rate of change, we are ready to use all that machinery to describe how drones move in 3D. The next section tackles this."
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## GTSAM 101\n",
"\n",
"> A deeper dive in the GTSAM concepts used above.\n",
"\n",
"The types `Point3` and `Rot3` are used for 3D position and rotation.\n",
"While `Point3` is really just an alias for a 3-dimensional numpy array, the `Rot3` type is a wrapper around a c++ GTSAM type that is not just a 3x3 matrix, which is how it represents the rotation internally, but also has a number of methods to interface with them.\n",
"For example, the `Rot3.Ypr` constructor creates a rotation from yaw, pitch, and roll angles:\n",
"\n",
"help(gtsam.Rot3.Ypr)\n",
"\n",
"```python\n",
"Ypr(...) method of builtins.PyCapsule instance\n",
" Ypr(y: float, p: float, r: float) -> gtsam.gtsam.Rot3\n",
"```"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"Using a 3D position and a 3D rotation we can create a 3D pose, represented by a `gtsam.Pose3`. As always, you can execute `help(gtsam.Pose3)` to get the full documentation of a class. Below is an excerpt with some useful methods. We have several constructors:\n",
"\n",
"```python\n",
" __init__(...)\n",
" __init__(*args, **kwargs)\n",
" Overloaded function.\n",
" \n",
" 1. __init__(self: Pose3) -> None\n",
" \n",
" 2. __init__(self: Pose3, other: Pose3) -> None\n",
" \n",
" 3. __init__(self: Pose3, r: Rot3, t: Point3) -> None\n",
" \n",
" 4. __init__(self: Pose3, pose2: Pose2) -> None\n",
" \n",
" 5. __init__(self: Pose3, mat: numpy.ndarray[numpy.float64[m, n]]) -> None\n",
"```\n",
"\n",
"where `t` above is just a `Point3`, and `mat` can be a 3x4 or 4x4 matrix. However, you are responsible for these matrices to satisfy the $SE(30$ constraints. We also used the `Pose3.TransformFrom` method above, and there is also a `Pose3.TransformTo` method:\n",
"\n",
"```python\n",
" transformFrom(...)\n",
" transformFrom(*args, **kwargs)\n",
" Overloaded function.\n",
" \n",
" 1. transformFrom(self: Pose3, point: Point3) -> Point3\n",
" ...\n",
" |\n",
" transformTo(...)\n",
" transformTo(*args, **kwargs)\n",
" Overloaded function.\n",
" \n",
" 1. transformTo(self: Pose3, point: Point3) -> Point3\n",
" ....\n",
"```\n",
"\n",
"Both have them have three overloads, but we show only the most commonly used one above."
]
}
],
"metadata": {
"colab": {
"collapsed_sections": [],
"include_colab_link": true,
"name": "S71_drone_state.ipynb",
"provenance": []
},
"interpreter": {
"hash": "c6e4e9f98eb68ad3b7c296f83d20e6de614cb42e90992a65aa266555a3137d0d"
},
"kernelspec": {
"display_name": "Python 3.9.6 64-bit ('base': conda)",
"name": "python3"
},
"language_info": {
"codemirror_mode": {
"name": "ipython",
"version": 3
},
"file_extension": ".py",
"mimetype": "text/x-python",
"name": "python",
"nbconvert_exporter": "python",
"pygments_lexer": "ipython3",
"version": "3.9.18"
},
"latex_metadata": {
"affiliation": "Georgia Institute of Technology",
"author": "Frank Dellaert and Seth Hutchinson",
"title": "Introduction to Robotics"
}
},
"nbformat": 4,
"nbformat_minor": 2
}