Inverse Kinematics for Mobile Robots Using QP Solver

Grégoire Passault

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:

solver = robot.make_solver()

T_world_trunk = np.eye(4)
trunk_task = solver.add_frame_task("trunk", T_world_trunk)
trunk_task.configure("trunk_task", "soft", 1.0, 1.0)

solver.add_regularization_task(1e-6)

• 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:

T_world_trunk[2, 3] = 0.25 + np.sin(t)*0.05
trunk_task.T_world_frame = T_world_trunk
qd = solver.solve(True)

We can now add a task for the left foot:

T_world_left = tf.translation((0., 0.055, 0.))
left_foot_task = solver.add_frame_task("left_foot", T_world_left)
left_foot_task.configure("left_foot_task", "soft", 1.0, 1.0)

And do the same for the right foot:

T_world_right = tf.translation((0., -0.055, 0.))
right_foot_task = solver.add_frame_task("right_foot", T_world_right)
right_foot_task.configure("right_foot_task", "soft", 1.0, 1.0)

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

com_task = solver.add_com_task(np.array([0., 0., 0.]))
com_task.configure("com_task", "soft", 1.0)

com_task.target_world = np.array([0., 0., 0.3 + np.sin(t)*0.03])

We can add a trunk_orientation task:

trunk_orientation_task = solver.add_orientation_task("trunk", np.eye(3))
trunk_orientation_task.configure("trunk_task", "soft", 1.0)

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

joints_task = solver.add_joints_task()
joints_task.configure("joints", "soft", 1.0)
joints_task.set_joints({
    "head_yaw": 0.,
    "head_pitch": 0.,
    "left_shoulder_pitch": 0.,
    "left_shoulder_roll": 0.,
    "left_elbow": -2.,
    "right_shoulder_pitch": 0.,
    "right_shoulder_roll": 0.,
    "right_elbow": -2.,
    
})

joints_task.set_joints({
    "left_shoulder_pitch":  np.sin(t*2.5)*0.5,
    "right_shoulder_pitch": np.sin(t*2.5)*0.5,
})

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 ?