Introduction

Planning produces some trajectories to follow

Those trajectories are expressed naturally in the task-space.

We need a way to control the robot to actually follow those trajectories:
this is the whole-body control problem

Overview

1. Robot modelling
2. Defining tasks
3. Inverse Kinematics
4. Quadratic Programming
5. In practice

Robot modelling

We model a robot with links and joints, and focus here only on serial robots.

Problem:
How to represent the fact that the robot can move around ?

In practice, robot geometry and dynamic properties can be represented in a robot description format.

Most popular are SDF and URDF.

Defining tasks

Task:
A task is an error that you want to take to .

Example: Position task

Example: Orientation task

Combining tasks

We can combine multiple tasks in one equation:

Inverse Kinematics

We want to solve for .

With a position task:

Numerical approach

Consider the (first-order) approximation of :

We can now solve the approximated:

This method is called the Newton-Raphson method

What if is not square, or not invertible?

System can be:

• not enough rows: Underconstrained
  There are infinite solutions to the equation,
• too many rows: Overconstrained
  The equation can't be solved at all,
• rank-deficient: Singular
  can be square, but not invertible.

We need to replace equation solving with score minimization.

This objective can be expanded to:

is a quadratic, intuitively looks like

We cancel its derivatives with respect to .

With numpy, you can use np.linalg.pinv

The pseudo-inverse gives you the best solution in the sense of least-square errors.

• Giving different "priorities" to different tasks?
• How can we handle constraints?
  • Maximum joint velocities:
  • Joint limits:

Quadratic Programming

Remember the score we wanted to minimize is:

It is exactly equivalent to computing the pseudo-inverse .

But, as a bonus, we can now add equality and inequality constraints!

Joint velocity limits

Joint limits

Tasks "soft" priorization

Instead of having , let's go back to our stack of tasks . We can then compute:

Separately for each task .

The objective function is then the sum of weighted sub-objectives:

Tasks "hard" priorization

Instead of adding a task in the objective function, we can add it the the equality constraints:

Under-constrained problem

QP formulation requires to be positive definite:
( should be non-zero for any non-zero )

If we don't have enough objectives, this will not be the case.

We add the task with a very low weight . This is called regularization

It is equivalent to adding to .

QP problem

The problem can then be formulated as:

Where the superscript refers to the soft tasks, and to the hard tasks.

Kinematics tasks we implemented

* Can be relative or absolute

In practice

Computing Jacobians can be done using Rigid Body algorithms library.

https://github.com/stack-of-tasks/pinocchio

Let's make some squats!

First, we can create the solver, and add two tasks:

• trunk_task is a frame task (it packages both position and orientation task)
• regularization_task is here to regularize the problem, because we have too many DoFs!

Then, we can update the trunk_task target in the main loop:

We can now add a task for the left foot:

And do the same for the right foot:

Now, let's remove the trunk_task and add a com_task:

We can add a trunk_orientation task:

We can now add a joints_task to impose target angles for the arms and the head:

Kicking using cubic spline and frame tasks:

Adding Centroidal Angular Momentum task to try to reduce the angular momentum:

Highly recommended reading

Modern Robotics

• Free PDF
• Good YouTube channel

Thanks!

Questions ?