Commit 48a274ce authored by Florent Lamiraux's avatar Florent Lamiraux
Browse files

Initial commit

parents
[submodule "cmake"]
path = cmake
url = git@github.com:jrl-umi3218/jrl-cmakemodules.git
# Copyright 2014, Florent Lamiraux, CNRS
#
# This file is part of hpp-wholebody-step.
# hpp-wholebody-step 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.
#
# hpp-wholebody-step 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
# General Lesser Public License for more details. You should have
# received a copy of the GNU Lesser General Public License along with
# hpp-wholebody-step. If not, see <http://www.gnu.org/licenses/>.
# Requires at least CMake 2.6 to configure the package.
CMAKE_MINIMUM_REQUIRED(VERSION 2.6)
SET(CUSTOM_HEADER_DIR "hpp/wholebody-step")
INCLUDE(cmake/base.cmake)
INCLUDE(cmake/boost.cmake)
SET(PROJECT_NAME hpp-wholebody-step)
SET(PROJECT_DESCRIPTION
"Path planning for humanoid robot by sliding on the groud.")
SET(PROJECT_URL "https://github.com/laas/hpp-wholebody-step")
SET(${PROJECT_NAME}_HEADERS
include/hpp/wholebody-step/fwd.hh
include/hpp/wholebody-step/static-stability-constraint.hh
)
SET(CXX_DISABLE_WERROR 1)
# Activate hpp-util logging if requested
SET (HPP_DEBUG FALSE CACHE BOOL "trigger hpp-util debug output")
IF (HPP_DEBUG)
SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DHPP_DEBUG")
ENDIF()
SETUP_PROJECT()
ADD_REQUIRED_DEPENDENCY("hpp-constraints")
# Add dependency toward hpp-wholebody-step library in pkg-config file.
PKG_CONFIG_APPEND_LIBS("hpp-wholebody-step")
ADD_SUBDIRECTORY(src)
SETUP_PROJECT_FINALIZE()
hpp-wholebody-step-planner
--------------------------
This package implements the tools to allow generic whole-body walk
planning for a humanoid robot.
Subproject commit 370de4f4f8ca6122c7ddc13583a7a9465ccc8ad6
\documentclass {article}
\usepackage{color}
\title {Rotation matrix as an exponential}
\newcommand\reals{\mathbf{R}}
\newcommand\rot{\mathbf{r}}
\newcommand\x{\mathbf{x}}
\newcommand\y{\mathbf{y}}
\newcommand\rcross{[\rot]_{\times}}
\newcommand\xcross{[\mathbf{x}]_{\times}}
\newcommand\ycross{[\mathbf{y}]_{\times}}
\newcommand\omegacross{[\omega]_{\times}}
\newcommand\rdotcross{\left[\dot{\rot}\right]_{\times}}
\newcommand\normr{\|\rot\|}
\newcommand\e{\mathbf{e}}
\newcommand\alphap{\alpha^{\prime}}
\begin {document}
\maketitle
\section {Introduction and notation}
Let $\rot\in\reals^3$ be a vector. We denote by
\begin{itemize}
\item $$
\rcross = \left(\begin{array}{ccc}
0 & -\rot_3 & \rot_2 \\
\rot_3 & 0 & -\rot_1 \\
-\rot_2 & \rot_1 & 0
\end{array}\right)
$$
the antisymmetric matrix corresponding to the cross product by $\rot$,
\item $R = \exp (\rcross)$ the rotation matrix corresponding to the rotation about
$\rot$.
\end{itemize}
According to Rodrigues formula, we have
\begin{equation}\label{eq:rodrigues}
R = \exp (\rcross) = I + \frac{\sin \normr}{\normr}\rcross +
\frac{1 - \cos \normr}{\normr^2}\rcross^2
\end{equation}
We define
\begin{eqnarray}
\label{eq:alpha1}
\alpha_1 (x) &=& \frac{\sin x}{x}\\
\label{eq:alpha2}
\alpha_2 (x) &=& \frac{1 - \cos x}{x^2}\\
\label{eq:alpha3}
\alphap_1 (x) &=& \frac{x \cos x - \sin x}{x^2}\\
\label{eq:alpha4}
\alphap_2 (x) &=& \frac{x \sin x -2(1-\cos x)}{x^3}
\end{eqnarray}
With this notation, we can write
\begin{equation}\label{eq:rodrigues2}
R = I + \alpha_1 (\normr) \rcross + \alpha_2 (\normr) \rcross^2
\end{equation}
\subsection {Powers of $\rcross$}
Let us compute the powers of $\rcross$:
\begin{eqnarray*}
\rcross^2 &=& \left(\begin{array}{ccc}
-\rot_2^2-\rot_3^2 & \rot_2\rot_1 & \rot_3\rot_1 \\
\rot_1\rot_2 & -\rot_1^2-\rot_3^2 & \rot_3\rot_2 \\
\rot_1\rot_3 & \rot_2\rot_3 & -\rot_1^2-\rot_2^2
\end{array}\right) =
\rot\rot^T - \normr^2 I_3\\
\rcross^3 &=& \rcross\rot\rot^T - \normr^2 \rcross = - \normr^2 \rcross\\
\rcross^4 &=& - \normr^2 \rcross^2
\end{eqnarray*}
Replacing in~(\ref{eq:rodrigues2}), we get
\begin{eqnarray}
R &=& I + \alpha_1 (\normr) \rcross + \alpha_2 (\normr) \rot\rot^T - \alpha_2 (\normr) \normr^2 I_3\\
\label{eq:rodrigues3}
&=&I + \alpha_1 (\normr) \rcross + \alpha_2 (\normr) \rot\rot^T + (\cos\normr-1) I_3
\end{eqnarray}
\section {Derivative}
We wish to find a relation between the derivative of $R$ as an antisymmetric
matrix and the derivative of vector $\rot$. We recall that
$$
\omegacross = \dot{R}R^T
$$
\begin{eqnarray*}
\normr = \sqrt{\rot^T\rot} && \frac{d}{dt}{\normr} = \frac{\rot^T\dot{\rot}}{\normr}
\end{eqnarray*}
Let us differentiate~(\ref{eq:rodrigues2})
$$
\dot{R} = \frac{\dot{\rot}^T\rot}{\normr} \alphap_1 (\normr) \rcross + \frac{\dot{\rot}^T\rot}{\normr}\alphap_2 (\normr) \rot\rot^T -\frac{\dot{\rot}^T\rot}{\normr} \sin\normr I_3 + \alpha_1 (\normr) \rdotcross + \alpha_2 (\normr) (\dot{\rot}\rot^T + \rot\dot{\rot}^T)
$$
To make notation lighter, we set $u = \frac{\rot^T\dot{\rot}}{\normr}$. Then
\begin{eqnarray*}
\omegacross &=& \dot{R}R^T\\
&=& \left(u \alphap_1 \rcross + u\alphap_2 \rot\rot^T -u \sin\normr I_3 + \alpha_1 \rdotcross + \alpha_2 (\dot{\rot}\rot^T + \rot\dot{\rot}^T)\right)\\
&&\left(I - \alpha_1 \rcross + \alpha_2 \rot\rot^T + (\cos\normr-1) I_3\right)\\
&=& u \alphap_1 \rcross - u \alphap_1\alpha_1 \rcross^2 + u \alphap_1\alpha_2 \rcross\rot\rot^T + u \alphap_1 (\cos\normr-1) \rcross\\
&& +u\alphap_2 \rot\rot^T - u\alpha_1\alphap_2\rot\rot^T\rcross + u\alphap_2\alpha_2\normr^2\rot\rot^T + u\alphap_2 (\cos\normr-1) \rot\rot^T\\
&& -u \sin\normr I_3 + u \sin\normr\alpha_1 \rcross - u \sin\normr\alpha_2 \rot\rot^T - u \sin\normr(\cos\normr-1) I_3\\
&& +\alpha_1 \rdotcross - \alpha_1^2 \rdotcross\rcross + \alpha_1 \alpha_2 \rdotcross\rot\rot^T + \alpha_1(\cos\normr-1)\rdotcross\\
&&+\alpha_2 (\dot{\rot}\rot^T + \rot\dot{\rot}^T) - \alpha_1 \alpha_2 (\dot{\rot}\rot^T + \rot\dot{\rot}^T)\rcross + \alpha_2 \alpha_2 (\dot{\rot}\rot^T + \rot\dot{\rot}^T)\rot\rot^T + (\cos\normr-1)\alpha_2 (\dot{\rot}\rot^T + \rot\dot{\rot}^T)\\
\end{eqnarray*}
We notice that $\rcross \rot = 0$ and that $\rot^T\rcross = 0$. Thus by substituting $\rcross^2$ by $\rot\rot^T-\normr^2 I_3$,
\begin{eqnarray*}
\omegacross &=&
u \alphap_1 \rcross - u \alphap_1\alpha_1 (\rot\rot^T-\normr^2 I_3) + u \alphap_1 (\cos\normr-1) \rcross\\
&& + u\alphap_2 \rot\rot^T + u\alphap_2\alpha_2\normr^2\rot\rot^T + u\alphap_2 (\cos\normr-1) \rot\rot^T\\
&& -u \sin\normr I_3 + u \sin\normr\alpha_1 \rcross - u \sin\normr\alpha_2 \rot\rot^T - u \sin\normr(\cos\normr-1) I_3\\
&& +\alpha_1 \rdotcross - \alpha_1^2 \rdotcross\rcross + \alpha_1 \alpha_2 \rdotcross\rot\rot^T + \alpha_1(\cos\normr-1)\rdotcross\\
&&+\alpha_2 (\dot{\rot}\rot^T + \rot\dot{\rot}^T) - \alpha_1 \alpha_2 \rot\dot{\rot}^T\rcross + \alpha_2^2\normr^2\dot{\rot}\rot^T + \alpha_2^2\rot\dot{\rot}^T\rot\rot^T + (\cos\normr-1)\alpha_2 (\dot{\rot}\rot^T + \rot\dot{\rot}^T)
\end{eqnarray*}
We notice that $\rot\dot{\rot}^T\rot\rot^T=u\normr\rot\rot^T$
\begin{eqnarray*}
\omegacross &=&
- u \alphap_1\alpha_1 (\rot\rot^T-\normr^2 I_3) + u \alphap_1 \cos\normr \rcross\\
&& + u\alphap_2 \rot\rot^T + u\alphap_2\alpha_2\normr^2\rot\rot^T + u\alphap_2 (\cos\normr-1) \rot\rot^T\\
&& + u \sin\normr\alpha_1 \rcross - u \sin\normr\alpha_2 \rot\rot^T - u \sin\normr\cos\normr I_3\\
&& +\alpha_1 \rdotcross - \alpha_1^2 \rdotcross\rcross + \alpha_1 \alpha_2 \rdotcross\rot\rot^T + \alpha_1(\cos\normr-1)\rdotcross\\
&&- \alpha_1 \alpha_2 \rot\dot{\rot}^T\rcross + \alpha_2^2\normr^2\dot{\rot}\rot^T + \alpha_2^2u\normr\rot\rot^T + \cos\normr\alpha_2 (\dot{\rot}\rot^T + \rot\dot{\rot}^T)
\end{eqnarray*}
$$
u\alphap_2\alpha_2\normr^2\rot\rot^T + u\alphap_2 (\cos\normr-1) \rot\rot^T =
u\alphap_2((1-\cos\normr)+(\cos\normr-1)) \rot\rot^T = 0
$$
\begin{eqnarray*}
\omegacross &=&
- u \alphap_1\alpha_1 (\rot\rot^T-\normr^2 I_3) + u \alphap_1 \cos\normr \rcross + u\alphap_2 \rot\rot^T\\
&& + u \sin\normr\alpha_1 \rcross - u \sin\normr\alpha_2 \rot\rot^T - u \sin\normr\cos\normr I_3\\
&& +\alpha_1 \rdotcross - \alpha_1^2 \rdotcross\rcross + \alpha_1 \alpha_2 \rdotcross\rot\rot^T + \alpha_1(\cos\normr-1)\rdotcross\\
&&- \alpha_1 \alpha_2 \rot\dot{\rot}^T\rcross + \alpha_2^2\normr^2\dot{\rot}\rot^T + \alpha_2^2u\normr\rot\rot^T + \cos\normr\alpha_2 (\dot{\rot}\rot^T + \rot\dot{\rot}^T)\\
&=& - u \alphap_1\alpha_1 \rot\rot^T + u \alphap_1\alpha_1\normr^2 I_3 + u \alphap_1 \cos\normr \rcross + u\alphap_2 \rot\rot^T\\
&& + u \sin\normr\alpha_1 \rcross - u \sin\normr\alpha_2 \rot\rot^T - u \sin\normr\cos\normr I_3\\
&& +\alpha_1 \rdotcross - \alpha_1^2 \rdotcross\rcross + \alpha_1 \alpha_2 \rdotcross\rot\rot^T + \alpha_1(\cos\normr-1)\rdotcross\\
&&- \alpha_1 \alpha_2 \rot\dot{\rot}^T\rcross + \alpha_2^2\normr^2\dot{\rot}\rot^T + \alpha_2^2u\normr\rot\rot^T + \cos\normr\alpha_2\dot{\rot}\rot^T + \cos\normr\alpha_2\rot\dot{\rot}^T\\
&=& u \left(-\alphap_1\alpha_1 + \alphap_2 - \sin\normr\alpha_2 + \alpha_2^2\normr\right)\rot\rot^T \\
&&+ u\left(\alphap_1 \cos\normr + \alpha_1\sin\normr\right) \rcross\\
&&+ u\left(\alphap_1\alpha_1\normr^2 - \sin\normr\cos\normr\right) I_3\\
&& +\alpha_1 \rdotcross - \alpha_1^2 \rdotcross\rcross + \alpha_1 \alpha_2 \rdotcross\rot\rot^T + \alpha_1(\cos\normr-1)\rdotcross\\
&&- \alpha_1 \alpha_2 \rot\dot{\rot}^T\rcross + \alpha_2^2\normr^2\dot{\rot}\rot^T + \cos\normr\alpha_2\dot{\rot}\rot^T + \cos\normr\alpha_2\rot\dot{\rot}^T\\
\end{eqnarray*}
\begin{eqnarray*}
-\alphap_1\alpha_1 + \alphap_2 - \sin\normr\alpha_2 + \alpha_2^2\normr
&=&
-\frac{\normr \cos \normr - \sin \normr}{\normr^2}\frac{\sin \normr}{\normr} + \frac{\normr \sin \normr -2(1-\cos \normr)}{\normr^3}\\
&& - \sin\normr\frac{1 - \cos \normr}{\normr^2} + \left(\frac{1 - \cos \normr}{\normr^2}\right)^2\normr\\
&=&\frac{-\normr \cos \normr \sin \normr + \sin^2 \normr}{\normr^3}+\frac{\normr \sin \normr -2+2\cos \normr}{\normr^3}\\
&& +\frac{-\normr\sin\normr + \normr\sin\normr\cos \normr}{\normr^3}\\
&&+\frac{1 - 2\cos \normr + \cos^2 \normr}{\normr^3}\\
&=& 0
\end{eqnarray*}
\begin{eqnarray*}
\alphap_1\alpha_1\normr^2 - \sin\normr\cos\normr &=&
\frac{\normr \cos \normr - \sin \normr}{\normr^2}\frac{\sin \normr}{\normr}\normr^2- \sin\normr\cos\normr\\
&=&
\frac{\normr\cos\normr\sin\normr - \sin^2\normr}{\normr}- \sin\normr\cos\normr\\&=&-\frac{\sin^2\normr}{\normr}
\end{eqnarray*}
\begin{eqnarray*}
\omegacross &=&
+ u\left(\alphap_1 \cos\normr + \alpha_1\sin\normr\right) \rcross -u \frac{\sin^2\normr}{\normr} I_3\\
&& -\alpha_1^2 \rdotcross\rcross + \alpha_1 \alpha_2 \rdotcross\rot\rot^T + \alpha_1\cos\normr\rdotcross\\
&&- \alpha_1 \alpha_2 \rot\dot{\rot}^T\rcross + \alpha_2^2\normr^2\dot{\rot}\rot^T + \cos\normr\alpha_2\dot{\rot}\rot^T + \cos\normr\alpha_2\rot\dot{\rot}^T\\
\end{eqnarray*}
\begin{eqnarray*}
\alpha_2^2\normr^2\dot{\rot}\rot^T + \cos\normr\alpha_2\dot{\rot}\rot^T &=&
\alpha_2\left(\alpha_2\normr^2 + \cos\normr\right)\dot{\rot}\rot^T\\
\alpha_2\normr^2 + \cos\normr &=& \frac{1 - \cos \normr}{\normr^2}\normr^2 + \cos\normr = 1
\end{eqnarray*}
\begin{eqnarray*}
\omegacross &=&
+ u\left(\alphap_1 \cos\normr + \alpha_1\sin\normr\right) \rcross -u \frac{\sin^2\normr}{\normr} I_3\\
&& -\alpha_1^2 \rdotcross\rcross + \alpha_1 \alpha_2 \rdotcross\rot\rot^T + \alpha_1\cos\normr\rdotcross\\
&&- \alpha_1 \alpha_2 \rot\dot{\rot}^T\rcross + \alpha_2\dot{\rot}\rot^T + \cos\normr\alpha_2\rot\dot{\rot}^T\\
\end{eqnarray*}
Using~(\ref{eq:xcrossycross}),
\begin{eqnarray*}
\omegacross &=&
u\left(\alphap_1 \cos\normr + \alpha_1\sin\normr\right) \rcross -u \frac{\sin^2\normr}{\normr} I_3\\
&& -\alpha_1^2 (\rot\dot{\rot}^T-\rot^T\dot{\rot} I_3) + \alpha_1 \alpha_2 \rdotcross\rot\rot^T + \alpha_1\cos\normr\rdotcross\\
&&- \alpha_1 \alpha_2 \rot\dot{\rot}^T\rcross + \alpha_2\dot{\rot}\rot^T + \cos\normr\alpha_2\rot\dot{\rot}^T\\
&=& u\left(\alphap_1 \cos\normr + \alpha_1\sin\normr\right) \rcross -u \frac{\sin^2\normr}{\normr} I_3\\
&& -\alpha_1^2\rot\dot{\rot}^T+\alpha_1^2\rot^T\dot{\rot} I_3 + \alpha_1 \alpha_2 \rdotcross\rot\rot^T + \alpha_1\cos\normr\rdotcross\\
&&- \alpha_1 \alpha_2 \rot\dot{\rot}^T\rcross + \alpha_2\dot{\rot}\rot^T + \cos\normr\alpha_2\rot\dot{\rot}^T\\
\end{eqnarray*}
\begin{eqnarray*}
-u \frac{\sin^2\normr}{\normr} I_3 + \alpha_1^2\rot^T\dot{\rot} I_3 &=&
-\frac{\rot^T\dot{\rot}}{\normr}\frac{\sin^2\normr}{\normr}+ \alpha_1^2\rot^T\dot{\rot} = 0
\end{eqnarray*}
\begin{eqnarray*}
\omegacross &=&
u\left(\alphap_1 \cos\normr + \alpha_1\sin\normr\right) \rcross\\
&& -\alpha_1^2\rot\dot{\rot}^T+ \alpha_1 \alpha_2 \rdotcross\rot\rot^T + \alpha_1\cos\normr\rdotcross\\
&&- \alpha_1 \alpha_2 \rot\dot{\rot}^T\rcross + \alpha_2\dot{\rot}\rot^T + \cos\normr\alpha_2\rot\dot{\rot}^T\\
\end{eqnarray*}
\begin{eqnarray*}
-\alpha_1^2\rot\dot{\rot}^T+ \cos\normr\alpha_2\rot\dot{\rot}^T &=&
\left(-\frac{\sin^2\normr}{\normr^2}+\cos\normr\frac{1-\cos\normr}{\normr^2}\right)\rot\dot{\rot}^T\\
&=&\frac{-\sin^2\normr-\cos^2\normr+\cos\normr}{\normr^2}\rot\dot{\rot}^T
=-\alpha_2\rot\dot{\rot}^T\\
\end{eqnarray*}
\begin{eqnarray*}
\omegacross &=&
u\left(\alphap_1 \cos\normr + \alpha_1\sin\normr\right) \rcross\\
&& + \alpha_2\left(\dot{\rot}\rot^T -\rot\dot{\rot}^T\right)+ \alpha_1 \alpha_2 \rdotcross\rot\rot^T + \alpha_1\cos\normr\rdotcross\\
&&- \alpha_1 \alpha_2 \rot\dot{\rot}^T\rcross\\
\end{eqnarray*}
Using~(\ref{eq:dotrrT-rdotrT}),
\begin{eqnarray*}
\omegacross &=&
u\left(\alphap_1 \cos\normr + \alpha_1\sin\normr\right) \rcross + \alpha_2\left[\rot\times\dot{\rot}\right]_{\times} + \alpha_1\cos\normr\rdotcross\\
&& + \alpha_1 \alpha_2 \left(\rdotcross\rot\rot^T - \rot\dot{\rot}^T\rcross\right)\\
\end{eqnarray*}
Finally, using~(\ref{eq:ouf}),
\begin{eqnarray*}
\omegacross &=&
u\left(\alphap_1 \cos\normr + \alpha_1\sin\normr\right) \rcross + \alpha_2\left[\rot\times\dot{\rot}\right]_{\times} + \alpha_1\cos\normr\rdotcross\\
&& + \alpha_1 \alpha_2 \left(\normr^2\rdotcross - \left[\rot\rot^T\dot{\rot}\right]_{\times}\right)\\
&=&u\left(\alphap_1 \cos\normr + \alpha_1\sin\normr\right) \rcross + \alpha_2\left[\rot\times\dot{\rot}\right]_{\times}\\
&& + \alpha_1\left(\cos\normr + \alpha_2\normr^2\right)\rdotcross - \alpha_1 \alpha_2\left[\rot\rot^T\dot{\rot}\right]_{\times}\\
&=&u\left(\alphap_1 \cos\normr + \alpha_1\sin\normr\right) \rcross + \alpha_2\left[\rot\times\dot{\rot}\right]_{\times} + \alpha_1\rdotcross - \alpha_1 \alpha_2\left[\rot\rot^T\dot{\rot}\right]_{\times}\\
&=&u\frac{\normr -\sin\normr\cos\normr}{\normr^2} \rcross + \alpha_2\left[\rot\times\dot{\rot}\right]_{\times} + \alpha_1\rdotcross - \alpha_1 \alpha_2\left[\rot\rot^T\dot{\rot}\right]_{\times}\\
\end{eqnarray*}
we get an expression of $\omega$ as a vector:
\begin{eqnarray*}
\omega &=&u\frac{\normr -\sin\normr\cos\normr}{\normr^2} \rot + \alpha_2\rot\times\dot{\rot} + \alpha_1\dot{\rot} - \alpha_1 \alpha_2\rot\rot^T\dot{\rot}\\
&=&\frac{\normr -\sin\normr\cos\normr}{\normr^3}(\rot^T\dot{\rot}) \rot + \alpha_2\rot\times\dot{\rot} + \alpha_1\dot{\rot} - \alpha_1 \alpha_2\rot\rot^T\dot{\rot}\\
&=&\frac{\normr -\sin\normr\cos\normr}{\normr^3}\rot\rot^T\dot{\rot} + \alpha_2\rot\times\dot{\rot} + \alpha_1\dot{\rot} - \alpha_1 \alpha_2\rot\rot^T\dot{\rot}\\
&=&\frac{\normr -\sin\normr}{\normr^3}\rot\rot^T\dot{\rot} + \alpha_2\rot\times\dot{\rot} + \alpha_1\dot{\rot}\\
\end{eqnarray*}
If we define the Jacobian matrix $J(\rot)$ such that
$$
\dot{\rot} = J(\rot)\omega
$$
we have
\begin{eqnarray*}
J^{-1} &=& \frac{\normr -\sin\normr}{\normr^3}\rot\rot^T + \alpha_2\rcross + \alpha_1 I_3\\
&=& \frac{\normr -\sin\normr}{\normr^3}\rot\rot^T + \frac{1 - \cos \normr}{\normr^2}\rcross + \frac{\sin \normr}{\normr} I_3\\
&=& \frac{\normr -\sin\normr}{\normr^3}(\rcross^2 + \normr^2 I_3) + \frac{1 - \cos \normr}{\normr^2}\rcross + \frac{\sin \normr}{\normr} I_3\\
&=& I_3 + \frac{1 - \cos \normr}{\normr^2}\rcross + \frac{\normr -\sin\normr}{\normr^3}\rcross^2\\
\end{eqnarray*}
\section{Computation of the Jacobian}
We need to inverse $J^{-1}$. For that, we make the assumption that $J$ is of the following form:
\begin{eqnarray*}
J &=& I_3 + \beta_1 \rcross + \beta_2 \rcross^2\\
JJ^{-1} &=& \left(I_3 + \beta_1 \rcross + \beta_2 \rcross^2\right)\left(I_3 + \frac{1 - \cos \normr}{\normr^2}\rcross + \frac{\normr -\sin\normr}{\normr^3}\rcross^2\right)\\
&=& I_3 + \frac{1 - \cos \normr}{\normr^2}\rcross + \frac{\normr -\sin\normr}{\normr^3}\rcross^2\\
&& + \beta_1 \rcross + \beta_1 \frac{1 - \cos \normr}{\normr^2}\rcross^2 + \beta_1 \frac{\normr -\sin\normr}{\normr^3}\rcross^3\\
&& + \beta_2 \rcross^2 + \beta_2 \frac{1 - \cos \normr}{\normr^2}\rcross^3 + \beta_2 \frac{\normr -\sin\normr}{\normr^3}\rcross^4\\
&=& I_3 + \left(\frac{1 - \cos \normr}{\normr^2} + \beta_1\right)\rcross + \left(\frac{\normr -\sin\normr}{\normr^3} + \beta_1 \frac{1 - \cos \normr}{\normr^2} + \beta_2\right)\rcross^2\\
&& + \left(\beta_1 \frac{\normr -\sin\normr}{\normr^3} + \beta_2 \frac{1 - \cos \normr}{\normr^2}\right)(- \normr^2 \rcross) + \beta_2 \frac{\normr -\sin\normr}{\normr^3}(- \normr^2 \rcross^2)\\
&=& I_3 + \left(\frac{1 - \cos \normr}{\normr^2} + \beta_1\right)\rcross + \left(\frac{\normr -\sin\normr}{\normr^3} + \beta_1 \frac{1 - \cos \normr}{\normr^2} + \beta_2\right)\rcross^2\\
&& + \left(- \beta_1 \frac{\normr -\sin\normr}{\normr} - \beta_2 (1 - \cos \normr)\right)\rcross - \beta_2 \frac{\normr -\sin\normr}{\normr}\rcross^2\\
&=& I_3 + \left(\frac{1 - \cos \normr}{\normr^2} + \beta_1 \frac{\sin\normr}{\normr} - \beta_2 (1 - \cos \normr)\right)\rcross \\
&& + \left(\frac{\normr -\sin\normr}{\normr^3} + \beta_1 \frac{1 - \cos \normr}{\normr^2} + \beta_2 \frac{\sin\normr}{\normr}\right)\rcross^2 \\
\end{eqnarray*}
We get the following linear system
\begin{eqnarray*}
\normr\sin\normr \beta_1 - (1 - \cos \normr)\normr^2 \beta_2 &=& \cos \normr - 1 \\
\normr(1 - \cos \normr)\beta_1 + \normr^2\sin\normr \beta_2 &=& \sin\normr - \normr
\end{eqnarray*}
$$
\Delta = \normr^3\sin^2\normr + \normr^3 (1-\cos\normr)^2 = 2\normr^3(1 -\cos\normr)
$$
Hence
\begin{eqnarray*}
\beta_1 &=& \frac{1}{2\normr^3(1 -\cos\normr)}\left(\normr^2\sin\normr (\cos \normr - 1) + (1 - \cos \normr)\normr^2(\sin\normr - \normr)\right) \\
\beta_2 &=& \frac{1}{2\normr^3(1 -\cos\normr)}\left(-\normr(1 - \cos \normr)(\cos \normr - 1) + \normr\sin\normr(\sin\normr - \normr)\right)\\
\beta_1 &=& \frac{1}{2\normr^3(1 -\cos\normr)}\left(-(1 - \cos \normr)\normr^3\right) = -\frac{1}{2}\\
\beta_2 &=& \frac{1}{2\normr^2(1 -\cos\normr)}\left(-2\cos \normr + 2 - \normr\sin\normr\right) = \frac{1}{\normr^2} - \frac{\sin\normr}{2\normr(1-\cos\normr)}\\
\end{eqnarray*}
and finally
$$
J = I_3 -\frac{1}{2}\rcross + \left(\frac{1}{\normr^2} - \frac{\sin\normr}{2\normr(1-\cos\normr)}\right)\rcross^2
$$
\section{Appendix}
\begin{eqnarray}
\xcross \ycross &=&
\left(\begin{array}{ccc}
-x_3y_3 - x_2y_2 & x_2y_1 & x_3y_1\\
x_1y_2 & -x_3y_3-x_1y_1 & x_3y_2 \\
x_1y_3 & x_2y_3 & -x_1y_1 - x_2y_2
\end{array}\right)\\
\label{eq:xcrossycross}
&=& \y\x^T-\x^T\y I_3
\end{eqnarray}
\begin{eqnarray}
\dot{\rot}\rot^T-\rot\dot{\rot}^T &=&
\left(\begin{array}{ccc}
0 & \dot{\rot}_1\rot_2 - \dot{\rot}_2\rot_1 & \dot{\rot}_1\rot_3 - \dot{\rot}_3\rot_1\\
\dot{\rot}_2\rot_1 - \dot{\rot}_1\rot_2 & 0 & \dot{\rot}_2\rot_3 - \dot{\rot}_3\rot_2\\
\dot{\rot}_3\rot_1 - \dot{\rot}_1\rot_3 & \dot{\rot}_3\rot_2 - \dot{\rot}_2\rot_3 & 0\end{array}\right)\\
\label{eq:dotrrT-rdotrT}
&=& \left[\rot\times\dot{\rot}\right]_{\times}
\end{eqnarray}
{\tiny
\begin{eqnarray*}
&\rdotcross\rot\rot^T-\rot\dot{\rot}^T\rcross&\\
&=&\\
&\left(\begin{array}{ccc}
0&-(\rot_1^2+\rot_2^2)\dot{\rot}_3+\rot_2\rot_3\dot{\rot}_2+\rot_1\rot_3\dot{\rot}_1 & (\rot_1^2+\rot_3^2)\dot{\rot}_2-\rot_1\rot_2\dot{\rot}_1-\rot_2\rot_3\dot{\rot}_3\\
+(\rot_1^2+\rot_2^2)\dot{\rot}_3-\rot_2\rot_3\dot{\rot}_2-\rot_1\rot_3\dot{\rot}_1 & 0 & -(\rot_2^2+\rot_3^2)\dot{\rot}_1+\rot_1\rot_2\dot{\rot}_2+\rot_1\rot_3\dot{\rot}_3\\
-(\rot_1^2+\rot_3^2)\dot{\rot}_2+\rot_1\rot_2\dot{\rot}_1+\rot_2\rot_3\dot{\rot}_3 & (\rot_2^2+\rot_3^2)\dot{\rot}_1-\rot_1\rot_2\dot{\rot}_2-\rot_1\rot_3\dot{\rot}_3 & 0
\end{array}\right)&
\end{eqnarray*}
}
\begin{eqnarray*}
\rdotcross\rot\rot^T-\rot\dot{\rot}^T\rcross&=&
\left[\begin{array}{c}
(\rot_2^2+\rot_3^2)\dot{\rot}_1-\rot_1\rot_2\dot{\rot}_2-\rot_1\rot_3\dot{\rot}_3\\
-\rot_1\rot_2\dot{\rot}_1+(\rot_1^2+\rot_3^2)\dot{\rot}_2-\rot_2\rot_3\dot{\rot}_3\\
-\rot_1\rot_3\dot{\rot}_1-\rot_2\rot_3\dot{\rot}_2+(\rot_1^2+\rot_2^2)\dot{\rot}_3\end{array}\right]_{\times}
\end{eqnarray*}
\begin{eqnarray*}
\left(\begin{array}{c}
(\rot_2^2+\rot_3^2)\dot{\rot}_1-\rot_1\rot_2\dot{\rot}_2-\rot_1\rot_3\dot{\rot}_3\\
-\rot_1\rot_2\dot{\rot}_1+(\rot_1^2+\rot_3^2)\dot{\rot}_2-\rot_2\rot_3\dot{\rot}_3\\
-\rot_1\rot_3\dot{\rot}_1-\rot_2\rot_3\dot{\rot}_2+(\rot_1^2+\rot_2^2)\dot{\rot}_3\end{array}\right) &=&
\left(\begin{array}{ccc}
\rot_2^2+\rot_3^2&-\rot_1\rot_2&-\rot_1\rot_3\\
-\rot_1\rot_2&\rot_1^2+\rot_3^2&-\rot_2\rot_3\\
-\rot_1\rot_3&-\rot_2\rot_3&\rot_1^2+\rot_2^2\end{array}\right) \dot{\rot}\\
\left(\begin{array}{ccc}
\rot_2^2+\rot_3^2&-\rot_1\rot_2&-\rot_1\rot_3\\
-\rot_1\rot_2&\rot_1^2+\rot_3^2&-\rot_2\rot_3\\
-\rot_1\rot_3&-\rot_2\rot_3&\rot_1^2+\rot_2^2\end{array}\right) &=& \normr^2 I_3 - \rot\rot^T
\end{eqnarray*}
Hence
\begin{eqnarray}\label{eq:ouf}
\rdotcross\rot\rot^T-\rot\dot{\rot}^T\rcross&=&
\normr^2\rdotcross - \left[\rot\rot^T\dot{\rot}\right]_{\times}
\end{eqnarray}
\end {document}
///
/// Copyright (c) 2014 CNRS
/// Authors: Florent Lamiraux
///
///
// This file is part of hpp-wholebody-step.
// hpp-wholebody-step-planner 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.
//
// hpp-wholebody-step-planner 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
// General Lesser Public License for more details. You should have
// received a copy of the GNU Lesser General Public License along with
// hpp-wholebody-step-planner. If not, see
// <http://www.gnu.org/licenses/>.
#ifndef HPP_WHOLEBODY_STEP_FWD_HH
# define HPP_WHOLEBODY_STEP_FWD_HH
# include <hpp/core/fwd.hh>
namespace hpp {
namespace wholebodyStep {
typedef model::JointPtr_t JointPtr_t;
typedef core::ConfigProjectorPtr_t ConfigProjectorPtr_t;
typedef core::ConfigProjector ConfigProjector;
typedef core::Configuration_t Configuration_t;
typedef core::DifferentiableFunction_t DifferentiableFunction_t;
typedef model::HumanoidRobotShPtr HumanoidRobotPtr_t;
typedef core::value_type value_type;
typedef core::size_type size_type;
} // wholeBodyStepPlanner
} // namespace hpp
#endif // HPP_WHOLEBODY_STEP_FWD_HH
///
/// Copyright (c) 2014 CNRS
/// Authors: Florent Lamiraux
///
///
// This file is part of hpp-wholebody-step.
// hpp-wholebody-step-planner 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.
//
// hpp-wholebody-step-planner 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
// General Lesser Public License for more details. You should have
// received a copy of the GNU Lesser General Public License along with
// hpp-wholebody-step-planner. If not, see
// <http://www.gnu.org/licenses/>.
#ifndef HPP_WHOLEBODY_STEP_STATIC_STABILITY_CONSTRAINT_HH
# define HPP_WHOLEBODY_STEP_STATIC_STABILITY_CONSTRAINT_HH
# include <hpp/model/humanoid-robot.hh>
# include <hpp/wholebody-step/fwd.hh>
# include <hpp/wholebody-step/config.hh>
namespace hpp {
namespace wholebodyStep {
/// Create quasi-static stability constraint for a humanoid robot
/// \param robot the humanoid robot,
/// \param configuration the configuration of the robot satisfying
/// the constraint,
/// \param errorThreshold norm threshold below which constraints are
/// considered satisfied,
/// \param maxNumberofIterations maximal number of iterations for
/// solving constraints.
/// The constraint makes the feet of the robot slide on a horizontal ground
/// and the center of mass project at a constant position with respect to
/// the feet.
ConfigProjectorPtr_t
createSlidingStabilityConstraint (const HumanoidRobotPtr_t& robot,
const Configuration_t& configuration,
value_type errorThreshold,
size_type maxNumberofIterations);
} // namespace wholebodyStep
} // namespace hpp
#endif // HPP_WHOLEBODY_STEP_STATIC_STABILITY_CONSTRAINT_HH
# Copyright (c) 2014 CNRS
# Author: Florent Lamiraux
#
# This file is part of hpp-wholebody-step.
# hpp-wholebody-step 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.
#
# hpp-wholebody-step 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
# General Lesser Public License for more details. You should have
# received a copy of the GNU Lesser General Public License along with
# hpp-wholebody-step. If not, see
# <http://www.gnu.org/licenses/>.
SET(SOURCES
static-stability-constraint.cc
)
ADD_LIBRARY(${PROJECT_NAME} SHARED ${SOURCES})
INSTALL(TARGETS ${PROJECT_NAME} DESTINATION lib)
PKG_CONFIG_USE_DEPENDENCY(${PROJECT_NAME} hpp-constraints)
///
/// Copyright (c) 2014 CNRS
/// Authors: Florent Lamiraux
///
///
// This file is part of hpp-wholebody-step.
// hpp-wholebody-step-planner 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.
//
// hpp-wholebody-step-planner 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
// General Lesser Public License for more details. You should have
// received a copy of the GNU Lesser General Public License along with
// hpp-wholebody-step-planner. If not, see
// <http://www.gnu.org/licenses/>.
#include <hpp/model/humanoid-robot.hh>
#include <hpp/model/joint.hh>
#include <hpp/core/config-projector.hh>
#include <hpp/constraints/orientation.hh>
#include <hpp/constraints/relative-orientation.hh>
#include <hpp/constraints/position.hh>
#include <hpp/constraints/relative-position.hh>
#include <hpp/wholebody-step/static-stability-constraint.hh>
namespace hpp {
namespace wholebodyStep {
using hpp::constraints::Orientation;
using hpp::constraints::RelativeOrientation;
using hpp::constraints::Position;
using hpp::constraints::RelativePosition;
using hpp::constraints::OrientationPtr_t;
using hpp::constraints::RelativeOrientationPtr_t;
using hpp::constraints::PositionPtr_t;
using hpp::constraints::RelativePositionPtr_t;
ConfigProjectorPtr_t
createSlidingStabilityConstraint (const HumanoidRobotPtr_t& robot,
const Configuration_t& configuration,
value_type errorThreshold,
size_type maxNumberofIterations)
{
robot->currentConfiguration (configuration);
robot->computeForwardKinematics ();
JointPtr_t joint1 = robot->getLeftAnkle ();
JointPtr_t joint2 = robot->getRightAnkle ();
const matrix4d& M1 = joint1->currentTransformation ();
const matrix4d& M2 = joint2->currentTransformation ();
ConfigProjectorPtr_t configProjector
(ConfigProjector::create (robot, "Sliding static stability",
errorThreshold, maxNumberofIterations));
// Relative orientation of the feet
matrix3d reference = M1.topLeftCorner <3,3> ().transpose ()*
M2.topLeftCorner <3,3> ();
configProjector->addConstraint
(RelativeOrientation::create (robot, joint1, joint2, reference));
// Relative position of the feet
vector3d local1; local1.setZero ();
vector3d global1 = M1.topRightCorner <3,1> ();