1 # Controllers formulation {#b_controllers_information}
2 \section NecessaryConditions Necessary constraints
3 \subsection SubSecDynamicModeling Dynamic modeling of a free-floating multi-body system
4 The dynamic model of a free-floating multi-body system can be written this way:
6 \f[ M(q) \ddot{q} + N(q,\dot{q}) \dot{q}+G(q)=Su \f]
8 with \f$q\f$ the robot state variable,
9 \f$ M(q) \f$ the inertia matrix of the multi-body system,
10 \f$ N (q,\dot{q}) \f$ the matrix of the non-linear dynamical effect (Centrifugal and Coriolis forces) of the system motion,
11 \f$ G(q) \f$ the gravity, \f$u\f$ the control vector, and \f$ S \f$ the matrix mapping the control vector on the system (also called the selection matrix).
13 The current software library we are using to computate such quantities and theirs derivatives is Pinocchio 1.
14 Pinocchio include dynamical quantities such as angular momentum and its derivatives.
15 \subsection SubSecContactModels Contact models
17 When considering a rigid contact model several constraints needs to be considered:
18 * No penetration in the ground
21 Both constraints generate inequalities at the contact points with respect to the state vector:
22 \f[ \Phi_{n,c} (q) \geq 0 \f]
23 \f[ \Phi_{t,c} (q) \geq 0 \f]
25 with \f$ \Phi_{n,c}(q) \f$ the normal force at contact point \f$ c \f$ expressed in the local reference frame of the contact point,
26 and \f$ \Phi_{t,c}(q) \f$ the tangential force at contact point \f$ c\f$ also expressed in the local reference frame of the contact point.
28 To avoid penetration velocity and acceleration of the points must be constrained too. Using differentiation we obtain:
29 \f[ C_n (q) \dot{q} = 0 \f]
30 \f[ C_n(q) \ddot{q}+s_n(q,\dot{q})=0 \f]
32 The same derivation can be realized to avoid sliding:
33 \f[ C_t(q)\dot{q}=0 \f]
34 \f[ C_t(q)\ddot{q}+s_n(q,\dot{q})=0 \f]
36 Self-collisiion is usually avoiding by formulating an inequality constraint on the distance between two robot bodies. To do that the closest two points between two shapes are tracked. The distance between two bodies is obtained through a flavor of the FCL library.
37 \section SecFunctions Functions used to generate a robot behavior
39 \subsection SubFuncFormulatingTasks Formulating tasks
40 \subsubsection subsubdef Definition
41 Task \f$ (e) \f$ are using features \f$(s)\f$. 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 \f$ robot\_model \f$
42 and a configuration vector \f$ q\f$ \f[ f_{CoM}:(robot\_model,q) \rightarrow c\f]
44 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
45 \f$ s(q) \f$ of the robot and a desired feature \f$s^*\f$:
48 \subsubsection subsubImposingTaskDyn Imposing the task dynamic
49 You can impose for instance that the task dynamic is an exponential decay:
52 \f[ \dot{e}=\dot{s}(q)−\dot{s}^∗\f]
54 If the desired feature does not move we have
57 in addition we can remark that
58 \f[\dot{s}(q)=\frac{\delta s}{\delta q}\dot{q}=J(q)\dot{q} \f]
60 \f[ \dot{e}=J(q)\dot{q}−\dot{s}^∗ \f]
64 a measurement of the feature
65 \f$ \hat{s}(t) \f$ can give us:
66 \f[ \hat{e}(t)=\hat{s}(t)−s^∗(t) \f]
68 \f[ \hat{e}(t)=e(t) \f]
70 \f[ \dot{e}(t)=−\lambda e(t)=−\lambda \hat{e}(t)= −\lambda ( \hat{s}(t)−s^*(t)) \f]
72 We will continue by dropping \f$ (t) \f$ for sake of clarity:
73 \f[ − \lambda ( \hat{s}− s^∗) = J(q)\dot{q} \f]
75 Therefore if \f$ J(q) \f$ is invertible we can find
76 \f$ \dot{q} \f$ using the following relationship:
77 \f[ \dot{q}=− \lambda J(q)^{−1}(\hat{s}−s^∗) \f]
79 In general \f$ J(q) \f$ is not square and then we have to use the Moore-Penrose pseudo inverse:
80 \f[ \dot{q}= −\lambda J(q)^+(\hat{s}−s^∗) \f]
82 But it can be also understood as the solution of the following optimization problem:
84 argmin_{ \dot{q}} || \dot{q} || & \\
85 s.t. & − \lambda (\hat{s} - s^∗) =J(q)\dot{q} \\
89 * The library implementing the task formulation in velocity domain is [sot-core](http://github.com/stack-of-tasks/sot-core) (Hierarchy).
90 * The library implementing the task formulation in acceleration is [sot-dyninv](http://github.com/stack-of-tasks/sot-dyninv) (Hierarchy with inequalities).
91 * The library implementing the task formulation in torque is [sot-torque-control](http://github.com/stack-of-tasks/sot-torque-control) (Weighted sum).
93 \subsection SubsectionFormulatingInstantaneousControl Formulating instantaneous control as optimization problem
95 The task function is allowing a very versatile formulation including:
96 * Hierarchy of tasks: Each task at a level \f$ i\f$ cannot interfer with the control at level \f$i-1\f$.
97 This concept has been introduced by Nakamura \cite Nakamura:ijrr:87 extended by Siciliano in \cite Siciliano:icarrue:91 .
98 * Weighted problems: Each task are at the same level i.e. they are all included in the same cost function, and they are weighted against each other.