Commit 2e0cffc4 authored by Nicolas Mansard's avatar Nicolas Mansard Committed by Valenza Florian
Browse files

IVIGIT.

parent c1678b07
#
# Copyright 2014 CNRS
#
CMAKE_MINIMUM_REQUIRED(VERSION 2.6)
INCLUDE(cmake/base.cmake)
INCLUDE(cmake/boost.cmake)
INCLUDE(cmake/cpack.cmake)
INCLUDE(cmake/eigen.cmake)
INCLUDE(cmake/python.cmake)
SET(PROJECT_NAME pinocchio)
SET(PROJECT_DESCRIPTION "Rigid multi body dynamics algorithms")
SET(PROJECT_URL "http://github.com/stac-of-tasks/pinocchio")
# Disable -Werror on Unix for now.
SET(CXX_DISABLE_WERROR True)
SET(CMAKE_VERBOSE_MAKEFILE True)
project(${PROJECT_NAME})
SETUP_PROJECT()
# ----------------------------------------------------
# --- DEPENDANCIES -----------------------------------
# ----------------------------------------------------
ADD_REQUIRED_DEPENDENCY("eigenpy >= v1.0.1")
# ----------------------------------------------------
# --- INCLUDE ----------------------------------------
# ----------------------------------------------------
SET(${PROJECT_NAME}_HEADERS
spatial/se3.hpp
spatial/motion.hpp
spatial/force.hpp
spatial/inertia.hpp
spatial/fwd.hpp
spatial/skew.hpp
multibody/joint.hpp
multibody/model.hpp
tools/timer.hpp
)
MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/include/pinocchio")
MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/include/pinocchio/spatial")
MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/include/pinocchio/multibody")
MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/include/pinocchio/tools")
FOREACH(header ${${PROJECT_NAME}_HEADERS})
GET_FILENAME_COMPONENT(headerName ${header} NAME)
IF(WIN32)
execute_process(COMMAND ${CMAKE_COMMAND} -E copy_if_different
${${PROJECT_NAME}_SOURCE_DIR}/src/${header}
${${PROJECT_NAME}_BINARY_DIR}/include/${PROJECT_NAME}/${header})
ELSE(WIN32)
execute_process(COMMAND ${CMAKE_COMMAND} -E create_symlink
${${PROJECT_NAME}_SOURCE_DIR}/src/${header}
${${PROJECT_NAME}_BINARY_DIR}/include/${PROJECT_NAME}/${header})
ENDIF(WIN32)
INSTALL(FILES ${${PROJECT_NAME}_SOURCE_DIR}/${header}
DESTINATION ${CMAKE_INSTALL_PREFIX}/include/${PROJECT_NAME}
PERMISSIONS OWNER_READ GROUP_READ WORLD_READ OWNER_WRITE)
ENDFOREACH(header)
# ----------------------------------------------------
# --- TARGETS ----------------------------------------
# ----------------------------------------------------
ADD_EXECUTABLE(tspatial EXCLUDE_FROM_ALL unittest/tspatial.cpp)
PKG_CONFIG_USE_DEPENDENCY(tspatial eigenpy)
ADD_EXECUTABLE(rnea EXCLUDE_FROM_ALL unittest/rnea.cpp)
PKG_CONFIG_USE_DEPENDENCY(rnea eigenpy)
SETUP_PROJECT_FINALIZE()
SETUP_PROJECT_CPACK()
GNU LESSER GENERAL PUBLIC LICENSE
Version 3, 29 June 2007
Copyright (C) 2007 Free Software Foundation, Inc. <http://fsf.org/>
Everyone is permitted to copy and distribute verbatim copies
of this license document, but changing it is not allowed.
This version of the GNU Lesser General Public License incorporates
the terms and conditions of version 3 of the GNU General Public
License, supplemented by the additional permissions listed below.
0. Additional Definitions.
As used herein, "this License" refers to version 3 of the GNU Lesser
General Public License, and the "GNU GPL" refers to version 3 of the GNU
General Public License.
"The Library" refers to a covered work governed by this License,
other than an Application or a Combined Work as defined below.
An "Application" is any work that makes use of an interface provided
by the Library, but which is not otherwise based on the Library.
Defining a subclass of a class defined by the Library is deemed a mode
of using an interface provided by the Library.
A "Combined Work" is a work produced by combining or linking an
Application with the Library. The particular version of the Library
with which the Combined Work was made is also called the "Linked
Version".
The "Minimal Corresponding Source" for a Combined Work means the
Corresponding Source for the Combined Work, excluding any source code
for portions of the Combined Work that, considered in isolation, are
based on the Application, and not on the Linked Version.
The "Corresponding Application Code" for a Combined Work means the
object code and/or source code for the Application, including any data
and utility programs needed for reproducing the Combined Work from the
Application, but excluding the System Libraries of the Combined Work.
1. Exception to Section 3 of the GNU GPL.
You may convey a covered work under sections 3 and 4 of this License
without being bound by section 3 of the GNU GPL.
2. Conveying Modified Versions.
If you modify a copy of the Library, and, in your modifications, a
facility refers to a function or data to be supplied by an Application
that uses the facility (other than as an argument passed when the
facility is invoked), then you may convey a copy of the modified
version:
a) under this License, provided that you make a good faith effort to
ensure that, in the event an Application does not supply the
function or data, the facility still operates, and performs
whatever part of its purpose remains meaningful, or
b) under the GNU GPL, with none of the additional permissions of
this License applicable to that copy.
3. Object Code Incorporating Material from Library Header Files.
The object code form of an Application may incorporate material from
a header file that is part of the Library. You may convey such object
code under terms of your choice, provided that, if the incorporated
material is not limited to numerical parameters, data structure
layouts and accessors, or small macros, inline functions and templates
(ten or fewer lines in length), you do both of the following:
a) Give prominent notice with each copy of the object code that the
Library is used in it and that the Library and its use are
covered by this License.
b) Accompany the object code with a copy of the GNU GPL and this license
document.
4. Combined Works.
You may convey a Combined Work under terms of your choice that,
taken together, effectively do not restrict modification of the
portions of the Library contained in the Combined Work and reverse
engineering for debugging such modifications, if you also do each of
the following:
a) Give prominent notice with each copy of the Combined Work that
the Library is used in it and that the Library and its use are
covered by this License.
b) Accompany the Combined Work with a copy of the GNU GPL and this license
document.
c) For a Combined Work that displays copyright notices during
execution, include the copyright notice for the Library among
these notices, as well as a reference directing the user to the
copies of the GNU GPL and this license document.
d) Do one of the following:
0) Convey the Minimal Corresponding Source under the terms of this
License, and the Corresponding Application Code in a form
suitable for, and under terms that permit, the user to
recombine or relink the Application with a modified version of
the Linked Version to produce a modified Combined Work, in the
manner specified by section 6 of the GNU GPL for conveying
Corresponding Source.
1) Use a suitable shared library mechanism for linking with the
Library. A suitable mechanism is one that (a) uses at run time
a copy of the Library already present on the user's computer
system, and (b) will operate properly with a modified version
of the Library that is interface-compatible with the Linked
Version.
e) Provide Installation Information, but only if you would otherwise
be required to provide such information under section 6 of the
GNU GPL, and only to the extent that such information is
necessary to install and execute a modified version of the
Combined Work produced by recombining or relinking the
Application with a modified version of the Linked Version. (If
you use option 4d0, the Installation Information must accompany
the Minimal Corresponding Source and Corresponding Application
Code. If you use option 4d1, you must provide the Installation
Information in the manner specified by section 6 of the GNU GPL
for conveying Corresponding Source.)
5. Combined Libraries.
You may place library facilities that are a work based on the
Library side by side in a single library together with other library
facilities that are not Applications and are not covered by this
License, and convey such a combined library under terms of your
choice, if you do both of the following:
a) Accompany the combined library with a copy of the same work based
on the Library, uncombined with any other library facilities,
conveyed under the terms of this License.
b) Give prominent notice with the combined library that part of it
is a work based on the Library, and explaining where to find the
accompanying uncombined form of the same work.
6. Revised Versions of the GNU Lesser General Public License.
The Free Software Foundation may publish revised and/or new versions
of the GNU Lesser General Public License from time to time. Such new
versions will be similar in spirit to the present version, but may
differ in detail to address new problems or concerns.
Each version is given a distinguishing version number. If the
Library as you received it specifies that a certain numbered version
of the GNU Lesser General Public License "or any later version"
applies to it, you have the option of following the terms and
conditions either of that published version or of any later version
published by the Free Software Foundation. If the Library as you
received it does not specify a version number of the GNU Lesser
General Public License, you may choose any version of the GNU Lesser
General Public License ever published by the Free Software Foundation.
If the Library as you received it specifies that a proxy can decide
whether future versions of the GNU Lesser General Public License shall
apply, that proxy's public statement of acceptance of any version is
permanent authorization for you to choose that version for the
Library.
pinocchio
=========
eigenpy
===========
Dynamic computations using Spatial Algebra
Setup
-----
To compile this package, it is recommended to create a separate build
directory:
mkdir _build
cd _build
cmake [OPTIONS] ..
make install
Please note that CMake produces a `CMakeCache.txt` file which should
be deleted to reconfigure a package from scratch.
### Dependencies
The matrix abstract layer depends on several packages which
have to be available on your machine.
- Libraries:
- eigen3
- System tools:
- CMake (>=2.6)
- pkg-config
- usual compilation tools (GCC/G++, make, etc.)
- Python 2.7
- Boost python
INPUT = @CMAKE_SOURCE_DIR@/src \
@CMAKE_SOURCE_DIR@/doc/additionalDoc
IMAGE_PATH = @CMAKE_SOURCE_DIR@/doc/pictures
FILE_PATTERNS = *.cc *.cpp *.h *.hpp *.hxx
TAGFILES = \
/*
* Copyright 2010,
* François Bleibel,
* Olivier Stasse,
*
* CNRS/AIST
*
* This file is part of sot-dynamic.
* sot-dynamic is free software: you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public License
* as published by the Free Software Foundation, either version 3 of
* the License, or (at your option) any later version.
* sot-dynamic is distributed in the hope that it will be
* useful, but WITHOUT ANY WARRANTY; without even the implied warranty
* of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU Lesser General Public License for more details. You should
* have received a copy of the GNU Lesser General Public License along
* with sot-dynamic. If not, see <http://www.gnu.org/licenses/>.
*/
/** \mainpage
\section soth_section_introduction Introduction
The soth package is ...
**/
<br><br>
<hr>
<center>
<img src="./pictures/footer.jpg" Height=100>
<br>soth library documentation</br>
</center>
<hr>
</center>
</body>
</head>
<HTML>
<HEAD>
<TITLE>soth library documentation</TITLE>
<LINK HREF="package.css" REL="stylesheet" TYPE="text/css">
</HEAD>
<BODY>
\documentclass[11pt,twoside,a4paper]{article}
\usepackage{amssymb}
\usepackage{amsmath}
\newcommand{\BIN}{\begin{bmatrix}}
\newcommand{\BOUT}{\end{bmatrix}}
\newcommand{\calR}{\mathcal{R}}
\newcommand{\calE}{\mathcal{E}}
\newcommand{\repr}{\cong}
\newcommand{\dpartial}[2]{\frac{\partial{#1}}{\partial{#2}}}
\newcommand{\ddpartial}[2]{\frac{\partial^2{#1}}{\partial{#2}^2}}
\newcommand{\aRb}{\ \leftidx{^A}R_B}
\newcommand{\aMb}{\ \leftidx{^A}M_B}
\newcommand{\amb}{\ \leftidx{^A}m_B}
\newcommand{\apb}{{\ \leftidx{^A}{AB}{}}}
\newcommand{\aXb}{\ \leftidx{^A}X_B}
\newcommand{\bRa}{\ \leftidx{^B}R_A}
\newcommand{\bMa}{\ \leftidx{^B}M_A}
\newcommand{\bma}{\ \leftidx{^B}m_A}
\newcommand{\bpa}{\ \leftidx{^B}{BA}{}}
\newcommand{\bXa}{\ \leftidx{^B}X_A}
\newcommand{\ap}{\ \leftidx{^A}p}
\newcommand{\bp}{\ \leftidx{^B}p}
\newcommand{\afs}{\ \leftidx{^A}\phi}
\newcommand{\bfs}{\ \leftidx{^B}\phi}
\newcommand{\af}{\ \leftidx{^A}f}
\renewcommand{\bf}{\ \leftidx{^B}f}
\newcommand{\an}{\ \leftidx{^A}\tau}
\newcommand{\bn}{\ \leftidx{^B}\tau}
\newcommand{\avs}{\ \leftidx{^A}\nu}
\newcommand{\bvs}{\ \leftidx{^B}\nu}
\newcommand{\w}{\omega}
\newcommand{\av}{\ \leftidx{^A}v}
\newcommand{\bv}{\ \leftidx{^B}v}
\newcommand{\aw}{\ \leftidx{^A}\w}
\newcommand{\bw}{\ \leftidx{^B}\w}
\newcommand{\aI}{\ \leftidx{^A}I}
\newcommand{\bI}{\ \leftidx{^B}I}
\newcommand{\cI}{\ \leftidx{^C}I}
\newcommand{\aY}{\ \leftidx{^A}Y}
\newcommand{\bY}{\ \leftidx{^B}Y}
\newcommand{\cY}{\ \leftidx{^c}Y}
\newcommand{\aXc}{\ \leftidx{^A}X_C}
\newcommand{\aMc}{\ \leftidx{^A}M_C}
\newcommand{\aRc}{\ \leftidx{^A}R_C}
\newcommand{\apc}{\ \leftidx{^A}{AC}{}}
\newcommand{\bXc}{\ \leftidx{^B}X_C}
\newcommand{\bRc}{\ \leftidx{^B}R_C}
\newcommand{\bMc}{\ \leftidx{^B}M_C}
\newcommand{\bpc}{\ \leftidx{^B}{BC}{}}
\usepackage{leftidx}
\begin{document}
\title{SE(3) operations}
\author{N. Mansard}
\date{}
\maketitle
\section{Rigid transformation}
$$m : p \in \calE(3) \rightarrow m(p) \in E(3)$$
Transformation from B to A:
$$\amb : \bp \in \calR^3 \repr \calE(3) \ \rightarrow\ \ap = \amb(\bp) = \aMb\ \bp$$
$$ \ap = \aRb \bp + \apb$$
$$\aMb = \BIN \aRb & \apb \\ 0 & 1 \BOUT $$
Transformation from A to B:
$$\bp = \aRb^T \ap + \bpa, \quad\textrm{with }\bpa = - \aRb^T \apb$$
$$\bMa = \BIN \aRb^T & - \aRb^T \apb \\ 0 & 1 \BOUT $$
For Featherstone, $E = \bRa =\aRb^T$ and $r = \apb$. Then:
$$\bMa = \BIN \bRa & -\bRa \apb \\ 0 & 1 \BOUT = \BIN E & -E r \\ 0 & 1 \BOUT $$
$$\aMb = \BIN \bRa^T & \apb \\ 0 & 1 \BOUT = \BIN E^T & r \\ 0 & 1 \BOUT $$
\section{Composition}
$$ \aMb \bMc = \BIN \aRb \bRc & \apb + \aRb \bpc \\ 0 & 1 \BOUT $$
$$ \aMb^{-1} \aMc = \BIN \aRb^T \aRc & \aRb^T (\apc - \apb) \\ 0 & 1 \BOUT $$
\section{Motion Application}
$$\avs = \BIN \av \\ \aw \BOUT$$
$$\bvs = \bXa\avs$$
$$ \aXb = \BIN \aRb & \apb_\times \aRb \\ 0 & \aRb \BOUT $$
$$ \aXb^{-1} = \bXa = \BIN \aRb^T & -\aRb^T \apb_\times \\ 0 & \aRb^T \BOUT $$
For Featherstone, $E = \bRa =\aRb^T$ and $r = \apb$. Then:
$$ \bXa = \BIN \bRa & - \bRa \apb_\times \\ 0 & \bRa \BOUT = \BIN E & -E r_\times \\ 0 & E \BOUT$$
$$ \aXb = \BIN \bRa^T & \apb_\times \bRa^T \\ 0 & \bRa^T \BOUT = \BIN E^T & r_\times E^T \\ 0 & E^T \BOUT$$
\section{Force Application}
$$\afs = \BIN \af \\ \an \BOUT$$
$$\bfs = \bXa^* \afs$$
For any $\phi,\nu$, $\phi\dot\nu = \afs^T \avs = \bfs^T \bvs$ and then:
$$\aXb^* = \aXb^{-T} = \BIN \aRb & 0 \\ \apb_\times \aRb & \aRb \BOUT$$
(because $\apb_\times^T = - \apb_\times$).
$$\aXb^{-*} = \bXa^* = \BIN \aRb^T & 0 \\ -\aRb^T \apb_\times & \aRb^T \BOUT$$
For Featherstone, $E = \bRa =\aRb^T$ and $r = \apb$. Then:
$$\bXa^* = \BIN \bRa & 0 \\ -\bRa \apb_\times & \bRa \BOUT = \BIN E & 0 \\ - E r_\times & E \BOUT $$
$$\aXb^* = \BIN \bRa^T & 0 \\ \apb_\times \bRa^T & \bRa^T \BOUT = \BIN E^T & 0 \\ r_\times E^T & E^T \BOUT $$
\section{Inertia application}
$$\aY: \avs \rightarrow \afs = \aY \avs$$
Coordinate transform:
$$\bY = \bXa^{*} \aY \bXa^{-1}$$
since:
$$\bfs = \bXa^* \bfs = \bXa^* \aI \aXb \bvs$$
Cannonical form. The inertia about the center of mass $c$ is:
$$\cY = \BIN m & 0 \\ 0 & \cI \BOUT$$
Expressed in any non-centered coordinate system $A$:
$$\aY = \aXc^* \cI \aXc^{-1} = \BIN m & m\ ^AAC_\times^T \\ m\ ^AAC_\times & \aI + m \apc_\times \apc\times^T \BOUT $$
Changing the coordinates system from $B$ to $A$:
$$\aY = \aXb^* \bXc^* \cI \bXc^{-1} \aXb^{-1} $$
$$ = \BIN m & m [\apb + \aRb \bpc]_\times^T \\ m [\apb + \aRb \bpc]_\times & \aRb \bI \aRb^T - m [\apb + \aRb \bpc]_\times^2 \BOUT$$
Representing the spatial inertia in $B$ by the triplet $(m,\bpc,\bI)$, the expression in $A$ is:
$$ \amb: \bY = (m,\bpc,\bI) \rightarrow \aY = (m,\apb+\aRb \bpc,\aRb \bI \aRb^T)$$
Similarly, the inverse action is:
$$ \amb^{-1}: \aY \rightarrow \bY = (m,\aRb^T(^AAC-\apb),\aRb^T\aI \aRb) $$
Motion-to-force map:
$$ Y \nu = \BIN m & mc_\times^T \\ mc_\times & I+mc_\times c_\times^T \BOUT \BIN v \\ \omega \BOUT
= \BIN m v - mc \times \omega \\ mc \times v + I \omega - mc \times ( c\times \omega) \BOUT$$
Nota: the square of the cross product is:
$$\BIN x\\y\\z\BOUT_ \times^2 = \BIN 0&-z&y \\ z&0&-x \\ -y&x&0 \BOUT^2 = \BIN -y^2-z^2&xy&xz \\ xy&-x^2-z^2&yz \\ xz&yz&-x^2-y^2 \BOUT$$
There is no computational interest in using it.
\section{Cross products}
Motion-motion product:
$$\nu_1 \times \nu_2 = \BIN v_1\\\omega_1\BOUT \times \BIN v_2\\\omega_2\BOUT = \BIN v_1 \times \omega_2 + \omega_1 \times v_2 \\ \omega_1 \times \omega_2 \BOUT $$
Motion-force product:
$$\nu \times \phi = \BIN v\\\omega\BOUT \times \BIN f\\ \tau \BOUT = \BIN \omega \times f \\ \omega \times \tau + v \times f \BOUT $$
A special form of the motion-force product is often used:
\begin{align*}\nu \times (Y \nu) &= \nu \times \BIN mv - mc\times \omega \\ mc\times v + I \omega - mc\times(c\times \omega) \BOUT \\&= \BIN m \omega\times v - \omega\times(mc\times \omega) \\ \omega \times ( mc \times v) + \omega \times (I\omega) -\omega \times(c \times( mc\times \omega)) -v\times(mc \times \omega)\BOUT\end{align*}
Setting $\beta=mc \times \omega$, this product can be written:
$$\nu \times (Y \nu) = \BIN \omega \times (m v - \beta) \\ \omega \times( c \times (mv-\beta)+I\omega) - v \times \beta \BOUT$$
This last form cost five $\times$, four $+$ and one $3\times3$ matrix-vector multiplication.
\section{Joint}
We denote by $1$ the coordinate system attached to the parent (predecessor) body at the joint input, ad by $2$ the coordinate system attached to the (child) successor body at the joint output. We neglect the possible time variation of the joint model (ie the bias velocity $\sigma = \nu(q,0)$ is null).
The joint geometry is expressed by the rigid transformation from the input to the ouput, parametrized by the joint coordinate system $q \in \mathcal{Q}$:
$$ ^2m_1 \repr \ ^2M_1(q)$$
The joint velocity (i.e. the velocity of the child wrt. the parent in the child coordinate system) is:
$$^2\nu_{12} = \nu_J(q,v_q) = \ ^2S(q) v_q $$
where $^2S$ is the joint Jacobian (or constraint matrix) that define the motion subspace allowed by the joint, and $v_q$ is the joint coordinate velocity (i.e. an element of the Lie algebra associated with the joint coordinate manifold), which would be $v_q=\dot q$ when $\dot q$ exists.
The joint acceleration is:
$$^2\alpha_{12} = S \dot v_q + c_J + \ ^2\nu_{1} \times \ ^2\nu_{12}$$
where $c_J = \sum_{i=1}^{n_q} \dpartial{S}{q_i} \dot q_i$ (null in the usual cases) and $^2\nu_{1}$ is the velocity of the parent body with respect to an absolute (Galilean) coordinate system\footnote{The abosulte velocity $\nu_{1}$ is also the relative velocity wrt. the Galilean coordinate system $\Omega$. The exhaustive notation should be $\nu_{\Omega1}$ but $\nu_1$ is prefered for simplicity.}.
The joint calculations take as input the joint position $q$ and velocity $v_q$ and should output $^2M_1$, $^2\nu_{12}$ and $^2c$ (this last vector being often a trivial $0_6$ vector). In addition, the joint model should store the position of the joint input in the central coordinate system of the previous joint $^0m_1$ which is a constant value.
The joint integrator computes the exponential map associated with the joint manifold. The function inputs are the initial position $q_0$, the velocity $v_q$ and the length of the integration interval $t$. It computes $q_t$ as:
$$ q_t = q_0 + \int_0^t v_q dt$$
For the simple vectorial case where $v_q=\dot q$, we have $q_t=q_0 + t v_q$. Written in the more general case of a Lie groupe, we have $q_t = q_0 exp(t v_q)$ where $exp$ denotes the exponential map (i.e. integration of a constant vector field from the Lie algebra into the Lie group). This integration only consider first order explicit Euler. More general integrators (e.g. Runge-Kutta in Lie groupes) remains to be written. Adequate references are welcome.
\section{RNEA}
\subsection{Initialization}
$$^0\nu_0 = 0 ; \ ^0\alpha_0 = -g$$
In the following, the coordinate system $i$ is attached to the output of the joint (child body), while $lambda(i)$ is the central coordinate system attached to the parent joint. The coordinated system associated with the joint input is denoted by $i_0$. The constant rigid transformation from $\lambda(i)$ to the joint input is then $^{\lambda(i)}M_{i_0}$.
\subsection{Forward loop}
For each joint $i$, update the joint calculation $\mathbf j_i$.calc($q,v_q$). This compute $\mathbf{j}.M = \ ^{\lambda(i)}M_{i_0}(q)$, $\mathbf{j}.\nu = \ ^i\nu_{{\lambda(i)}i}(q,v_q)$, $\mathbf{j}.S = \ ^iS(q)$ and $\mathbf{j}.c = \sum_{k=1}^{n_q} \dpartial{^iS}{q_k} \dot q_k$. Attached to the joint is also its placement in body $\lambda(i)$ denoted by $\mathbf{j}.M_0 =\ ^{\lambda(i)}M_{i_0}$. Then:
$$^{\lambda(i)}M_i = \mathbf{j}.M_0 \ \mathbf{j}.M $$
$$^0M_i = \ ^0M_{\lambda(i)} \ ^{\lambda(i)}M_i$$
$$^i\nu_{i}= \ ^{\lambda(i)}X_i^{-1} \ ^{\lambda(i)}\nu_{{\lambda(i)}} + \mathbf{j}.\nu$$
$$^i\alpha_{i}= \ ^{\lambda(i)}X_i^{-1} \ ^{\lambda(i)}\alpha_{{\lambda(i)}} + \mathbf{j}.S \dot v_q + \mathbf{j}.c + \ ^i\nu_{i} \times \mathbf{j}.\nu$$
$$^i\phi_i= \ ^iY_i \ ^i\alpha_i + \ ^i\nu_i \times \ ^iY_i \ ^i\nu_i - \ ^0X_i^{-*}\ ^0\phi_i^{ext}$$
\subsection{Backward loop}
For each joint $i$ from leaf to root, do:
$$\tau_i = \mathbf{j}.S^T \ ^i\phi_i$$
$$^{\lambda(i)}\phi_{\lambda(i)} \ +\!\!= \ ^{\lambda(i)}X_i^{*} \ ^i\phi_i$$
\subsection{Nota}
It is more efficient to apply $X^{-1}$ than $X$. Similarly, it is more efficient to apply $X^{-*}$ than $X^*$. Therefore, it is better to store the transformations $^{\lambda(i)}m_i$ and $^0m_i$ than $^im_{\lambda(i)}$ and $^im_0$.
\end{document}
body {
font-family: 'Lucida Grande','Lucida Sans Unicode',Verdana,Sans-Serif;
color: #5D5D5D;
}
dl {
border: 1.5px #82b6d7 solid;
width: 97%;
padding: 5px;
color: #330077;
}
code {
color: #3C9A35;
}
td.md {
color: #0066CC;
}
h1 {
padding-top: 50px;
padding: 0px;
font-family: 'Lucida Grande','Lucida Sans Unicode',Verdana,Sans-Serif;
font-variant: small-caps;
color:#0066CC;
text-align: center;
}
h2,h3,hr {
magin-top: 15px;
padding: 0px;
font-family: 'Lucida Grande','Lucida Sans Unicode',Verdana,Sans-Serif;
font-variant: small-caps;
color:#0066CC;
}
h4 {
color: #3C9A35;
}
a:link {
font-weight: bold;
text-decoration: none;
color:#0066CC
}
a:hover, a:active {
text-decoration: underline;
color: #3C9A35;
}
a:visited {
font-weight: bold;
color: #3C9A35;
text-decoration: none;
}
DIV.memitem
{
spacing: 10px;
width:100%;
background:#FFFFFF;
font-size:100%;
line-height:normal;
border-width: 1px;
border-style: solid;
border-color: #808080;
-moz-border-radius: 8px 8px 8px 8px;
}
DIV.memproto