Successfully reported this slideshow.
Introduction
Theoretical foundations
Software
The stack of tasks
Florent Lamiraux, Olivier Stasse and Nicolas Mansard
CNRS...
Introduction
Theoretical foundations
Software
The stack of tasks
Introduction
Theoretical foundations
Software
The stack o...
Introduction
Theoretical foundations
Software
Outline
Introduction
Theoretical foundations
Software
The stack of tasks
Introduction
Theoretical foundations
Software
Introduction
The stack of tasks provides a control framework for real-time
r...
Introduction
Theoretical foundations
Software
Introduction
The stack of tasks provides a control framework for real-time
r...
Introduction
Theoretical foundations
Software
Introduction
The stack of tasks provides a control framework for real-time
r...
Introduction
Theoretical foundations
Software
Introduction
The stack of tasks provides a control framework for real-time
r...
Introduction
Theoretical foundations
Software
Introduction
The stack of tasks provides a control framework for real-time
r...
Introduction
Theoretical foundations
Software
Outline
Introduction
Theoretical foundations
Software
The stack of tasks
Introduction
Theoretical foundations
Software
Rigid body B
◮ Configuration represented by an homogeneous matrix
MB =
RB tB
...
Introduction
Theoretical foundations
Software
Rigid body B
◮ Configuration represented by an homogeneous matrix
MB =
RB tB
...
Introduction
Theoretical foundations
Software
Rigid body B
◮ Configuration represented by an homogeneous matrix
MB =
RB tB
...
Introduction
Theoretical foundations
Software
Rigid body B
◮ Velocity represented by (vB, ωB) ∈ R6 where
˙RB = ˆωBRB
and
ˆ...
Introduction
Theoretical foundations
Software
Rigid body B
◮ Velocity represented by (vB, ωB) ∈ R6 where
˙RB = ˆωBRB
and
ˆ...
Introduction
Theoretical foundations
Software
Rigid body B
◮ Velocity represented by (vB, ωB) ∈ R6 where
˙RB = ˆωBRB
and
ˆ...
Introduction
Theoretical foundations
Software
Configuration space
◮ Robot: set of rigid-bodies linked by
joints B0, · · · B...
Introduction
Theoretical foundations
Software
Configuration space
◮ Robot: set of rigid-bodies linked by
joints B0, · · · B...
Introduction
Theoretical foundations
Software
Configuration space
◮ Robot: set of rigid-bodies linked by
joints B0, · · · B...
Introduction
Theoretical foundations
Software
Velocity
◮ Velocity:
˙q = ( ˙x, ˙y, ˙z, ωx , ωy , ωz, ˙θ1, · · · ˙θn−6)
ω ∈ ...
Introduction
Theoretical foundations
Software
Velocity
◮ Velocity:
˙q = ( ˙x, ˙y, ˙z, ωx , ωy , ωz, ˙θ1, · · · ˙θn−6)
ω ∈ ...
Introduction
Theoretical foundations
Software
Velocity
◮ Velocity:
˙q = ( ˙x, ˙y, ˙z, ωx , ωy , ωz, ˙θ1, · · · ˙θn−6)
ω ∈ ...
Introduction
Theoretical foundations
Software
Task
◮ Definition: function of the
◮ robot configuration,
◮ time and
◮ possibl...
Introduction
Theoretical foundations
Software
Task
◮ Definition: function of the
◮ robot configuration,
◮ time and
◮ possibl...
Introduction
Theoretical foundations
Software
Task
◮ Definition: function of the
◮ robot configuration,
◮ time and
◮ possibl...
Introduction
Theoretical foundations
Software
Task
◮ Definition: function of the
◮ robot configuration,
◮ time and
◮ possibl...
Introduction
Theoretical foundations
Software
Task
◮ Definition: function of the
◮ robot configuration,
◮ time and
◮ possibl...
Introduction
Theoretical foundations
Software
Hierarchical task based control
Given
◮ a configuration q,
◮ two tasks of dec...
Introduction
Theoretical foundations
Software
Hierarchical task based control
Given
◮ a configuration q,
◮ two tasks of dec...
Introduction
Theoretical foundations
Software
Hierarchical task based control
Jacobian:
◮ we denote
◮ Ji = ∂Ti
∂q for i ∈ ...
Introduction
Theoretical foundations
Software
Hierarchical task based control
Jacobian:
◮ we denote
◮ Ji = ∂Ti
∂q for i ∈ ...
Introduction
Theoretical foundations
Software
Hierarchical task based control
Jacobian:
◮ we denote
◮ Ji = ∂Ti
∂q for i ∈ ...
Introduction
Theoretical foundations
Software
Hierarchical task based control
Jacobian:
◮ we denote
◮ Ji = ∂Ti
∂q for i ∈ ...
Introduction
Theoretical foundations
Software
Hierarchical task based control
Jacobian:
◮ we denote
◮ Ji = ∂Ti
∂q for i ∈ ...
Introduction
Theoretical foundations
Software
Moore Penrose pseudo-inverse
Given a matrix A ∈ Rm×n, the Moore Penrose pseu...
Introduction
Theoretical foundations
Software
Moore Penrose pseudo-inverse
Given a matrix A ∈ Rm×n, the Moore Penrose pseu...
Introduction
Theoretical foundations
Software
Moore Penrose pseudo-inverse
Given a matrix A ∈ Rm×n, the Moore Penrose pseu...
Introduction
Theoretical foundations
Software
Moore Penrose pseudo-inverse
Given a matrix A ∈ Rm×n, the Moore Penrose pseu...
Introduction
Theoretical foundations
Software
Hierarchical task based control
Resolution of the first constraint:
˙T1 = J1 ...
Introduction
Theoretical foundations
Software
Hierarchical task based control
Resolution of the first constraint:
˙T1 = J1 ...
Introduction
Theoretical foundations
Software
Hierarchical task based control
Resolution of the first constraint:
˙T1 = J1 ...
Introduction
Theoretical foundations
Software
Hierarchical task based control
Resolution of the first constraint:
˙T1 = J1 ...
Introduction
Theoretical foundations
Software
Hierarchical task based control
Resolution of the first constraint:
˙T1 = J1 ...
Introduction
Theoretical foundations
Software
Hierarchical task based control
In fact
∀u ∈ Rn
, J1 ˙q1+(In − J+
1 J1)u = J...
Introduction
Theoretical foundations
Software
Hierarchical task based control
In fact
∀u ∈ Rn
, J1 ˙q1+(In − J+
1 J1)u = J...
Introduction
Theoretical foundations
Software
Hierarchical task based control
In fact
∀u ∈ Rn
, J1 ˙q1+(In − J+
1 J1)u = J...
Introduction
Theoretical foundations
Software
Hierarchical task based control
In fact
∀u ∈ Rn
, J1 ˙q1+(In − J+
1 J1)u = J...
Introduction
Theoretical foundations
Software
Controlling the second task
We have
˙q = ˙q1 + P1u
˙T2 = J2 ˙q +
∂T2
∂t
˙T2 ...
Introduction
Theoretical foundations
Software
Controlling the second task
We have
˙q = ˙q1 + P1u
˙T2 = J2 ˙q +
∂T2
∂t
˙T2 ...
Introduction
Theoretical foundations
Software
Controlling the second task
We have
˙q = ˙q1 + P1u
˙T2 = J2 ˙q +
∂T2
∂t
˙T2 ...
Introduction
Theoretical foundations
Software
Controlling the second task
Thus
−λ2T2 = J2 ˙q1 +
∂T2
∂t
+ J2P1u
J2P1u = −λ2...
Introduction
Theoretical foundations
Software
Controlling the second task
Thus
−λ2T2 = J2 ˙q1 +
∂T2
∂t
+ J2P1u
J2P1u = −λ2...
Introduction
Theoretical foundations
Software
Example
◮ T1: position of the feet +
projection of center of
mass,
◮ T2: pos...
Introduction
Theoretical foundations
Software
Outline
Introduction
Theoretical foundations
Software
The stack of tasks
Introduction
Theoretical foundations
Software
Architecture overview
The stack of tasks
Introduction
Theoretical foundations
Software
Libraries
◮ jrl-mathtools: implementation of small size matrices,
◮ to be re...
Introduction
Theoretical foundations
Software
Libraries
◮ jrl-mathtools: implementation of small size matrices,
◮ to be re...
Introduction
Theoretical foundations
Software
Libraries
◮ jrl-mathtools: implementation of small size matrices,
◮ to be re...
Introduction
Theoretical foundations
Software
Libraries
◮ jrl-mathtools: implementation of small size matrices,
◮ to be re...
Introduction
Theoretical foundations
Software
Libraries
◮ jrl-mathtools: implementation of small size matrices,
◮ to be re...
Introduction
Theoretical foundations
Software
dynamic-graph
◮ Entity
◮ Signal: synchronous interface
◮ Command: asynchrono...
Introduction
Theoretical foundations
Software
dynamic-graph
◮ Entity
◮ Signal: synchronous interface
◮ Command: asynchrono...
Introduction
Theoretical foundations
Software
dynamic-graph
◮ Entity
◮ Signal: synchronous interface
◮ Command: asynchrono...
Introduction
Theoretical foundations
Software
Signal
Synchronous interface storing a given data type
◮ output signals:
◮ r...
Introduction
Theoretical foundations
Software
Signal
Synchronous interface storing a given data type
◮ output signals:
◮ r...
Introduction
Theoretical foundations
Software
Signal
Synchronous interface storing a given data type
◮ output signals:
◮ r...
Introduction
Theoretical foundations
Software
Signal
Synchronous interface storing a given data type
◮ output signals:
◮ r...
Introduction
Theoretical foundations
Software
Signal
Synchronous interface storing a given data type
◮ dependency relation...
Introduction
Theoretical foundations
Software
Signal
Synchronous interface storing a given data type
◮ dependency relation...
Introduction
Theoretical foundations
Software
Signal
Synchronous interface storing a given data type
◮ dependency relation...
Introduction
Theoretical foundations
Software
Signal
Synchronous interface storing a given data type
◮ dependency relation...
Introduction
Theoretical foundations
Software
Signal
Synchronous interface storing a given data type
◮ dependency relation...
Introduction
Theoretical foundations
Software
Signal
Synchronous interface storing a given data type
◮ dependency relation...
Introduction
Theoretical foundations
Software
Command
Asynchronous interface
◮ input in a fixed set of types,
◮ trigger an ...
Introduction
Theoretical foundations
Software
dynamic-graph-python
Python bindings to dynamic-graph
◮ module dynamic graph...
Introduction
Theoretical foundations
Software
dynamic-graph-python
Python bindings to dynamic-graph
◮ module dynamic graph...
Introduction
Theoretical foundations
Software
dynamic-graph-python
Python bindings to dynamic-graph
◮ module dynamic graph...
Introduction
Theoretical foundations
Software
dynamic-graph-python
Python bindings to dynamic-graph
◮ module dynamic graph...
Introduction
Theoretical foundations
Software
dynamic-graph-python
Python bindings to dynamic-graph
◮ module dynamic graph...
Introduction
Theoretical foundations
Software
dynamic-graph-tutorial
Simple use case for illustration
◮ Definition of 2 ent...
Introduction
Theoretical foundations
Software
dynamic-graph-tutorial
>>> from dynamic graph.tutorial import InvertedPendul...
Introduction
Theoretical foundations
Software
dynamic-graph-tutorial
>>> from dynamic graph.tutorial import InvertedPendul...
Introduction
Theoretical foundations
Software
dynamic-graph-tutorial
>>> from dynamic graph.tutorial import InvertedPendul...
Introduction
Theoretical foundations
Software
dynamic-graph-tutorial
>>> from dynamic graph.tutorial import InvertedPendul...
Introduction
Theoretical foundations
Software
dynamic-graph-tutorial
>>> from dynamic graph.tutorial import InvertedPendul...
Introduction
Theoretical foundations
Software
dynamic-graph-tutorial
Package provides
◮ C++ code of classes InvertedPendul...
Introduction
Theoretical foundations
Software
dynamic-graph-tutorial
Package provides
◮ C++ code of classes InvertedPendul...
Introduction
Theoretical foundations
Software
dynamic-graph-tutorial
Package provides
◮ C++ code of classes InvertedPendul...
Introduction
Theoretical foundations
Software
dynamic-graph-tutorial
Package provides
◮ C++ code of classes InvertedPendul...
Introduction
Theoretical foundations
Software
dynamic-graph-tutorial
Package provides
◮ C++ code of classes InvertedPendul...
Introduction
Theoretical foundations
Software
sot-core
Class FeatureAbstract
◮ function of the robot and environment state...
Introduction
Theoretical foundations
Software
sot-core
Class FeatureAbstract
◮ function of the robot and environment state...
Introduction
Theoretical foundations
Software
sot-core
Class FeatureAbstract
◮ function of the robot and environment state...
Introduction
Theoretical foundations
Software
sot-core
Class FeatureAbstract
◮ function of the robot and environment state...
Introduction
Theoretical foundations
Software
Feature
When paired with a reference, features become tasks.
◮ Example
◮ err...
Introduction
Theoretical foundations
Software
Feature
When paired with a reference, features become tasks.
◮ Example
◮ err...
Introduction
Theoretical foundations
Software
Feature
When paired with a reference, features become tasks.
◮ Example
◮ err...
Introduction
Theoretical foundations
Software
Task
◮ Collection of features with a control gain,
◮ implements abstraction ...
Introduction
Theoretical foundations
Software
Solver SOT
Hierarchical task solver
◮ computes robot joint velocity
The stac...
Introduction
Theoretical foundations
Software
sot-dynamic
dynamic graph.sot.dynamics.Dynamic builds a
kinematic chain from...
Introduction
Theoretical foundations
Software
sot-pattern-generator
dynamic graph.sot.pattern generator
◮ Entity PatternGe...
Introduction
Theoretical foundations
Software
sot-application
dynamic graph.sot.application
◮ Provide scripts for standard...
Introduction
Theoretical foundations
Software
Packages specific to robots
sot-hrp2
◮ defines a class Robot that provides
◮ r...
Introduction
Theoretical foundations
Software
Utilities
◮ dynamic graph.writeGraph (filename): writes the
current graph in...
Introduction
Theoretical foundations
Software
Utilities
◮ dynamic graph.writeGraph (filename): writes the
current graph in...
Introduction
Theoretical foundations
Software
Utilities
◮ dynamic graph.writeGraph (filename): writes the
current graph in...
Introduction
Theoretical foundations
Software
Utilities
◮ dynamic graph.writeGraph (filename): writes the
current graph in...
Introduction
Theoretical foundations
Software
Utilities
◮ dynamic graph.writeGraph (filename): writes the
current graph in...
Introduction
Theoretical foundations
Software
Utilities
◮ dynamic graph.writeGraph (filename): writes the
current graph in...
Introduction
Theoretical foundations
Software
Utilities
◮ dynamic graph.writeGraph (filename): writes the
current graph in...
Introduction
Theoretical foundations
Software
Installation
Through robotpkg
◮ git clone http://trac.laas.fr/git/robots/rob...
Introduction
Theoretical foundations
Software
Installation
Through github:
◮ git clone --recursive git://github.com/jrl-um...
Introduction
Theoretical foundations
Software
Installation
Through github:
◮ git clone --recursive git://github.com/jrl-um...
Introduction
Theoretical foundations
Software
Installation
Through installation script
◮ git clone git://github.com/stack-...
Introduction
Theoretical foundations
Software
Running the stack of tasks into OpenHRP-3.1
You need to install:
◮ ros-elect...
Introduction
Theoretical foundations
Software
Running the stack of tasks into OpenHRP-3.0.7
Assumptions
◮ OpenHRP 3.0.7 is...
Introduction
Theoretical foundations
Software
Running the stack of tasks into OpenHRP-3.0.7
Initialize the application: cr...
Introduction
Theoretical foundations
Software
Running the stack of tasks into OpenHRP-3.0.7
Build the graph including the ...
Introduction
Theoretical foundations
Software
Running the stack of tasks into OpenHRP-3.0.7
Create the graph
>>> CreateEve...
Introduction
Theoretical foundations
Software
Running the stack of tasks into OpenHRP-3.0.7
Switch to the new graph
>>> wa...
Introduction
Theoretical foundations
Software
Software structure - Conceptual view
The stack of tasks
Introduction
Theoretical foundations
Software
Software structure - Link with Model
The stack of tasks
Introduction
Theoretical foundations
Software
Software structure - Link with Model
The stack of tasks
Introduction
Theoretical foundations
Software
Software structure - Link with Model
The stack of tasks
Introduction
Theoretical foundations
Software
Software structure - Link with Model
The stack of tasks
Introduction
Theoretical foundations
Software
Software structure - Link with Model
The stack of tasks
Introduction
Theoretical foundations
Software
Software structure - Link with Model
The stack of tasks
Introduction
Theoretical foundations
Software
Software structure - Repositories
The stack of tasks
Upcoming SlideShare
Loading in …5
×

Stack of Tasks Course

3,730 views

Published on

Published in: Technology, Business
  • Be the first to comment

Stack of Tasks Course

  1. 1. Introduction Theoretical foundations Software The stack of tasks Florent Lamiraux, Olivier Stasse and Nicolas Mansard CNRS-LAAS, Toulouse, France The stack of tasks
  2. 2. Introduction Theoretical foundations Software The stack of tasks Introduction Theoretical foundations Software The stack of tasks
  3. 3. Introduction Theoretical foundations Software Outline Introduction Theoretical foundations Software The stack of tasks
  4. 4. Introduction Theoretical foundations Software Introduction The stack of tasks provides a control framework for real-time redundant manipulator control ◮ implementation of a data-flow, ◮ control of the graph by python scripting, ◮ task-based hierarchical control, ◮ portable: tested on HRP-2, Nao, Romeo. The stack of tasks
  5. 5. Introduction Theoretical foundations Software Introduction The stack of tasks provides a control framework for real-time redundant manipulator control ◮ implementation of a data-flow, ◮ control of the graph by python scripting, ◮ task-based hierarchical control, ◮ portable: tested on HRP-2, Nao, Romeo. The stack of tasks
  6. 6. Introduction Theoretical foundations Software Introduction The stack of tasks provides a control framework for real-time redundant manipulator control ◮ implementation of a data-flow, ◮ control of the graph by python scripting, ◮ task-based hierarchical control, ◮ portable: tested on HRP-2, Nao, Romeo. The stack of tasks
  7. 7. Introduction Theoretical foundations Software Introduction The stack of tasks provides a control framework for real-time redundant manipulator control ◮ implementation of a data-flow, ◮ control of the graph by python scripting, ◮ task-based hierarchical control, ◮ portable: tested on HRP-2, Nao, Romeo. The stack of tasks
  8. 8. Introduction Theoretical foundations Software Introduction The stack of tasks provides a control framework for real-time redundant manipulator control ◮ implementation of a data-flow, ◮ control of the graph by python scripting, ◮ task-based hierarchical control, ◮ portable: tested on HRP-2, Nao, Romeo. The stack of tasks
  9. 9. Introduction Theoretical foundations Software Outline Introduction Theoretical foundations Software The stack of tasks
  10. 10. Introduction Theoretical foundations Software Rigid body B ◮ Configuration represented by an homogeneous matrix MB = RB tB 0 0 0 1 ∈ SE(3) RB ∈ SO(3) ⇔ RT B RB = I3 Point x ∈ R3 in local frame of B is moved to y ∈ R3 in global frame: y 1 = MB x 1 The stack of tasks
  11. 11. Introduction Theoretical foundations Software Rigid body B ◮ Configuration represented by an homogeneous matrix MB = RB tB 0 0 0 1 ∈ SE(3) RB ∈ SO(3) ⇔ RT B RB = I3 Point x ∈ R3 in local frame of B is moved to y ∈ R3 in global frame: y 1 = MB x 1 The stack of tasks
  12. 12. Introduction Theoretical foundations Software Rigid body B ◮ Configuration represented by an homogeneous matrix MB = RB tB 0 0 0 1 ∈ SE(3) RB ∈ SO(3) ⇔ RT B RB = I3 Point x ∈ R3 in local frame of B is moved to y ∈ R3 in global frame: y 1 = MB x 1 The stack of tasks
  13. 13. Introduction Theoretical foundations Software Rigid body B ◮ Velocity represented by (vB, ωB) ∈ R6 where ˙RB = ˆωBRB and ˆω =   0 −ω3 ω2 ω3 0 −ω1 −ω2 ω1 0   is the matrix corresponding to the cross product operator ◮ Velocity of point P on B vp = ˙tB + ωB × OBP where OB is the origin of the local frame of B. The stack of tasks
  14. 14. Introduction Theoretical foundations Software Rigid body B ◮ Velocity represented by (vB, ωB) ∈ R6 where ˙RB = ˆωBRB and ˆω =   0 −ω3 ω2 ω3 0 −ω1 −ω2 ω1 0   is the matrix corresponding to the cross product operator ◮ Velocity of point P on B vp = ˙tB + ωB × OBP where OB is the origin of the local frame of B. The stack of tasks
  15. 15. Introduction Theoretical foundations Software Rigid body B ◮ Velocity represented by (vB, ωB) ∈ R6 where ˙RB = ˆωBRB and ˆω =   0 −ω3 ω2 ω3 0 −ω1 −ω2 ω1 0   is the matrix corresponding to the cross product operator ◮ Velocity of point P on B vp = ˙tB + ωB × OBP where OB is the origin of the local frame of B. The stack of tasks
  16. 16. Introduction Theoretical foundations Software Configuration space ◮ Robot: set of rigid-bodies linked by joints B0, · · · Bm. ◮ Configuration: position in space of each body. q = (qwaist , θ1, · · · θn−6) ∈ SE(3) × Rn−6 qwaist = (x, y, z, roll, pitch, yaw) ◮ Position of Bi depends on q: MBi (q) ∈ SE(3) The stack of tasks
  17. 17. Introduction Theoretical foundations Software Configuration space ◮ Robot: set of rigid-bodies linked by joints B0, · · · Bm. ◮ Configuration: position in space of each body. q = (qwaist , θ1, · · · θn−6) ∈ SE(3) × Rn−6 qwaist = (x, y, z, roll, pitch, yaw) ◮ Position of Bi depends on q: MBi (q) ∈ SE(3) The stack of tasks
  18. 18. Introduction Theoretical foundations Software Configuration space ◮ Robot: set of rigid-bodies linked by joints B0, · · · Bm. ◮ Configuration: position in space of each body. q = (qwaist , θ1, · · · θn−6) ∈ SE(3) × Rn−6 qwaist = (x, y, z, roll, pitch, yaw) ◮ Position of Bi depends on q: MBi (q) ∈ SE(3) The stack of tasks
  19. 19. Introduction Theoretical foundations Software Velocity ◮ Velocity: ˙q = ( ˙x, ˙y, ˙z, ωx , ωy , ωz, ˙θ1, · · · ˙θn−6) ω ∈ R3 ◮ Velocity of Bi vBi ωBi (q, ˙q) = JBi (q). ˙q ∈ R6 The stack of tasks
  20. 20. Introduction Theoretical foundations Software Velocity ◮ Velocity: ˙q = ( ˙x, ˙y, ˙z, ωx , ωy , ωz, ˙θ1, · · · ˙θn−6) ω ∈ R3 ◮ Velocity of Bi vBi ωBi (q, ˙q) = JBi (q). ˙q ∈ R6 The stack of tasks
  21. 21. Introduction Theoretical foundations Software Velocity ◮ Velocity: ˙q = ( ˙x, ˙y, ˙z, ωx , ωy , ωz, ˙θ1, · · · ˙θn−6) ω ∈ R3 ◮ Velocity of Bi vBi ωBi (q, ˙q) = JBi (q). ˙q ∈ R6 The stack of tasks
  22. 22. Introduction Theoretical foundations Software Task ◮ Definition: function of the ◮ robot configuration, ◮ time and ◮ possibly external parameters that should converge to 0: T ∈ C∞ (C × R, Rm ) ◮ Example: position tracking of an end-effector Bee ◮ M(q) ∈ SE(3) position of the end-effector, ◮ M∗ (t) ∈ SE(3) reference position T(q, t) = t(M∗−1(t)M(q)) uθ(R∗−1(t)R(q)) where ◮ t() is the translation part of an homogeneous matrix, ◮ R and R∗ are the rotation part of M and M∗ . The stack of tasks
  23. 23. Introduction Theoretical foundations Software Task ◮ Definition: function of the ◮ robot configuration, ◮ time and ◮ possibly external parameters that should converge to 0: T ∈ C∞ (C × R, Rm ) ◮ Example: position tracking of an end-effector Bee ◮ M(q) ∈ SE(3) position of the end-effector, ◮ M∗ (t) ∈ SE(3) reference position T(q, t) = t(M∗−1(t)M(q)) uθ(R∗−1(t)R(q)) where ◮ t() is the translation part of an homogeneous matrix, ◮ R and R∗ are the rotation part of M and M∗ . The stack of tasks
  24. 24. Introduction Theoretical foundations Software Task ◮ Definition: function of the ◮ robot configuration, ◮ time and ◮ possibly external parameters that should converge to 0: T ∈ C∞ (C × R, Rm ) ◮ Example: position tracking of an end-effector Bee ◮ M(q) ∈ SE(3) position of the end-effector, ◮ M∗ (t) ∈ SE(3) reference position T(q, t) = t(M∗−1(t)M(q)) uθ(R∗−1(t)R(q)) where ◮ t() is the translation part of an homogeneous matrix, ◮ R and R∗ are the rotation part of M and M∗ . The stack of tasks
  25. 25. Introduction Theoretical foundations Software Task ◮ Definition: function of the ◮ robot configuration, ◮ time and ◮ possibly external parameters that should converge to 0: T ∈ C∞ (C × R, Rm ) ◮ Example: position tracking of an end-effector Bee ◮ M(q) ∈ SE(3) position of the end-effector, ◮ M∗ (t) ∈ SE(3) reference position T(q, t) = t(M∗−1(t)M(q)) uθ(R∗−1(t)R(q)) where ◮ t() is the translation part of an homogeneous matrix, ◮ R and R∗ are the rotation part of M and M∗ . The stack of tasks
  26. 26. Introduction Theoretical foundations Software Task ◮ Definition: function of the ◮ robot configuration, ◮ time and ◮ possibly external parameters that should converge to 0: T ∈ C∞ (C × R, Rm ) ◮ Example: position tracking of an end-effector Bee ◮ M(q) ∈ SE(3) position of the end-effector, ◮ M∗ (t) ∈ SE(3) reference position T(q, t) = t(M∗−1(t)M(q)) uθ(R∗−1(t)R(q)) where ◮ t() is the translation part of an homogeneous matrix, ◮ R and R∗ are the rotation part of M and M∗ . The stack of tasks
  27. 27. Introduction Theoretical foundations Software Hierarchical task based control Given ◮ a configuration q, ◮ two tasks of decreasing priorities: ◮ T1 ∈ C∞ (C × R, Rm1 ), ◮ T2 ∈ C∞ (C × R, Rm2 ), compute a control vector ˙q ◮ that makes T1 converge toward 0 and ◮ that makes T2 converge toward 0 if possible. The stack of tasks
  28. 28. Introduction Theoretical foundations Software Hierarchical task based control Given ◮ a configuration q, ◮ two tasks of decreasing priorities: ◮ T1 ∈ C∞ (C × R, Rm1 ), ◮ T2 ∈ C∞ (C × R, Rm2 ), compute a control vector ˙q ◮ that makes T1 converge toward 0 and ◮ that makes T2 converge toward 0 if possible. The stack of tasks
  29. 29. Introduction Theoretical foundations Software Hierarchical task based control Jacobian: ◮ we denote ◮ Ji = ∂Ti ∂q for i ∈ {1, 2} ◮ then ◮ ∀q ∈ C, ∀t ∈ R, ∀ ˙q ∈ Rn , ˙Ti = Ji (q, t) ˙q + ∂Ti ∂t (q, t) We try to enforce ◮ ˙T1 = −λ1T1 ⇒ T1(t) = e−λ1t T1(0) → 0 ◮ ˙T2 = −λ2T2 ⇒ T2(t) = e−λ2t T2(0) → 0 ◮ λ1 and λ2 are called the gains associated to T1 and T2. The stack of tasks
  30. 30. Introduction Theoretical foundations Software Hierarchical task based control Jacobian: ◮ we denote ◮ Ji = ∂Ti ∂q for i ∈ {1, 2} ◮ then ◮ ∀q ∈ C, ∀t ∈ R, ∀ ˙q ∈ Rn , ˙Ti = Ji (q, t) ˙q + ∂Ti ∂t (q, t) We try to enforce ◮ ˙T1 = −λ1T1 ⇒ T1(t) = e−λ1t T1(0) → 0 ◮ ˙T2 = −λ2T2 ⇒ T2(t) = e−λ2t T2(0) → 0 ◮ λ1 and λ2 are called the gains associated to T1 and T2. The stack of tasks
  31. 31. Introduction Theoretical foundations Software Hierarchical task based control Jacobian: ◮ we denote ◮ Ji = ∂Ti ∂q for i ∈ {1, 2} ◮ then ◮ ∀q ∈ C, ∀t ∈ R, ∀ ˙q ∈ Rn , ˙Ti = Ji (q, t) ˙q + ∂Ti ∂t (q, t) We try to enforce ◮ ˙T1 = −λ1T1 ⇒ T1(t) = e−λ1t T1(0) → 0 ◮ ˙T2 = −λ2T2 ⇒ T2(t) = e−λ2t T2(0) → 0 ◮ λ1 and λ2 are called the gains associated to T1 and T2. The stack of tasks
  32. 32. Introduction Theoretical foundations Software Hierarchical task based control Jacobian: ◮ we denote ◮ Ji = ∂Ti ∂q for i ∈ {1, 2} ◮ then ◮ ∀q ∈ C, ∀t ∈ R, ∀ ˙q ∈ Rn , ˙Ti = Ji (q, t) ˙q + ∂Ti ∂t (q, t) We try to enforce ◮ ˙T1 = −λ1T1 ⇒ T1(t) = e−λ1t T1(0) → 0 ◮ ˙T2 = −λ2T2 ⇒ T2(t) = e−λ2t T2(0) → 0 ◮ λ1 and λ2 are called the gains associated to T1 and T2. The stack of tasks
  33. 33. Introduction Theoretical foundations Software Hierarchical task based control Jacobian: ◮ we denote ◮ Ji = ∂Ti ∂q for i ∈ {1, 2} ◮ then ◮ ∀q ∈ C, ∀t ∈ R, ∀ ˙q ∈ Rn , ˙Ti = Ji (q, t) ˙q + ∂Ti ∂t (q, t) We try to enforce ◮ ˙T1 = −λ1T1 ⇒ T1(t) = e−λ1t T1(0) → 0 ◮ ˙T2 = −λ2T2 ⇒ T2(t) = e−λ2t T2(0) → 0 ◮ λ1 and λ2 are called the gains associated to T1 and T2. The stack of tasks
  34. 34. Introduction Theoretical foundations Software Moore Penrose pseudo-inverse Given a matrix A ∈ Rm×n, the Moore Penrose pseudo inverse A+ ∈ Rn×m of A is the unique matrix satisfying: AA+ A = A A+ AA+ = A+ (AA+ )T = AA+ (A+ A)T = A+ A Given a linear system: Ax = b, A ∈ Rm×n , x ∈ Rn , b ∈ Rm x = A+b minimizes ◮ Ax − b over Rn, ◮ x over argmin Ax − b . The stack of tasks
  35. 35. Introduction Theoretical foundations Software Moore Penrose pseudo-inverse Given a matrix A ∈ Rm×n, the Moore Penrose pseudo inverse A+ ∈ Rn×m of A is the unique matrix satisfying: AA+ A = A A+ AA+ = A+ (AA+ )T = AA+ (A+ A)T = A+ A Given a linear system: Ax = b, A ∈ Rm×n , x ∈ Rn , b ∈ Rm x = A+b minimizes ◮ Ax − b over Rn, ◮ x over argmin Ax − b . The stack of tasks
  36. 36. Introduction Theoretical foundations Software Moore Penrose pseudo-inverse Given a matrix A ∈ Rm×n, the Moore Penrose pseudo inverse A+ ∈ Rn×m of A is the unique matrix satisfying: AA+ A = A A+ AA+ = A+ (AA+ )T = AA+ (A+ A)T = A+ A Given a linear system: Ax = b, A ∈ Rm×n , x ∈ Rn , b ∈ Rm x = A+b minimizes ◮ Ax − b over Rn, ◮ x over argmin Ax − b . The stack of tasks
  37. 37. Introduction Theoretical foundations Software Moore Penrose pseudo-inverse Given a matrix A ∈ Rm×n, the Moore Penrose pseudo inverse A+ ∈ Rn×m of A is the unique matrix satisfying: AA+ A = A A+ AA+ = A+ (AA+ )T = AA+ (A+ A)T = A+ A Given a linear system: Ax = b, A ∈ Rm×n , x ∈ Rn , b ∈ Rm x = A+b minimizes ◮ Ax − b over Rn, ◮ x over argmin Ax − b . The stack of tasks
  38. 38. Introduction Theoretical foundations Software Hierarchical task based control Resolution of the first constraint: ˙T1 = J1 ˙q + ∂T1 ∂t = −λ1T1 (1) J1 ˙q = −λ1T1 − ∂T1 ∂t (2) ˙q1 −J+ 1 (λ1T1 + ∂T1 ∂t ) (3) Where J+ 1 is the (Moore Penrose) pseudo-inverse of J1. ˙q1 minimizes ◮ J1 ˙q + λ1T1 + ∂T1 ∂t = ˙T1 + λ1T1 ◮ ˙q over argmin J1 ˙q + λ1T1 + ∂T1 ∂t Hence, ◮ if λ1T1 + ∂T1 ∂t is in Im(J1), (1) is satisfied The stack of tasks
  39. 39. Introduction Theoretical foundations Software Hierarchical task based control Resolution of the first constraint: ˙T1 = J1 ˙q + ∂T1 ∂t = −λ1T1 (1) J1 ˙q = −λ1T1 − ∂T1 ∂t (2) ˙q1 −J+ 1 (λ1T1 + ∂T1 ∂t ) (3) Where J+ 1 is the (Moore Penrose) pseudo-inverse of J1. ˙q1 minimizes ◮ J1 ˙q + λ1T1 + ∂T1 ∂t = ˙T1 + λ1T1 ◮ ˙q over argmin J1 ˙q + λ1T1 + ∂T1 ∂t Hence, ◮ if λ1T1 + ∂T1 ∂t is in Im(J1), (1) is satisfied The stack of tasks
  40. 40. Introduction Theoretical foundations Software Hierarchical task based control Resolution of the first constraint: ˙T1 = J1 ˙q + ∂T1 ∂t = −λ1T1 (1) J1 ˙q = −λ1T1 − ∂T1 ∂t (2) ˙q1 −J+ 1 (λ1T1 + ∂T1 ∂t ) (3) Where J+ 1 is the (Moore Penrose) pseudo-inverse of J1. ˙q1 minimizes ◮ J1 ˙q + λ1T1 + ∂T1 ∂t = ˙T1 + λ1T1 ◮ ˙q over argmin J1 ˙q + λ1T1 + ∂T1 ∂t Hence, ◮ if λ1T1 + ∂T1 ∂t is in Im(J1), (1) is satisfied The stack of tasks
  41. 41. Introduction Theoretical foundations Software Hierarchical task based control Resolution of the first constraint: ˙T1 = J1 ˙q + ∂T1 ∂t = −λ1T1 (1) J1 ˙q = −λ1T1 − ∂T1 ∂t (2) ˙q1 −J+ 1 (λ1T1 + ∂T1 ∂t ) (3) Where J+ 1 is the (Moore Penrose) pseudo-inverse of J1. ˙q1 minimizes ◮ J1 ˙q + λ1T1 + ∂T1 ∂t = ˙T1 + λ1T1 ◮ ˙q over argmin J1 ˙q + λ1T1 + ∂T1 ∂t Hence, ◮ if λ1T1 + ∂T1 ∂t is in Im(J1), (1) is satisfied The stack of tasks
  42. 42. Introduction Theoretical foundations Software Hierarchical task based control Resolution of the first constraint: ˙T1 = J1 ˙q + ∂T1 ∂t = −λ1T1 (1) J1 ˙q = −λ1T1 − ∂T1 ∂t (2) ˙q1 −J+ 1 (λ1T1 + ∂T1 ∂t ) (3) Where J+ 1 is the (Moore Penrose) pseudo-inverse of J1. ˙q1 minimizes ◮ J1 ˙q + λ1T1 + ∂T1 ∂t = ˙T1 + λ1T1 ◮ ˙q over argmin J1 ˙q + λ1T1 + ∂T1 ∂t Hence, ◮ if λ1T1 + ∂T1 ∂t is in Im(J1), (1) is satisfied The stack of tasks
  43. 43. Introduction Theoretical foundations Software Hierarchical task based control In fact ∀u ∈ Rn , J1 ˙q1+(In − J+ 1 J1)u = J1 ˙q1 therefore, ˙q = ˙q1 + (In − J+ 1 J1)u also minimizes J1 ˙q + λ1T1 + ∂T1 ∂t . P1 = (In − J+ 1 J1) is a projector on J1 kernel: J1P1 = 0 ∀u ∈ Rn, if ˙q = P1u, then, ˙T1 = ∂T1 ∂t . The stack of tasks
  44. 44. Introduction Theoretical foundations Software Hierarchical task based control In fact ∀u ∈ Rn , J1 ˙q1+(In − J+ 1 J1)u = J1 ˙q1 therefore, ˙q = ˙q1 + (In − J+ 1 J1)u also minimizes J1 ˙q + λ1T1 + ∂T1 ∂t . P1 = (In − J+ 1 J1) is a projector on J1 kernel: J1P1 = 0 ∀u ∈ Rn, if ˙q = P1u, then, ˙T1 = ∂T1 ∂t . The stack of tasks
  45. 45. Introduction Theoretical foundations Software Hierarchical task based control In fact ∀u ∈ Rn , J1 ˙q1+(In − J+ 1 J1)u = J1 ˙q1 therefore, ˙q = ˙q1 + (In − J+ 1 J1)u also minimizes J1 ˙q + λ1T1 + ∂T1 ∂t . P1 = (In − J+ 1 J1) is a projector on J1 kernel: J1P1 = 0 ∀u ∈ Rn, if ˙q = P1u, then, ˙T1 = ∂T1 ∂t . The stack of tasks
  46. 46. Introduction Theoretical foundations Software Hierarchical task based control In fact ∀u ∈ Rn , J1 ˙q1+(In − J+ 1 J1)u = J1 ˙q1 therefore, ˙q = ˙q1 + (In − J+ 1 J1)u also minimizes J1 ˙q + λ1T1 + ∂T1 ∂t . P1 = (In − J+ 1 J1) is a projector on J1 kernel: J1P1 = 0 ∀u ∈ Rn, if ˙q = P1u, then, ˙T1 = ∂T1 ∂t . The stack of tasks
  47. 47. Introduction Theoretical foundations Software Controlling the second task We have ˙q = ˙q1 + P1u ˙T2 = J2 ˙q + ∂T2 ∂t ˙T2 = J2 ˙q1 + ∂T2 ∂t + J2P1u We want ˙T2 = −λ2T2 Thus −λ2T2 = J2 ˙q1 + ∂T2 ∂t + J2P1u J2P1u = −λ2T2 − J2 ˙q1 − ∂T2 ∂t The stack of tasks
  48. 48. Introduction Theoretical foundations Software Controlling the second task We have ˙q = ˙q1 + P1u ˙T2 = J2 ˙q + ∂T2 ∂t ˙T2 = J2 ˙q1 + ∂T2 ∂t + J2P1u We want ˙T2 = −λ2T2 Thus −λ2T2 = J2 ˙q1 + ∂T2 ∂t + J2P1u J2P1u = −λ2T2 − J2 ˙q1 − ∂T2 ∂t The stack of tasks
  49. 49. Introduction Theoretical foundations Software Controlling the second task We have ˙q = ˙q1 + P1u ˙T2 = J2 ˙q + ∂T2 ∂t ˙T2 = J2 ˙q1 + ∂T2 ∂t + J2P1u We want ˙T2 = −λ2T2 Thus −λ2T2 = J2 ˙q1 + ∂T2 ∂t + J2P1u J2P1u = −λ2T2 − J2 ˙q1 − ∂T2 ∂t The stack of tasks
  50. 50. Introduction Theoretical foundations Software Controlling the second task Thus −λ2T2 = J2 ˙q1 + ∂T2 ∂t + J2P1u J2P1u = −λ2T2 − J2 ˙q1 − ∂T2 ∂t u = −(J2P1)+ (λ2T2 + J2 ˙q1 + ∂T2 ∂t ) ˙q2 ˙q1 + P1u = ˙q1 − P1(J2P1)+ (λ2T2 + J2 ˙q1 + ∂T2 ∂t )) minimizes ˙T2 + λ2T2 over ˙q1 + Ker J1. The stack of tasks
  51. 51. Introduction Theoretical foundations Software Controlling the second task Thus −λ2T2 = J2 ˙q1 + ∂T2 ∂t + J2P1u J2P1u = −λ2T2 − J2 ˙q1 − ∂T2 ∂t u = −(J2P1)+ (λ2T2 + J2 ˙q1 + ∂T2 ∂t ) ˙q2 ˙q1 + P1u = ˙q1 − P1(J2P1)+ (λ2T2 + J2 ˙q1 + ∂T2 ∂t )) minimizes ˙T2 + λ2T2 over ˙q1 + Ker J1. The stack of tasks
  52. 52. Introduction Theoretical foundations Software Example ◮ T1: position of the feet + projection of center of mass, ◮ T2: position of the right wrist. The stack of tasks
  53. 53. Introduction Theoretical foundations Software Outline Introduction Theoretical foundations Software The stack of tasks
  54. 54. Introduction Theoretical foundations Software Architecture overview The stack of tasks
  55. 55. Introduction Theoretical foundations Software Libraries ◮ jrl-mathtools: implementation of small size matrices, ◮ to be replaced by Eigen ◮ jrl-mal: abstract layer for matrices, ◮ to be replaced by Eigen ◮ abstract-robot-dynamics: abstraction for humanoid robot description, ◮ jrl-dynamics: implementation of the above abstract interfaces, ◮ jrl-walkgen: ZMP based dynamic walk generation. The stack of tasks
  56. 56. Introduction Theoretical foundations Software Libraries ◮ jrl-mathtools: implementation of small size matrices, ◮ to be replaced by Eigen ◮ jrl-mal: abstract layer for matrices, ◮ to be replaced by Eigen ◮ abstract-robot-dynamics: abstraction for humanoid robot description, ◮ jrl-dynamics: implementation of the above abstract interfaces, ◮ jrl-walkgen: ZMP based dynamic walk generation. The stack of tasks
  57. 57. Introduction Theoretical foundations Software Libraries ◮ jrl-mathtools: implementation of small size matrices, ◮ to be replaced by Eigen ◮ jrl-mal: abstract layer for matrices, ◮ to be replaced by Eigen ◮ abstract-robot-dynamics: abstraction for humanoid robot description, ◮ jrl-dynamics: implementation of the above abstract interfaces, ◮ jrl-walkgen: ZMP based dynamic walk generation. The stack of tasks
  58. 58. Introduction Theoretical foundations Software Libraries ◮ jrl-mathtools: implementation of small size matrices, ◮ to be replaced by Eigen ◮ jrl-mal: abstract layer for matrices, ◮ to be replaced by Eigen ◮ abstract-robot-dynamics: abstraction for humanoid robot description, ◮ jrl-dynamics: implementation of the above abstract interfaces, ◮ jrl-walkgen: ZMP based dynamic walk generation. The stack of tasks
  59. 59. Introduction Theoretical foundations Software Libraries ◮ jrl-mathtools: implementation of small size matrices, ◮ to be replaced by Eigen ◮ jrl-mal: abstract layer for matrices, ◮ to be replaced by Eigen ◮ abstract-robot-dynamics: abstraction for humanoid robot description, ◮ jrl-dynamics: implementation of the above abstract interfaces, ◮ jrl-walkgen: ZMP based dynamic walk generation. The stack of tasks
  60. 60. Introduction Theoretical foundations Software dynamic-graph ◮ Entity ◮ Signal: synchronous interface ◮ Command: asynchronous interface ◮ Factory ◮ builds a new entity of requested type, ◮ new entity types can be dynamically added (advanced). ◮ Pool ◮ stores all instances of entities, ◮ return reference to entity of given name. The stack of tasks
  61. 61. Introduction Theoretical foundations Software dynamic-graph ◮ Entity ◮ Signal: synchronous interface ◮ Command: asynchronous interface ◮ Factory ◮ builds a new entity of requested type, ◮ new entity types can be dynamically added (advanced). ◮ Pool ◮ stores all instances of entities, ◮ return reference to entity of given name. The stack of tasks
  62. 62. Introduction Theoretical foundations Software dynamic-graph ◮ Entity ◮ Signal: synchronous interface ◮ Command: asynchronous interface ◮ Factory ◮ builds a new entity of requested type, ◮ new entity types can be dynamically added (advanced). ◮ Pool ◮ stores all instances of entities, ◮ return reference to entity of given name. The stack of tasks
  63. 63. Introduction Theoretical foundations Software Signal Synchronous interface storing a given data type ◮ output signals: ◮ recomputed by a callback function, or ◮ set to constant value ◮ warning: setting to constant value deactivate callback, ◮ input signals: ◮ plugged by an output signal, or ◮ set to constant value, ◮ warning: setting to constant value unplugs, The stack of tasks
  64. 64. Introduction Theoretical foundations Software Signal Synchronous interface storing a given data type ◮ output signals: ◮ recomputed by a callback function, or ◮ set to constant value ◮ warning: setting to constant value deactivate callback, ◮ input signals: ◮ plugged by an output signal, or ◮ set to constant value, ◮ warning: setting to constant value unplugs, The stack of tasks
  65. 65. Introduction Theoretical foundations Software Signal Synchronous interface storing a given data type ◮ output signals: ◮ recomputed by a callback function, or ◮ set to constant value ◮ warning: setting to constant value deactivate callback, ◮ input signals: ◮ plugged by an output signal, or ◮ set to constant value, ◮ warning: setting to constant value unplugs, The stack of tasks
  66. 66. Introduction Theoretical foundations Software Signal Synchronous interface storing a given data type ◮ output signals: ◮ recomputed by a callback function, or ◮ set to constant value ◮ warning: setting to constant value deactivate callback, ◮ input signals: ◮ plugged by an output signal, or ◮ set to constant value, ◮ warning: setting to constant value unplugs, The stack of tasks
  67. 67. Introduction Theoretical foundations Software Signal Synchronous interface storing a given data type ◮ dependency relation: s1 depends on s2 if s1 callback needs the value of s2, ◮ each signal s stores time of last recomputation in member s.t ◮ s is said outdated at time t if ◮ t > s.t , and ◮ one dependency s dep of s ◮ is out-dated or ◮ has been recomputed later than s: s dep.t > s.t . ◮ reading an out-dated signal triggers recomputation. ◮ New types can be dynamically added (advanced) The stack of tasks
  68. 68. Introduction Theoretical foundations Software Signal Synchronous interface storing a given data type ◮ dependency relation: s1 depends on s2 if s1 callback needs the value of s2, ◮ each signal s stores time of last recomputation in member s.t ◮ s is said outdated at time t if ◮ t > s.t , and ◮ one dependency s dep of s ◮ is out-dated or ◮ has been recomputed later than s: s dep.t > s.t . ◮ reading an out-dated signal triggers recomputation. ◮ New types can be dynamically added (advanced) The stack of tasks
  69. 69. Introduction Theoretical foundations Software Signal Synchronous interface storing a given data type ◮ dependency relation: s1 depends on s2 if s1 callback needs the value of s2, ◮ each signal s stores time of last recomputation in member s.t ◮ s is said outdated at time t if ◮ t > s.t , and ◮ one dependency s dep of s ◮ is out-dated or ◮ has been recomputed later than s: s dep.t > s.t . ◮ reading an out-dated signal triggers recomputation. ◮ New types can be dynamically added (advanced) The stack of tasks
  70. 70. Introduction Theoretical foundations Software Signal Synchronous interface storing a given data type ◮ dependency relation: s1 depends on s2 if s1 callback needs the value of s2, ◮ each signal s stores time of last recomputation in member s.t ◮ s is said outdated at time t if ◮ t > s.t , and ◮ one dependency s dep of s ◮ is out-dated or ◮ has been recomputed later than s: s dep.t > s.t . ◮ reading an out-dated signal triggers recomputation. ◮ New types can be dynamically added (advanced) The stack of tasks
  71. 71. Introduction Theoretical foundations Software Signal Synchronous interface storing a given data type ◮ dependency relation: s1 depends on s2 if s1 callback needs the value of s2, ◮ each signal s stores time of last recomputation in member s.t ◮ s is said outdated at time t if ◮ t > s.t , and ◮ one dependency s dep of s ◮ is out-dated or ◮ has been recomputed later than s: s dep.t > s.t . ◮ reading an out-dated signal triggers recomputation. ◮ New types can be dynamically added (advanced) The stack of tasks
  72. 72. Introduction Theoretical foundations Software Signal Synchronous interface storing a given data type ◮ dependency relation: s1 depends on s2 if s1 callback needs the value of s2, ◮ each signal s stores time of last recomputation in member s.t ◮ s is said outdated at time t if ◮ t > s.t , and ◮ one dependency s dep of s ◮ is out-dated or ◮ has been recomputed later than s: s dep.t > s.t . ◮ reading an out-dated signal triggers recomputation. ◮ New types can be dynamically added (advanced) The stack of tasks
  73. 73. Introduction Theoretical foundations Software Command Asynchronous interface ◮ input in a fixed set of types, ◮ trigger an action, ◮ returns a result in the same set of types. The stack of tasks
  74. 74. Introduction Theoretical foundations Software dynamic-graph-python Python bindings to dynamic-graph ◮ module dynamic graph linked to libdynamic-graph.so ◮ class Entity ◮ each C++ entity class declared in the factory generates a python class of the same name, ◮ signals are instance members, ◮ commands are bound to instance methods ◮ method help lists commands ◮ method displaySignals displays signals ◮ class Signal ◮ property value to set and get signal value ◮ remote interpreter to be embedded into a robot controller (advanced) The stack of tasks
  75. 75. Introduction Theoretical foundations Software dynamic-graph-python Python bindings to dynamic-graph ◮ module dynamic graph linked to libdynamic-graph.so ◮ class Entity ◮ each C++ entity class declared in the factory generates a python class of the same name, ◮ signals are instance members, ◮ commands are bound to instance methods ◮ method help lists commands ◮ method displaySignals displays signals ◮ class Signal ◮ property value to set and get signal value ◮ remote interpreter to be embedded into a robot controller (advanced) The stack of tasks
  76. 76. Introduction Theoretical foundations Software dynamic-graph-python Python bindings to dynamic-graph ◮ module dynamic graph linked to libdynamic-graph.so ◮ class Entity ◮ each C++ entity class declared in the factory generates a python class of the same name, ◮ signals are instance members, ◮ commands are bound to instance methods ◮ method help lists commands ◮ method displaySignals displays signals ◮ class Signal ◮ property value to set and get signal value ◮ remote interpreter to be embedded into a robot controller (advanced) The stack of tasks
  77. 77. Introduction Theoretical foundations Software dynamic-graph-python Python bindings to dynamic-graph ◮ module dynamic graph linked to libdynamic-graph.so ◮ class Entity ◮ each C++ entity class declared in the factory generates a python class of the same name, ◮ signals are instance members, ◮ commands are bound to instance methods ◮ method help lists commands ◮ method displaySignals displays signals ◮ class Signal ◮ property value to set and get signal value ◮ remote interpreter to be embedded into a robot controller (advanced) The stack of tasks
  78. 78. Introduction Theoretical foundations Software dynamic-graph-python Python bindings to dynamic-graph ◮ module dynamic graph linked to libdynamic-graph.so ◮ class Entity ◮ each C++ entity class declared in the factory generates a python class of the same name, ◮ signals are instance members, ◮ commands are bound to instance methods ◮ method help lists commands ◮ method displaySignals displays signals ◮ class Signal ◮ property value to set and get signal value ◮ remote interpreter to be embedded into a robot controller (advanced) The stack of tasks
  79. 79. Introduction Theoretical foundations Software dynamic-graph-tutorial Simple use case for illustration ◮ Definition of 2 entity types ◮ InvertedPendulum ◮ input signal: force ◮ output signal: state ◮ FeedbackController ◮ input signal: state ◮ output signal: force The stack of tasks
  80. 80. Introduction Theoretical foundations Software dynamic-graph-tutorial >>> from dynamic graph.tutorial import InvertedPendulum, FeedbackController >>> a = InvertedPendulum (’IP’) >>> b = FeedbackController (’K’) >>> a.displaySignals () --- <IP> signal list: |-- <Sig:InvertedPendulum(IP)::input(double)::force (Type Cst) AUTOPLUGGED ‘-- <Sig:InvertedPendulum(IP)::output(vector)::state (Type Cst) >>> a.help () Classical inverted pendulum dynamic model List of commands: ----------------- getCartMass: Get cart mass getPendulumLength: Get pendulum length getPendulumMass: Get pendulum mass incr: Integrate dynamics for time step provided as input setCartMass: Set cart mass setPendulumLength: Set pendulum length setPendulumMass: Set pendulum mass >>> a.help (’incr’) incr: Integrate dynamics for time step provided as input take one floating point number as input >>> The stack of tasks
  81. 81. Introduction Theoretical foundations Software dynamic-graph-tutorial >>> from dynamic graph.tutorial import InvertedPendulum, FeedbackController >>> a = InvertedPendulum (’IP’) >>> b = FeedbackController (’K’) >>> a.displaySignals () --- <IP> signal list: |-- <Sig:InvertedPendulum(IP)::input(double)::force (Type Cst) AUTOPLUGGED ‘-- <Sig:InvertedPendulum(IP)::output(vector)::state (Type Cst) >>> a.help () Classical inverted pendulum dynamic model List of commands: ----------------- getCartMass: Get cart mass getPendulumLength: Get pendulum length getPendulumMass: Get pendulum mass incr: Integrate dynamics for time step provided as input setCartMass: Set cart mass setPendulumLength: Set pendulum length setPendulumMass: Set pendulum mass >>> a.help (’incr’) incr: Integrate dynamics for time step provided as input take one floating point number as input >>> The stack of tasks
  82. 82. Introduction Theoretical foundations Software dynamic-graph-tutorial >>> from dynamic graph.tutorial import InvertedPendulum, FeedbackController >>> a = InvertedPendulum (’IP’) >>> b = FeedbackController (’K’) >>> a.displaySignals () --- <IP> signal list: |-- <Sig:InvertedPendulum(IP)::input(double)::force (Type Cst) AUTOPLUGGED ‘-- <Sig:InvertedPendulum(IP)::output(vector)::state (Type Cst) >>> a.help () Classical inverted pendulum dynamic model List of commands: ----------------- getCartMass: Get cart mass getPendulumLength: Get pendulum length getPendulumMass: Get pendulum mass incr: Integrate dynamics for time step provided as input setCartMass: Set cart mass setPendulumLength: Set pendulum length setPendulumMass: Set pendulum mass >>> a.help (’incr’) incr: Integrate dynamics for time step provided as input take one floating point number as input >>> The stack of tasks
  83. 83. Introduction Theoretical foundations Software dynamic-graph-tutorial >>> from dynamic graph.tutorial import InvertedPendulum, FeedbackController >>> a = InvertedPendulum (’IP’) >>> b = FeedbackController (’K’) >>> a.displaySignals () --- <IP> signal list: |-- <Sig:InvertedPendulum(IP)::input(double)::force (Type Cst) AUTOPLUGGED ‘-- <Sig:InvertedPendulum(IP)::output(vector)::state (Type Cst) >>> a.help () Classical inverted pendulum dynamic model List of commands: ----------------- getCartMass: Get cart mass getPendulumLength: Get pendulum length getPendulumMass: Get pendulum mass incr: Integrate dynamics for time step provided as input setCartMass: Set cart mass setPendulumLength: Set pendulum length setPendulumMass: Set pendulum mass >>> a.help (’incr’) incr: Integrate dynamics for time step provided as input take one floating point number as input >>> The stack of tasks
  84. 84. Introduction Theoretical foundations Software dynamic-graph-tutorial >>> from dynamic graph.tutorial import InvertedPendulum, FeedbackController >>> a = InvertedPendulum (’IP’) >>> b = FeedbackController (’K’) >>> a.displaySignals () --- <IP> signal list: |-- <Sig:InvertedPendulum(IP)::input(double)::force (Type Cst) AUTOPLUGGED ‘-- <Sig:InvertedPendulum(IP)::output(vector)::state (Type Cst) >>> a.help () Classical inverted pendulum dynamic model List of commands: ----------------- getCartMass: Get cart mass getPendulumLength: Get pendulum length getPendulumMass: Get pendulum mass incr: Integrate dynamics for time step provided as input setCartMass: Set cart mass setPendulumLength: Set pendulum length setPendulumMass: Set pendulum mass >>> a.help (’incr’) incr: Integrate dynamics for time step provided as input take one floating point number as input >>> The stack of tasks
  85. 85. Introduction Theoretical foundations Software dynamic-graph-tutorial Package provides ◮ C++ code of classes InvertedPendulum and FeedbackController, ◮ explanation about how to create a new entity type in C++, ◮ information about how to create a command in C++, ◮ information about how to create a python module defining the bindings in cmake, ◮ python script that runs an example. The stack of tasks
  86. 86. Introduction Theoretical foundations Software dynamic-graph-tutorial Package provides ◮ C++ code of classes InvertedPendulum and FeedbackController, ◮ explanation about how to create a new entity type in C++, ◮ information about how to create a command in C++, ◮ information about how to create a python module defining the bindings in cmake, ◮ python script that runs an example. The stack of tasks
  87. 87. Introduction Theoretical foundations Software dynamic-graph-tutorial Package provides ◮ C++ code of classes InvertedPendulum and FeedbackController, ◮ explanation about how to create a new entity type in C++, ◮ information about how to create a command in C++, ◮ information about how to create a python module defining the bindings in cmake, ◮ python script that runs an example. The stack of tasks
  88. 88. Introduction Theoretical foundations Software dynamic-graph-tutorial Package provides ◮ C++ code of classes InvertedPendulum and FeedbackController, ◮ explanation about how to create a new entity type in C++, ◮ information about how to create a command in C++, ◮ information about how to create a python module defining the bindings in cmake, ◮ python script that runs an example. The stack of tasks
  89. 89. Introduction Theoretical foundations Software dynamic-graph-tutorial Package provides ◮ C++ code of classes InvertedPendulum and FeedbackController, ◮ explanation about how to create a new entity type in C++, ◮ information about how to create a command in C++, ◮ information about how to create a python module defining the bindings in cmake, ◮ python script that runs an example. The stack of tasks
  90. 90. Introduction Theoretical foundations Software sot-core Class FeatureAbstract ◮ function of the robot and environment states ◮ position of an end-effector, ◮ position of a feature in an image (visual servoing) ◮ with values in a Lie group G (SO(3), SE(3), Rn,...), ◮ with a mapping e from G into Rm such that e(0G) = 0 The stack of tasks
  91. 91. Introduction Theoretical foundations Software sot-core Class FeatureAbstract ◮ function of the robot and environment states ◮ position of an end-effector, ◮ position of a feature in an image (visual servoing) ◮ with values in a Lie group G (SO(3), SE(3), Rn,...), ◮ with a mapping e from G into Rm such that e(0G) = 0 The stack of tasks
  92. 92. Introduction Theoretical foundations Software sot-core Class FeatureAbstract ◮ function of the robot and environment states ◮ position of an end-effector, ◮ position of a feature in an image (visual servoing) ◮ with values in a Lie group G (SO(3), SE(3), Rn,...), ◮ with a mapping e from G into Rm such that e(0G) = 0 The stack of tasks
  93. 93. Introduction Theoretical foundations Software sot-core Class FeatureAbstract ◮ function of the robot and environment states ◮ position of an end-effector, ◮ position of a feature in an image (visual servoing) ◮ with values in a Lie group G (SO(3), SE(3), Rn,...), ◮ with a mapping e from G into Rm such that e(0G) = 0 The stack of tasks
  94. 94. Introduction Theoretical foundations Software Feature When paired with a reference, features become tasks. ◮ Example ◮ error = e (value.position⊖reference.position) ◮ errordot: derivative of error when value.position is constant. The stack of tasks
  95. 95. Introduction Theoretical foundations Software Feature When paired with a reference, features become tasks. ◮ Example ◮ error = e (value.position⊖reference.position) ◮ errordot: derivative of error when value.position is constant. The stack of tasks
  96. 96. Introduction Theoretical foundations Software Feature When paired with a reference, features become tasks. ◮ Example ◮ error = e (value.position⊖reference.position) ◮ errordot: derivative of error when value.position is constant. The stack of tasks
  97. 97. Introduction Theoretical foundations Software Task ◮ Collection of features with a control gain, ◮ implements abstraction TaskAbstract ◮ task = −controlGain.error The stack of tasks
  98. 98. Introduction Theoretical foundations Software Solver SOT Hierarchical task solver ◮ computes robot joint velocity The stack of tasks
  99. 99. Introduction Theoretical foundations Software sot-dynamic dynamic graph.sot.dynamics.Dynamic builds a kinematic chain from a file and ◮ computes forward kinematics ◮ position and Jacobian of end effectors (wrists, ankles), ◮ position of center of mass ◮ computes dynamics ◮ inertia matrix. The stack of tasks
  100. 100. Introduction Theoretical foundations Software sot-pattern-generator dynamic graph.sot.pattern generator ◮ Entity PatternGenerator produces walk motions as ◮ position and velocity of the feet ◮ position and velocity of the center of mass The stack of tasks
  101. 101. Introduction Theoretical foundations Software sot-application dynamic graph.sot.application ◮ Provide scripts for standard control graph initialization ◮ depends on application: control mode (velocity, acceleration) The stack of tasks
  102. 102. Introduction Theoretical foundations Software Packages specific to robots sot-hrp2 ◮ defines a class Robot that provides ◮ ready to use features for feet, hands, gaze and center of mass, ◮ ready to use tasks for the same end effectors, ◮ an entity Dynamic, ◮ an entity Device (interface with the robot control system) sot-hrprtc-hrp2 ◮ provide an RTC component to integrate sot-hrp2 into the robot controller. The stack of tasks
  103. 103. Introduction Theoretical foundations Software Utilities ◮ dynamic graph.writeGraph (filename): writes the current graph in a file using graphviz dot format. ◮ dynamic graph.sot.core.FeaturePosition wraps two FeaturePoint6d: a value and a reference, ◮ MetaTask6d: ◮ MetaTaskPosture: ◮ MetaTaskKine6d: ◮ MetaTaskKinePosture: ◮ MetaTaskCom: The stack of tasks
  104. 104. Introduction Theoretical foundations Software Utilities ◮ dynamic graph.writeGraph (filename): writes the current graph in a file using graphviz dot format. ◮ dynamic graph.sot.core.FeaturePosition wraps two FeaturePoint6d: a value and a reference, ◮ MetaTask6d: ◮ MetaTaskPosture: ◮ MetaTaskKine6d: ◮ MetaTaskKinePosture: ◮ MetaTaskCom: The stack of tasks
  105. 105. Introduction Theoretical foundations Software Utilities ◮ dynamic graph.writeGraph (filename): writes the current graph in a file using graphviz dot format. ◮ dynamic graph.sot.core.FeaturePosition wraps two FeaturePoint6d: a value and a reference, ◮ MetaTask6d: ◮ MetaTaskPosture: ◮ MetaTaskKine6d: ◮ MetaTaskKinePosture: ◮ MetaTaskCom: The stack of tasks
  106. 106. Introduction Theoretical foundations Software Utilities ◮ dynamic graph.writeGraph (filename): writes the current graph in a file using graphviz dot format. ◮ dynamic graph.sot.core.FeaturePosition wraps two FeaturePoint6d: a value and a reference, ◮ MetaTask6d: ◮ MetaTaskPosture: ◮ MetaTaskKine6d: ◮ MetaTaskKinePosture: ◮ MetaTaskCom: The stack of tasks
  107. 107. Introduction Theoretical foundations Software Utilities ◮ dynamic graph.writeGraph (filename): writes the current graph in a file using graphviz dot format. ◮ dynamic graph.sot.core.FeaturePosition wraps two FeaturePoint6d: a value and a reference, ◮ MetaTask6d: ◮ MetaTaskPosture: ◮ MetaTaskKine6d: ◮ MetaTaskKinePosture: ◮ MetaTaskCom: The stack of tasks
  108. 108. Introduction Theoretical foundations Software Utilities ◮ dynamic graph.writeGraph (filename): writes the current graph in a file using graphviz dot format. ◮ dynamic graph.sot.core.FeaturePosition wraps two FeaturePoint6d: a value and a reference, ◮ MetaTask6d: ◮ MetaTaskPosture: ◮ MetaTaskKine6d: ◮ MetaTaskKinePosture: ◮ MetaTaskCom: The stack of tasks
  109. 109. Introduction Theoretical foundations Software Utilities ◮ dynamic graph.writeGraph (filename): writes the current graph in a file using graphviz dot format. ◮ dynamic graph.sot.core.FeaturePosition wraps two FeaturePoint6d: a value and a reference, ◮ MetaTask6d: ◮ MetaTaskPosture: ◮ MetaTaskKine6d: ◮ MetaTaskKinePosture: ◮ MetaTaskCom: The stack of tasks
  110. 110. Introduction Theoretical foundations Software Installation Through robotpkg ◮ git clone http://trac.laas.fr/git/robots/robotpkg.git cd robotpkg ./bootstrap/bootstrap --prefix=<your prefix> cd motion/sot-dynamic make install The stack of tasks
  111. 111. Introduction Theoretical foundations Software Installation Through github: ◮ git clone --recursive git://github.com/jrl-umi3218/jrl-mal.git git clone --recursive git://github.com/jrl-umi3218/jrl-mathtools.git git clone --recursive git://github.com/laas/abstract-robot-dynamics.git git clone --recursive git://github.com/jrl-umi3218/jrl-dynamics.git git clone --recursive git://github.com/jrl-umi3218/jrl-walkgen.git git clone --recursive git://github.com/jrl-umi3218/dynamic-graph.git git clone --recursive git://github.com/jrl-umi3218/dynamic-graph-python.git git clone --recursive git://github.com/jrl-umi3218/sot-core.git git clone --recursive git://github.com/laas/sot-tools.git git clone --recursive git://github.com/jrl-umi3218/sot-dynamic.git git clone --recursive git://github.com/jrl-umi3218/sot-pattern-generator.git git clone --recursive git://github.com/stack-of-tasks/sot-application.git git clone --recursive git://github.com/laas/sot-hrp2.git git clone --recursive git://github.com/stack-of-tasks/sot-hrprtc-hrp2.git ◮ for each package, mkdir package/build cd package/build cmake -DCMAKE INSTALL PREFIX=<your prefix> .. make install The stack of tasks
  112. 112. Introduction Theoretical foundations Software Installation Through github: ◮ git clone --recursive git://github.com/jrl-umi3218/jrl-mal.git git clone --recursive git://github.com/jrl-umi3218/jrl-mathtools.git git clone --recursive git://github.com/laas/abstract-robot-dynamics.git git clone --recursive git://github.com/jrl-umi3218/jrl-dynamics.git git clone --recursive git://github.com/jrl-umi3218/jrl-walkgen.git git clone --recursive git://github.com/jrl-umi3218/dynamic-graph.git git clone --recursive git://github.com/jrl-umi3218/dynamic-graph-python.git git clone --recursive git://github.com/jrl-umi3218/sot-core.git git clone --recursive git://github.com/laas/sot-tools.git git clone --recursive git://github.com/jrl-umi3218/sot-dynamic.git git clone --recursive git://github.com/jrl-umi3218/sot-pattern-generator.git git clone --recursive git://github.com/stack-of-tasks/sot-application.git git clone --recursive git://github.com/laas/sot-hrp2.git git clone --recursive git://github.com/stack-of-tasks/sot-hrprtc-hrp2.git ◮ for each package, mkdir package/build cd package/build cmake -DCMAKE INSTALL PREFIX=<your prefix> .. make install The stack of tasks
  113. 113. Introduction Theoretical foundations Software Installation Through installation script ◮ git clone git://github.com/stack-of-tasks/install-sot.git cd install-sot/scripts ./install sot.sh The stack of tasks
  114. 114. Introduction Theoretical foundations Software Running the stack of tasks into OpenHRP-3.1 You need to install: ◮ ros-electric ◮ OpenHRP-3.1 you will find instructions in https://wiki.laas.fr/robots/HRP/Software Then follow instructions in sot-hrprtc/README.md: https://github.com/stack-of-tasks/sot-hrprtc-hrp2 The stack of tasks
  115. 115. Introduction Theoretical foundations Software Running the stack of tasks into OpenHRP-3.0.7 Assumptions ◮ OpenHRP 3.0.7 is installed ◮ The Stack of Tasks has been installed thanks to previous slide with install sot.sh in the directory: /home/ user / devel / ros unstable ◮ Your /opt/grx3.0/HRP2LAAS/bin/config.sh is well setup. The golden commands $>roscore #Launching HRP2 simulation with OpenHPR $>roslaunch hrp2 bringup openhrp bridge . launch robot := hrp2 14 mode:= d g w i t h s t a b i l i z e r simulation := true $>rosrun dynamic graph bridge run command $>>> . . . # create the solver ( see next s l i d e ) $>rosservice c a l l / start dynamic graph The stack of tasks
  116. 116. Introduction Theoretical foundations Software Running the stack of tasks into OpenHRP-3.0.7 Initialize the application: create tracer and solver [ INFO ] [ WallTime : 1370854858.786392] waiting for service . . . I n t e r a c t i n g with remote server . >>> from dynamic graph . sot . a p p l i c a t i o n . v e l o c i t y . precomputed tasks import i n i t i a l i z e >>> solver = i n i t i a l i z e ( robot ) >>> robot . i n i t i a l i z e T r a c e r ( ) The stack of tasks
  117. 117. Introduction Theoretical foundations Software Running the stack of tasks into OpenHRP-3.0.7 Build the graph including the pattern generator [ INFO ] [ WallTime : 1370854858.786392] waiting for service . . . I n t e r a c t i n g with remote server . >>> from dynamic graph . sot . pattern generator . walking import CreateEverythingForPG , walkFewSteps With meta selector The stack of tasks
  118. 118. Introduction Theoretical foundations Software Running the stack of tasks into OpenHRP-3.0.7 Create the graph >>> CreateEverythingForPG ( robot , solver ) At t h i s stage ( ’ modelDir : ’ , ’ ˜ / devel / ros−unstable / i n s t a l l / share / hrp2−14 ’ ) ( ’modelName : ’ , ’ HRP2JRLmainsmall . wrl ’ ) ( ’ s p e c i f i c i t i e s P a t h : ’ , ’ HRP2SpecificitiesSmall . xml ’ ) ( ’ jointRankPath : ’ , ’ HRP2LinkJointRankSmall . xml ’ ) After Task for Right and Left Feet The stack of tasks
  119. 119. Introduction Theoretical foundations Software Running the stack of tasks into OpenHRP-3.0.7 Switch to the new graph >>> walkFewSteps ( robot ) >>> The stack of tasks
  120. 120. Introduction Theoretical foundations Software Software structure - Conceptual view The stack of tasks
  121. 121. Introduction Theoretical foundations Software Software structure - Link with Model The stack of tasks
  122. 122. Introduction Theoretical foundations Software Software structure - Link with Model The stack of tasks
  123. 123. Introduction Theoretical foundations Software Software structure - Link with Model The stack of tasks
  124. 124. Introduction Theoretical foundations Software Software structure - Link with Model The stack of tasks
  125. 125. Introduction Theoretical foundations Software Software structure - Link with Model The stack of tasks
  126. 126. Introduction Theoretical foundations Software Software structure - Link with Model The stack of tasks
  127. 127. Introduction Theoretical foundations Software Software structure - Repositories The stack of tasks

×