Introduction
Writing a controller
The goal of the Stack-of-Tasks (SoT) is to provide a SDK in order to:- Write control problem for complex robots such as humanoid robots
- Test them quickly using geometric settings (rviz, gepetto-viewer), dynamic simulators (ODE,Bullet), system simulator (Gazebo)
- Deploy them in real robots (TALOS, HRP-2)
All our controllers are defined as optimization problems. We are using either off-the-shelf solvers or our own solvers, depending of the targeted applications. The free variables are the control vector $\mathbf{u}$ to apply to the system. They are constraints which can be of several kinds: the mechanical constraints of the system with the environment, the behavior that we want the robot to follow. The next two following sections introduce our software framework to take into account those two constraints.
Necessary Constraints
Dynamic modeling of a free-floating multi-body system
The dynamic model of a free-floating multi-body system can be written this way: $$\mathbf{M}(\mathbf{q})\mathbf{\ddot{q}} + \mathbf{N}(\mathbf{q},\mathbf{\dot{q}})\mathbf{\dot{q}}+ \mathbf{G}(\mathbf{q}) = \mathbf{T}(\mathbf{q})\mathbf{u}$$ with $\mathbf{q}$ the robot state variable, $\mathbf{M}(\mathbf{q})$ the inertia matrix of the multi-body system, $N(\mathbf{q},\mathbf{\dot{q}})$ the matrix of the non-linear dynamical effect (Centrifugal and Coriolis forces) of the system motion, $\mathbf{G}(\mathbf{q})$ the gravity, $\mathbf{u}$ a control vector, and $\mathbf{T}(\mathbf{q})$ the matrix mapping the control vector on the system.The current software library we are using to computate such quantities and theirs derivatives is Pinocchio 1. Pinocchio include dynamical quantities such as angular momentum and its derivatives.
Contact models
When considering a rigid contact model several constraints needs to be considered:- No penetration in the ground
- No sliding
Functions used to generate a robot behavior
Formulating tasks
Definition
Task ($\mathbf{e}$) are using features ($\mathbf{s}$). Features might be end-effector posision (hands, feet), the robot center-of-mass, or the center-of-gravity of an object projected in the image. We assume that a function relates the current state of the robot typically the configuration, speed, acceleration to a feature. For instance, we may want to consider the Center-Of-Mass of a robot specified by a model $robot\_model$ and a configuration vector $\mathbf{q}$: $$ \mathbf{f}_{CoM}: (robot\_model, \mathbf{q}) \rightarrow \mathbf{c}$$ We further assume that this function is at least differentiable once. A task is then defined by the regulation between the feature depending on the current state $\mathbf{s}(\mathbf{q})$ of the robot and a desired feature $\mathbf{s}^*$: $$ \mathbf{e} = \mathbf{s}(\mathbf{q})- \mathbf{s}^* $$Imposing the task dynamic
You can impose for instance that the task dynamic is an exponential decay: $$ \mathbf{\dot{e}} = - \lambda \mathbf{e}$$ But we also have: $$\mathbf{\dot{e}} = \mathbf{\dot{s}}(\mathbf{q}) - \mathbf{\dot{s}}^*$$ If the desired feature does not move we have $\mathbf{\dot{s}}^*=0$, in addition we can remark that $\mathbf{\dot{s}}(\mathbf{q})=\frac{\delta s}{\delta q}\mathbf{\dot{q}} = \mathbf{J}(\mathbf{q})\mathbf{\dot{q}}$ Thus $$\mathbf{\dot{e}} = \mathbf{J}(\mathbf{q})\mathbf{\dot{q}} - \mathbf{\dot{s}}^*$$ At a given time $t=k\Delta T$ a measurement of the feature $\hat{\mathbf{s}}(t)$ can give us: $$ \hat{\mathbf{e}}(t) = \hat{\mathbf{s}}(t) - \mathbf{s}^*(t)$$ By posing $\hat{\mathbf{e}}(t)=\mathbf{e}(t)$ we have: $$ \mathbf{\dot{e}}(t) = - \lambda \mathbf{e}(t) = - \lambda \hat{\mathbf{e}}(t) = - \lambda (\hat{\mathbf{s}}(t) - \mathbf{s}^*(t))$$ We will continue by dropping $(t)$ for sake of clarity: $$ - \lambda (\hat{\mathbf{s}} - \mathbf{s}^*) = \mathbf{J}(\mathbf{q})\mathbf{\dot{q}} \label{eq:equa_cst}$$ Therefore if $\mathbf{J}(\mathbf{q})$ is invertible we can find $\mathbf{\dot{q}}$ using the following relationship: $$ \mathbf{\dot{q}} = - \lambda \mathbf{J}(\mathbf{q})^{-1}(\hat{\mathbf{s}} - \mathbf{s}^*) $$ In general $\mathbf{J}(\mathbf{q})$ is not square and then we have to use the Moore-Penrose pseudo inverse: $$ \mathbf{\dot{q}} = - \lambda \mathbf{J}(\mathbf{q})^{+}(\hat{\mathbf{s}} - \mathbf{s}^*) $$ But it can be also understood as the solution of the following optimization problem: $$ \begin{array} & argmin_{\mathbf{\dot{q}}} \parallel \mathbf{\dot{q}} \parallel \\ s.t. & - \lambda (\hat{\mathbf{s}} - \mathbf{s}^*) = \mathbf{J}(\mathbf{q})\mathbf{\dot{q}} \\ \end{array} $$- The library implementing the task formulation in velocity domain is sot-core (Hierarchy).
- The library implementing the task formulation in acceleration is sot-dyninv (Hierarchy with inequalities).
- The library implementing the task formulation in torque is sot-torque-control (Weighted sum).
Formulating instantaneous control as optimization problem
The task function is allowing a very versatile formulation including- Hierarchy of tasks
- Weighted problems