LAB

<latex>{\fontsize{16pt}\selectfont \textbf{Direct transcription for legged robots }} </latex>
<latex>{\fontsize{16pt}\selectfont \textbf{using contact models}} </latex>

<latex>{\fontsize{12pt}\selectfont \textbf{Bob Hoffmann}} </latex>
<latex>{\fontsize{10pt}\selectfont \textit{Semester Project, RSC}} </latex>

<latex> {\fontsize{12pt}\selectfont \textbf{Abstract} </latex>

In this report of a semester project, we present an extension of a trajectory optimization framework to legged robots. Contact dynamics are modeled using hard, as well as using soft contact models, while assuring that the optimization problem remains solvable using direct transcription. For soft contacts, continuously differentiable models are considered and for hard contacts, the number of optimization variables is extended by the contact forces, which are then determined through normal and tangential contact constraints embedded in the optimization. The methods are verified using three dynamical systems: a Planar-Mono-Hopper, a 3D-Mono-Hopper and a bipedal walker with point feet, which are performing basic locomotion tasks. It can be shown that the through-contact optimization is feasible using both contact modelling approaches.

<latex> {\fontsize{12pt}\selectfont \textbf{Background on ground contacts} </latex>

The main difficulty of legged robotic locomotion are ground contacts. The interaction with the environment is constantly changing and this leads to a non-smooth state evolution. Therefore one classifies legged robots as hybrid systems, which make the trajectory optimization problem even more difficult. In order to deal with hybrid systems there are two main methods: In multi-phase methods one defines a sequence of transitions, solves a smooth problem in each phase and enforces a transition event. Whereas in through-contact methods one deals with the full hybrid dynamics during the optimization. Each method has certain advantages, but also disadvantages.
Multi-phase methods are fast and accurate, but require a predefined sequence of transitions. Whereas through-contact methods can handle arbitrary sequences of contacts, but are slower, less accurate and they need to deal with contact dynamics.
One way to implement a multi-phase method is the projection of the EoM on the null-space of the constraint Jacobian for different contact configurations, i.e. giving the constraint/support consistent dynamics.
For through-contact methods, the contact dynamics can be modeled using either penalty-based or analytical methods.
Analytical methods are also called time-stepping in simulation tools, or “hard contacts”. Contacts conditions are set as constraints in an optimization problem in which one solves for the constraint forces.
In penalty-based methods the contact forces are usually calculated explicitly using a model, based on the state of the system.

<latex> {\fontsize{12pt}\selectfont \textbf{Trajectory optimization} </latex>
In trajectory optimization, we have a dynamical system, a robot, which can be described by a set of differential equations: $\mathbf{\dot{x}}=\mathbf{f}(\mathbf{x},\mathbf{u})$
With $\mathbf{x}$ being the state and $\mathbf{u}$ the control input.

We now want this robot to achieve a certain goal. The robot starts at a given initial position $\mathbf{x}(t_0)$ and has possibly a fixed terminal position $\mathbf{x}(t_f)$. Then we have a cost function $J$, which we want to minimize, for example: energy consumption. Additionally there might be constraints $\boldsymbol{\phi(\cdot)}$, which the robot has to fulfil, such as avoid obstacles.

The task of finding an optimal solution for the state and the control trajectory to this problem, is called trajectory optimization.

The trajectory optimization framework, shown below, can be described as follows: We have a dynamical system, an objective function and path constraints. These are passed to the optimization. Here direct transcription is used to transform the trajectory optimization problem into a nonlinear program, which is then solved using a solver, like IPOPT or SNOPT.

<latex> {\fontsize{10pt}\selectfont \textbf{General framework} </latex>

The continuous time formulation of a trajectory optimization problem is as follows:
<latex> \begin{flalign*} \underset{\mathbf{u}(t),\mathbf{x}(t)}{\min} &J = \Psi (\mathbf{x}(t_f))+\int_0^{t_f} \psi (\mathbf{u}(t),\mathbf{x}(t),t) \mathrm{d}t && \numberthis \label{eqn:to_problem}
\text{s.t.} \qquad &\mathbf{\dot{x}}=\mathbf{f}(\mathbf{x},\mathbf{u}) &&
&\mathbf{g}_{0,l}\leq \mathbf{g}_0(\mathbf{x}(t_0),\mathbf{u}(t_0),t_0)\leq \mathbf{g}_{0,u}&&
&\mathbf{g}_{f,l}\leq \mathbf{g}_f(\mathbf{x}(t_f),\mathbf{u}(t_f),t_f)\leq \mathbf{g}_{f,u}&&
&\boldsymbol{\phi}_l \leq \boldsymbol{\phi}(\mathbf{x}(t),\mathbf{u}(t),t)\leq \boldsymbol{\phi}_{u}&&
&\mathbf{x}_{\min}\leq \mathbf{x}(t) \leq \mathbf{x}_{\max} \qquad \mathbf{u}_{\min}\leq \mathbf{u}(t) \leq \mathbf{u}_{\max}&& \end{flalign*} </latex>

with

<latex> \begin{table}[H] \begin{tabular}{ll} $\mathbf{x}(t)$ &Finite-time state trajectory $\qquad \mathbf{x} \in \mathbb{R}^{n_x}$
$\mathbf{u}(t)$ &Finite-time control trajectory $\qquad \mathbf{u} \in \mathbb{R}^{n_u}$
$t$ &Time: $t \in [t_0,t_f]$
$J$ &Criteria to be minimized
$\psi(\cdot)$ &Intermediate cost
$\Psi(\cdot)$ &Final Cost
$\mathbf{\dot{x}}=\mathbf{f}(\mathbf{x},\mathbf{u})$ &Dynamics of the system
$\mathbf{g}(\cdot)$ &Boundary conditions
$\boldsymbol{\phi(\cdot)}$ &Path constraints
$\mathbf{x}_{\min}\leq \mathbf{x}(t) \leq \mathbf{x}_{\max}$ &Bounds on the state variables
$\mathbf{u}_{\min}\leq \mathbf{u}(t) \leq \mathbf{u}_{\max}$ &Bounds on the control variables
\end{tabular} \end{table} </latex>

<latex> {\fontsize{10pt}\selectfont \textbf{Direct transcription} </latex>

One method to solve a trajectory optimization problem is direct transcription. The infinite dimensional optimal control problem is transformed into a mathematical optimization problem with a finite number of variables that can be solved using an Nonlinear Programming Solver.
In fact, the problem is discretized on $N$ nodes, the dynamics are approximated, for example using the Hermite-Simpson method, and then the trajectory optimization problem can be solved numerically.

<latex>\begin{flalign*} \underset{\mathbf{y}}{\min} \quad &f_0(\mathbf{y})&& \numberthis \label{eqn:direct_transcription}
\text{s.t.} \qquad &\boldsymbol{\zeta}(\mathbf{y}) = 0&&
&\mathbf{b}_{\min}\leq \mathbf{b}(\mathbf{y}) \leq \mathbf{b}_{\max} &&
&\mathbf{y}_{\min}\leq \mathbf{y} \leq \mathbf{y}_{\max}&& \end{flalign*} </latex>

<latex>\begin{table}[H] \begin{tabular}{lp{10cm}} $\mathbf{y}$ &Decision variables $\mathbf{y} \in \mathbb{R}^{p}=\mathbb{R}^{N(n_x+n_u)+N-1}$: discrete values of the robot state and control trajectories sampled at certain points or nodes: $\mathbf{y} = \{\mathbf{x}_k,\mathbf{u}_k,\Delta T_k\}$
$\Delta T$ &Time between nodes (variable)
$N$ &Total number of nodes
$k$ &Discrete time index: $k = 1, \ldots,N$
$f_0 (\mathbf{y})$ &Scalar and derivable objective function
$\mathbf{b}(\cdot)$ &Boundary and path constraints
$[\mathbf{y}_{\min},\mathbf{y}_{\max}]$ &Bounds on the state and control variables
$\boldsymbol{\zeta}$ &Dynamic constraints: $\boldsymbol{\zeta} (\cdot) \in \mathbb{R}^{(N-1)\times n_x} \qquad \boldsymbol{\zeta} = \mathbf{x}_{k+1}-\mathbf{x}_{k} - \Delta T_k \hat{\mathbf{f}}$
$\hat{\mathbf{f}}$: &Approximation of the dynamics
\end{tabular} \end{table} </latex>

<latex> {\fontsize{10pt}\selectfont \textbf{Approximation of the dynamics} </latex>

There exist different possibilities to approximate the state evolution of a dynamical system during direct transcription. One can in fact chose any method, which can used for the numerical integration of differential equations.

<latex>\begin{table}[H] \begin{tabular}{ll} Forward Euler: &$\boldsymbol{\zeta} = \mathbf{x}_{k+1}-\mathbf{x}_{k} - \Delta T_k f(\mathbf{x}_{k},\mathbf{u}_{k})$
Backward Euler: &$\boldsymbol{\zeta} = \mathbf{x}_{k+1}-\mathbf{x}_{k} - \Delta T_k f(\mathbf{x}_{k+1},\mathbf{u}_{k+1})$
Trapezoidal: &$\boldsymbol{\zeta} = \mathbf{x}_{k+1}-\mathbf{x}_{k} - \frac{\Delta T_k}{2}[f(\mathbf{x}_{k},\mathbf{u}_{k})+f(\mathbf{x}_{k+1},\mathbf{u}_{k+1})]$
Hermite Simpson: &$\boldsymbol{\zeta} = \mathbf{x}_{k+1}-\mathbf{x}_{k} - \frac{\Delta T_k}{6}[f(\mathbf{x}_{k},\mathbf{u}_{k})+f(\mathbf{x}_{c},\mathbf{u}_{c})+f(\mathbf{x}_{k+1},\mathbf{u}_{k+1})]$
\end{tabular} \end{table}</latex>

Generally one can find, that from top to bottom, the accuracy of the approximation becomes better, but as the resulting dynamical defects become more complicated, the solving time increases. When using the Hermite Simpson methods one also calls direct transcription direct collocation method.

<latex> {\fontsize{10pt}\selectfont \textbf{Numerical solvers} </latex>

Two different solvers were used in this project: SNOPT and IPOPT, which rely on different working principles. IPOPT is based on an interior point algorithm, whereas SNOPT is based on sequential quadratic programming. A common restriction of both methods is that they need smooth objective and constraint functions in order to reliably find solutions.

<latex> {\fontsize{12pt}\selectfont \textbf{Dynamics of legged robots} </latex>
In general the dynamics of a floating base rigid multi-body system, having $n$ scleronomic and holonomic $1$ DoF bindings, which do not form closed kinematic chains, and where the system is interacting with the environment, can be written in this standard form:

<latex> \begin{flalign*} \mathbf{M}(\mathbf{q})\ddot{\mathbf{q}}+\mathbf{h}(\mathbf{q},\dot{\mathbf{q}})=\mathbf{S}^T \boldsymbol{\tau} + \mathbf{F}_c = \mathbf{S}^T \boldsymbol{\tau} + \mathbf{J}_c^T \boldsymbol{\lambda}&& \end{flalign*} </latex>

where:

<latex>\begin{table}[H] \begin{tabular}{lp{12cm}} $\mathbf{q}$ &Generalized coordinates $\mathbf{q} \in \mathbb{R}^{n+6}$
$\mathbf{M}$ &Inertia matrix $\mathbf{M}\in \mathbb{R}^{(n+6)\times(n+6)}$
$\mathbf{h}$ &Gravitational, friction, coriolis and centrifugal effects $\mathbf{h}\in \mathbb{R}^{n+6}$
$\boldsymbol{\tau}$ &Generalized forces (projection of moments and forces on $\mathbf{q}$): $\boldsymbol{\tau} \in \mathbb{R}^{n}$
$\mathbf{S}$ &Joint selection matrix (reflects the underactuation) $\mathbf{S}\in \mathbb{R}^{n\times(n+6)}$
$\mathbf{F}_c$ &Generalized constraint force acting on the robot: $\mathbf{F}_c \in \mathbb{R}^{n+6},\quad \mathbf{F}_c = \mathbf{J}_c^T \mathbf{\lambda}$
$\mathbf{J}_c$ &Constraint Jacobian $\mathbf{J}_c \in \mathbb{R}^{(3m)\times(n+6)} \qquad \mathbf{J}_c = \frac{\partial\boldsymbol{\Phi}(\mathbf{q})}{\partial\mathbf{q}}$
$\boldsymbol{\Phi}$ &Stacked distance between the contact points.
$\boldsymbol{\lambda}$ &Constraint forces from $m$ contacts $\boldsymbol{\lambda} \in \mathbb{R}^{3m}$
\end{tabular} \end{table}</latex>

Note: such a floating base system has $n+6$ degrees of freedom. Each binding has $1$ DoF and the base has $6$ DoFs, i.e. $3$ translational and $3$ rotational.

<latex> {\fontsize{10pt}\selectfont \textbf{Implicit dynamics} </latex>

In order to write the equation of motion in the form required by the trajectory optimization problem setting, the following two substitutions are made:

$\mathbf{x} = \begin{pmatrix} \mathbf{q}
\dot{\mathbf{q}} \end{pmatrix} \qquad\mathbf{u} = \mathbf{\tau}$

Rearranging and premultiplying by $\mathbf{M}^{-1}$, with $\mathbf{M}$ being positive definite, gives:

<latex>\begin{flalign*} \begin{pmatrix} \ddot{\mathbf{q}}
\dot{\mathbf{q}} \end{pmatrix} &= \begin{pmatrix} \mathbf{M}^{-1}(\mathbf{q}) \left(-\mathbf{h}(\mathbf{q},\dot{\mathbf{q}}) + \mathbf{S}^T \boldsymbol{\tau} + \mathbf{J}_c^T \boldsymbol{\lambda}\right)
\mathbf{\dot{q}} \end{pmatrix} &&
\dot{\mathbf{x}} &= \mathbf{f(x,u,\boldsymbol{\lambda})} && \end{flalign*}</latex>

These are the so called implicit dynamics of the system. The constraint forces $\boldsymbol{\lambda}$ are not known a priori and must be determined based on modelling assumptions.

<latex> {\fontsize{10pt}\selectfont \textbf{Examples of dynamical systems} </latex>

<latex> {\fontsize{12pt}\selectfont \textbf{Modelling of contact forces} </latex>
The two main methods how contacts can be modelled are:

<latex>\begin{itemize} \item Soft contacts \item Hard contacts \end{itemize}</latex>

<latex> {\fontsize{10pt}\selectfont \textbf{Free body diagram} </latex>

In order to model contacts, we first sketch the problem for two rigid bodies, which might come in contact at the points P and Q respectively. The force of magnitude $\lambda$, which might act along the normal $\vec{n}$ between these two points, can be modelled using a generic force element K. In general, this force depends on the elongation $\Phi$ and of it's derivatives.

<latex> {\fontsize{10pt}\selectfont \textbf{Soft contacts} </latex>

“Soft contacts” are also called penalty-based, here the force is an explicit function of the state ($\lambda=\lambda(x)$), for example of the ground penetration. For our case we further distinguish between smooth and non-smooth functions.

A non-smooth example is modelling the ground as a linear spring-damper combined with an if-then statement.

<latex>\begin{flalign*} F_z &= \begin{cases} -k_g z_0 - b_g \dot{z}_0
0 \end{cases} \begin{matrix}\text{if } z_0 < 0
\text{otherwise}\end{matrix} &&
F_x &= \begin{cases} -b_g \dot{x}_0
0 \end{cases} \quad\qquad \begin{matrix}\text{if } z_0 < 0
\text{otherwise}\end{matrix} && \end{flalign*}</latex>

In simulations one usually uses a finite-state machine to distinguish phases:

<latex>\begin{itemize} \item No collision, no friction \item Escaping collision \item Collision, static friction \item Collision, kinetic friction \end{itemize}</latex>

One of the numerical difficulties is to accurately evaluate penetration depth and velocity, which are required for the phase transitions. The forces, in normal direction as well as the friction force, are calculated using a model, e.g. linear spring-damper.

For smooth models, one can consider non-linear models, which usually are exponential in the ground penetration. These are defined in the whole state space and often have the following behaviour: If $\Phi$ becomes large the force quickly goes to zero, whereas when $\Phi$ becomes negative, the force approaches infinity.

<latex> {\fontsize{10pt}\selectfont \textbf{Hard contacts} </latex>

In “Hard contacts”, one adds contact conditions as constraints to the equation of motion. These are no longer ordinary differential equations, but become differential algebraic equations, usually also with inequality constraints.

First we consider two set-value functions $Upr$ and $Sgn$:

In the normal direction, one imposes that the distance $\Phi$ between the two bodies must be positive and also that the normal force must be positive, i.e. there is no penetration and no adhesion. Also there can only be a force if both bodies are in contact, so if $\Phi=0$. This can be expressed by the following set of constraints:

<latex> \begin{flalign*} {\Phi(\mathbf{q})} \lambda_n &= 0 &&
{\Phi(\mathbf{q})} &\geq 0 &&
\lambda_n &\geq 0 &&
\Leftrightarrow -\lambda_n &\in Upr(\boldsymbol{\Phi(\mathbf{q})}) && \end{flalign*} </latex>

with:

<latex> \begin{table}[H] \begin{tabular}{ll} $\lambda_n$ &Magnitude of the contact force in the normal direction
${\Phi(\mathbf{q})}$ &Normal distance from contact
\end{tabular} \end{table} </latex>

In the tangential direction we formulate the classical coulomb friction:

<latex> \begin{flalign*}

1. \lambda_t&= \mu \lambda_n \qquad &\text{if} \quad {\Upsilon_t}(\mathbf{q},\mathbf{\dot{q}}) > 0 \quad&\text{``Forward-sliding}&&
-\lambda_t &\in [-\mu \lambda_n,\mu \lambda_N] \qquad &\text{if} \quad {\Upsilon_t}(\mathbf{q},\mathbf{\dot{q}}) = 0 \quad&\text{Sticking}&&
-\lambda_t& = -\mu \lambda_n \qquad &\text{if} \quad {\Upsilon_t}(\mathbf{q},\mathbf{\dot{q}}) < 0 \quad&\text{``Backward-sliding}&&
   

\Leftrightarrow -\lambda_t &\in \mu \lambda_n Sgn({\Upsilon_t(\mathbf{q},\mathbf{\dot{q}})})&&& \end{flalign*} </latex>

with:

<latex> \begin{table}[H] \begin{tabular}{ll} $\lambda_t$ &Magnitude of the contact force in the tangential direction
${\Upsilon_t}(\mathbf{q},\mathbf{\dot{q}})$ &Tangential velocity at contact: $\boldsymbol{\Upsilon_t}(\mathbf{q},\mathbf{\dot{q}})=\vec{t}^T(\vec{v}_Q-\vec{v}_P)$
$\mu$ &Friction coefficient \end{tabular} \end{table} </latex>

An equivalent formulation is given by:

<latex>\begin{flalign*} \lambda_t^+ , \lambda_t^- , \gamma &\geq 0 &&
\mu \lambda_n - \lambda_t^+ - \lambda_t^- &\geq 0 &&
\gamma + \Upsilon_t (\mathbf{q} , \dot{\mathbf{q}}) &\geq 0 &&
\gamma - \Upsilon_t (\mathbf{q} , \dot{\mathbf{q}}) &\geq 0 &&
(\mu \lambda_n - \lambda_t^+ - \lambda_t^-) \gamma &= 0 &&
(\gamma + \Upsilon_t (\mathbf{q} , \dot{\mathbf{q}})) \lambda_t^+ &= 0 &&
(\gamma - \Upsilon_t (\mathbf{q} , \dot{\mathbf{q}})) \lambda_t^- &= 0 &&
\end{flalign*} </latex>
with: $\gamma$ being a helper variable.

If one wants to limit the motion to “No sliding” if there is contact, one obtains the following reduced set of constraints

<latex> \begin{flalign*} \Upsilon_t (\mathbf{q} , \dot{\mathbf{q}}) \lambda_n &= 0 &&
\boldsymbol{\Phi(\mathbf{q})} \lambda_t &= 0 &&
\mu \lambda_n - \lambda_t &\geq 0 &&
\mu \lambda_n + \lambda_t &\geq 0 && \end{flalign*} </latex>

<latex> {\fontsize{10pt}\selectfont \textbf{Collision models} </latex>

The above contact constraints are sufficient for describing motions of rigid body systems which are in contact, or which break contact. However they do not handle the case in which contact is made: Additional collision models are needed.
If we denote by ${\Upsilon_n}(\mathbf{q},\mathbf{\dot{q}})=\vec{n}^T(\vec{v}_Q-\vec{v}_P)$ the normal velocity at the contact and we further use $(\cdot)^-$ for the value before and $(\cdot)^+$ for the value after the contact, we can write the classical Newton collision law as:

<latex> \begin{flalign*} {\Upsilon_n^+}(\mathbf{q},\mathbf{\dot{q}}) &= -\varepsilon_n\Upsilon_n^-(\mathbf{q},\mathbf{\dot{q}}) && \end{flalign*} </latex>

with: $\varepsilon_n \in [0,1]$ being the normal collision coefficient.

In fact, one can write a collision law for the normal and the tangential direction in inequality form, using the previously defined set-value functions and by denoting with $\Lambda$ impulsive contact forces:

<latex> \begin{flalign*} \xi_n &= \Upsilon_n^+ +\varepsilon_n \Upsilon_n^- \quad;\quad -\Lambda_n \in Upr(\xi_n) &&
\xi_t &= \Upsilon_t^+ +\varepsilon_t \Upsilon_t^- \quad;\quad -\Lambda_t \in \mu \Lambda_n Sgn(\xi_t) \end{flalign*} </latex>

If one wants to integrate forward the equations of motion of rigid body systems with contacts, one can for example use a time-stepping algorithm. This involves solving a linear complementarity problem in each time step.

<latex> {\fontsize{10pt}\selectfont \textbf{Joint limits} </latex>

Joint limits can be modeled in the same way: either using soft or hard contacts. An example for hard joint limits with $q\leq q_{\max}$ is:

<latex> \begin{flalign*} \Phi(q)=q_{\max} - q &\geq 0 &&
-\lambda &\geq 0 &&
\Phi(q)\lambda &= 0 && \end{flalign*} </latex>

<latex> {\fontsize{10pt}\selectfont \textbf{Spatial contacts} </latex>

The above analysis was carried out for the planar case. In three dimensions, the problem is in general more complex and would require a wider mathematical framework. However if we limit the contact problem to a point coming in contact with a planar surface, we can use a polyhedral to approximate the friction cone. To further simplify the modelling, it was decided to consider the $x$ and $y$ direction independently, i.e. use a friction square.

<latex> {\fontsize{12pt}\selectfont \textbf{Implementation of contact models} </latex>

Contact models as described in the previous chapter cannot be naively used, when one wants to solve trajectory optimization problems using direct transcription, as the functions describing the constraints of the NLP should be differentiable.

<latex> {\fontsize{10pt}\selectfont \textbf{Soft contacts} </latex>

The following functions were used to describe the normal and the tangential force:

<latex> \begin{flalign*} \lambda_n ({\Phi}(\mathbf{q}))&= \mathrm{f}_0 \log_2 \left( 1+ 2^{-\frac{\kappa}{f_0}{\Phi}(\mathbf{q})} \right) &&
\lambda_t({\Phi}(\mathbf{q}),\Upsilon_t (\mathbf{q} , \dot{\mathbf{q}})) &= - \mu \lambda_n ({\Phi}(\mathbf{q})) \tanh\left(\frac{\Upsilon_t (\mathbf{q} , \dot{\mathbf{q}})}{\hat{\Upsilon}} \right) && \end{flalign*} </latex>

with: $\mathrm{f}_0, \kappa, \hat{\Upsilon}$ being tuning parameters.

In the normal direction they approximate a linear spring element with if then statement and in the tangential direction they approximate Coulomb friction.

In order to use these, we plug the function $\lambda(\mathbf{x})$ in the implicit formulation $\dot{\mathbf{x}} = \mathbf{f(x,u,\boldsymbol{\lambda})}$ of the dynamics and then get the dynamics in explicit form $\dot{\mathbf{x}}= \mathbf{\tilde{f}(x,u)}$: Using the resulting smooth, explicit dynamics the trajectory optimization problem can be solved.

<latex> {\fontsize{10pt}\selectfont \textbf{Hard contacts} </latex>

For the implementation of hard contacts, one needs to add the constraint forces in each time step as optimization variables to the NLP. One easy way to implement this, is to extend the control dimension by the number of needed constraint forces:

<latex> \begin{flalign*} \mathbf{u}_{\text{augmented}} &= \begin{pmatrix} \mathbf{u}
\mathbf{u}_\mathbf{\lambda} \end{pmatrix} && \end{flalign*} </latex>

Helper variables can be included in the same way.

In order to model contacts, the contact constraints need to be added to the equation of motion. This is done by adding a set of constraints for the normal and tangential direction to the path constraints of the optimization.

<latex> {\fontsize{10pt}\selectfont \textbf{Approximation of inelastic collisions} </latex>

By using the backward euler time-discretisation scheme for the dynamics and by solving a trajectory optimization problem with a set of contact constraints, one approximates inelastic collisions. If we apply the backward euler discretisation to the implicit description of our dynamics, we get:

<latex>\begin{flalign*} &\mathbf{x_{k+1}-x_{k}}- \Delta T_k \mathbf{f(x_{k+1},u_{k+1})} = \mathbf{0} &&
&{\begin{pmatrix} \dot{\mathbf{q}}_{k+1}
{\mathbf{q}}_{k+1} \end{pmatrix}} - {\begin{pmatrix} \dot{\mathbf{q}}_{k}
\mathbf{q}_{k} \end{pmatrix}} - \Delta T_k \begin{pmatrix} \mathbf{M}^{-1}(\mathbf{q_{k+1}}) \left(-\mathbf{h}(\mathbf{q_{k+1}},\mathbf{\dot{q}_{k+1}}) + \mathbf{S}^T \boldsymbol{\tau}_{k+1} + \mathbf{J}_c^T \mathbf{\lambda_{k+1}}\right)
\mathbf{\dot{q}_{k+1}} \end{pmatrix} = \mathbf{0} && \nonumber
&\Leftrightarrow {\begin{pmatrix} \mathbf{M}(\mathbf{q_{k+1}}) (\dot{\mathbf{q}}_{k+1} - \dot{\mathbf{q}}_{k})
{\mathbf{q}}_{k} - {\mathbf{q}}_{k+1} \end{pmatrix}} + \begin{pmatrix} \Delta T_k\left(\mathbf{h}(\mathbf{q_{k+1}},\mathbf{\dot{q}_{k+1}}) -\mathbf{S}^T \boldsymbol{\tau}_{k+1} - \mathbf{J}_c^T \mathbf{\lambda_{k+1}}\right)
\Delta T_k \mathbf{\dot{q}_{k+1}} \end{pmatrix} = \mathbf{0} && \end{flalign*} </latex>

However as described in, one might use another approximation of the dynamics. In this case it is not clear a priori, if the resulting trajectory represents a motion with inelastic collisions. This would require further analysis. However it seems to be the case.

<latex> {\fontsize{10pt}\selectfont \textbf{Joint limits} </latex>

In order to model the dynamics of the series elastic actuator with the mechanical stop in the leg of the hopper a switching linear spring-damper formulation can be used. However, as we need a differentiable model, both linear functions are connected smoothly using an Tangens Hyperbolicus function.

<latex> \begin{flalign*} F_k &= \begin{cases}k_l r_{s\Delta} = f_1
k_{stop} r_{s\Delta} - b_{stop} \dot{r} = f_2 \end{cases} \begin{matrix}\text{if } r_{s\Delta}>0
\text{otherwise}\end{matrix} &&
\Rightarrow {F}_{k,smooth} &= f_1 + \frac{1+\tanh(\kappa \cdot r_{s\Delta})}{2}(f_2-f_1) && \end{flalign*}</latex>

with: $\kappa$ being a tuning parameter determining the smoothness of the connection.

<latex> {\fontsize{12pt}\selectfont \textbf{Results} </latex>

In order to test different contact formulations, we have set-up the following problem:

We did not use a fixed terminal state, but we used the desired final state as quadratic cost function. $\mathbf{H}$ penalizes the final state deviation and $\mathbf{Q}$ penalizes the state error in each time step, and is therefore rewarding a quick arrival at the goal position. This is a greedy cost function to reach the final state, not taking into account any control effort.

<latex> \begin{flalign*} J &= (\mathbf{x}(t_f)-\mathbf{x}_{goal})^T \mathbf{H} (\mathbf{x}(t_f)-\mathbf{x}_{goal}) + \int_0^{t_f}(\mathbf{x}(t)-\mathbf{x}_{goal})^T \mathbf{Q} (\mathbf{x}(t)-\mathbf{x}_{goal})dt && \end{flalign*}</latex>

To model ground contacts, both the continuously differentiable contact models and the formulations using complementarity conditions without sliding and were evaluated. The mechanical stop of the series elastic actuator was modelled by the smooth connection of two linear functions. For the approximation of the dynamics we used the Hermite Simpson method, except once the backward Euler method for the planar-mono-hopper.

<latex> {\fontsize{10pt}\selectfont \textbf{Planar-Hopper: Continuously differentiable contact models} </latex>

The initial state of the Planar-Mono-Hopper was floating above the ground and the goal was to advance by one meter and be finally standing on the ground.

For the analysis of the results for the case of using continuously differentiable contact models, the figures below show the evolution of the center of mass, the spatial foot position, the ground reaction force and the ideal control inputs are used. In the first phase between $t = 0s$ and $t=0.3s$ the vertical position center of mass follows a parabola and the horizontal stays at $0$ as one would expect in free-fall. However the foot of the hopper is moved such that it has a backward velocity during the first touch-down. The resulting ground reaction force, makes the center of mass move forward and the hopper then does multiple small jumps until the goal position. There the hopper balances by applying rapidly changing torques.

<latex> {\fontsize{10pt}\selectfont \textbf{Planar-Hopper: Complementarity conditions without sliding} </latex>

When modelling the ground using complementarity conditions without sliding, the hopper first lands on the ground and then stays there (inelastic collision). There is some sliding as the solver violates the no-sliding condition between the nodes, which is an intrinsic problem to direct transcription. The hopper slowly lowers the position of the center of mass and then in the last few moments, it jumps to the final position.

<latex> {\fontsize{10pt}\selectfont \textbf{Planar-Hopper: Backward-Euler} </latex>

As previously explained, the approximation of inelastic collision holds when using the backward Euler discretisation. However in general the approximation of the dynamics is not as accurate. If we take a look at the evolution of the center of mass in the phase between $t = 0s$ and $t=0.3s$, we observe that the horizontal position increases, despite that there has not yet been ground contact, i.e. $z_{foot}(t)>0 \text{ for } t\in[0,0.3]$. This observation violates the linear momentum principle in horizontal direction during free-fall and is therefore non-physical, i.e. with $x_{CoM}(0)=0$ and $\dot{x}_{CoM}(0)=0$: $m_{total}\ddot{x}_{CoM} = F_{x,external} = 0 \overset{t\in[0,0.3]}{\Rightarrow} x_{CoM}(t)=0$. The non-physical behaviour is probably due to the fact that the solver violates the dynamical constraints in between the nodes when using the backward Euler discretisation. This is not as easily possible when using the Hermite Simpson method, which imposes stricter constraints on the dynamics and enforces that the derivative in between the nodes is correct.

<latex> {\fontsize{10pt}\selectfont \textbf{3D-Mono-Hopper} </latex>

For the three dimensional case, the setup was similar as for the Planar-Mono-Hopper. The ground contact was modelled using continuously differentiable contact models, the starting position was floating above the ground and the goal was to move half a meter in x and y direction.

For the evolution of the centre of mass in vertical direction, one observes a behaviour similar to a bouncing ball, with the ground forces causing the re-bouncing at the ground. The horizontal components of the position of the centre of mass only change after the first ground contact and then move towards the goal position. The control inputs applied to the ball-socket joint between the leg and the upper body are rapidly changing. This behaviour is likely due to a local optimality of bang-bang solutions, but it should be avoided as it is likely not feasible for a physical system. Therefore an extension of the cost function, which penalizes this behaviour should be considered.

<latex> {\fontsize{10pt}\selectfont \textbf{Bipedal walker with point-feet} </latex>

The goal which was set by the quadratic cost function was to advance by one meter with the ground being modelled using complementarity conditions without sliding. The bipedal walker with point-feet starts floating in the air, falls on the ground and does one step forward. However the solver only found a feasible solution in which the biped advanced by $0.65m$.

<latex> {\fontsize{12pt}\selectfont \textbf{Conclusion} </latex>

<latex> {\fontsize{10pt}\selectfont \textbf{Contributions} </latex>

The goal of this semester project was to find a way to deal with the non-smooth dynamics of legged robots:
How can contacts with the ground be modelled in such a way that the resulting trajectory optimization problem is solvable using the existing direct transcription implementation, without the need to specify a contact sequence.

After analysing modelling of contacts using soft and hard contacts models and adapting the methods to the needs of a trajectory optimization framework, it was found that one can solve trajectory optimization problems with direct transcription using continuously differentiable models and also using complementarity conditions. Continuously differentiable models can be plugged in the implicit description of the dynamics and for hard contacts the control dimension is augmented and the path constraints are extended by the contact constraints. It is then possible to find feasible, from a NLP perspective, solutions using both methods for different dynamical systems. The found trajectories seem to satisfy basic physical intuition and might therefore be used in practice.

<latex> {\fontsize{10pt}\selectfont \textbf{Discussion} </latex>

However, finding feasible solutions does not work robustly. The solver does not always find solutions and is sensitive to changes in parameters, e.g initial, final conditions, cost function, etc. . Especially the method used to approximate the dynamics influences the results: Choosing the Hermite Simpson method produces results which can be physically interpreted. But it seems hard for the solver to find such solutions. IPOPT needs up to a couple of hours to find solutions, whereas SNOPT is generally faster, but seems to get stuck for difficult problems (oscillating between two infeasible points). When using the backward Euler or the Trapezoidal method, SNOPT is usually capable of finding solutions in a couple of minutes. However these solutions are violating the physical intuition, by exploiting the less strict constraints.

Besides, the demonstrated tasks are not really complex compared to possible real world applications.

Another problem, which is intrinsic to direct transcription, is that the solutions satisfy the constraints only on the nodes and the dynamics are only approximated, mostly using the Hermite Simpson method. So the results from chapter \ref{chap:results} were just visualizations of the ideal state trajectory. If we now simulate the system using the control trajectory from the optimization, the open loop system is unstable, due to the approximations from the direct transcription and due to the modelling assumptions of ground contact. In fact one can and should use different contact models during the optimization and the simulation in order to evaluate the robustness.