1 of 23

NOpenSoT:

RT Whole-Body Control Framework

Enrico Mingo Hoffman, Arturo Laurenzi and Luca Muratore

Fondazione Istituto Italiano di Tecnologia

2 of 23

OpenSoT

  • Framework for instantaneous control
    • RT safe
    • Different control formulations
    • Intuitive formulation of complex control problems
    • Takes inspiration from Stack of Tasks

​

​

3 of 23

OpenSoT Features

​

Easy To Use: ease formulation of different control problems

​

Model Independent: use your own model library

​

​

Standards: based on Eigen and KDL, framework independent

​

Robot Independent: integrate easily robots through URDF

​

​

​

Interfaces: common interfaces to write you own Tasks, Constraints and Solvers

​

Fast and RT Safe: C++ API for optimal RT performances

​

Solvers, Tasks and Constraints: ready to use for common robotic control problems

​

​

​

Tools: for log, debug and mathematical utilities

​

4 of 23

OpenSoT Components

MODEL

TASKS & CONSTRAINTS

SOLVERS

UTILITIES

5 of 23

Controller Structure

model.update()

tasks.update()

constraints.update()

solver.solve()

control

state

Real-Time safe & fast (~1-10 ms)

Inverse Dynamics

Cartesian Impedance Ctrl

Operational Space

6 of 23

Controller Structure

model.update()

tasks.update()

constraints.update()

solver.solve()

state

Real-Time safe & fast (~1-10 ms)

Open-Loop IK

control

7 of 23

Controller Structure

model.update()

tasks.update()

constraints.update()

solver.solve()

state ol

Real-Time safe & fast (~1-10 ms)

state cl

Admittance Ctrl

Partial Feedback IK

control

8 of 23

Model

ModelInterface

      • INPUT: URDF and SRDF files
      • OUTPUT: FK, Jacobians, ID, Inertia Matrix, CoM, ...

​

ModelInterface

srdf

urdf

update()

setJointPosition()

getJacobian()

...

ModelInterfaceRBDL

RBDL

9 of 23

Tasks and Constraints

Cost Function

    • Interfaces for Task formulation
      • INPUT: a ModelInterface
      • OUTPUT: A matrix and b vector
      • LP formulation with c vector

​

Constraints (Linear)

    • Interfaces for Constraints formulation
      • INPUT: a ModelInterface
      • OUTPUT: C matrix and c vectors

​

Cartesian Velocity Task:

Joint Velocity Limits Constraint:

10 of 23

Tasks and Constraints C++ API

TaskInterface

update()

_A, _b, _c, _W, ...

...

ConstraintInterface

update()

_Aineq, _bLower, _bUpper, _Aeq, ...

...

CartesianImpedance

Self-Collision Avoidance

...

...

object

11 of 23

Utilities: Variables

A Variable is an Affine transformation:

​

which can be used to model optimization variables.

​

​

Joint Accelerations + Contact Forces:

//// Update aux task matrices and vectors

_A = …

_b = ...

​

///// HERE A and b are updated

auto v = _A*_qdot - _b

A = v.getM();

b = -v.getq();

///////////

​

update()

12 of 23

Utilities: Math of Tasks

Math of Tasks: C++ operators to handle Tasks and Constraints in synthetic but detailed way.

SYMBOL

EFFECT

SYNTAX

+

SOFT priority between Tasks (Task Augmentation)

T3 = T1 + T2

/

HARD priority between Tasks (Null-Space)

S1 = T1/T2

*

Relative Task Gain

W*T1

%

Select Task Rows

T2 = T1%{1,2}

<<

Insert Constraint

​

T1<<C1

S1<<C1

13 of 23

Floating Base Robots

  • Floating Base IK:

​

Augmented joint-space

Constraint: contacts

c1

c2

w

bl

1st priority task: CoM

2nd priority task: end-effectors

3rd priority task: postural

14 of 23

Utilities: Math of Tasks

Math of Tasks: C++ operators to handle Tasks and Constraints in synthetic but detailed way.

Whole-Body IK Example:

15 of 23

API Example

left_arm.reset(new Cartesian("left_arm", q, *model, “l_ee”, “base”));

right_arm.reset(new Cartesian("right_arm", q, *model, “r_ee”, “base”));

postural.reset(new Postural(q));

joint_lims.reset(new JointLimits(q, qmax, qmin));

vel_lims.reset(new VelocityLimits(2., dT, q.size()));

self_collision_avoidance.reset(new SelfCollisionAvoidance(q, *model, “base”));

self_collision_avoidance->setCollisionWhiteList(whiteList);

​

auto_stack = (left_arm + right_arm)/(postural)<<joint_lims<<vel_lims<<self_collision_avoidance;

​

​

16 of 23

Solvers

FRONT-END

BACK-END

  • eHQP
  • iHQP/LP
  • ...

​

Prepare Problems

​

  • qpOASES
  • OSQP
  • GLPK
  • ...

​

Solve Problems

MoT Problem

Solution

  • Compute cost function
  • Apply HARD priority strategy
  • Select QP/LP solver

17 of 23

Solvers: Front-End iHQP/LP

BACK-END

solution 0

problem 0

solution 1

problem 1

problem 2

18 of 23

Solvers: C++ API

SolverInterface

solve()

Back-End

init()

solve()

getOptions()

iHQP/LP (Front-End)

qpOASESBackEnd

...

plugin

object

19 of 23

API Example

left_arm.reset(new Cartesian("left_arm", q, *model, “l_ee”, “base”));

right_arm.reset(new Cartesian("right_arm", q, *model, “r_ee”, “base”));

postural.reset(new Postural(q));

joint_lims.reset(new JointLimits(q, qmax, qmin));

vel_lims.reset(new VelocityLimits(2., dT, q.size()));

self_collision_avoidance.reset(new SelfCollisionAvoidance(q, *model, “base”));

self_collision_avoidance->setCollisionWhiteList(whiteList);

​

auto_stack = (left_arm + right_arm)/(postural)<<joint_lims<<vel_lims<<self_collision_avoidance;

​

solver.reset(new iHQP(auto_stack->getStack(), auto_stack->getBounds(),1e9, OSQP));

while(ros::ok()){

model->setJointPosition(q);

model->update();

auto_stack->update(q);

​

if(solver->solve(dq))

q+=dq;

}

​

​

​

20 of 23

API Example

21 of 23

Other Formulations

22 of 23

Other Formulations

23 of 23

THANK YOU!

​

QUESTIONS?