Task-Space Inverse Dynamics: Implementation · Introduction This document explains the...
Transcript of Task-Space Inverse Dynamics: Implementation · Introduction This document explains the...
![Page 1: Task-Space Inverse Dynamics: Implementation · Introduction This document explains the implementation of the control framework Task-Space Inverse Dynamics(TSID). To simplify the job](https://reader033.fdocuments.in/reader033/viewer/2022043000/5f746933f5165e1ed71d5ced/html5/thumbnails/1.jpg)
Task-Space Inverse Dynamics:
Implementation
Quadratic-Programming based Control for Legged Robots
Andrea Del Prete
University of Trento
![Page 2: Task-Space Inverse Dynamics: Implementation · Introduction This document explains the implementation of the control framework Task-Space Inverse Dynamics(TSID). To simplify the job](https://reader033.fdocuments.in/reader033/viewer/2022043000/5f746933f5165e1ed71d5ced/html5/thumbnails/2.jpg)
Table of contents
1. Introduction
2. Details
3. Python Example
4. Exercises
1
![Page 3: Task-Space Inverse Dynamics: Implementation · Introduction This document explains the implementation of the control framework Task-Space Inverse Dynamics(TSID). To simplify the job](https://reader033.fdocuments.in/reader033/viewer/2022043000/5f746933f5165e1ed71d5ced/html5/thumbnails/3.jpg)
Introduction
![Page 4: Task-Space Inverse Dynamics: Implementation · Introduction This document explains the implementation of the control framework Task-Space Inverse Dynamics(TSID). To simplify the job](https://reader033.fdocuments.in/reader033/viewer/2022043000/5f746933f5165e1ed71d5ced/html5/thumbnails/4.jpg)
Introduction
This document explains the implementation of the control framework
Task-Space Inverse Dynamics (TSID).
To simplify the job we rely on the open-source C++ library TSID1.
TSID (currently) relies on:
• Eigen for linear algebra
• Pinocchio for multi-body dynamics computations
• Eiquadprog for solving Quadratic Programs
1https://github.com/stack-of-tasks/tsid
2
![Page 5: Task-Space Inverse Dynamics: Implementation · Introduction This document explains the implementation of the control framework Task-Space Inverse Dynamics(TSID). To simplify the job](https://reader033.fdocuments.in/reader033/viewer/2022043000/5f746933f5165e1ed71d5ced/html5/thumbnails/5.jpg)
Introduction
This document explains the implementation of the control framework
Task-Space Inverse Dynamics (TSID).
To simplify the job we rely on the open-source C++ library TSID1.
TSID (currently) relies on:
• Eigen for linear algebra
• Pinocchio for multi-body dynamics computations
• Eiquadprog for solving Quadratic Programs
1https://github.com/stack-of-tasks/tsid
2
![Page 6: Task-Space Inverse Dynamics: Implementation · Introduction This document explains the implementation of the control framework Task-Space Inverse Dynamics(TSID). To simplify the job](https://reader033.fdocuments.in/reader033/viewer/2022043000/5f746933f5165e1ed71d5ced/html5/thumbnails/6.jpg)
Introduction
This document explains the implementation of the control framework
Task-Space Inverse Dynamics (TSID).
To simplify the job we rely on the open-source C++ library TSID1.
TSID (currently) relies on:
• Eigen for linear algebra
• Pinocchio for multi-body dynamics computations
• Eiquadprog for solving Quadratic Programs
1https://github.com/stack-of-tasks/tsid
2
![Page 7: Task-Space Inverse Dynamics: Implementation · Introduction This document explains the implementation of the control framework Task-Space Inverse Dynamics(TSID). To simplify the job](https://reader033.fdocuments.in/reader033/viewer/2022043000/5f746933f5165e1ed71d5ced/html5/thumbnails/7.jpg)
Main features: Pros & Cons
CONS
• Not mature (Feb 2017)
• Many missing features you may
need for your application
• Hierarchy
• Fixed-base robots
• Joint pos-vel limits
• Actuation limits
• Bilateral contacts
• Line contacts
• ...
PROS
• Efficient (<0.6 ms for
humanoid)
• Tested in simulation & on
HRP-2
• Open source
• Modular design
• → easy to extend
• Python bindings
• No alternative (AFAIK)
3
![Page 8: Task-Space Inverse Dynamics: Implementation · Introduction This document explains the implementation of the control framework Task-Space Inverse Dynamics(TSID). To simplify the job](https://reader033.fdocuments.in/reader033/viewer/2022043000/5f746933f5165e1ed71d5ced/html5/thumbnails/8.jpg)
Main features: Pros & Cons
CONS
• Not mature (Feb 2017)
• Many missing features you may
need for your application
• Hierarchy
• Fixed-base robots
• Joint pos-vel limits
• Actuation limits
• Bilateral contacts
• Line contacts
• ...
PROS
• Efficient (<0.6 ms for
humanoid)
• Tested in simulation & on
HRP-2
• Open source
• Modular design
• → easy to extend
• Python bindings
• No alternative (AFAIK)
3
![Page 9: Task-Space Inverse Dynamics: Implementation · Introduction This document explains the implementation of the control framework Task-Space Inverse Dynamics(TSID). To simplify the job](https://reader033.fdocuments.in/reader033/viewer/2022043000/5f746933f5165e1ed71d5ced/html5/thumbnails/9.jpg)
Key Concepts
Task
• Motion
• Force
• Actuation
Rigid Contact
• similar to Task, but
• associated to reaction forces
Inverse Dynamics Formulation
• collects Tasks and
RigidContacts
• translates them into HQP
HQP Solver
• solves a HQP
4
![Page 10: Task-Space Inverse Dynamics: Implementation · Introduction This document explains the implementation of the control framework Task-Space Inverse Dynamics(TSID). To simplify the job](https://reader033.fdocuments.in/reader033/viewer/2022043000/5f746933f5165e1ed71d5ced/html5/thumbnails/10.jpg)
Key Concepts
Task
• Motion
• Force
• Actuation
Rigid Contact
• similar to Task, but
• associated to reaction forces
Inverse Dynamics Formulation
• collects Tasks and
RigidContacts
• translates them into HQP
HQP Solver
• solves a HQP
4
![Page 11: Task-Space Inverse Dynamics: Implementation · Introduction This document explains the implementation of the control framework Task-Space Inverse Dynamics(TSID). To simplify the job](https://reader033.fdocuments.in/reader033/viewer/2022043000/5f746933f5165e1ed71d5ced/html5/thumbnails/11.jpg)
Key Concepts
Task
• Motion
• Force
• Actuation
Rigid Contact
• similar to Task, but
• associated to reaction forces
Inverse Dynamics Formulation
• collects Tasks and
RigidContacts
• translates them into HQP
HQP Solver
• solves a HQP
4
![Page 12: Task-Space Inverse Dynamics: Implementation · Introduction This document explains the implementation of the control framework Task-Space Inverse Dynamics(TSID). To simplify the job](https://reader033.fdocuments.in/reader033/viewer/2022043000/5f746933f5165e1ed71d5ced/html5/thumbnails/12.jpg)
Key Concepts
Task
• Motion
• Force
• Actuation
Rigid Contact
• similar to Task, but
• associated to reaction forces
Inverse Dynamics Formulation
• collects Tasks and
RigidContacts
• translates them into HQP
HQP Solver
• solves a HQP
4
![Page 13: Task-Space Inverse Dynamics: Implementation · Introduction This document explains the implementation of the control framework Task-Space Inverse Dynamics(TSID). To simplify the job](https://reader033.fdocuments.in/reader033/viewer/2022043000/5f746933f5165e1ed71d5ced/html5/thumbnails/13.jpg)
Other Concepts
Constraint
• affine function
• purely mathematical
• used to represent HQP
Robot Wrapper
• contains robot model
• provides utility functions to
compute robot quantities
• e.g., mass matrix, Jacobians
Trajectory
• maps time to vector values
• pos, vel, acc
• position and velocity can have
different sizes (Lie groups)
5
![Page 14: Task-Space Inverse Dynamics: Implementation · Introduction This document explains the implementation of the control framework Task-Space Inverse Dynamics(TSID). To simplify the job](https://reader033.fdocuments.in/reader033/viewer/2022043000/5f746933f5165e1ed71d5ced/html5/thumbnails/14.jpg)
Other Concepts
Constraint
• affine function
• purely mathematical
• used to represent HQP
Robot Wrapper
• contains robot model
• provides utility functions to
compute robot quantities
• e.g., mass matrix, Jacobians
Trajectory
• maps time to vector values
• pos, vel, acc
• position and velocity can have
different sizes (Lie groups)
5
![Page 15: Task-Space Inverse Dynamics: Implementation · Introduction This document explains the implementation of the control framework Task-Space Inverse Dynamics(TSID). To simplify the job](https://reader033.fdocuments.in/reader033/viewer/2022043000/5f746933f5165e1ed71d5ced/html5/thumbnails/15.jpg)
Other Concepts
Constraint
• affine function
• purely mathematical
• used to represent HQP
Robot Wrapper
• contains robot model
• provides utility functions to
compute robot quantities
• e.g., mass matrix, Jacobians
Trajectory
• maps time to vector values
• pos, vel, acc
• position and velocity can have
different sizes (Lie groups)
5
![Page 16: Task-Space Inverse Dynamics: Implementation · Introduction This document explains the implementation of the control framework Task-Space Inverse Dynamics(TSID). To simplify the job](https://reader033.fdocuments.in/reader033/viewer/2022043000/5f746933f5165e1ed71d5ced/html5/thumbnails/16.jpg)
Details
![Page 17: Task-Space Inverse Dynamics: Implementation · Introduction This document explains the implementation of the control framework Task-Space Inverse Dynamics(TSID). To simplify the job](https://reader033.fdocuments.in/reader033/viewer/2022043000/5f746933f5165e1ed71d5ced/html5/thumbnails/17.jpg)
Robot Wrapper 1/2
Interface for computing robot-related quantities:
RobotWrapper(string filename, vector<string> package_dirs,
JointModelVariant rootJoint);
int nq(); // size of configuration vector q
int nv(); // size of velocity vector v
Model & model(); // reference to robot model (Pinocchio)
// Compute all quantities and store them into data
void computeAllTerms(Data &data, Vector q, Vector v);
6
![Page 18: Task-Space Inverse Dynamics: Implementation · Introduction This document explains the implementation of the control framework Task-Space Inverse Dynamics(TSID). To simplify the job](https://reader033.fdocuments.in/reader033/viewer/2022043000/5f746933f5165e1ed71d5ced/html5/thumbnails/18.jpg)
Robot Wrapper 2/2
Vector rotor_inertias();
Vector gear_ratios();
Vector3 com(Data data);
Vector3 com_vel(Data data);
Vector3 com_acc(Data data);
Matrix3x Jcom(Data data);
Matrix mass(Data data);
Vector nonLinearEffects(Data data);
SE3 position(Data data, JointIndex index);
Motion velocity(Data data, JointIndex index);
Motion acceleration(Data data, JointIndex index);
Matrix6x jacobianWorld(Data data, JointIndex index);
Matrix6x jacobianLocal(Data data, JointIndex index);7
![Page 19: Task-Space Inverse Dynamics: Implementation · Introduction This document explains the implementation of the control framework Task-Space Inverse Dynamics(TSID). To simplify the job](https://reader033.fdocuments.in/reader033/viewer/2022043000/5f746933f5165e1ed71d5ced/html5/thumbnails/19.jpg)
ConstraintBase
• A linear (affine) function
• Purely mathematical object
• “Unaware” of what the function represents
Three kinds of constraints:
• Equalities, represented by matrix A and vector a:
Ax = a
• Inequalities, represented by matrix A and vectors lb and ub:
lb ≤ Ax ≤ ub
• Bounds, represented by vectors lb and ub:
lb ≤ x ≤ ub
8
![Page 20: Task-Space Inverse Dynamics: Implementation · Introduction This document explains the implementation of the control framework Task-Space Inverse Dynamics(TSID). To simplify the job](https://reader033.fdocuments.in/reader033/viewer/2022043000/5f746933f5165e1ed71d5ced/html5/thumbnails/20.jpg)
ConstraintBase
• A linear (affine) function
• Purely mathematical object
• “Unaware” of what the function represents
Three kinds of constraints:
• Equalities, represented by matrix A and vector a:
Ax = a
• Inequalities, represented by matrix A and vectors lb and ub:
lb ≤ Ax ≤ ub
• Bounds, represented by vectors lb and ub:
lb ≤ x ≤ ub
8
![Page 21: Task-Space Inverse Dynamics: Implementation · Introduction This document explains the implementation of the control framework Task-Space Inverse Dynamics(TSID). To simplify the job](https://reader033.fdocuments.in/reader033/viewer/2022043000/5f746933f5165e1ed71d5ced/html5/thumbnails/21.jpg)
ConstraintBase
• A linear (affine) function
• Purely mathematical object
• “Unaware” of what the function represents
Three kinds of constraints:
• Equalities, represented by matrix A and vector a:
Ax = a
• Inequalities, represented by matrix A and vectors lb and ub:
lb ≤ Ax ≤ ub
• Bounds, represented by vectors lb and ub:
lb ≤ x ≤ ub
8
![Page 22: Task-Space Inverse Dynamics: Implementation · Introduction This document explains the implementation of the control framework Task-Space Inverse Dynamics(TSID). To simplify the job](https://reader033.fdocuments.in/reader033/viewer/2022043000/5f746933f5165e1ed71d5ced/html5/thumbnails/22.jpg)
ConstraintBase
• A linear (affine) function
• Purely mathematical object
• “Unaware” of what the function represents
Three kinds of constraints:
• Equalities, represented by matrix A and vector a:
Ax = a
• Inequalities, represented by matrix A and vectors lb and ub:
lb ≤ Ax ≤ ub
• Bounds, represented by vectors lb and ub:
lb ≤ x ≤ ub
8
![Page 23: Task-Space Inverse Dynamics: Implementation · Introduction This document explains the implementation of the control framework Task-Space Inverse Dynamics(TSID). To simplify the job](https://reader033.fdocuments.in/reader033/viewer/2022043000/5f746933f5165e1ed71d5ced/html5/thumbnails/23.jpg)
ConstraintBase
• A linear (affine) function
• Purely mathematical object
• “Unaware” of what the function represents
Three kinds of constraints:
• Equalities, represented by matrix A and vector a:
Ax = a
• Inequalities, represented by matrix A and vectors lb and ub:
lb ≤ Ax ≤ ub
• Bounds, represented by vectors lb and ub:
lb ≤ x ≤ ub
8
![Page 24: Task-Space Inverse Dynamics: Implementation · Introduction This document explains the implementation of the control framework Task-Space Inverse Dynamics(TSID). To simplify the job](https://reader033.fdocuments.in/reader033/viewer/2022043000/5f746933f5165e1ed71d5ced/html5/thumbnails/24.jpg)
ConstraintBase
• A linear (affine) function
• Purely mathematical object
• “Unaware” of what the function represents
Three kinds of constraints:
• Equalities, represented by matrix A and vector a:
Ax = a
• Inequalities, represented by matrix A and vectors lb and ub:
lb ≤ Ax ≤ ub
• Bounds, represented by vectors lb and ub:
lb ≤ x ≤ ub
8
![Page 25: Task-Space Inverse Dynamics: Implementation · Introduction This document explains the implementation of the control framework Task-Space Inverse Dynamics(TSID). To simplify the job](https://reader033.fdocuments.in/reader033/viewer/2022043000/5f746933f5165e1ed71d5ced/html5/thumbnails/25.jpg)
ConstraintBase
ConstraintBase(string name, int rows, int cols);
bool isEquality();
bool isInequality();
bool isBound();
Matrix matrix();
Vector vector();
Vector lowerBound();
Vector upperBound();
bool setMatrix(Matrix A);
bool setVector(Vector b);
bool setLowerBound(Vector lb);
bool setUpperBound(Vector ub);
bool checkConstraint(Vector x);
9
![Page 26: Task-Space Inverse Dynamics: Implementation · Introduction This document explains the implementation of the control framework Task-Space Inverse Dynamics(TSID). To simplify the job](https://reader033.fdocuments.in/reader033/viewer/2022043000/5f746933f5165e1ed71d5ced/html5/thumbnails/26.jpg)
TaskBase
Interface of TaskBase:
TaskBase(string name, Model model);
Constraint compute(double t, Vector q, Vector v, Data data);
Three kinds of task:
• TaskMotion: linear function of robot accelerations
• TaskContactForce: linear function of contact forces
• TaskActuation: linear function of joint torques
Tasks can compute either:
• equality constraints, e.g., TaskComEquality, TaskJointPosture,
TaskSE3Equality
• bounds, e.g., TaskJointBounds (not implemented yet)
• inequality constraints, e.g., friction cones
10
![Page 27: Task-Space Inverse Dynamics: Implementation · Introduction This document explains the implementation of the control framework Task-Space Inverse Dynamics(TSID). To simplify the job](https://reader033.fdocuments.in/reader033/viewer/2022043000/5f746933f5165e1ed71d5ced/html5/thumbnails/27.jpg)
TaskBase
Interface of TaskBase:
TaskBase(string name, Model model);
Constraint compute(double t, Vector q, Vector v, Data data);
Three kinds of task:
• TaskMotion: linear function of robot accelerations
• TaskContactForce: linear function of contact forces
• TaskActuation: linear function of joint torques
Tasks can compute either:
• equality constraints, e.g., TaskComEquality, TaskJointPosture,
TaskSE3Equality
• bounds, e.g., TaskJointBounds (not implemented yet)
• inequality constraints, e.g., friction cones
10
![Page 28: Task-Space Inverse Dynamics: Implementation · Introduction This document explains the implementation of the control framework Task-Space Inverse Dynamics(TSID). To simplify the job](https://reader033.fdocuments.in/reader033/viewer/2022043000/5f746933f5165e1ed71d5ced/html5/thumbnails/28.jpg)
TaskBase
Interface of TaskBase:
TaskBase(string name, Model model);
Constraint compute(double t, Vector q, Vector v, Data data);
Three kinds of task:
• TaskMotion: linear function of robot accelerations
• TaskContactForce: linear function of contact forces
• TaskActuation: linear function of joint torques
Tasks can compute either:
• equality constraints, e.g., TaskComEquality, TaskJointPosture,
TaskSE3Equality
• bounds, e.g., TaskJointBounds (not implemented yet)
• inequality constraints, e.g., friction cones
10
![Page 29: Task-Space Inverse Dynamics: Implementation · Introduction This document explains the implementation of the control framework Task-Space Inverse Dynamics(TSID). To simplify the job](https://reader033.fdocuments.in/reader033/viewer/2022043000/5f746933f5165e1ed71d5ced/html5/thumbnails/29.jpg)
TaskBase
Interface of TaskBase:
TaskBase(string name, Model model);
Constraint compute(double t, Vector q, Vector v, Data data);
Three kinds of task:
• TaskMotion: linear function of robot accelerations
• TaskContactForce: linear function of contact forces
• TaskActuation: linear function of joint torques
Tasks can compute either:
• equality constraints, e.g., TaskComEquality, TaskJointPosture,
TaskSE3Equality
• bounds, e.g., TaskJointBounds (not implemented yet)
• inequality constraints, e.g., friction cones
10
![Page 30: Task-Space Inverse Dynamics: Implementation · Introduction This document explains the implementation of the control framework Task-Space Inverse Dynamics(TSID). To simplify the job](https://reader033.fdocuments.in/reader033/viewer/2022043000/5f746933f5165e1ed71d5ced/html5/thumbnails/30.jpg)
TaskBase
Interface of TaskBase:
TaskBase(string name, Model model);
Constraint compute(double t, Vector q, Vector v, Data data);
Three kinds of task:
• TaskMotion: linear function of robot accelerations
• TaskContactForce: linear function of contact forces
• TaskActuation: linear function of joint torques
Tasks can compute either:
• equality constraints, e.g., TaskComEquality, TaskJointPosture,
TaskSE3Equality
• bounds, e.g., TaskJointBounds (not implemented yet)
• inequality constraints, e.g., friction cones
10
![Page 31: Task-Space Inverse Dynamics: Implementation · Introduction This document explains the implementation of the control framework Task-Space Inverse Dynamics(TSID). To simplify the job](https://reader033.fdocuments.in/reader033/viewer/2022043000/5f746933f5165e1ed71d5ced/html5/thumbnails/31.jpg)
ContactBase
Interface of ContactBase:
ContactBase(name, Kp, Kd, bodyName, regWeight);
ConstraintBase computeMotionTask(t, q, v, data);
InequalityConstraint computeForceTask(t, q, v, data);
ConstraintBase computeForceRegularizationTask(t, q, v, data);
Matrix computeForceGeneratorMatrix();
Motion task:
• represents motion constraint caused by rigid contact
• Jvq = −Jvq −Kpe − Kd e
Force task:
• represents inequality constraints acting on contact forces
• e.g., friction cone constraints
• Af ≤ a
11
![Page 32: Task-Space Inverse Dynamics: Implementation · Introduction This document explains the implementation of the control framework Task-Space Inverse Dynamics(TSID). To simplify the job](https://reader033.fdocuments.in/reader033/viewer/2022043000/5f746933f5165e1ed71d5ced/html5/thumbnails/32.jpg)
ContactBase
Interface of ContactBase:
ContactBase(name, Kp, Kd, bodyName, regWeight);
ConstraintBase computeMotionTask(t, q, v, data);
InequalityConstraint computeForceTask(t, q, v, data);
ConstraintBase computeForceRegularizationTask(t, q, v, data);
Matrix computeForceGeneratorMatrix();
Motion task:
• represents motion constraint caused by rigid contact
• Jvq = −Jvq
−Kpe − Kd e
Force task:
• represents inequality constraints acting on contact forces
• e.g., friction cone constraints
• Af ≤ a
11
![Page 33: Task-Space Inverse Dynamics: Implementation · Introduction This document explains the implementation of the control framework Task-Space Inverse Dynamics(TSID). To simplify the job](https://reader033.fdocuments.in/reader033/viewer/2022043000/5f746933f5165e1ed71d5ced/html5/thumbnails/33.jpg)
ContactBase
Interface of ContactBase:
ContactBase(name, Kp, Kd, bodyName, regWeight);
ConstraintBase computeMotionTask(t, q, v, data);
InequalityConstraint computeForceTask(t, q, v, data);
ConstraintBase computeForceRegularizationTask(t, q, v, data);
Matrix computeForceGeneratorMatrix();
Motion task:
• represents motion constraint caused by rigid contact
• Jvq = −Jvq −Kpe − Kd e
Force task:
• represents inequality constraints acting on contact forces
• e.g., friction cone constraints
• Af ≤ a
11
![Page 34: Task-Space Inverse Dynamics: Implementation · Introduction This document explains the implementation of the control framework Task-Space Inverse Dynamics(TSID). To simplify the job](https://reader033.fdocuments.in/reader033/viewer/2022043000/5f746933f5165e1ed71d5ced/html5/thumbnails/34.jpg)
ContactBase
Interface of ContactBase:
ContactBase(name, Kp, Kd, bodyName, regWeight);
ConstraintBase computeMotionTask(t, q, v, data);
InequalityConstraint computeForceTask(t, q, v, data);
ConstraintBase computeForceRegularizationTask(t, q, v, data);
Matrix computeForceGeneratorMatrix();
Motion task:
• represents motion constraint caused by rigid contact
• Jvq = −Jvq −Kpe − Kd e
Force task:
• represents inequality constraints acting on contact forces
• e.g., friction cone constraints
• Af ≤ a
11
![Page 35: Task-Space Inverse Dynamics: Implementation · Introduction This document explains the implementation of the control framework Task-Space Inverse Dynamics(TSID). To simplify the job](https://reader033.fdocuments.in/reader033/viewer/2022043000/5f746933f5165e1ed71d5ced/html5/thumbnails/35.jpg)
ContactBase
Interface of ContactBase:
ContactBase(name, Kp, Kd, bodyName, regWeight);
ConstraintBase computeMotionTask(t, q, v, data);
InequalityConstraint computeForceTask(t, q, v, data);
ConstraintBase computeForceRegularizationTask(t, q, v, data);
Matrix computeForceGeneratorMatrix();
Force Regularization task:
• regularizes contact forces
• e.g., keep them close to friction cone center
Force-Generator matrix T :
• maps force variables to motion constraint representation
• Dynamic: Mvq + h = S>τ + J>Tf
• Motion constraint: Jvq = −Jvq• Friction cones: Af ≤ a
12
![Page 36: Task-Space Inverse Dynamics: Implementation · Introduction This document explains the implementation of the control framework Task-Space Inverse Dynamics(TSID). To simplify the job](https://reader033.fdocuments.in/reader033/viewer/2022043000/5f746933f5165e1ed71d5ced/html5/thumbnails/36.jpg)
ContactBase
Interface of ContactBase:
ContactBase(name, Kp, Kd, bodyName, regWeight);
ConstraintBase computeMotionTask(t, q, v, data);
InequalityConstraint computeForceTask(t, q, v, data);
ConstraintBase computeForceRegularizationTask(t, q, v, data);
Matrix computeForceGeneratorMatrix();
Force Regularization task:
• regularizes contact forces
• e.g., keep them close to friction cone center
Force-Generator matrix T :
• maps force variables to motion constraint representation
• Dynamic: Mvq + h = S>τ + J>Tf
• Motion constraint: Jvq = −Jvq• Friction cones: Af ≤ a
12
![Page 37: Task-Space Inverse Dynamics: Implementation · Introduction This document explains the implementation of the control framework Task-Space Inverse Dynamics(TSID). To simplify the job](https://reader033.fdocuments.in/reader033/viewer/2022043000/5f746933f5165e1ed71d5ced/html5/thumbnails/37.jpg)
Contact6d
• unilateral plane contact (polygonal shape)
• 6d motion constraint → no motion allowed in any direction
PROBLEM
• minimal force representation would be 6d (3d force + 3d moment)
• hard to write friction constraints with 6d force representation
(especially for non-rectangular contact shapes)
• easy to write friction constraints if reaction force represented as
collection of 3d forces applied at vertices of contact surface
• redundant representation, e.g., 4-vertex surface → 12 variables
• redundancy is an issue for motion constraint if HQP solver does not
handle redundant constraints (as eiQuadProg).
SOLUTION
• use 6d representation for motion constraint Jvq = −Jvq ∈ R6
• but 12d representation for force variable f ∈ R12
• force-generator matrix T ∈ R6×12 defines mapping between two
representations: τcontact = J>Tf
13
![Page 38: Task-Space Inverse Dynamics: Implementation · Introduction This document explains the implementation of the control framework Task-Space Inverse Dynamics(TSID). To simplify the job](https://reader033.fdocuments.in/reader033/viewer/2022043000/5f746933f5165e1ed71d5ced/html5/thumbnails/38.jpg)
Contact6d
• unilateral plane contact (polygonal shape)
• 6d motion constraint → no motion allowed in any direction
PROBLEM
• minimal force representation would be 6d (3d force + 3d moment)
• hard to write friction constraints with 6d force representation
(especially for non-rectangular contact shapes)
• easy to write friction constraints if reaction force represented as
collection of 3d forces applied at vertices of contact surface
• redundant representation, e.g., 4-vertex surface → 12 variables
• redundancy is an issue for motion constraint if HQP solver does not
handle redundant constraints (as eiQuadProg).
SOLUTION
• use 6d representation for motion constraint Jvq = −Jvq ∈ R6
• but 12d representation for force variable f ∈ R12
• force-generator matrix T ∈ R6×12 defines mapping between two
representations: τcontact = J>Tf
13
![Page 39: Task-Space Inverse Dynamics: Implementation · Introduction This document explains the implementation of the control framework Task-Space Inverse Dynamics(TSID). To simplify the job](https://reader033.fdocuments.in/reader033/viewer/2022043000/5f746933f5165e1ed71d5ced/html5/thumbnails/39.jpg)
Contact6d
• unilateral plane contact (polygonal shape)
• 6d motion constraint → no motion allowed in any direction
PROBLEM
• minimal force representation would be 6d (3d force + 3d moment)
• hard to write friction constraints with 6d force representation
(especially for non-rectangular contact shapes)
• easy to write friction constraints if reaction force represented as
collection of 3d forces applied at vertices of contact surface
• redundant representation, e.g., 4-vertex surface → 12 variables
• redundancy is an issue for motion constraint if HQP solver does not
handle redundant constraints (as eiQuadProg).
SOLUTION
• use 6d representation for motion constraint Jvq = −Jvq ∈ R6
• but 12d representation for force variable f ∈ R12
• force-generator matrix T ∈ R6×12 defines mapping between two
representations: τcontact = J>Tf
13
![Page 40: Task-Space Inverse Dynamics: Implementation · Introduction This document explains the implementation of the control framework Task-Space Inverse Dynamics(TSID). To simplify the job](https://reader033.fdocuments.in/reader033/viewer/2022043000/5f746933f5165e1ed71d5ced/html5/thumbnails/40.jpg)
Contact6d
• unilateral plane contact (polygonal shape)
• 6d motion constraint → no motion allowed in any direction
PROBLEM
• minimal force representation would be 6d (3d force + 3d moment)
• hard to write friction constraints with 6d force representation
(especially for non-rectangular contact shapes)
• easy to write friction constraints if reaction force represented as
collection of 3d forces applied at vertices of contact surface
• redundant representation, e.g., 4-vertex surface → 12 variables
• redundancy is an issue for motion constraint if HQP solver does not
handle redundant constraints (as eiQuadProg).
SOLUTION
• use 6d representation for motion constraint Jvq = −Jvq ∈ R6
• but 12d representation for force variable f ∈ R12
• force-generator matrix T ∈ R6×12 defines mapping between two
representations: τcontact = J>Tf
13
![Page 41: Task-Space Inverse Dynamics: Implementation · Introduction This document explains the implementation of the control framework Task-Space Inverse Dynamics(TSID). To simplify the job](https://reader033.fdocuments.in/reader033/viewer/2022043000/5f746933f5165e1ed71d5ced/html5/thumbnails/41.jpg)
Contact6d
• unilateral plane contact (polygonal shape)
• 6d motion constraint → no motion allowed in any direction
PROBLEM
• minimal force representation would be 6d (3d force + 3d moment)
• hard to write friction constraints with 6d force representation
(especially for non-rectangular contact shapes)
• easy to write friction constraints if reaction force represented as
collection of 3d forces applied at vertices of contact surface
• redundant representation, e.g., 4-vertex surface → 12 variables
• redundancy is an issue for motion constraint if HQP solver does not
handle redundant constraints (as eiQuadProg).
SOLUTION
• use 6d representation for motion constraint Jvq = −Jvq ∈ R6
• but 12d representation for force variable f ∈ R12
• force-generator matrix T ∈ R6×12 defines mapping between two
representations: τcontact = J>Tf
13
![Page 42: Task-Space Inverse Dynamics: Implementation · Introduction This document explains the implementation of the control framework Task-Space Inverse Dynamics(TSID). To simplify the job](https://reader033.fdocuments.in/reader033/viewer/2022043000/5f746933f5165e1ed71d5ced/html5/thumbnails/42.jpg)
Contact6d
• unilateral plane contact (polygonal shape)
• 6d motion constraint → no motion allowed in any direction
PROBLEM
• minimal force representation would be 6d (3d force + 3d moment)
• hard to write friction constraints with 6d force representation
(especially for non-rectangular contact shapes)
• easy to write friction constraints if reaction force represented as
collection of 3d forces applied at vertices of contact surface
• redundant representation, e.g., 4-vertex surface → 12 variables
• redundancy is an issue for motion constraint if HQP solver does not
handle redundant constraints (as eiQuadProg).
SOLUTION
• use 6d representation for motion constraint Jvq = −Jvq ∈ R6
• but 12d representation for force variable f ∈ R12
• force-generator matrix T ∈ R6×12 defines mapping between two
representations: τcontact = J>Tf
13
![Page 43: Task-Space Inverse Dynamics: Implementation · Introduction This document explains the implementation of the control framework Task-Space Inverse Dynamics(TSID). To simplify the job](https://reader033.fdocuments.in/reader033/viewer/2022043000/5f746933f5165e1ed71d5ced/html5/thumbnails/43.jpg)
Contact6d
• unilateral plane contact (polygonal shape)
• 6d motion constraint → no motion allowed in any direction
PROBLEM
• minimal force representation would be 6d (3d force + 3d moment)
• hard to write friction constraints with 6d force representation
(especially for non-rectangular contact shapes)
• easy to write friction constraints if reaction force represented as
collection of 3d forces applied at vertices of contact surface
• redundant representation, e.g., 4-vertex surface → 12 variables
• redundancy is an issue for motion constraint if HQP solver does not
handle redundant constraints (as eiQuadProg).
SOLUTION
• use 6d representation for motion constraint Jvq = −Jvq ∈ R6
• but 12d representation for force variable f ∈ R12
• force-generator matrix T ∈ R6×12 defines mapping between two
representations: τcontact = J>Tf
13
![Page 44: Task-Space Inverse Dynamics: Implementation · Introduction This document explains the implementation of the control framework Task-Space Inverse Dynamics(TSID). To simplify the job](https://reader033.fdocuments.in/reader033/viewer/2022043000/5f746933f5165e1ed71d5ced/html5/thumbnails/44.jpg)
Contact6d
• unilateral plane contact (polygonal shape)
• 6d motion constraint → no motion allowed in any direction
PROBLEM
• minimal force representation would be 6d (3d force + 3d moment)
• hard to write friction constraints with 6d force representation
(especially for non-rectangular contact shapes)
• easy to write friction constraints if reaction force represented as
collection of 3d forces applied at vertices of contact surface
• redundant representation, e.g., 4-vertex surface → 12 variables
• redundancy is an issue for motion constraint if HQP solver does not
handle redundant constraints (as eiQuadProg).
SOLUTION
• use 6d representation for motion constraint Jvq = −Jvq ∈ R6
• but 12d representation for force variable f ∈ R12
• force-generator matrix T ∈ R6×12 defines mapping between two
representations: τcontact = J>Tf
13
![Page 45: Task-Space Inverse Dynamics: Implementation · Introduction This document explains the implementation of the control framework Task-Space Inverse Dynamics(TSID). To simplify the job](https://reader033.fdocuments.in/reader033/viewer/2022043000/5f746933f5165e1ed71d5ced/html5/thumbnails/45.jpg)
Contact6d
• unilateral plane contact (polygonal shape)
• 6d motion constraint → no motion allowed in any direction
PROBLEM
• minimal force representation would be 6d (3d force + 3d moment)
• hard to write friction constraints with 6d force representation
(especially for non-rectangular contact shapes)
• easy to write friction constraints if reaction force represented as
collection of 3d forces applied at vertices of contact surface
• redundant representation, e.g., 4-vertex surface → 12 variables
• redundancy is an issue for motion constraint if HQP solver does not
handle redundant constraints (as eiQuadProg).
SOLUTION
• use 6d representation for motion constraint Jvq = −Jvq ∈ R6
• but 12d representation for force variable f ∈ R12
• force-generator matrix T ∈ R6×12 defines mapping between two
representations: τcontact = J>Tf
13
![Page 46: Task-Space Inverse Dynamics: Implementation · Introduction This document explains the implementation of the control framework Task-Space Inverse Dynamics(TSID). To simplify the job](https://reader033.fdocuments.in/reader033/viewer/2022043000/5f746933f5165e1ed71d5ced/html5/thumbnails/46.jpg)
Contact6d
• unilateral plane contact (polygonal shape)
• 6d motion constraint → no motion allowed in any direction
PROBLEM
• minimal force representation would be 6d (3d force + 3d moment)
• hard to write friction constraints with 6d force representation
(especially for non-rectangular contact shapes)
• easy to write friction constraints if reaction force represented as
collection of 3d forces applied at vertices of contact surface
• redundant representation, e.g., 4-vertex surface → 12 variables
• redundancy is an issue for motion constraint if HQP solver does not
handle redundant constraints (as eiQuadProg).
SOLUTION
• use 6d representation for motion constraint Jvq = −Jvq ∈ R6
• but 12d representation for force variable f ∈ R12
• force-generator matrix T ∈ R6×12 defines mapping between two
representations: τcontact = J>Tf 13
![Page 47: Task-Space Inverse Dynamics: Implementation · Introduction This document explains the implementation of the control framework Task-Space Inverse Dynamics(TSID). To simplify the job](https://reader033.fdocuments.in/reader033/viewer/2022043000/5f746933f5165e1ed71d5ced/html5/thumbnails/47.jpg)
InverseDynamicsFormulationBase
Central class of the whole library
Methods to add tasks:
addMotionTask(MotionTask task, double weight, int priority);
addForceTask(ForceTask task, double weight, int priority);
addTorqueTask(TorqueTask task, double weight, int priority);
Method to add rigid contacts:
addRigidContact(RigidContact contact);
Methods to convert TSID problem into (Hierarchical) QP:
HqpData computeProblemData(double time, Vector q, Vector v);
HqpData defined as:
#typedef vector<pair<double, ConstraintBase>> ConstraintLevel
#typedef vector<ConstraintLevel> HqpData
14
![Page 48: Task-Space Inverse Dynamics: Implementation · Introduction This document explains the implementation of the control framework Task-Space Inverse Dynamics(TSID). To simplify the job](https://reader033.fdocuments.in/reader033/viewer/2022043000/5f746933f5165e1ed71d5ced/html5/thumbnails/48.jpg)
InverseDynamicsFormulationBase
Central class of the whole library
Methods to add tasks:
addMotionTask(MotionTask task, double weight, int priority);
addForceTask(ForceTask task, double weight, int priority);
addTorqueTask(TorqueTask task, double weight, int priority);
Method to add rigid contacts:
addRigidContact(RigidContact contact);
Methods to convert TSID problem into (Hierarchical) QP:
HqpData computeProblemData(double time, Vector q, Vector v);
HqpData defined as:
#typedef vector<pair<double, ConstraintBase>> ConstraintLevel
#typedef vector<ConstraintLevel> HqpData
14
![Page 49: Task-Space Inverse Dynamics: Implementation · Introduction This document explains the implementation of the control framework Task-Space Inverse Dynamics(TSID). To simplify the job](https://reader033.fdocuments.in/reader033/viewer/2022043000/5f746933f5165e1ed71d5ced/html5/thumbnails/49.jpg)
InverseDynamicsFormulationBase
Central class of the whole library
Methods to add tasks:
addMotionTask(MotionTask task, double weight, int priority);
addForceTask(ForceTask task, double weight, int priority);
addTorqueTask(TorqueTask task, double weight, int priority);
Method to add rigid contacts:
addRigidContact(RigidContact contact);
Methods to convert TSID problem into (Hierarchical) QP:
HqpData computeProblemData(double time, Vector q, Vector v);
HqpData defined as:
#typedef vector<pair<double, ConstraintBase>> ConstraintLevel
#typedef vector<ConstraintLevel> HqpData
14
![Page 50: Task-Space Inverse Dynamics: Implementation · Introduction This document explains the implementation of the control framework Task-Space Inverse Dynamics(TSID). To simplify the job](https://reader033.fdocuments.in/reader033/viewer/2022043000/5f746933f5165e1ed71d5ced/html5/thumbnails/50.jpg)
InverseDynamicsFormulationBase
Central class of the whole library
Methods to add tasks:
addMotionTask(MotionTask task, double weight, int priority);
addForceTask(ForceTask task, double weight, int priority);
addTorqueTask(TorqueTask task, double weight, int priority);
Method to add rigid contacts:
addRigidContact(RigidContact contact);
Methods to convert TSID problem into (Hierarchical) QP:
HqpData computeProblemData(double time, Vector q, Vector v);
HqpData defined as:
#typedef vector<pair<double, ConstraintBase>> ConstraintLevel
#typedef vector<ConstraintLevel> HqpData
14
![Page 51: Task-Space Inverse Dynamics: Implementation · Introduction This document explains the implementation of the control framework Task-Space Inverse Dynamics(TSID). To simplify the job](https://reader033.fdocuments.in/reader033/viewer/2022043000/5f746933f5165e1ed71d5ced/html5/thumbnails/51.jpg)
InverseDynamicsFormulationBase
Central class of the whole library
Methods to add tasks:
addMotionTask(MotionTask task, double weight, int priority);
addForceTask(ForceTask task, double weight, int priority);
addTorqueTask(TorqueTask task, double weight, int priority);
Method to add rigid contacts:
addRigidContact(RigidContact contact);
Methods to convert TSID problem into (Hierarchical) QP:
HqpData computeProblemData(double time, Vector q, Vector v);
HqpData defined as:
#typedef vector<pair<double, ConstraintBase>> ConstraintLevel
#typedef vector<ConstraintLevel> HqpData
14
![Page 52: Task-Space Inverse Dynamics: Implementation · Introduction This document explains the implementation of the control framework Task-Space Inverse Dynamics(TSID). To simplify the job](https://reader033.fdocuments.in/reader033/viewer/2022043000/5f746933f5165e1ed71d5ced/html5/thumbnails/52.jpg)
HQP Solvers
Using InverseDynamicsFormulationBase you get an HqpData object.
Then you need to solve this HQP.
All HQP solvers implement this interface (SolverHQPBase):
void resize(int nVar, int nEq, int nIn);
HqpOutput solve(HqpData data);
HqpOutput is defined as:
class HqpOutput
{
QpStatusFlag flag;
Vector x, lambda;
}
15
![Page 53: Task-Space Inverse Dynamics: Implementation · Introduction This document explains the implementation of the control framework Task-Space Inverse Dynamics(TSID). To simplify the job](https://reader033.fdocuments.in/reader033/viewer/2022043000/5f746933f5165e1ed71d5ced/html5/thumbnails/53.jpg)
HQP Solvers
Using InverseDynamicsFormulationBase you get an HqpData object.
Then you need to solve this HQP.
All HQP solvers implement this interface (SolverHQPBase):
void resize(int nVar, int nEq, int nIn);
HqpOutput solve(HqpData data);
HqpOutput is defined as:
class HqpOutput
{
QpStatusFlag flag;
Vector x, lambda;
}
15
![Page 54: Task-Space Inverse Dynamics: Implementation · Introduction This document explains the implementation of the control framework Task-Space Inverse Dynamics(TSID). To simplify the job](https://reader033.fdocuments.in/reader033/viewer/2022043000/5f746933f5165e1ed71d5ced/html5/thumbnails/54.jpg)
HQP Solvers
Using InverseDynamicsFormulationBase you get an HqpData object.
Then you need to solve this HQP.
All HQP solvers implement this interface (SolverHQPBase):
void resize(int nVar, int nEq, int nIn);
HqpOutput solve(HqpData data);
HqpOutput is defined as:
class HqpOutput
{
QpStatusFlag flag;
Vector x, lambda;
}
15
![Page 55: Task-Space Inverse Dynamics: Implementation · Introduction This document explains the implementation of the control framework Task-Space Inverse Dynamics(TSID). To simplify the job](https://reader033.fdocuments.in/reader033/viewer/2022043000/5f746933f5165e1ed71d5ced/html5/thumbnails/55.jpg)
HQP Solvers
Using InverseDynamicsFormulationBase you get an HqpData object.
Then you need to solve this HQP.
All HQP solvers implement this interface (SolverHQPBase):
void resize(int nVar, int nEq, int nIn);
HqpOutput solve(HqpData data);
HqpOutput is defined as:
class HqpOutput
{
QpStatusFlag flag;
Vector x, lambda;
}
15
![Page 56: Task-Space Inverse Dynamics: Implementation · Introduction This document explains the implementation of the control framework Task-Space Inverse Dynamics(TSID). To simplify the job](https://reader033.fdocuments.in/reader033/viewer/2022043000/5f746933f5165e1ed71d5ced/html5/thumbnails/56.jpg)
Available HQP Solvers
• Several solvers currently implemented
• None of them supports hierarchy
• → HQP problems can only have two hierarchy levels.
• All solvers based on EiQuadProg: a modified version of
uQuadProg++ working with Eigen
• To improve efficiency, two optimized versions have been developed:
• EiquadprogRealTime: the fastest, but matrix sizes known at
compile time
• EiquadprogFast: dynamic matrix sizes (memory allocation
performed only when resizing)
Results on HRP-2’s computer (very old):
60 variables, 18 equalities, 40 inequalities
*** PROFILING RESULTS [ms] (min - avg - max ) ***
Eiquadprog ................... 0.651 0.704 0.870
Eiquadprog Fast .............. 0.563 0.605 0.810
Eiquadprog Real Time ......... 0.543 0.592 0.712
active inequalities .... 16.0 19.8 26.0
16
![Page 57: Task-Space Inverse Dynamics: Implementation · Introduction This document explains the implementation of the control framework Task-Space Inverse Dynamics(TSID). To simplify the job](https://reader033.fdocuments.in/reader033/viewer/2022043000/5f746933f5165e1ed71d5ced/html5/thumbnails/57.jpg)
Available HQP Solvers
• Several solvers currently implemented
• None of them supports hierarchy
• → HQP problems can only have two hierarchy levels.
• All solvers based on EiQuadProg: a modified version of
uQuadProg++ working with Eigen
• To improve efficiency, two optimized versions have been developed:
• EiquadprogRealTime: the fastest, but matrix sizes known at
compile time
• EiquadprogFast: dynamic matrix sizes (memory allocation
performed only when resizing)
Results on HRP-2’s computer (very old):
60 variables, 18 equalities, 40 inequalities
*** PROFILING RESULTS [ms] (min - avg - max ) ***
Eiquadprog ................... 0.651 0.704 0.870
Eiquadprog Fast .............. 0.563 0.605 0.810
Eiquadprog Real Time ......... 0.543 0.592 0.712
active inequalities .... 16.0 19.8 26.0
16
![Page 58: Task-Space Inverse Dynamics: Implementation · Introduction This document explains the implementation of the control framework Task-Space Inverse Dynamics(TSID). To simplify the job](https://reader033.fdocuments.in/reader033/viewer/2022043000/5f746933f5165e1ed71d5ced/html5/thumbnails/58.jpg)
Available HQP Solvers
• Several solvers currently implemented
• None of them supports hierarchy
• → HQP problems can only have two hierarchy levels.
• All solvers based on EiQuadProg: a modified version of
uQuadProg++ working with Eigen
• To improve efficiency, two optimized versions have been developed:
• EiquadprogRealTime: the fastest, but matrix sizes known at
compile time
• EiquadprogFast: dynamic matrix sizes (memory allocation
performed only when resizing)
Results on HRP-2’s computer (very old):
60 variables, 18 equalities, 40 inequalities
*** PROFILING RESULTS [ms] (min - avg - max ) ***
Eiquadprog ................... 0.651 0.704 0.870
Eiquadprog Fast .............. 0.563 0.605 0.810
Eiquadprog Real Time ......... 0.543 0.592 0.712
active inequalities .... 16.0 19.8 26.0
16
![Page 59: Task-Space Inverse Dynamics: Implementation · Introduction This document explains the implementation of the control framework Task-Space Inverse Dynamics(TSID). To simplify the job](https://reader033.fdocuments.in/reader033/viewer/2022043000/5f746933f5165e1ed71d5ced/html5/thumbnails/59.jpg)
Available HQP Solvers
• Several solvers currently implemented
• None of them supports hierarchy
• → HQP problems can only have two hierarchy levels.
• All solvers based on EiQuadProg: a modified version of
uQuadProg++ working with Eigen
• To improve efficiency, two optimized versions have been developed:
• EiquadprogRealTime: the fastest, but matrix sizes known at
compile time
• EiquadprogFast: dynamic matrix sizes (memory allocation
performed only when resizing)
Results on HRP-2’s computer (very old):
60 variables, 18 equalities, 40 inequalities
*** PROFILING RESULTS [ms] (min - avg - max ) ***
Eiquadprog ................... 0.651 0.704 0.870
Eiquadprog Fast .............. 0.563 0.605 0.810
Eiquadprog Real Time ......... 0.543 0.592 0.712
active inequalities .... 16.0 19.8 26.0
16
![Page 60: Task-Space Inverse Dynamics: Implementation · Introduction This document explains the implementation of the control framework Task-Space Inverse Dynamics(TSID). To simplify the job](https://reader033.fdocuments.in/reader033/viewer/2022043000/5f746933f5165e1ed71d5ced/html5/thumbnails/60.jpg)
Available HQP Solvers
• Several solvers currently implemented
• None of them supports hierarchy
• → HQP problems can only have two hierarchy levels.
• All solvers based on EiQuadProg: a modified version of
uQuadProg++ working with Eigen
• To improve efficiency, two optimized versions have been developed:
• EiquadprogRealTime: the fastest, but matrix sizes known at
compile time
• EiquadprogFast: dynamic matrix sizes (memory allocation
performed only when resizing)
Results on HRP-2’s computer (very old):
60 variables, 18 equalities, 40 inequalities
*** PROFILING RESULTS [ms] (min - avg - max ) ***
Eiquadprog ................... 0.651 0.704 0.870
Eiquadprog Fast .............. 0.563 0.605 0.810
Eiquadprog Real Time ......... 0.543 0.592 0.712
active inequalities .... 16.0 19.8 26.0
16
![Page 61: Task-Space Inverse Dynamics: Implementation · Introduction This document explains the implementation of the control framework Task-Space Inverse Dynamics(TSID). To simplify the job](https://reader033.fdocuments.in/reader033/viewer/2022043000/5f746933f5165e1ed71d5ced/html5/thumbnails/61.jpg)
Available HQP Solvers
• Several solvers currently implemented
• None of them supports hierarchy
• → HQP problems can only have two hierarchy levels.
• All solvers based on EiQuadProg: a modified version of
uQuadProg++ working with Eigen
• To improve efficiency, two optimized versions have been developed:
• EiquadprogRealTime: the fastest, but matrix sizes known at
compile time
• EiquadprogFast: dynamic matrix sizes (memory allocation
performed only when resizing)
Results on HRP-2’s computer (very old):
60 variables, 18 equalities, 40 inequalities
*** PROFILING RESULTS [ms] (min - avg - max ) ***
Eiquadprog ................... 0.651 0.704 0.870
Eiquadprog Fast .............. 0.563 0.605 0.810
Eiquadprog Real Time ......... 0.543 0.592 0.712
active inequalities .... 16.0 19.8 26.0
16
![Page 62: Task-Space Inverse Dynamics: Implementation · Introduction This document explains the implementation of the control framework Task-Space Inverse Dynamics(TSID). To simplify the job](https://reader033.fdocuments.in/reader033/viewer/2022043000/5f746933f5165e1ed71d5ced/html5/thumbnails/62.jpg)
Available HQP Solvers
• Several solvers currently implemented
• None of them supports hierarchy
• → HQP problems can only have two hierarchy levels.
• All solvers based on EiQuadProg: a modified version of
uQuadProg++ working with Eigen
• To improve efficiency, two optimized versions have been developed:
• EiquadprogRealTime: the fastest, but matrix sizes known at
compile time
• EiquadprogFast: dynamic matrix sizes (memory allocation
performed only when resizing)
Results on HRP-2’s computer (very old):
60 variables, 18 equalities, 40 inequalities
*** PROFILING RESULTS [ms] (min - avg - max ) ***
Eiquadprog ................... 0.651 0.704 0.870
Eiquadprog Fast .............. 0.563 0.605 0.810
Eiquadprog Real Time ......... 0.543 0.592 0.712
active inequalities .... 16.0 19.8 26.0
16
![Page 63: Task-Space Inverse Dynamics: Implementation · Introduction This document explains the implementation of the control framework Task-Space Inverse Dynamics(TSID). To simplify the job](https://reader033.fdocuments.in/reader033/viewer/2022043000/5f746933f5165e1ed71d5ced/html5/thumbnails/63.jpg)
Available HQP Solvers
• Several solvers currently implemented
• None of them supports hierarchy
• → HQP problems can only have two hierarchy levels.
• All solvers based on EiQuadProg: a modified version of
uQuadProg++ working with Eigen
• To improve efficiency, two optimized versions have been developed:
• EiquadprogRealTime: the fastest, but matrix sizes known at
compile time
• EiquadprogFast: dynamic matrix sizes (memory allocation
performed only when resizing)
Results on HRP-2’s computer (very old):
60 variables, 18 equalities, 40 inequalities
*** PROFILING RESULTS [ms] (min - avg - max ) ***
Eiquadprog ................... 0.651 0.704 0.870
Eiquadprog Fast .............. 0.563 0.605 0.810
Eiquadprog Real Time ......... 0.543 0.592 0.712
active inequalities .... 16.0 19.8 26.0 16
![Page 64: Task-Space Inverse Dynamics: Implementation · Introduction This document explains the implementation of the control framework Task-Space Inverse Dynamics(TSID). To simplify the job](https://reader033.fdocuments.in/reader033/viewer/2022043000/5f746933f5165e1ed71d5ced/html5/thumbnails/64.jpg)
Python Example
![Page 65: Task-Space Inverse Dynamics: Implementation · Introduction This document explains the implementation of the control framework Task-Space Inverse Dynamics(TSID). To simplify the job](https://reader033.fdocuments.in/reader033/viewer/2022043000/5f746933f5165e1ed71d5ced/html5/thumbnails/65.jpg)
Description
• Code snippets
• Biped robot with both feet on the ground (double support)
• Control center of mass (CoM) for balance
• Control joint angles (posture) for whole-body stability
• Good starting point before moving to more complex scenarios
17
![Page 66: Task-Space Inverse Dynamics: Implementation · Introduction This document explains the implementation of the control framework Task-Space Inverse Dynamics(TSID). To simplify the job](https://reader033.fdocuments.in/reader033/viewer/2022043000/5f746933f5165e1ed71d5ced/html5/thumbnails/66.jpg)
Description
• Code snippets
• Biped robot with both feet on the ground (double support)
• Control center of mass (CoM) for balance
• Control joint angles (posture) for whole-body stability
• Good starting point before moving to more complex scenarios
17
![Page 67: Task-Space Inverse Dynamics: Implementation · Introduction This document explains the implementation of the control framework Task-Space Inverse Dynamics(TSID). To simplify the job](https://reader033.fdocuments.in/reader033/viewer/2022043000/5f746933f5165e1ed71d5ced/html5/thumbnails/67.jpg)
Description
• Code snippets
• Biped robot with both feet on the ground (double support)
• Control center of mass (CoM) for balance
• Control joint angles (posture) for whole-body stability
• Good starting point before moving to more complex scenarios
17
![Page 68: Task-Space Inverse Dynamics: Implementation · Introduction This document explains the implementation of the control framework Task-Space Inverse Dynamics(TSID). To simplify the job](https://reader033.fdocuments.in/reader033/viewer/2022043000/5f746933f5165e1ed71d5ced/html5/thumbnails/68.jpg)
Description
• Code snippets
• Biped robot with both feet on the ground (double support)
• Control center of mass (CoM) for balance
• Control joint angles (posture) for whole-body stability
• Good starting point before moving to more complex scenarios
17
![Page 69: Task-Space Inverse Dynamics: Implementation · Introduction This document explains the implementation of the control framework Task-Space Inverse Dynamics(TSID). To simplify the job](https://reader033.fdocuments.in/reader033/viewer/2022043000/5f746933f5165e1ed71d5ced/html5/thumbnails/69.jpg)
Description
• Code snippets
• Biped robot with both feet on the ground (double support)
• Control center of mass (CoM) for balance
• Control joint angles (posture) for whole-body stability
• Good starting point before moving to more complex scenarios
17
![Page 70: Task-Space Inverse Dynamics: Implementation · Introduction This document explains the implementation of the control framework Task-Space Inverse Dynamics(TSID). To simplify the job](https://reader033.fdocuments.in/reader033/viewer/2022043000/5f746933f5165e1ed71d5ced/html5/thumbnails/70.jpg)
Create Robot Wrapper
import pinocchio as se3
from tsid import RobotWrapper, ...
path = ’/../models/romeo’
urdf = path + ’/urdf/romeo.urdf’
vec = se3.StdVec_StdString()
vec.extend(item for item in path)
robot = RobotWrapper(urdf, vec, se3.JointModelFreeFlyer(),
False)
18
![Page 71: Task-Space Inverse Dynamics: Implementation · Introduction This document explains the implementation of the control framework Task-Space Inverse Dynamics(TSID). To simplify the job](https://reader033.fdocuments.in/reader033/viewer/2022043000/5f746933f5165e1ed71d5ced/html5/thumbnails/71.jpg)
Create Inverse Dynamics Formulation
invdyn = InverseDynamicsFormulationAccForce("tsid", robot,
False)
q = getNeutralConfigurationFromSrdf(robot.model(), srdf, False)
v = matlib.zeros(robot.nv).T
invdyn.computeProblemData(t, q, v)
data = invdyn.data()
19
![Page 72: Task-Space Inverse Dynamics: Implementation · Introduction This document explains the implementation of the control framework Task-Space Inverse Dynamics(TSID). To simplify the job](https://reader033.fdocuments.in/reader033/viewer/2022043000/5f746933f5165e1ed71d5ced/html5/thumbnails/72.jpg)
Create Contact
contactRF = Contact6d("contact_rfoot", robot, rf_frame_name,
contact_points, contact_normal, mu,
fMin, fMax, w_forceReg)
contactRF.setKp(kp_contact * matlib.ones(6).T)
contactRF.setKd(2*sqrt(kp_contact) * matlib.ones(6).T)
rf_joint_id = robot.model().getJointId(rf_frame_name)
H_rf_ref = robot.position(data, rf_joint_id)
contactRF.setReference(H_rf_ref)
invdyn.addRigidContact(contactRF)
# repeat for other contact(s)
20
![Page 73: Task-Space Inverse Dynamics: Implementation · Introduction This document explains the implementation of the control framework Task-Space Inverse Dynamics(TSID). To simplify the job](https://reader033.fdocuments.in/reader033/viewer/2022043000/5f746933f5165e1ed71d5ced/html5/thumbnails/73.jpg)
Create Center-of-Mass Task
comTask = TaskComEquality("task-com", robot)
comTask.setKp(kp_com * matlib.ones(3).T)
comTask.setKd(2*sqrt(kp_com) * matlib.ones(3).T)
invdyn.addMotionTask(comTask, w_com, 1, 0.0)
21
![Page 74: Task-Space Inverse Dynamics: Implementation · Introduction This document explains the implementation of the control framework Task-Space Inverse Dynamics(TSID). To simplify the job](https://reader033.fdocuments.in/reader033/viewer/2022043000/5f746933f5165e1ed71d5ced/html5/thumbnails/74.jpg)
Create Posture Task
postureTask = TaskJointPosture("task-posture", robot)
postureTask.setKp(kp_posture*matlib.ones(robot.nv-6).T)
postureTask.setKd(2*sqrt(kp_posture)*matlib.ones(robot.nv-6).T)
invdyn.addMotionTask(postureTask, w_posture, 1, 0.0)
22
![Page 75: Task-Space Inverse Dynamics: Implementation · Introduction This document explains the implementation of the control framework Task-Space Inverse Dynamics(TSID). To simplify the job](https://reader033.fdocuments.in/reader033/viewer/2022043000/5f746933f5165e1ed71d5ced/html5/thumbnails/75.jpg)
Create Reference Task Trajectories
com_ref = robot.com(data)
trajCom = TrajectoryEuclidianConstant("traj_com", com_ref)
q_ref = q[7:]
trajPosture = TrajectoryEuclidianConstant("traj_joint", q_ref)
23
![Page 76: Task-Space Inverse Dynamics: Implementation · Introduction This document explains the implementation of the control framework Task-Space Inverse Dynamics(TSID). To simplify the job](https://reader033.fdocuments.in/reader033/viewer/2022043000/5f746933f5165e1ed71d5ced/html5/thumbnails/76.jpg)
Create HQP Solver
solver = SolverHQuadProg("qp solver")
solver.resize(invdyn.nVar, invdyn.nEq, invdyn.nIn)
24
![Page 77: Task-Space Inverse Dynamics: Implementation · Introduction This document explains the implementation of the control framework Task-Space Inverse Dynamics(TSID). To simplify the job](https://reader033.fdocuments.in/reader033/viewer/2022043000/5f746933f5165e1ed71d5ced/html5/thumbnails/77.jpg)
Control Loop
for i in range(0, N_SIMULATION_STEPS):
comTask.setReference(trajCom.computeNext())
postureTask.setReference(trajPosture.computeNext())
# get current state estimation
(q, v) = ...
HQPData = invdyn.computeProblemData(t, q, v)
sol = solver.solve(HQPData)
tau = invdyn.getActuatorForces(sol)
# send desired joint torques (tau) to actuators
...
25
![Page 78: Task-Space Inverse Dynamics: Implementation · Introduction This document explains the implementation of the control framework Task-Space Inverse Dynamics(TSID). To simplify the job](https://reader033.fdocuments.in/reader033/viewer/2022043000/5f746933f5165e1ed71d5ced/html5/thumbnails/78.jpg)
Simulation Loop
for i in range(0, N_SIMULATION_STEPS):
...
# assuming perfect torque-acceleration tracking...
dv = invdyn.getAccelerations(sol)
# integrate desired accelerations
q = se3.integrate(robot.model(), q, dt*v)
v += dt*dv
# increase time
t += dt
26
![Page 79: Task-Space Inverse Dynamics: Implementation · Introduction This document explains the implementation of the control framework Task-Space Inverse Dynamics(TSID). To simplify the job](https://reader033.fdocuments.in/reader033/viewer/2022043000/5f746933f5165e1ed71d5ced/html5/thumbnails/79.jpg)
Exercises
![Page 80: Task-Space Inverse Dynamics: Implementation · Introduction This document explains the implementation of the control framework Task-Space Inverse Dynamics(TSID). To simplify the job](https://reader033.fdocuments.in/reader033/viewer/2022043000/5f746933f5165e1ed71d5ced/html5/thumbnails/80.jpg)
Exercise 1: CoM Set-Point Regulation
• Run provided example (tsid/exercizes/ex 1.py) and check that
the robot does not move
• Change references of CoM/posture and look what happens
• Change CoM/posture gains and see effect
• Change CoM/posture weights and see effect
• Set reference CoM outside support polygon (e.g., 20 cm to the
side), what happens? Why?
27
![Page 81: Task-Space Inverse Dynamics: Implementation · Introduction This document explains the implementation of the control framework Task-Space Inverse Dynamics(TSID). To simplify the job](https://reader033.fdocuments.in/reader033/viewer/2022043000/5f746933f5165e1ed71d5ced/html5/thumbnails/81.jpg)
Exercise 1: CoM Set-Point Regulation
• Run provided example (tsid/exercizes/ex 1.py) and check that
the robot does not move
• Change references of CoM/posture and look what happens
• Change CoM/posture gains and see effect
• Change CoM/posture weights and see effect
• Set reference CoM outside support polygon (e.g., 20 cm to the
side), what happens? Why?
27
![Page 82: Task-Space Inverse Dynamics: Implementation · Introduction This document explains the implementation of the control framework Task-Space Inverse Dynamics(TSID). To simplify the job](https://reader033.fdocuments.in/reader033/viewer/2022043000/5f746933f5165e1ed71d5ced/html5/thumbnails/82.jpg)
Exercise 1: CoM Set-Point Regulation
• Run provided example (tsid/exercizes/ex 1.py) and check that
the robot does not move
• Change references of CoM/posture and look what happens
• Change CoM/posture gains and see effect
• Change CoM/posture weights and see effect
• Set reference CoM outside support polygon (e.g., 20 cm to the
side), what happens? Why?
27
![Page 83: Task-Space Inverse Dynamics: Implementation · Introduction This document explains the implementation of the control framework Task-Space Inverse Dynamics(TSID). To simplify the job](https://reader033.fdocuments.in/reader033/viewer/2022043000/5f746933f5165e1ed71d5ced/html5/thumbnails/83.jpg)
Exercise 1: CoM Set-Point Regulation
• Run provided example (tsid/exercizes/ex 1.py) and check that
the robot does not move
• Change references of CoM/posture and look what happens
• Change CoM/posture gains and see effect
• Change CoM/posture weights and see effect
• Set reference CoM outside support polygon (e.g., 20 cm to the
side), what happens? Why?
27
![Page 84: Task-Space Inverse Dynamics: Implementation · Introduction This document explains the implementation of the control framework Task-Space Inverse Dynamics(TSID). To simplify the job](https://reader033.fdocuments.in/reader033/viewer/2022043000/5f746933f5165e1ed71d5ced/html5/thumbnails/84.jpg)
Exercise 1: CoM Set-Point Regulation
• Run provided example (tsid/exercizes/ex 1.py) and check that
the robot does not move
• Change references of CoM/posture and look what happens
• Change CoM/posture gains and see effect
• Change CoM/posture weights and see effect
• Set reference CoM outside support polygon (e.g., 20 cm to the
side), what happens? Why?
27
![Page 85: Task-Space Inverse Dynamics: Implementation · Introduction This document explains the implementation of the control framework Task-Space Inverse Dynamics(TSID). To simplify the job](https://reader033.fdocuments.in/reader033/viewer/2022043000/5f746933f5165e1ed71d5ced/html5/thumbnails/85.jpg)
Exercise 2: CoM Sinusoidal Tracking
• Run provided code (tsid/exercizes/ex 2.py) and check the
sinusoidal reference CoM tracking
• Increase CoM frequency until tracking gets bad. Why does that
happen?
• Set contact feedback gains to zero, what happens? Why?
• Add contact on hand
28
![Page 86: Task-Space Inverse Dynamics: Implementation · Introduction This document explains the implementation of the control framework Task-Space Inverse Dynamics(TSID). To simplify the job](https://reader033.fdocuments.in/reader033/viewer/2022043000/5f746933f5165e1ed71d5ced/html5/thumbnails/86.jpg)
Exercise 2: CoM Sinusoidal Tracking
• Run provided code (tsid/exercizes/ex 2.py) and check the
sinusoidal reference CoM tracking
• Increase CoM frequency until tracking gets bad. Why does that
happen?
• Set contact feedback gains to zero, what happens? Why?
• Add contact on hand
28
![Page 87: Task-Space Inverse Dynamics: Implementation · Introduction This document explains the implementation of the control framework Task-Space Inverse Dynamics(TSID). To simplify the job](https://reader033.fdocuments.in/reader033/viewer/2022043000/5f746933f5165e1ed71d5ced/html5/thumbnails/87.jpg)
Exercise 2: CoM Sinusoidal Tracking
• Run provided code (tsid/exercizes/ex 2.py) and check the
sinusoidal reference CoM tracking
• Increase CoM frequency until tracking gets bad. Why does that
happen?
• Set contact feedback gains to zero, what happens? Why?
• Add contact on hand
28
![Page 88: Task-Space Inverse Dynamics: Implementation · Introduction This document explains the implementation of the control framework Task-Space Inverse Dynamics(TSID). To simplify the job](https://reader033.fdocuments.in/reader033/viewer/2022043000/5f746933f5165e1ed71d5ced/html5/thumbnails/88.jpg)
Exercise 2: CoM Sinusoidal Tracking
• Run provided code (tsid/exercizes/ex 2.py) and check the
sinusoidal reference CoM tracking
• Increase CoM frequency until tracking gets bad. Why does that
happen?
• Set contact feedback gains to zero, what happens? Why?
• Add contact on hand
28
![Page 89: Task-Space Inverse Dynamics: Implementation · Introduction This document explains the implementation of the control framework Task-Space Inverse Dynamics(TSID). To simplify the job](https://reader033.fdocuments.in/reader033/viewer/2022043000/5f746933f5165e1ed71d5ced/html5/thumbnails/89.jpg)
Exercise 3: Taking a Step
• Extend code to make robot take a step (solution in
tsid/demo/demo romeo.py)
29