# Impedance Optimization for Uncertain Contact Interactions Through Risk
Sensitive Optimal Control

Bilal Hammoud1,2 , Majid Khadiv2 and Ludovic Righetti1,2 *This work was
supported by New York University, the European Union’s Horizon 2020 research
and innovation program (grant agreement 780684 and European Research Council’s
grant 637935) and the National Science Foundation (grants 1825993, 1932187 and
2026479).1Tandon School of Engineering, New York University, USA2Max Planck
Institute for Intelligent Systems, Tübingen, Germany
<EMAIL_ADDRESS>
###### Abstract

This paper addresses the problem of computing optimal impedance schedules for
legged locomotion tasks involving complex contact interactions. We formulate
the problem of impedance regulation as a trade-off between disturbance
rejection and measurement uncertainty. We extend a stochastic optimal control
algorithm known as Risk Sensitive Control to take into account measurement
uncertainty and propose a formal way to include such uncertainty for unknown
contact locations. The approach can efficiently generate optimal state and
control trajectories along with local feedback control gains, i.e. impedance
schedules. Extensive simulations demonstrate the capabilities of the approach
in generating meaningful stiffness and damping modulation patterns before and
after contact interaction. For example, contact forces are reduced during
early contacts, damping increases to anticipate a high impact event and
tracking is automatically traded-off for increased stability. In particular,
we show a significant improvement in performance during jumping and trotting
tasks with a simulated quadruped robot.

##  I Introduction

State of the art locomotion controllers include a model predictive control
scheme that computes trajectories of some reduced model. This model predictive
scheme is then realized through a pre-designed impedance controller or a QP
based inverse dynamics solver. These strategies have proven to be successful
in completely structured and controlled environments. However, current robot
control strategies still lack the ability to reason about uncertainty in the
environment. High stiffness feedback controllers are usually used to track
precise trajectories. This approach is usually limiting for a robot in multi-
contact scenarios where the robot depends on intermittent contact interactions
to move itself or some object around. Contact interactions increase the
complexity of the control design problem. A stiff controller will counteract
an unpredicted contact by increasing the control input to ensure tracking,
which might generate high contact forces that destabilize the system. On the
other hand, excessive compliance could lead to large deviations from the
desired task. Studies from the field of neuroscience suggest that human beings
modulate their impedance during contact interactions [1, 2]. Other studies
suggest that sensorimotor commands are the result of an optimal feedback
control mechanism that controls a trade-off between accuracy and stability
[3]. A more recent study [4] suggests that this impedance ”sweet spot” is a
result of reasoning not only about the desired task, but also the
uncertainties present during contact interactions.

Impedance control for robotics was introduced by Hogan [5] and the controller
was derived later [6]. Hogan [7] demonstrated experimentally that this
controller is capable of stabilizing contact interactions if proper impedance
parameters were to be chosen. Following the results presented by Hogan, Park
[8] used a similar approach to design a bipedal walking controller. The
results show a biped capable of walking on uneven terrain. The results
achieved through impedance control have proven to be superior for control
strategies involving contact interactions [9]. However, the mentioned
approaches design the impedance schedules through an exhaustive trial and
error process. It remains an open question on how to systematically optimize
impedance profiles for robotic tasks involving contact interactions.

Optimal feedback control theory has many promising aspects that could help
approach this problem. Mayne [10] introduced an algorithm that computes local
quadratic approximations of both the dynamics and the cost functions and then
iteratively solves the nonlinear optimal control problem. This algorithm is
commonly known as the Differential Dynamic Programming Algorithm (DDP). It has
the advantage of providing an optimal control trajectory together with local
feedback controller. Many variations of DDP appeared later in the literature
[11, 12, 13, 14, 15, 16]. However, all the mentioned variations are
deterministic in nature and favor tracking over stability, making them prone
to failure in situations where tracking cannot be perfectly achieved, and in
attempting to do so, the controller can destabilize the system, uncertain
contact interactions being a clear example.

Todorov [17] added multiplicative process noise to the optimal control problem
violating the certainty equivalence principle. This led to control policies
that are dependent on the process noise. Li [18] derived similar results for
partially observable systems with control constraints resulting in control
policies that are a function of both process and measurement uncertainties.
Another method to break the certainty equivalence principle is achieved
through an exponential transformation of the cost function, this was first
introduced by Jacobson [19] for linear systems and later extended for
nonlinear optimal control by Farshidian [20]. This formally synthesizes a
controller that could obtain risk neutral, risk sensitive or risk seeking
behaviors depending on the parameterization of the role of the uncertainty in
the cost function. Medina [21] used the exponential cost transformation with
process noise to perform manipulation tasks through a model predictive control
scheme. The exponential transformation was extended to accommodate for
measurement uncertainties in [22] obtaining a risk sensitive optimal control
algorithm that accounts for higher order statistics in both process and
measurement models making it a suitable framework for designing feedback
controllers that can trade-off disturbance rejection and measurement
uncertainty. The approach was only tested on toy problems and never used for
more complex robotic tasks.

This paper builds on the ideas of Ponton [22] to propose a systematic method
for computing impedance schedules for legged robots. We extend the algorithm
to work with hard contact transitions and introduce a way to incorporate
contact measurement uncertainty into the whole-body optimal control
formulation. This results in systematically optimized impedance profiles that
exhibit desirable stiffness and damping patterns to handle uncertain, high
impact and contact transitions. Extensive numerical simulations demonstrate
the properties of the approach when compared to usual DDP algorithms and other
measurement noise models. In particular, we show a significant increase in
performance for hard impacts for jumping and trotting over uneven terrains.

##  II Background

This section provides background on the robot and contact models, risk-
sensitive stochastic optimal control and its extension to include measurement
uncertainty.

###  II-A Multi-Contact Robot Dynamics

The dynamics of a legged robot in contact with its environment is described
using the following equation

$\displaystyle\DIFdelbegin\DIFaddbegin\underbrace{\frac{d}{dt}\begin{bmatrix}q\\\
v\end{bmatrix}}_{{\color[rgb]{0,0,0}\dot{x}}}=\DIFdelbegin\DIFaddbegin\underbrace{\begin{bmatrix}v\\\
M^{-1}(\tau-h+J^{T}\lambda_{ext})\end{bmatrix}}_{{\color[rgb]{0,0,0}f(x,u)}}$
(1)

where $q\in SE(3)\times R^{nj}$ is the vector of generalized coordinates with
$nj$ being the number of robot joints, $v\in R^{nv}$ is the vector of
generalized velocities with $nv=nj+6$. $M\left(q\right)$ is the inertial
matrix, $h\left(q,v\right)$ is the vector combining the nonlinear terms such
as Coriolis acceleration and gravity, $S^{T}$ is the selection matrix mapping
the controls to the actuated degrees of freedom, $\lambda_{ext}$ is the vector
of contact forces and $J\left(q\right)$ is the contact Jacobian. The notation
indicating the dependence on $q$ and $v$ will be omitted for the remainder of
the text. Let $x^{T}=\left[q^{T},v^{T}\right]$ define the state vector, hence
the discrete time state transitions become

$\displaystyle{\color[rgb]{0,0,0}x_{t+1}=f_{t}\left(x_{t},u_{t}\right)=x_{t}\oplus\underbrace{\left(\delta
t.f(x,u)\right)}_{dx_{t}}}$ (2)

Then $dx_{t}$ represents the change in the state vector during a time interval
$\delta t$ and $\oplus$ handles the Lie group composition operation for the
base orientation.

###  II-B Rigid Contact Model

While different contact models can be chosen to compute the contact forces
$\lambda_{ext}$ [23], a rigid contact model is chosen for the optimal control
computation [24]. Let $p$, $\dot{p}$ and $\ddot{p}$ denote any contact point
position, velocity and acceleration respectively, then during an active
contact phase, the rigid contact assumption can be stated as

$\displaystyle\dot{p}\DIFdelbegin=Jv=0\DIFdelbegin\DIFaddbegin\quad\textrm{{\color[rgb]{0,0,0}and}}\quad\ddot{p}\DIFdelbegin=\dot{J}v+J\dot{v}=0\DIFdelbegin\DIFaddbegin{\color[rgb]{0,0,0}.}$
(3)

In order to resolve the contact forces that guarantee the no-motion
constraints of all active contacts, the robot dynamics (1) is projected to the
contact space using $\left(JM^{-1}J^{T}\right)^{-1}JM^{-1}=\Lambda JM^{-1}$ to
result in

$\displaystyle\lambda_{ext}=-\Lambda\dot{J}v+\Lambda JM^{-1}h-\Lambda
JM^{-1}S^{T}\tau$ (4)

Once $\lambda_{ext}$ is obtained, the motion vector $dx_{t}$ can be computed
and the state vector $x_{t+1}$ can be obtained from (2).

###  II-C Risk Sensitive Optimal Control

We consider the stochastic optimal control approach known as Risk Sensitive
Control to explicitly reason about uncertainty. A nonlinear iterative risk
sensitive optimal control formulation [25, 20, 22] explicitly takes into
account the distribution of uncertainty while being numerically efficient for
nonlinear problems. Consider the following dynamics written as a nonlinear
stochastic difference equation

$\displaystyle
x_{t+1}=f\DIFaddbegin{\color[rgb]{0,0,0}{}_{t}}\left(x_{t},u_{t}\right)+F\left(x_{t},u_{t}\right)\omega_{t}$
(5)

where $\omega_{t}\sim\mathcal{N}\left(0,\Omega_{t}\right)$ is the process
noise which accounts for unmodeled disturbances and $F(x_{t},u_{t})$ maps the
noise to the full state. Consider an objective function of the form

$\displaystyle\mathcal{L}\left(\DIFdelbegin\DIFaddbegin{\color[rgb]{0,0,0}\mathcal{X}},\DIFdelbegin\DIFaddbegin{\color[rgb]{0,0,0}\mathcal{U}}\right)\DIFdelbegin=l_{T}\left(x_{T}\right)+\sum_{0}^{T-1}l_{t}\left(x_{t},\DIFdelbegin\DIFaddbegin{\color[rgb]{0,0,0}u}_{t}\right)\DIFdelbegin$
(6)

where $\mathcal{X}=[x_{0},...,x_{T}]$ and $\mathcal{U}=[u_{0},...,u_{T-1}]$
denote the state and control trajectories respectively,
$l_{T}\left(x_{T}\right)$ is the terminal cost, and
$l_{t}\left(x_{t},u_{t}\right)$ is the cost at time $t$. Typical optimal
control approaches minimize the expectation of the objective function,
however, risk sensitive optimal control instead minimizes the expectation of
the exponential transformation of the objective. This affords the
consideration of the higher-order statistics of the cost

$\displaystyle\mathcal{J}^{*}\ $
$\displaystyle=\min\DIFdelbegin_{\DIFaddbegin{\color[rgb]{0,0,0}\mathcal{X},\mathcal{U}}}\
\mathbb{E}\left[\DIFdelbegin\DIFaddbegin{\color[rgb]{0,0,0}\exp{\left(\sigma\mathcal{L}\left(\mathcal{X},\mathcal{U}\right)\right)}}\right]$
(7)

where $\sigma$ is the sensitivity scalar. Farshidian [20] proved that the
cumulant generating function of $\mathcal{J}$ can be expressed as

$\displaystyle\frac{1}{\sigma}\log{\mathcal{J}}=\mathbb{E}\left[\mathcal{L}\right]+\frac{\sigma}{2}\mu_{2}\left[\mathcal{L}\right]+\frac{\sigma^{2}}{6}\mu_{3}\left[\mathcal{L}\right]+...$
(8)

where $\mu_{i}\left[\mathcal{L}\right]$ is the $i$’th moment of the random
variable $\mathcal{L}$. The risk sensitivity parameter $\sigma$ then provides
a tool to control the contribution of the higher order moments on the cost.
When $\sigma<0$, the control is risk-seeking and higher cost variances will be
preferred. When $\sigma>0$, the control is risk-averse since a high variance
of the cost distribution will be more penalized. When $\sigma\to 0$, the
problem reduces to a normal (risk-neutral) optimal control problem and only
the expectation of the objective is minimized.

The problem can be solved globally with Ricatti-like equations for linear
dynamics and a quadratic cost function [19], the solution being a linear
feedback controller. However, finding a global minimizer
$\mathcal{X}^{*},\mathcal{U}^{*}$ for the cost function (7) is generally
intractable for nonlinear dynamics and non quadratic costs. The method
presented in this paper computes locally optimal solutions through iterative
linearizations of the dynamics and a quadratic approximation of the objective
functions, a common technique used in deriving iterative nonlinear optimal
control algorithms [10, 14, 20, 26]. The local deviations from nominal state
and control trajectories, denoted by superscript $n$, are written as

$\displaystyle{\color[rgb]{0,0,0}\delta x_{t}=x_{t}\ominus
x^{n}_{t},\quad\quad\delta u_{t}=u_{t}-u^{n}_{t}}$ (9)

with $\ominus$ representing the suitable difference on the state manifold. The
system dynamics can be linearized in terms of the deviations as

$\displaystyle\delta
x\DIFaddbegin{\color[rgb]{0,0,0}{}_{t+1}}=\DIFdelbegin\DIFaddbegin{\color[rgb]{0,0,0}A}_{t}\DIFaddbegin{\color[rgb]{0,0,0}\delta}x_{t}\DIFaddbegin{\color[rgb]{0,0,0}+B_{t}}\delta
u_{t}\DIFaddbegin{\color[rgb]{0,0,0}+C}_{t}\DIFaddbegin{\color[rgb]{0,0,0}\omega}_{t}\DIFaddbegin$
(10)

where $A_{t}$, $B_{t}$ and $C_{t}$ are the respective linearization of
$f_{t}(\delta x_{t},\delta u_{t})$ and $F_{t}(\delta x_{t},\delta u_{t})$ with
respect to the state and control terms. Similarly a quadratic approximation of
the cost function $l_{t}(x_{t},u_{t})$ can be obtained. For a linear dynamics
and a quadratic cost under the risk sensitive exponential transformation, the
value function takes the form of an exponential with a quadratic argument in
the state [19]

$\displaystyle{\color[rgb]{0,0,0}V\left(\delta x_{t}\right)}$
$\displaystyle{\color[rgb]{0,0,0}=\exp{\Bigg{\\{}\sigma\left(\frac{1}{2}\delta
x_{t}^{T}S_{t}\delta x_{t}+\delta
x_{t}^{T}s_{t}+\bar{s}_{t}\right)\Bigg{\\}}}}$ (11)

Importantly, the value function here holds a completely different form than
that of DDP and the principle of optimality needs to be written in
multiplicative form as

$\displaystyle\DIFaddbegin{\color[rgb]{0,0,0}V}\left(\delta\DIFdelbegin\DIFaddbegin{\color[rgb]{0,0,0}x}_{t}\DIFaddbegin\right)$
$\displaystyle=\DIFdelbegin\DIFaddbegin{\color[rgb]{0,0,0}\min_{\delta
u_{t}}}\Big{\\{}{\color[rgb]{0,0,0}\exp{\left[\sigma l_{t}\left(\delta
x_{t},\delta u_{t}\right)\right]}\
\mathbb{E}}\left[{\color[rgb]{0,0,0}V}\left(\delta
x\DIFdelbegin\DIFaddbegin{\color[rgb]{0,0,0}{}_{t+1}}\right)\right]\Big{\\}}$
(12)

In order to obtain the solution, the expectation of the value function at time
$t+1$ must be computed. Finally the value function at time $t$ is approximated
by computing $S_{t},s_{t}$ and $\bar{s}_{t}$ along with $\delta u_{t}$
recursively backward in time. Since $\delta u_{t}$ is the argument minimizing
a quadratic in $\delta x_{t}$, then the locally optimal controller $\delta
u_{t}=K_{t}\delta x_{t}+k_{t}$ is linear in the state deviations. Here,
$K_{t}$ is the error feedback gains while $k_{t}$ is the feedforward command.
The solution of the recursive risk sensitive value function is discussed in
details in [19, 20]. The optimized deviations are used to iteratively improve
the solution until convergence is reached. It is important to note that the
control law explicitly incorporates the covariance of the noise distribution
in $K_{t}$ and $k_{t}$ [20].

###  II-D Including Measurement Uncertainty

The formulation presented above does not integrate the uncertainty coming from
a nonlinear measurement model

$\displaystyle{\color[rgb]{0,0,0}y_{t+1}=g\left(x_{t},u_{t}\right)+H\left(x_{t},u_{t}\right)\gamma_{t}}$
(13)

where $\gamma_{t}\sim\mathcal{N}\left(0,\Gamma_{t}\right)$ is the measurement
noise. Measurement uncertainty is crucial for our approach as we need to take
into account the uncertainty about contact locations. Speyer [27] incorporated
measurement noise into the risk sensitive formulation by introducing a state
vector that grows at each time step to include the entire history of the
states. Recently, Ponton [22] suggested that this could be avoided by
augmenting the linearized system dynamics with that of an Extended Kalman
Filter (EKF). Assuming an observable system, an EKF can be used to compute the
estimate deviations $\delta\hat{x}_{t}$ along the nominal trajectory at each
iteration. Let $F_{t}$ and $D_{t}$ be the linear approximations of
$g\left(x_{t},u_{t}\right)$ and $H\left(x_{t},u_{t}\right)$ from (13)
respectively. Then we can compute the Kalman gains $G_{t}$ during the forward
pass of the iterative algorithm (i.e. when updating the nominal state and
control trajectories) and use them to define an augmented linear system, which
includes both the real, $\delta x_{t}$, and estimated, $\delta\hat{x}_{t}$,
states of the system. With the extended state $\delta\tilde{x}_{t}=[\delta
x_{t},\delta\hat{x}_{t}]$, the augmented system dynamics becomes

$\displaystyle{\color[rgb]{0,0,0}\begin{bmatrix}\delta x_{t+1}\\\
\delta\hat{x}_{t+1}\end{bmatrix}=}$
$\displaystyle{\color[rgb]{0,0,0}\begin{bmatrix}A_{t}\delta x_{t}+B_{t}\delta
u_{t}\\\ A_{t}\delta x_{t}+B_{t}\delta u_{t}+G_{t}F_{t}\left(\delta
x_{t}-\delta\hat{x}_{t}\right)\end{bmatrix}}$
$\displaystyle{\color[rgb]{0,0,0}+\begin{bmatrix}C_{t}&0\\\
0&G_{t}D_{t}\end{bmatrix}\begin{bmatrix}\omega_{t}\\\
\gamma_{t}\end{bmatrix}}$ (14)

This augmented linear dynamical system is used, in place of (10), to solve the
risk sensitive problem. The resulting locally optimal control policy is then

$\displaystyle{\color[rgb]{0,0,0}\delta
u_{t}^{*}=k_{t}+\begin{bmatrix}K_{t}^{x}&K_{t}^{\hat{x}}\end{bmatrix}\delta\tilde{x}_{t}}$
(15)

As the true state deviations $\delta x_{t}$ are unknown, then following Li
[18] and Ponton [22], we take the expectation of $\delta u_{t}^{*}$ with
$\delta x_{t}$ conditioned on $\delta\hat{x}_{t}$ to give the optimal policy

$\displaystyle{\color[rgb]{0,0,0}\mathbb{E}_{\delta
x_{t}|\delta\hat{x}_{t}}\left[\delta
u_{t}^{*}\right]=k_{t}+\left(K_{t}^{x}+K_{t}^{\hat{x}}\right)\delta\hat{x}_{t}}$
(16)

Detailed derivations of the complete recursive algorithm follow the work of
Ponton [22] and can be found together with our software implementation in our
open-source repository111https://github.com/machines-in-
motion/risk_sensitive_control. It is also important to note that the backward
recursion has the same complexity as that of DDP. The only added complexity is
the computation of the estimator gains in the forward pass. The convergence of
iterative risk sensitive solvers is studied in the work of Roulet [28].

##  III Multi-Contact Risk Sensitive Control

Now we detail how we use the risk-sensitive optimal control algorithm
including measurement uncertainties to optimize motions and impedance
schedules for legged robots.

###  III-A Including Multiple Contact Switching

For the rigid contact model (3), a contact switch causes a change in the
dimensions of the contact Jacobian $J$. Given a predefined contact sequence
and timing we can define the switching in the overall system dynamics denoted
$f_{n}$ where the subscript $n$ indicates a predefined contact phase. The
contact switching sequence is obtained with an initial guess for the open-loop
optimal trajectory using an existing kino-dynamic optimizer [29].

The contact transitions are also aligned with the collocation points such that
the objective function and dynamics of the problem can be written as

$\displaystyle\min_{\delta X,\delta U}$
$\displaystyle\DIFdelbegin\DIFaddbegin{\color[rgb]{0,0,0}\mathbb{E}}\left[{\color[rgb]{0,0,0}\exp{\left(\sigma
l_{T}\left(\delta
x_{T}\right)+\sigma\sum_{n=0}^{N}\sum_{t=0}^{T}l_{t}\left(\delta x_{t},\delta
u_{t}\right)\right)}}\right]$ (17) $\displaystyle\DIFaddbegin$ $\displaystyle
s.t.\DIFdelbegin\quad\delta x_{t+1}=f_{n}\left(\delta x_{t},\delta
u_{t}\right)$ (18)

where $\delta\mathcal{X}$ and $\delta\mathcal{U}$ are the trajectories of the
state and control deviations respectively. $N$ is the number of contact
switches along the trajectory, and $T$ is the horizon of each phase. This
procedure avoids non-smooth switches in the contacts by solving the optimal
control problem on multiple smooth intervals. Consistency at the transition
between the switching intervals is ensured through the forward integration of
the trajectory along with high penalty on tracking the switching states. In
particular, We compute analytical derivatives for all the quantities,
including the contacts, using the Pinocchio and Crocoddyl libraries [30, 26].

###  III-B Error State Kalman Filter on Smooth Manifolds

It is desired to design an EKF to augment the linearized dynamics of the robot
at each time step, hence, the same dynamical model $f_{n}$ is used, i.e. at
each time step, the predefined contact sequence defines the dynamical model of
choice. Let the discrete time EKF of the state deviations $\delta\hat{x}_{t}$
take the form of the second row of (14), then following [22] the error
dynamics $\delta e_{t+1}=\delta x_{t+1}-\delta\hat{x}_{t+1}$ is given by

$\displaystyle{\color[rgb]{0,0,0}\delta
e_{t+1}=\left(A_{t}-G_{t}F_{t}\right)\delta
e_{t}+C_{t}\omega_{t}-G_{t}D_{t}\gamma_{t}}$ (19)

This is the simplest estimator of choice and can be replaced with any other
estimator as long as it can be written locally as a linear dynamical system.
The optimal estimation gains are then given by

$\displaystyle{\color[rgb]{0,0,0}G_{t}=A_{t}\Sigma_{t}F_{t}^{T}\left(F_{t}\Sigma_{t}F_{t}^{T}+D_{t}\Gamma_{t}D_{t}^{T}\right)^{-1}}$
(20)

where $\Sigma_{t}$ is the error dynamics covariance. As legged robots have a
free floating base modeled as an $SE(3)$ element, the proper Lie group
operations must be utilized during the propagation of the covariance of the
local error dynamics. A full treatment of EKFs on Lie groups can be found in
[31].

###  III-C Uncertainty in Contact Interactions

Consider a robot during a dynamic contact scenario as depicted in Fig. 1. A
control algorithm computes actuation torques based on the perception of the
robot’s own states along with its surrounding environment. Both the perception
of the robot environment and its states are susceptible to sensor noise and
perception errors, introducing uncertainty in the measurements. We propose to
model the contact uncertainty as an uncertainty at the tip of the swing foot
$\Gamma_{c}$ as shown in the ellipsoid in Fig. 1. This has the advantage of
avoiding the need to keep track of the environment model or the next contact
location in the state vector. We then map this uncertainty back to the space
where the full state covariance matrix $\Gamma_{fs}$ is defined. Adding both
covariances results in the total uncertainty in the contact interaction.

Figure 1: Uncertainty in Contact Interactions

A deviation in the swinging end-effector of the robot can be linearly mapped
to a deviation in its state vector through

$\displaystyle\begin{bmatrix}\delta p\\\
\delta\dot{p}\end{bmatrix}=\underbrace{\begin{bmatrix}J_{s}&0\\\
\dot{J}_{s}&J_{s}\end{bmatrix}}_{A_{s}}\begin{bmatrix}\delta q\\\ \delta
v\end{bmatrix}$ (21)

where $J_{s}$ is the jacobian of the swinging foot. Then the minimum norm
change in the state vector corresponding to a change in the end-effector is
given by

$\displaystyle\begin{bmatrix}\delta q\\\ \delta
v\end{bmatrix}=A^{\dagger}_{s}\begin{bmatrix}\delta p\\\
\delta\dot{p}\end{bmatrix}$ (22)

where $A^{\dagger}_{s}=A^{T}_{s}\left(A_{s}.A^{T}_{s}\right)^{-1}$ is the
Moore-Penrose inverse of $A_{s}$. Different norms could be chosen using a
weighted inverse if desired. Now that the deviations of the end-effector are
in the same vector space as the full state errors, it is possible to add the
noise resulting from the robot states such as joint encoders along with the
contact uncertainty. However, a deviation in the swing foot might induce a
deviation in the feet that are actively in contact and break our rigid
constraint assumption in (3). To avoid inconsistency, the deviations of the
swing foot must be projected to the null space of the feet actively in contact
using the null space projector $P_{c}$. The final form of the deviations
transformation can then be written as

$\displaystyle\DIFdelbegin\DIFaddbegin\begin{bmatrix}\delta q\\\ \delta
v\end{bmatrix}=\DIFdelbegin\DIFaddbegin\underbrace{\left(I-A_{c}^{\dagger}A_{c}\right)}_{{\color[rgb]{0,0,0}P_{c}}}A\DIFdelbegin\DIFaddbegin{\color[rgb]{0,0,0}{}_{s}^{\dagger}}\begin{bmatrix}\delta
p\\\ \delta\dot{p}\end{bmatrix}$ (23)

where $A_{c}$ is similar in structure to $A_{s}$ however containing the
Jacobians of the active contacts described in (3). This transformation maps
the mean of the deviations in a certain end-effector frame to that of the full
state vector of the robot. Since our noise model is Gaussian, the covariance
matrix can also be transformed using the affine property of Gaussians. The
total covariance of the state vector becomes

$\displaystyle\Gamma=\Gamma_{fs}+P_{c}A^{\dagger}\DIFaddbegin{\color[rgb]{0,0,0}{}_{s}}\Gamma_{c}A^{\dagger^{T}}\DIFaddbegin{\color[rgb]{0,0,0}{}_{s}}P_{c}^{T}$
(24)

In summary, we define Gaussian noise models both in end-effector and state
space, with respective covariance $\Gamma_{c}$ and $\Gamma_{fs}$. We combine
them using (24). This approach allows to modulate the at the swing feet just
before and after contact.

##  IV Simulations Results

To demonstrate the capabilities of the proposed method for controlling multi-
contact interactions under contact uncertainty, we present three different
simulation experiments using an accurate model of our open-source quadruped
robot Solo [32]. In the first experiment, we study the effect of different
measurement noise models on the computed impedance profiles and resulting
impact forces when encountering an unexpected contact. With the second
experiment, we explore how the trade-off between stiffness and damping changes
when using our risk-sensitive control approach when compared to standard DDP
methods. Finally, in the third experiment, we systematically quantify how the
stability of the system is favored relative to the accuracy of the controller
in the risk sensitive case during locomotion tasks.

The DDP controller used as a baseline in the presented experiments is from
[26]. The kino-dynamic optimizer described in [29] is used to generate
reference trajectories around which both iterative controllers DDP and Risk
Sensitive are initialized. A linear spring damper contact model is used for
the simulations with an explicit Euler integration scheme [33] at a time step
$\delta t_{sim}=$110-4$$\mathrm{s}$$ with a spring stiffness parameter of
$k=$1105\text{\,}\mathrm{N}\mathrm{/}\mathrm{m}$$ and a spring damping
parameter of $b=$3102$$\mathrm{N}\mathrm{s}\mathrm{/}\mathrm{m}$$. The
coefficient of static friction used in simulation is $\mu=0.7$. The simulated
feedback control frequency runs at $1$\mathrm{k}\mathrm{H}\mathrm{z}$$ and the
discretization step for the optimal control problems is set to $\delta
t_{opt}=$110-2\text{\,}\mathrm{s}$$. Moreover, the legs of the robot will be
referred to as $FL,FR,HL,HR$ denoting FrontLeft, FrontRight, HindLeft and
HindRight respectively. Each leg of the robot in turn consists of three joints
as it can be seen in Fig. 1.

For all the experiments, the same cost function, weights and reference
trajectories are used for both DDP and Risk Sensitive Control. All the
planning and control is designed for perfectly flat floor in all three
experiments. This results in the same whole body trajectories $x_{t}$ and the
same feedforward torque control profiles $\tau$ for both DDP and Risk
Sensitive control in each experiment. The sensitivity parameter is set to
$\sigma=10$ for the Risk Sensitive solver, leaving the uncertainty models as
the only variable in the experiments. The only differences in the optimized
plans are the impedance profiles (i.e. the feedback gains $K_{t}$).

For all the experiments presented, the uncertainty parameters can be described
as follows. The process noise $\Omega_{t}=diag($110-6$)$ through out the whole
experiment. The full state measurement noise $\Gamma_{fs}=diag($510-3$)$ for
all the experiments. $\Gamma_{c}=0$ during any lift off phase of the foot. For
the landing phase $\Gamma_{c}$ is increased to $510-4$ for the diagonal
elements corresponding to $p$ and $110-4$ for the diagonal elements
corresponding to $\dot{p}$.

###  IV-A Effect of Noise Models on Impedance Regulation

In this experiment, the task is to swing a single leg forward with a maximum
height of $10\text{\,}\mathrm{c}\mathrm{m}$ and a step length of
$8\text{\,}\mathrm{c}\mathrm{m}$ similar to what is shown in Fig. 1. A
$3\text{\,}\mathrm{c}\mathrm{m}$ high block is added at the next contact
location to simulate an unpredicted contact of $9.2\%$ of the total leg
length. The contact with the block occurs at $t=$0.43\text{\,}\mathrm{s}$$
whereas the contact with flat ground was planned for
$t=$0.55\text{\,}\mathrm{s}$$.

(a) Position error norm

(b) Velocity error norm

(c) Stiffness norm

(d) Damping norm

Figure 2: Tracking error and feedback norms for uncertainty models and DDP.
The grey zone corresponds to the time between the unexpected contact and the
planned one.

The first uncertainty model, Risk-Uniform, is simply a diagonal matrix with
equal variance on all of its entries. In the second uncertainty model, Risk-
SwingJoints, the variance terms on the joints of the swinging foot are
increased. In the third model, Risk-Unconstrained, we add a contact noise term
similar to (22) without using the nullspace projection due to the active
contacts. The last model, Risk-Contact, includes the projection of the swing
contact uncertainty into the null space of the active contacts (23).

Figure 3: Front feet normal forces for different uncertainty models and DDP.

Figure 2 compares the tracking behavior of each control scheme. The state
error is divided into the error in the configuration $\delta q$ ( Fig. 2(a))
and the error in the velocity tracking $\delta v$ (Fig. 2(b)). Similarly the
optimized feedback gains are divided into the feedback gains associated to the
configuration $K_{p}$ and the feedback gains from the velocity $K_{d}$. Their
norms are shown in Fig. 2(c) and Fig. 2(d) respectively. The vector norm
$\|v\|=\sqrt{v^{T}.v}$ is used to compute the state error norms whereas the
Frobenius Norm $\|M\|_{F}=\sqrt{Trace\left(M.M^{T}\right)}$ is used to compute
the feedback norms.

We notice in Fig. 2 that the risk sensitive control with Risk-Uniform and
Risk-SwingJoint noise models diverge after the unexpected impact. DDP also
diverges but accumulates less error than these two risk sensitive schemes. All
three controllers have high gain norms at the time of impact $|Kp|\approx 400$
and a stiffness to damping ratio at $\nicefrac{{|Kp|}}{{|Kd|}}\approx 20$. It
is important to note that such gains are too high and would not be usable on
the real robot [32]. The maximum reduction in the overall stiffness and
damping is obtained by using the Contact uncertainty model where
$\nicefrac{{|Kp_{DDP}|}}{{|Kp_{Contact}|}}\approx 10$ and
$\nicefrac{{|Kd_{DDP}|}}{{|Kd_{Contact}|}}\approx 4$. With this reduction in
the impedance magnitudes, a less accurate tracking is observed for both
Unconstrained and Contact earlier in time $t=$0.3\text{\,}\mathrm{s}$$.
However, thanks to the less aggressive feedback gains, the robot can handle
the unpredicted impact. Importantly, these reduced gains fit well within
ranges acceptable for execution on the real robot.

(a) Base Stiffness

(b) Base Damping

Figure 4: Part of the feedback gains mapping base errors to joint torques at
the time of unexpected impact t = $0.43\text{\,}\mathrm{s}$

The effect of the controllers is clearer when looking at the normal contact
forces (Fig. 3) where the impact force on the swinging leg $FL$ after
$t=$0.43\text{\,}\mathrm{s}$$ is lowest for Risk-Contact. Additionally, the
impact force propagates its effect to the front right leg for DDP, Risk-
Uniform and Risk-SwingJoints. Whereas for the case where the uncertainty about
the contact is included (Risk-Unconstrained and Risk-Contact), we see that the
robot absorbs the impact force and we do not notice any propagation of the
disturbance to the other feet.

(a) Base Stiffness

(b) Base Damping

Figure 5: Part of the feedback gains mapping base errors to joint torques at
the time of unexpected impact t = $0.43\text{\,}\mathrm{s}$

Inspecting the structure of the feedback matrices computed at the time of
impact can shed light on these differences in behavior. The blocks of the
feedback matrix that map the base states to the control are depicted in Fig.
5. Risk Sensitive control changes the structure of the gain matrices, where
the base error is modulated mainly through the feet on the ground namely FR,
HL and HR, which is expected. The advantage of Risk-Contact is observed in the
portion of the feedback matrix that maps the joint errors to the control
commands (Fig. LABEL:fig:exp01_feedback_joint). DDP has aggressive Kp gain to
track the motion of the swinging foot FL, that is the first three diagonal
elements of Kp-DDP relative to the remaining diagonal elements corresponding
to the remaining joints. Unlike Kp-DDP and Kp-Unconstrained, Kp-Contact has
lower gains on the joints of FL relative to the joints of the support feet FR
and HL. This allows the swing foot to behave as a soft spring relative to the
support feet which explains the significantly lower impact force observed and
the good tracking of the contact forces on the other feet. These results
underline the importance of choosing appropriate noise models and supports the
choice of the Risk-Contact noise model.

###  IV-B Stiffness vs. Damping & Impact Forces

We now only consider the Risk-Contact noise model. This experiment discusses
how the introduced model results in an improvement of the performance of an
extremely dynamic motion, a jump of a total height of
$0.5\text{\,}\mathrm{m}$. Halfway through the flight phase, a block of
$3\text{\,}\mathrm{c}\mathrm{m}$ height is placed on the floor.

(a) Base $|\delta q|$

(b) Joint $|\delta q|$

(c) Base $|K_{p}|$

(d) Joint $|K_{p}|$

Figure 6: Stiffness norm and position errors.

(a) Base $|\delta v|$

(b) Joint $|\delta v|$

(c) Base $|K_{d}|$

(d) Joint $|K_{d}|$

Figure 7: Damping and Velocity Errors Figure 8: FL Normal Contact Forces

(a) Base Height

(b) Front Left Foot Height

Figure 9: Base and Foot Height Comparison

Both $DDP$ and Risk-Contact feedback policies achieve the desired jump height
with a slightly better performance for the DDP controller. However, inspecting
the tracking and impedance profiles reveals a more natural behavior emerging
in the Risk Sensitive case. During the flight phase between
$t=$1\text{\,}\mathrm{s}$$ and $t=$1.5\text{\,}\mathrm{s}$$ both controllers
exhibit a significant decrease in both the base and joint stiffness as shown
in Fig. 6. For the damping portion of the feedback matrix, the base damping
also shows a decrease during the flight phase. Remarkably, we notice a stark
difference in the damping modulation of the joints. DDP significantly
decreases damping on the joints while Risk Sensitive significantly increases
it. This substantial increase in the joint damping allows to more quickly
absorb the unexpected impact, i.e. an abrupt change in the velocity of the
feet. As a result, we notice $20\%$ lower impact forces (Fig. 8), which in
turn avoids the bouncing behavior observed with the DDP controller (Fig. 9).
This comes however at the cost of larger deviations for the joint positions.
When measuring the stiffness to damping ratio of both the base and the joints,
we notice that during active contact phases the base stiffness to damping
ratio is relatively the same for DDP and risk sensitive. However, for the
joints, during the support phase, the stiffness to damping ratio of risk
sensitive is 1.5 times higher than that of DDP, explaining the good tracking
with lower overall gain profiles.

Figure 10: Snapshots of the trotting gait and the contact disturbances.

###  IV-C Trade-off between Stability & Accuracy

In this experiment, we study the capabilities of our controller when trotting
on unknown terrain. Both DDP and risk sensitive policies are optimized to
track a trotting gait including a total of 14 contact switches. During
simulations, we introduce unexpected blocks at contact locations (Fig. 10) in
order to study the policies robustness. An execution is successful if the
robot manages to reach the desired terminal base configuration and velocity
without falling.

| Experiment 1 | Experiment 2
---|---|---
# of Simulations | 500 | 500
Maximum # of blocks | 14 | 14
Maximum block height | 45 mm | 34 mm
% of leg length | 14 % | 11 %
Method | DDP | Risk | DDP | Risk
# of Successful Sim. | 237 | 325 | 358 | 458
% of Successful Sim. | 47.4% | 65% | 72.6% | 91.6%
TABLE I: Parameters and results of the trotting experiments

(a) Position error norm

(b) Orientation error norm

Figure 11: Trotting Base Tracking Error Norms Statistics

We conduct 1000 simulations, divided into two batches. In the first batch, the
contact disturbances are sampled to represent up to $14\%$ of the total leg
length, which is quite aggressive. In the second batch, the maximum contact
variation is $11\%$ of the leg length. The results are summarized in Table I.
In both cases, we observe a significant increase in trotting performance when
using the risk sensitive controller. Moreover, the magnitude of the gains of
the DDP controller are outside of the range of gains admissible on the real
robot while the risk sensitive controller gains are not (based on our
preliminary investigations on the real robot). This result demonstrates that
the risk-sensitive controller effectively improves the robustness to contact
uncertainties.

For successful executions, we show the distribution of base position tracking
errors in Fig. 11. When successful, the risk sensitive control policy finishes
the trotting gait with larger deviations in the base configuration to generate
higher number of stable execution, underlying the trade-off between accurate
tracking and robustness to contact uncertainty.

##  V Conclusion

In this paper, we extended the idea of risk sensitive optimal control with
measurement uncertainty to legged locomotion problems. We showed the
importance of the choice of the noise models to generate meaningful impedance
modulation patterns. Through extensive simulations, we demonstrated that our
approach can generate stiffness and damping profiles that lead to better
responses in face of hard impacts and contact uncertainty when compared to
typical DDP algorithms. The computed gains are significantly smaller, and
within ranges that are realistically executable on the real robot. Our
approach provides a systematic approach to automatically compute optimal
impedance modulations, at the same computational cost as modern DDP
algorithms. Future work will include real robot experiments.

## References

  * [1] E. Burdet, R. Osu, D. W. Franklin, T. E. Milner, and M. Kawato, “The central nervous system stabilizes unstable dynamics by learning optimal impedance,” _Nature_ , vol. 414, no. 6862, pp. 446–449, 2001.
  * [2] H. Lee, E. J. Rouse, and H. I. Krebs, “Summary of human ankle mechanical impedance during walking,” _IEEE journal of translational engineering in health and medicine_ , vol. 4, pp. 1–7, 2016.
  * [3] D. Liu and E. Todorov, “Evidence for the flexible sensorimotor strategies predicted by optimal feedback control,” _Journal of Neuroscience_ , vol. 27, no. 35, pp. 9354–9368, 2007.
  * [4] A. J. Nagengast, D. A. Braun, and D. M. Wolpert, “Risk-sensitivity and the mean-variance trade-off: decision making in sensorimotor control,” _Proceedings of the Royal Society B: Biological Sciences_ , vol. 278, no. 1716, pp. 2325–2332, 2011.
  * [5] N. Hogan, “Impedance control: An approach to manipulation: Part i—theory,” 1985\.
  * [6] ——, “Impedance control: An approach to manipulation: Part ii—implementation,” 1985.
  * [7] ——, “Stable execution of contact tasks using impedance control,” in _1987 IEEE International Conference on Robotics and Automation_ , vol. 4. IEEE, 1987, pp. 1047–1054.
  * [8] J. H. Park, “Impedance control for biped robot locomotion,” _IEEE Trans. on Robotics and Automation_ , vol. 17, no. 6, pp. 870–882, 2001.
  * [9] J. Lee, D. J. Hyun, J. Ahn, S. Kim, and N. Hogan, “On the dynamics of a quadruped robot model with impedance control: Self-stabilizing high speed trot-running and period-doubling bifurcations,” in _IEEE/RSJ International Conference on Intelligent Robots and Systems_ , 2014.
  * [10] D. Mayne, “A second-order gradient method for determining optimal trajectories of non-linear discrete-time systems,” _International Journal of Control_ , vol. 3, no. 1, pp. 85–95, 1966.
  * [11] W. Li and E. Todorov, “Iterative linear quadratic regulator design for nonlinear biological movement systems.” in _ICINCO_ , 2004.
  * [12] A. Sideris and J. E. Bobrow, “An efficient sequential linear quadratic algorithm for solving nonlinear optimal control problems,” in _American Control Conference_. IEEE, 2005, pp. 2275–2280.
  * [13] Y. Tassa, T. Erez, and E. Todorov, “Synthesis and stabilization of complex behaviors through online trajectory optimization,” in _IEEE/RSJ International Conference on Intelligent Robots and Systems_ , 2012.
  * [14] Y. Tassa, N. Mansard, and E. Todorov, “Control-limited differential dynamic programming,” in _2014 IEEE International Conference on Robotics and Automation (ICRA)_. IEEE, 2014, pp. 1168–1175.
  * [15] R. Grandia, F. Farshidian, A. Dosovitskiy, R. Ranftl, and M. Hutter, “Frequency-aware model predictive control,” _IEEE Robotics and Automation Letters_ , vol. 4, no. 2, pp. 1517–1524, 2019.
  * [16] R. Grandia, F. Farshidian, R. Ranftl, and M. Hutter, “Feedback mpc for torque-controlled legged robots,” in _IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS 2019)_ , 2019.
  * [17] E. Todorov and W. Li, “A generalized iterative lqg method for locally-optimal feedback control of constrained nonlinear stochastic systems,” in _American Control Conference_ , 2005, pp. 300–306.
  * [18] W. Li and E. Todorov, “Iterative linearization methods for approximately optimal control and estimation of non-linear stochastic system,” _International Journal of Control_ , vol. 80, no. 9, pp. 1439–1453, 2007\.
  * [19] D. Jacobson, “Optimal stochastic linear systems with exponential performance criteria and their relation to deterministic differential games,” _IEEE Tran on Aut. control_ , vol. 18, no. 2, pp. 124–131, 1973.
  * [20] F. Farshidian and J. Buchli, “Risk sensitive, nonlinear optimal control: Iterative linear exponential-quadratic optimal control with gaussian noise,” _arXiv preprint arXiv:1512.07173_ , 2015.
  * [21] J. R. Medina and S. Hirche, “Considering uncertainty in optimal robot control through high-order cost statistics,” _IEEE Transactions on Robotics_ , vol. 34, no. 4, pp. 1068–1081, 2018.
  * [22] B. Pontón, S. Schaal, and L. Righetti, “On the effects of measurement uncertainty in optimal control of contact interactions,” in _Algorithmic Foundations of Robotics XII_. Springer, 2020, pp. 784–799.
  * [23] G. Gilardi and I. Sharf, “Literature survey of contact dynamics modelling,” _Mechanism and machine theory_ , vol. 37, no. 10, 2002.
  * [24] L. Righetti, J. Buchli, M. Mistry, M. Kalakrishnan, and S. Schaal, “Optimal distribution of contact forces with inverse-dynamics control,” _The International Journal of Robotics Research_ , vol. 32, no. 3, 2013.
  * [25] K. Dvijotham and E. Todorov, “A unifying framework for linearly solvable control,” in _Proceedings of the Twenty-Seventh Conference on Uncertainty in Artificial Intelligence_ , 2011, pp. 179–186.
  * [26] C. Mastalli, R. Budhiraja, W. Merkt, G. Saurel, B. Hammoud, M. Naveau, J. Carpentier, L. Righetti, S. Vijayakumar, and N. Mansard, “Crocoddyl: An efficient and versatile framework for multi-contact optimal control,” in _2020 IEEE International Conference on Robotics and Automation (ICRA)_. IEEE, 2020, pp. 2536–2542.
  * [27] J. Speyer, J. Deyst, and D. Jacobson, “Optimization of stochastic linear systems with additive measurement and process noise using exponential performance criteria,” _IEEE Transactions on Automatic Control_ , vol. 19, no. 4, pp. 358–366, 1974.
  * [28] V. Roulet, M. Fazel, S. Srinivasa, and Z. Harchaoui, “On the convergence of the iterative linear exponential quadratic gaussian algorithm to stationary points,” in _IEEE American Control Conference_ , 2020.
  * [29] B. Ponton, A. Herzog, A. Del Prete, S. Schaal, and L. Righetti, “On time optimization of centroidal momentum dynamics,” in _IEEE International Conference on Robotics and Automation_. IEEE, 2018.
  * [30] J. Carpentier, G. Saurel, G. Buondonno, J. Mirabel, F. Lamiraux, O. Stasse, and N. Mansard, “The pinocchio c++ library: A fast and flexible implementation of rigid body dynamics algorithms and their analytical derivatives,” in _IEEE/SICE International Symposium on System Integration_ , 2019, pp. 614–619.
  * [31] J. Sola, “Quaternion kinematics for the error-state kalman filter,” _arXiv preprint arXiv:1711.02508_ , 2017.
  * [32] F. Grimminger, A. Meduri, M. Khadiv, J. Viereck, M. Wüthrich, M. Naveau, V. Berenz, S. Heim, F. Widmaier, T. Flayols _et al._ , “An open torque-controlled modular robot architecture for legged locomotion research,” _IEEE Robotics and Automation Letters_ , vol. 5, no. 2, pp. 3650–3657, 2020.
  * [33] B. Hammoud, L. Olivieri, L. Righetti, J. Carpentier, and A. Del Prete, “Fast and accurate multi-body simulation with stiff viscoelastic contacts,” _arXiv preprint arXiv:2101.06846_ , 2021.