Create a `TaskJointPosture` to control the joint configuration of the robot. Set the proportional and derivative gains $k_p$ and $k_d$. Add the task to the formulation object with a user-specified weight to the priority level 1 (1 corresponds to the cost function, 0 corresponds to the constraints).
Create a TaskJointBounds to specify the joint velocity limits and add it to the formulation object with priority level 0 (which means it is added as a hard constraint in the QP).
In this notebook is presented a simple use of the TSID framework on a humanoid robot, Talos.
### Notations and Definitions
The robot system has a state $x$ and we denote the control inputs as $u$. <br>
The choice of the robot model depends both on the __robot__ and the **task** to perform. For legged robots the typical model of the actuators is a torque source because we will need to control the contact forces exchanged between robot and environment. Therefore the control input $u$ are motor torques $\tau$. <br>
With the robot configuration $q$ and its velocity $\dot{q}$ we have the robot state $x$:
$$ x \triangleq (q, \dot{q}) $$
%% Cell type:markdown id: tags:
### Under-Actuated Systems
A legged robot is __under actuated__, its number of actuators is less than its number of degrees of freedom (DoFs). <br>
We denotes $n_{va}$ the number of actuators and $n_v$ the number of DoFs:
$$ n_{va} < n_v $$
Assume elements of $q$ are ordered, $q \triangleq (q_u, q_a)$, where:
- $q_u \in \mathbb{R}^{n_{qu}} $ are the passive (unactuated) joints
- $q_a \in \mathbb{R}^{n_{qa}} $ are the actuated joints
Similarly, $v \triangleq (v_u, v_a)$, where $v_u \in \mathbb{R}^{n_{vu}}$ and $v_a \in \mathbb{R}^{n_{va}}$.
$ S \triangleq [\ 0_{n_{va} \ \times \ n_{vu}} \ \ I_{n_{va}}] $ is a selection matrix associated to the actuated joints:
$$ v_a = S \ v $$
The dynamic of an under-actuated mechanical system is:
where $M(q) \in \mathbb{R}^{n_v × n_v}$ is the mass matrix, $h(q,v) \in \mathbb{R}^{n_v}$ are the bias forces, $\tau \in \mathbb{R}^{n_{va}}$ are the joint torques, $f \in \mathbb{R}^{n_f}$ are the contact forces, and $J(q) \in \mathbb{R}^{n_f×n_v}$ is the contact Jacobian.
This dynamic is often decomposed into unactuated and actuated parts:
where $\ominus$ is the difference operator in the output space (it is the standard difference if the output space is Euclidian).
We can now define the desired dynamics of the task.
Usually, when the robot is controlled in velocity, the desired dynamics is set to:
$$ \dot{e}^* = - K e $$
When the robot is controlled at the acceleration level (which is our case because we assume a torque controlled robot), the desired task dynamics is typically specified with a PID control law:
$$
\ddot{e}^* = K_P e + K_D \dot{e} + K_I \displaystyle \int_{j=0}^{j=k} e(j) dj
$$
with $K_P$ the error gain, $K_D$ the derivative of the error gain, and $K_I$ the integral gain.
### QP Optimisation problem with acceleration and torque
The transcription of the motion reference from the task space to the whole-body control is naturally written as a quadratic program (QP). A QP is composed of two layers, namely the constraints and the cost. It can be seen as a hierarchy of two levels, the constraint having priority over the cost. Inequalities can also be taken into account directly as constraints, or indirectly in the cost function.
Let us simplify the equations of motion based on the rigid body dynamics assuming there is no contact:
$$M \dot{v} + h = S^T \tau $$
Let us assume we have a task error $e$ regulating an output function $y$.
We can introduce a slack variable $s$ (an implicit optimization variable) to represent the difference between desired and real error dynamics:
$$\ddot{e} - \ddot{e}^*\triangleq s $$
A simple formulation of the QP problem can then be expressed as:
$$ \underset{\dot{v},\tau}{\min} \quad \| s \|^2 \\
\textrm{s.t.} \quad M \dot{v} + h = S^T \tau $$
If the system is in contact with environment, its dynamic must account for contact forces $f_{ext}$. If contacts are soft, measured/estimated contact forces $\hat{f}_{ext}$ can be easily included:
$$ \underset{\dot{v},\tau}{\min} \quad \| s \|^2 \\
\textrm{s.t.} \quad M \dot{v} + h = S^T \tau + J_c^T \hat{f}_{ext} $$
But if contacts are rigid, they constrain the motion. They are usually modelled as nonlinear functions, which are differentiated twice:
$$ c(q) = 0 \,\Leftrightarrow \text{Contact point do not move}$$
$$ J_c \, v = 0 \,\Leftrightarrow \text{Contact point velocities are null}$$
$$ J_c \,\dot{v} + \dot{J_c} \, v = 0 \,\Leftrightarrow \text{Contact point accelerations are null}$$
This leads to the following optimization problem:
$$ \underset{\dot{v},\tau}{\min} \quad \| s \|^2 \\
\textrm{s.t.} \quad \Bigg[
\begin{array}{lll}
J_c & 0 & 0 \\
M & -J_c^T & -S^T
\end{array}
\Bigg] \,\Bigg[\begin{array}{l}
\dot{v} \\
f \\
\tau
\end{array}
\Bigg] \, = \,\Bigg[\begin{array}{l}
-\dot{J_c} \, v \\
-h
\end{array}
\Bigg]$$
The main benefit of QP solvers is that they can handle inequality constraints. Inequalities are mainly used to defined boundaries of the system, such as joint torque, velocity or position limits, or friction cones for the contact forces.
%% Cell type:markdown id: tags:
### Weighted Sum
Complex robots are typically redundant with respect to the main task they must perform, this redundancy can be used to execute secondary tasks. This multi-objective optimization can be achieved by setting respective weights between the tasks (strategy used in TSID), or by imposing a strict hierarchy between them (cascade of QP or HQP).
Assume robot must perform N tasks, each defined by a task function $g_i(\cdot)$:
This problem remains standard computationally-efficient (compared to Least Square Programming). However, finding proper weights can be hard because too extreme weights lead to numerical issues.
%% Cell type:markdown id: tags:
# An example: CoM Sinusoidal trajectory on the robot TALOS
The goal of this exercise is to create a set of tasks allowing the tracking of a sinusoidal trajectory with the CoM of the biped robot Talos.
In the following we describe the process to create, initialise and solve the QP problem defined by the tasks.
%% Cell type:code id: tags:
``` python
# Python import needed by the exercise
importmatplotlib.pyplotasplt
importnumpyasnp
fromnumpy.linalgimportnorm
importos
importtimeastmp
# import the library TSID for the Whole-Body Controller
importtsid
# import the pinocchio library for the mathematical methods (Lie algebra) and multi-body dynamics computations.
importpinocchioaspin
# import example_example_robot_data to get the current path for the robot models
