orcisir_ISIRController 0.1
ISIR controller in xde framework to control robots, based on LQP solver.
ORCISIR_ISIRController API

Introduction

The goal of the controller is to compute a torque $ \torque $ that will be applied to the robot at each time step.

To compute this torque, we use a well-known Linear-Quadratic Program (LQP: a convex optimization program). It solves the (generic) following problem:

\begin{align*} \argmin{\x} &: \; \frac{1}{2} \x\tp Q \x + \vec{p}\tp \x + r \\ & \A \x + \b = \vec{0} \\ & \G \x + \h \leq \vec{0} \end{align*}

There are two possible ways to solve the control problem of a dynamic system. The first one relies on a redundant formalism (more readable), the second on an independent formalism (more efficient).

Control problem - Redundant formalism

The dynamic problem can be defined with the following variable $ \x = [ \ddq \; \torque \; \force_c ] $ where $ \ddq $ is the generalized acceleration vector, $ \torque $ is the output torque, and $ \force_c $ is the concatenation vector of all the contact forces.

Then, given a set of tasks $ (T_i(\x)) $ the control problem can be written as follows:

\begin{align*} \argmin{\x} &: \; \sum_i T_i(\x) \quad = \; \frac{1}{2} \x\tp Q \x + \vec{p}\tp \x + r & & \\ \end{align*}

With the following set of constraints:

\begin{align*} & M \ddq + N \dq = \vec{g} + S \torque + \J_c\tp \force_c & \quad \quad & [M \; -S \; -\J_c\tp] \x + N \dq - \vec{g} = \vec{0} & &\text{equation of motion} \\ & -\torque_{max} \leq \torque \leq \torque_{max} & & -\torque_{max} \leq [0 \; \Id \; 0] \x \leq \torque_{max} & & \text{torque limits} \\ & & \Rightarrow \quad & & & \\ & \q_{min} \leq F_{jl}(\ddq) \leq \q_{max} & & \q_{min} \leq [F_{jl} \; 0 \; 0] \x \leq \q_{max} & & \text{joint limits} \\ & \J_c \ddq + \dJ_c \dq = \vec{0} & & [\J_c \; 0 \; 0] \x + \dJ_c \dq = \vec{0} & & \text{contact point acceleration (should be full rank!)} \\ & C \force_c \leq \vec{0} & & [0 \; 0 \; C] \x \leq \vec{0} & & \text{contact point forces in friction cones} \\ \end{align*}

Control problem - Independent formalism

The dynamic problem defined above uses a redundant variable because they are linked in the equation of motion. The following variable $ \av = [ \torque \; \force_c ] $ can be used as an independent variable. The dynamic equation is no more in the constraints set, it is written in every tasks and constraints (see Redundant to independent formalisms) and the problem becomes:

\begin{align*} \argmin{\av} &: \; \sum_i T_i(\av) \quad = \; \av\tp Q \av + \vec{p}\tp \av + r & & \\ \end{align*}

With the following constraint set:

\begin{align*} & -\torque_{max} \leq \torque \leq \torque_{max} & & -\torque_{max} \leq [\Id \; 0] \av \leq \torque_{max} & & \text{torque limits} \\ & & \Rightarrow \quad & & & \\ & \q_{min} \leq F_{jl}(\ddq) \leq \q_{max} & & \q_{min} \leq F1(F_{jl}) \av \leq \q_{max} & & \text{joint limits} \\ & \J_c \ddq + \dJ_c \dq = \vec{0} & & F2_{cp}(\J_c) \av + F3_{cp}(\dJ_c \dq) = \vec{0} & & \text{contact point acceleration (should be full rank!)} \\ & C \force_c \leq \vec{0} & & [0 \; C] \av \leq \vec{0} & & \text{contact point forces in friction cones} \\ \end{align*}

where $ F1, F2, F3 $ are functions that transform matrices from redundant formalism to indepedent formalism.

What we need

To sum up, we need the following data for the constraints:

About tasks

 All Classes Namespaces Files Functions Variables Typedefs Defines