Joint-Space Control

Optimization-based Robot Control

Andrea Del Prete

University of Trento
Introduction

This document explains the control framework known as Task-Space

Inverse Dynamics (TSID).
TSID is a popular control framework for legged robots.

1
Introduction

This document explains the control framework known as Task-Space

Inverse Dynamics (TSID).
TSID is a popular control framework for legged robots.
It all started in 1987 with this paper by Oussama Khatib: “A unified
approach for motion and force control of robot manipulators: The
operational space formulation” [5]

1
Introduction

This document explains the control framework known as Task-Space

Inverse Dynamics (TSID).
TSID is a popular control framework for legged robots.
It all started in 1987 with this paper by Oussama Khatib: “A unified
approach for motion and force control of robot manipulators: The
operational space formulation” [5]
Very active research topic between 2004 and 2015 [10, 6, 7, 9, 4, 3].

1
Introduction

This document explains the control framework known as Task-Space

Inverse Dynamics (TSID).
TSID is a popular control framework for legged robots.
It all started in 1987 with this paper by Oussama Khatib: “A unified
approach for motion and force control of robot manipulators: The
operational space formulation” [5]
Very active research topic between 2004 and 2015 [10, 6, 7, 9, 4, 3].
Now not so active anymore (i.e. problem solved), but widely used.

1
Schedule

1. Theory of Joint Space Control (≈ 1:15 hour)

2. Implementation (≈ 1 hour)
3. Theory of Task Space Control (≈ 1:15 hour)
4. Implementation (≈ 1 hour)

2
Notation & Definitions

The state of the system is denoted x , (q, v ).

Configuration vector q ∈ Rnq of (relative) joint angles.
Velocity vector v = q̇ ∈ Rnv of (relative) joint velocities.
The control inputs are denoted u , τ (joint torques).

3
Notation & Definitions

The state of the system is denoted x , (q, v ).

Configuration vector q ∈ Rnq of (relative) joint angles.
Velocity vector v = q̇ ∈ Rnv of (relative) joint velocities.
The control inputs are denoted u , τ (joint torques).
The identity matrix is denoted I .
The zero matrix is denoted 0.
When needed, the size of the matrix is written as index, e.g., I3 .

3
Table of contents

1. Joint-Space Inverse Dynamics Control

2. Inverse Dynamics Control as Optimization Problem

4
Joint-Space Inverse Dynamics
Control
Robot Manipulator

Given (nonlinear) manipulator dynamics:

M(q)v̇ + h(q, v ) = τ (1)

Problem
Find τ (t) so that q(t) follows reference q r (t).

5
Robot Manipulator

Given (nonlinear) manipulator dynamics:

M(q)v̇ + h(q, v ) = τ (1)

Problem
Find τ (t) so that q(t) follows reference q r (t).
Assumption
We know dynamics and can measure q and v .

5
Robot Manipulator

Given (nonlinear) manipulator dynamics:

M(q)v̇ + h(q, v ) = τ (1)

Problem
Find τ (t) so that q(t) follows reference q r (t).
Assumption
We know dynamics and can measure q and v .
Solution
Set τ = M(q)v̇ d + h(q, v ) → closed-loop dynamics is v̇ = v̇ d .

5
Robot Manipulator

Given (nonlinear) manipulator dynamics:

M(q)v̇ + h(q, v ) = τ (1)

Problem
Find τ (t) so that q(t) follows reference q r (t).
Assumption
We know dynamics and can measure q and v .
Solution
Set τ = M(q)v̇ d + h(q, v ) → closed-loop dynamics is v̇ = v̇ d .
Select v̇ d so that q(t) follows q r (t):

5
Robot Manipulator

Given (nonlinear) manipulator dynamics:

M(q)v̇ + h(q, v ) = τ (1)

Problem
Find τ (t) so that q(t) follows reference q r (t).
Assumption
We know dynamics and can measure q and v .
Solution
Set τ = M(q)v̇ d + h(q, v ) → closed-loop dynamics is v̇ = v̇ d .
Select v̇ d so that q(t) follows q r (t):

v̇ d = v̇ r

5
Robot Manipulator

Given (nonlinear) manipulator dynamics:

M(q)v̇ + h(q, v ) = τ (1)

Problem
Find τ (t) so that q(t) follows reference q r (t).
Assumption
We know dynamics and can measure q and v .
Solution
Set τ = M(q)v̇ d + h(q, v ) → closed-loop dynamics is v̇ = v̇ d .
Select v̇ d so that q(t) follows q r (t):

v̇ d = v̇ r − Kd (v − v r ) − Kp (q − q r ) (2)

where Kp , Kd are diagonal positive-definite gain matrices.

5
Convergence

Show that q(t) converges to q r (t).

6
Convergence

Show that q(t) converges to q r (t).

Closed-loop dynamics is

v̇ = v̇ r − Kd (v − v r ) −Kp (q − q r )
| {z } | {z }
ė e

6
Convergence

Show that q(t) converges to q r (t).

Closed-loop dynamics is

v̇ = v̇ r − Kd (v − v r ) −Kp (q − q r )
| {z } | {z }
ė e

ë = −Kd ė − Kp e

6
Convergence

Show that q(t) converges to q r (t).

Closed-loop dynamics is

v̇ = v̇ r − Kd (v − v r ) −Kp (q − q r )
| {z } | {z }
ė e

ë = −Kd ė − Kp e
" # " #" #
ė 0 I e
=
ë −Kp −Kd ė
|{z} | {z } |{z}
ẋ A x

6
Convergence

Show that q(t) converges to q r (t).

Closed-loop dynamics is

v̇ = v̇ r − Kd (v − v r ) −Kp (q − q r )
| {z } | {z }
ė e

ë = −Kd ė − Kp e
" # " #" #
ė 0 I e
=
ë −Kp −Kd ė
|{z} | {z } |{z}
ẋ A x

A is Hurwitz if Kp and Kd are diagonal and positive-definite →

limt→∞ x(t) = 0 → limt→∞ q(t) = q r (t)

6
Many names for the same approach

This control law:

τ = M(v̇ r − Kd ė − Kp e) + h (3)
is known as:

• Inverse-Dynamics (ID) Control: because based on inverse dynamics

computation.
• Computed Torque: because it computes torques needed to get
desired accelerations.
• Feedback Linearization (from control theory): because it uses state
feedback to linearize closed-loop dynamics.

7
Many names for the same approach

This control law:

τ = M(v̇ r − Kd ė − Kp e) + h (3)
is known as:

• Inverse-Dynamics (ID) Control: because based on inverse dynamics

computation.
• Computed Torque: because it computes torques needed to get
desired accelerations.
• Feedback Linearization (from control theory): because it uses state
feedback to linearize closed-loop dynamics.

Another variant (with similar properties) exists:

τ = M v̇ r − Kd ė − Kp e + h (4)

7
Other Control Laws for Manipulators

Simpler control laws often used for manipulators.

A common option is PD+gravity compensation:

τ = −Kd ė − Kp e + g (q)
| {z } |{z} (5)
PD gravity compensation

8
Other Control Laws for Manipulators

Simpler control laws often used for manipulators.

A common option is PD+gravity compensation:

τ = −Kd ė − Kp e + g (q)
| {z } |{z} (5)
PD gravity compensation

Another (even simpler) option is PID control:

Z t
τ = −Kd ė − Kp e − Ki e(s)ds (6)
0

where integral replaces gravity compensation.

8
Other Control Laws for Manipulators

Simpler control laws often used for manipulators.

A common option is PD+gravity compensation:

τ = −Kd ė − Kp e + g (q)
| {z } |{z} (5)
PD gravity compensation

Another (even simpler) option is PID control:

Z t
τ = −Kd ė − Kp e − Ki e(s)ds (6)
0

where integral replaces gravity compensation.

Both control laws are stable (so q → q r ).

8
Other Control Laws for Manipulators

Simpler control laws often used for manipulators.

A common option is PD+gravity compensation:

τ = −Kd ė − Kp e + g (q)
| {z } |{z} (5)
PD gravity compensation

Another (even simpler) option is PID control:

Z t
τ = −Kd ė − Kp e − Ki e(s)ds (6)
0

where integral replaces gravity compensation.

Both control laws are stable (so q → q r ).
In theory “ID control“ outperforms “PD+gravity”, which outperforms
“PID”.

8
Other Control Laws for Manipulators

Simpler control laws often used for manipulators.

A common option is PD+gravity compensation:

τ = −Kd ė − Kp e + g (q)
| {z } |{z} (5)
PD gravity compensation

Another (even simpler) option is PID control:

Z t
τ = −Kd ė − Kp e − Ki e(s)ds (6)
0

where integral replaces gravity compensation.

Both control laws are stable (so q → q r ).
In theory “ID control“ outperforms “PD+gravity”, which outperforms
“PID”.
In practice the opposite could occur because of model errors.
8
Inverse Dynamics Control as
Optimization Problem
Inverse Dynamics (ID) Control as Least-Squares Problem

Solution of optimization problem:

(τ ∗ , v̇ ∗ ) = argmin ||v̇ − v̇ d ||2

τ,v̇
(7)
subject to M v̇ + h = τ

with v̇ d = v̇ r − Kd ė − Kp e

9
Inverse Dynamics (ID) Control as Least-Squares Problem

Solution of optimization problem:

(τ ∗ , v̇ ∗ ) = argmin ||v̇ − v̇ d ||2

τ,v̇
(7)
subject to M v̇ + h = τ

with v̇ d = v̇ r − Kd ė − Kp e, is exactly the ID control law:

τ ∗ = M v̇ d + h, (8)

9
Inverse Dynamics (ID) Control as Least-Squares Problem

Solution of optimization problem:

(τ ∗ , v̇ ∗ ) = argmin ||v̇ − v̇ d ||2

τ,v̇
(7)
subject to M v̇ + h = τ

with v̇ d = v̇ r − Kd ė − Kp e, is exactly the ID control law:

τ ∗ = M v̇ d + h, (8)

No advantage in solving (7) to compute (8), but (7) is starting point to

solve more complex problems.

9
Inverse Dynamics (ID) Control as Least-Squares Problem

Solution of optimization problem:

(τ ∗ , v̇ ∗ ) = argmin ||v̇ − v̇ d ||2

τ,v̇
(7)
subject to M v̇ + h = τ

with v̇ d = v̇ r − Kd ė − Kp e, is exactly the ID control law:

τ ∗ = M v̇ d + h, (8)

No advantage in solving (7) to compute (8), but (7) is starting point to

solve more complex problems.
Problem (7) is Least-Squares Program/Problem (LSP).

9
Taxonomy of Convex Optimization Problems

Least-Squares Programs (LSP) have:

• linear equality/inequality constraints (Ax ≤ b, or Ax = b)

• 2-norm of linear cost function (||Ax − b||2 )

10
Taxonomy of Convex Optimization Problems

Least-Squares Programs (LSP) have:

• linear equality/inequality constraints (Ax ≤ b, or Ax = b)

• 2-norm of linear cost function (||Ax − b||2 )

LSPs are subclass of convex Quadratic Programs (QPs), which have:

• linear equality/inequality constraints (Ax ≤ b, or Ax = b)

• convex quadratic cost function (x > Hx + h> x, with H ≥ 0)

10
Taxonomy of Convex Optimization Problems

Least-Squares Programs (LSP) have:

• linear equality/inequality constraints (Ax ≤ b, or Ax = b)

• 2-norm of linear cost function (||Ax − b||2 )

LSPs are subclass of convex Quadratic Programs (QPs), which have:

• linear equality/inequality constraints (Ax ≤ b, or Ax = b)

• convex quadratic cost function (x > Hx + h> x, with H ≥ 0)

LSPs and convex QPs can be solved extremely fast with off-the-shelf
softwares

10
Taxonomy of Convex Optimization Problems

Least-Squares Programs (LSP) have:

• linear equality/inequality constraints (Ax ≤ b, or Ax = b)

• 2-norm of linear cost function (||Ax − b||2 )

LSPs are subclass of convex Quadratic Programs (QPs), which have:

• linear equality/inequality constraints (Ax ≤ b, or Ax = b)

• convex quadratic cost function (x > Hx + h> x, with H ≥ 0)

LSPs and convex QPs can be solved extremely fast with off-the-shelf
softwares
→ We can solve LSP/QPs inside 1 kHz control loops!

10
Adding Torque Limits to ID Control

Take the ID control LSP:

minimize ||v̇ − v̇ d ||2

τ,v̇
(9)
subject to M v̇ + h = τ

11
Adding Torque Limits to ID Control

Take the ID control LSP:

minimize ||v̇ − v̇ d ||2

τ,v̇
(9)
subject to M v̇ + h = τ

LSPs allow for linear inequality constraints → we can add torque limits:

minimize ||v̇ − v̇ d ||2

τ,v̇

subject to M v̇ + h = τ (10)
τ min ≤ τ ≤ τ max

11
Adding Torque Limits to ID Control

Take the ID control LSP:

minimize ||v̇ − v̇ d ||2

τ,v̇
(9)
subject to M v̇ + h = τ

LSPs allow for linear inequality constraints → we can add torque limits:

minimize ||v̇ − v̇ d ||2

τ,v̇

subject to M v̇ + h = τ (10)
τ min ≤ τ ≤ τ max

Main advantage of optimization: inequality constraints.

11
Adding Current Limits for Electric Motors

In electric motors current i is proportional to torque τ :

i = kτ τ (11)

12
Adding Current Limits for Electric Motors

In electric motors current i is proportional to torque τ :

i = kτ τ (11)

Add current limits:

minimize ||v̇ − v̇ d ||2

τ,v̇

subject to M v̇ + h = τ
(12)
τ min ≤ τ ≤ τ max
i min ≤ kτ τ ≤ i max

12
Adding Joint Velocity Limits

Assuming constant accelerations v̇ during time step ∆t:

v (t + ∆t) = v (t) + ∆t v̇ (13)

13
Adding Joint Velocity Limits

Assuming constant accelerations v̇ during time step ∆t:

v (t + ∆t) = v (t) + ∆t v̇ (13)

Add joint velocity limits:

minimize ||v̇ − v̇ d ||2

τ,v̇

subject to M v̇ + h = τ
τ min ≤ τ ≤ τ max (14)
i min ≤ kτ τ ≤ i max
v min ≤ v + ∆t v̇ ≤ v max

13
Adding Joint Position Limits

Could use same trick for position limits:

1
q(t + ∆t) = q(t) + ∆t v (t) + ∆t 2 v̇ (15)
2

14
Adding Joint Position Limits

Could use same trick for position limits:

1
q(t + ∆t) = q(t) + ∆t v (t) + ∆t 2 v̇ (15)
2
However, this can result in high accelerations, typically incompatible with
torque/current limits → unfeasible LSP.

14
Adding Joint Position Limits

Could use same trick for position limits:

1
q(t + ∆t) = q(t) + ∆t v (t) + ∆t 2 v̇ (15)
2
However, this can result in high accelerations, typically incompatible with
torque/current limits → unfeasible LSP.
Better approaches exist [1, 8, 2], but we don’t discuss them here.

14
Summary

Inverse-Dynamics Control: τ = M(v̇ r − Kd ė − Kp e) + h

15
Summary

Inverse-Dynamics Control: τ = M(v̇ r − Kd ė − Kp e) + h

Other version: τ = M v̇ r − Kd ė − Kp e + h

15
Summary

Inverse-Dynamics Control: τ = M(v̇ r − Kd ė − Kp e) + h

Other version: τ = M v̇ r − Kd ė − Kp e + h
PD + gravity compensation: τ = −Kd ė − Kp e + g (q)

15
Summary

Inverse-Dynamics Control: τ = M(v̇ r − Kd ė − Kp e) + h

Other version: τ = M v̇ r − Kd ė − Kp e + h
PD + gravity compensation: τ = −Kd ė − Kp e + g (q)
Z t
PID: τ = −Kd ė − Kp e − Ki e(s)ds
0

15
Summary

Inverse-Dynamics Control: τ = M(v̇ r − Kd ė − Kp e) + h

Other version: τ = M v̇ r − Kd ė − Kp e + h
PD + gravity compensation: τ = −Kd ė − Kp e + g (q)
Z t
PID: τ = −Kd ė − Kp e − Ki e(s)ds
0

ID Control as LSP:
minimize ||v̇ − v̇ d ||2
τ,v̇

subject to M v̇ + h = τ

15
Summary

Inverse-Dynamics Control: τ = M(v̇ r − Kd ė − Kp e) + h

Other version: τ = M v̇ r − Kd ė − Kp e + h
PD + gravity compensation: τ = −Kd ė − Kp e + g (q)
Z t
PID: τ = −Kd ė − Kp e − Ki e(s)ds
0

ID Control as LSP:
minimize ||v̇ − v̇ d ||2
τ,v̇

subject to M v̇ + h = τ
τ min ≤ τ ≤ τ max

15
Summary

Inverse-Dynamics Control: τ = M(v̇ r − Kd ė − Kp e) + h

Other version: τ = M v̇ r − Kd ė − Kp e + h
PD + gravity compensation: τ = −Kd ė − Kp e + g (q)
Z t
PID: τ = −Kd ė − Kp e − Ki e(s)ds
0

ID Control as LSP:
minimize ||v̇ − v̇ d ||2
τ,v̇

subject to M v̇ + h = τ
τ min ≤ τ ≤ τ max
i min ≤ kτ τ ≤ i max

15
Summary

Inverse-Dynamics Control: τ = M(v̇ r − Kd ė − Kp e) + h

Other version: τ = M v̇ r − Kd ė − Kp e + h
PD + gravity compensation: τ = −Kd ė − Kp e + g (q)
Z t
PID: τ = −Kd ė − Kp e − Ki e(s)ds
0

ID Control as LSP:
minimize ||v̇ − v̇ d ||2
τ,v̇

subject to M v̇ + h = τ
τ min ≤ τ ≤ τ max
i min ≤ kτ τ ≤ i max
v min ≤ v + ∆t v̇ ≤ v max

15
References i

W. Decré, R. Smits, H. Bruyninckx, and J. De Schutter.

Extending iTaSC to support inequality constraints and
non-instantaneous task specification.
In IEEE International Conference on Robotics and Automation
(ICRA), 2009.
A. Del Prete.
Joint Position and Velocity Bounds in Discrete-Time
Acceleration / Torque Control of Robot Manipulators.
IEEE Robotics and Automation Letters, 3(1), 2018.
A. Del Prete, F. Nori, G. Metta, and L. Natale.
Prioritized Motion-Force Control of Constrained
Fully-Actuated Robots: ”Task Space Inverse Dynamics”.
Robotics and Autonomous Systems, 63:150–157, 2015.

16
References ii

A. Escande, N. Mansard, and P.-B. Wieber.

Hierarchical Quadratic Programming: Fast Online
Humanoid-Robot Motion Generation.
International Journal of Robotics Research, 33(7):1006–1028, 2014.
O. Khatib.
A unified approach for motion and force control of robot
manipulators: The operational space formulation.
IEEE Journal on Robotics and Automation, 3(1):43–53, feb 1987.
M. Mistry, J. Buchli, and S. Schaal.
Inverse dynamics control of floating base systems using
orthogonal decomposition.
2010 IEEE International Conference on Robotics and Automation,
(3):3406–3412, may 2010.

17
References iii

L. Righetti, J. Buchli, M. Mistry, M. Kalakrishnan, and S. Schaal.

Optimal distribution of contact forces with inverse dynamics
control.
The International Journal of Robotics Research, (January), jan 2013.
S. Rubrecht, V. Padois, P. Bidaud, M. Broissia, and M. Da Silva
Simoes.
Motion safety and constraints compatibility for multibody
robots.
Autonomous Robots, 32(3):333–349, 2012.
L. Saab, O. E. Ramos, N. Mansard, P. Soueres, and J.-y. Fourquet.
Dynamic Whole-Body Motion Generation under Rigid
Contacts and other Unilateral Constraints.
IEEE Transactions on Robotics, 29(2):346–362, 2013.

18
References iv

L. Sentis and O. Khatib.

Synthesis of whole-body behaviors through hierarchical control
of behavioral primitives.
International Journal of Humanoid Robotics, 2(4):505–518, 2005.

19