wassname/pyrobolearn
1
0
Fork 0
mirror of https://github.com/wassname/pyrobolearn.git synced 2026-09-09 11:31:38 +08:00
Code Issues Packages Projects Releases Wiki Activity

update priorities: doc + signatures of kinematic and dynamic tasks

Browse Source
This commit is contained in:
Brian Delhaisse
2019-04-14 16:17:40 +02:00
parent 12880e1bf1
commit ab9a3e18a1
9 changed files with 585 additions and 23 deletions
Download Patch File Download Diff File Expand all files Collapse all files
pyrobolearn/priorities/README.md
+41 -9
View File
@@ -1,24 +1,56 @@
## Priority Tasks
In this folder, you will find the code for priority "tasks". The "tasks" defined here are different from the tasks defined in the `pyrobolearn/tasks` folder which defines robot learning tasks. The tasks defined here can be more seen as "constraints"; for instance, the constraint for the robot to maintain its balance (i.e. have its center of mass above the support polygon), the constraint for the robot to have a certain pose, the constraint for the robot's end-effectors to track a certain trajectory, the constraint for the robot to not have collisions between its links, the constraint for the robot to respect the equation of motions, etc. In the robotics community field, these are known as "tasks" and concepts around them such as the stack of tasks [2] have been defined. In *pyrobolearn*, we keep a more abstract definition of a task, which is also probably more related to what people have in mind when talking about a robot performing a certain task.
In this folder, you will find the code for priority "tasks". The "tasks" defined here are different from the tasks
defined in the `pyrobolearn/tasks` folder which defines robot learning tasks. The tasks defined here can be more seen
as "constraints"; for instance, the constraint for the robot to maintain its balance (i.e. have its center of mass
above the support polygon), the constraint for the robot to have a certain pose, the constraint for the robot's
end-effectors to track a certain trajectory, the constraint for the robot to not have collisions between its links,
the constraint for the robot to respect the equation of motions, etc. In the robotics community field, these are known
as "tasks" and concepts around them such as the stack of tasks [2] have been defined. In *pyrobolearn*, we keep a
more abstract definition of a task, which is also probably more related to what people have in mind when talking
about a robot performing a certain task.
Most of these "tasks" are represented as a constrained optimization problem, where the "task" consists to minimize a certain objective function while respecting certain equality and inequality constraints. This is the reason why they are not called "constraints" to avoid the confusion with the (inequality and equality) constraints defined in the optimization problem. Most of the time, they are formulated as quadratic programming (QP) optimization problem [1]. Priority tasks are divided between kinematic and dynamic tasks, where the former only takes into account position and velocity information, while the latter also include dynamic information (forces and torques applied on the various bodies). The variables that are thus optimized by the optimization problem depends on the type of problem (kinematic or dynamic) we are dealing with. In the case of a kinematic task, the variables are often the joint (or end-effector) positions and/or velocities, while in the dynamic case, the variables are the joint accelerations and the (reaction) forces applied on the robot.
Most of these "tasks" are represented as a constrained optimization problem, where the "task" consists to minimize
a certain objective function while respecting certain equality and inequality constraints. This is the reason why
they are not called "constraints" to avoid the confusion with the (inequality and equality) constraints defined in
the optimization problem. Most of the time, they are formulated as quadratic programming (QP) optimization problem [1].
Priority tasks are divided between kinematic and dynamic tasks, where the former only takes into account position and
velocity information, while the latter also include dynamic information (forces and torques applied on the various
bodies). The variables that are thus optimized by the optimization problem depends on the type of problem (kinematic
or dynamic) we are dealing with. In the case of a kinematic task, the variables are often the joint (or end-effector)
positions and/or velocities, while in the dynamic case, the variables are the joint accelerations and the (reaction)
forces applied on the robot.
Priorities can be divided into two categories: soft and hard priorities.
* Soft priorities: each objective function is weigthed by an importance weight where higher weights mean that we give more importance to the corresponding objective function. For instance, we might have a humanoid robot with two arms where each arm has to follow a specific trajectory and where we give the same importance to both "tasks". Soft priorities use task augmentation.
* hard priorities: the most important constrained optimization problem is first solved, and then the next most important one is solved with an additional (optimization) constraint that the solution has to be in the solution space of the previous one. For instance, it is more important for a humanoid robot to maintain its balance than to follow perfectly a trajectory with its end-effector. This way of putting "tasks" on top of each other is known as the stack of tasks in the robotics community [2]. Hard priorities exploit the null-space of higher priority tasks.
* Soft priorities: each objective function is weigthed by an importance weight where higher weights mean that we give
more importance to the corresponding objective function. For instance, we might have a humanoid robot with two arms
where each arm has to follow a specific trajectory and where we give the same importance to both "tasks". Soft
priorities use task augmentation.
* hard priorities: the most important constrained optimization problem is first solved, and then the next most
important one is solved with an additional (optimization) constraint that the solution has to be in the solution space
of the previous one. For instance, it is more important for a humanoid robot to maintain its balance than to follow
perfectly a trajectory with its end-effector. This way of putting "tasks" on top of each other is known as the stack
of tasks in the robotics community [2]. Hard priorities exploit the null-space of higher priority tasks.
Soft and hard priorities can be mixed together as done in the following C++ framework [3].
The code presented here (and the architecture) is partially inspired by [3], but we decouple it from its tight coupling with other frameworks/middlewares (i.e. superbuild, XBotControl, ROS/Yarp), write it in Python using optimization libraries (that are generally written in C++ and provide Python wrappers), and provide kinematic and dynamic "tasks" as well.
The code presented here (and the architecture) is partially inspired by [3, 4] (the papers and slides). Compared to
this framework, we write it in Python using popular optimization libraries (that are usually written in C/C++ and
provide Python wrappers), we decouple it from other frameworks/middlewares (such as superbuild, XBotControl,
ROS/Yarp, etc.), and make it free and open-source (currently, the original code seems to be on a private repo).
In what follows, to avoid confusion with the vocabulary used in the robotics community, we will keep the notions of "tasks", (optimization) "constraints", and "solvers". The solvers can be found in the `pyrobolearn/optimizers` folder.
In what follows, to avoid confusion with the vocabulary used in the robotics community, we will keep the notions of
"tasks", (optimization) "constraints", and "solvers". The solvers can be found in the `pyrobolearn/optimizers` folder.
## References
1. Quadratic Programming (Wikipedia): https://en.wikipedia.org/wiki/Quadratic_programming
2. "A Versatile Generalized Inverted Kinematics Implementationfor Collaborative Working Humanoid Robots: The Stack of Tasks" ([code](https://stack-of-tasks.github.io/)), Mansard et al., 2009
3. "OpenSoT: A whole-body control library for the compliant humanoid robot COMAN" ([code](https://opensot.wixsite.com/opensot), [slides](https://docs.google.com/presentation/d/1kwJsAnVi_3ADtqFSTP8wq3JOGLcvDV_ypcEEjPHnCEA), [tutorial video](https://www.youtube.com/watch?v=yFon-ZDdSyg), [old code](https://github.com/songcheng/OpenSoT)), Rocchi et al., 2015
2. "A Versatile Generalized Inverted Kinematics Implementationfor Collaborative Working Humanoid Robots: The Stack of
Tasks" ([code](https://stack-of-tasks.github.io/)), Mansard et al., 2009
3. "OpenSoT: A whole-body control library for the compliant humanoid robot COMAN" (
[code](https://opensot.wixsite.com/opensot),
[slides](https://docs.google.com/presentation/d/1kwJsAnVi_3ADtqFSTP8wq3JOGLcvDV_ypcEEjPHnCEA),
[tutorial video](https://www.youtube.com/watch?v=yFon-ZDdSyg), [old code](https://github.com/songcheng/OpenSoT)),
Rocchi et al., 2015
4. "Robot Control for Dummies: Insights and Examples using OpenSoT", Hoffman et al., 2017
pyrobolearn/priorities/constraints/README.md
+7 -2
View File
@@ -1,10 +1,15 @@
## Constraints
In this folder, we define the most common inequality and equality optimization constraints used in robotics for priority tasks. Several of them were provided in [1].
In this folder, we define the most common inequality and equality optimization constraints used in robotics for
priority tasks. Several of them were provided in [1].
Constraints include joint limits, joint velocity limits, collision avoidance, and others.
References:
1. "OpenSoT: A whole-body control library for the compliant humanoid robot COMAN" ([code](https://opensot.wixsite.com/opensot), [slides](https://docs.google.com/presentation/d/1kwJsAnVi_3ADtqFSTP8wq3JOGLcvDV_ypcEEjPHnCEA), [tutorial video](https://www.youtube.com/watch?v=yFon-ZDdSyg), [old code](https://github.com/songcheng/OpenSoT)), Rocchi et al., 2015
1. "OpenSoT: A whole-body control library for the compliant humanoid robot COMAN" (
[code](https://opensot.wixsite.com/opensot),
[slides](https://docs.google.com/presentation/d/1kwJsAnVi_3ADtqFSTP8wq3JOGLcvDV_ypcEEjPHnCEA),
[tutorial video](https://www.youtube.com/watch?v=yFon-ZDdSyg),
[old code](https://github.com/songcheng/OpenSoT)), Rocchi et al., 2015
2. "Robot Control for Dummies: Insights and Examples using OpenSoT", Hoffman et al., 2017
pyrobolearn/priorities/constraints/constraint.py
+111 -3
View File
@@ -1,5 +1,93 @@
#!/usr/bin/env python
"""Provide the various constraints used in QP.
r"""Provide the various constraints used in QP.
Provide the various optimization constraints (:math:`G, h, F, c` in the upcoming formulation) used in QP.
A quadratic program (QP) is written in standard form [1] as:
.. math::
x^* &= \arg \min_x \frac{1}{2} x^T Q x + p^T x \\ \text{subj. to}
& Gx \leq h \\
& Fx = c
where :math:`x` is the vector being optimized (in robotics, it can be joint positions, velocities, torques, ...),
"the matrix :math:`Q` and vector :math:`p` are used to define any quadratic objective function of these variables,
while the matrix-vector couples :math:`(G,h)` and :math:`(F,c)` respectively define inequality and equality
constraints" [1]. Inequality constraints can include the lower bounds and upper bounds of :math`x` by setting
:math:`G` to be the identity matrix or minus this one, and :math:`h` to be the upper or lower bounds.
For instance, the quadratic objective function :math:`||Ax - b||^2_{W}` (where :math:`W` is a weight matrix) is given
in the standard form as:
.. math:: ||Ax - b||^2_{W} = (Ax - b)^\top W (Ax - b) = x^\top A^\top W A x - 2 b^\top W A x + b^\top W b
where the last term :math:`b^\top W b` can be removed as it does not depend on the variables we are optimizing (i.e.
:math:`x`). We thus have :math:`Q = A^\top W A` a symmetric matrix and :math:`p = -2 b^\top W A`.
Many control problems in robotics can be formulated as a quadratic programming problem.
For instance, let's assume that we want to optimize the joint velocities :math:`\dot{q}` given the end-effector's
desired position and velocity in task space. We can define the quadratic problem as:
.. math:: || J(q) \dot{q} - \dot{x} ) ||^2
where using a PD reference, :math:`\dot{x} = \dot{x}_d + K (x_d - x)`, where :math:`x_d` and :math:`x` are the desired
and current end-effector's position respectively, and :math:`\dot{x}_d` is the desired velocity.
* Soft priority tasks: with soft-priority tasks, the quadratic programming problem being minimized for n such tasks
is given by:
.. math::
x^* &= \arg \min_x ||A_1 x - b_1||^2_{W_1} + ||A_2 x - b_2 ||^2_{W_2} + ... + ||A_n x - b_n ||^2_{W_n} \\
\text{subj. to} & Gx \leq h \\
& Fx = c
Often, the weight matrices :math:`W_i` are just scalars :math:`w_i`. This problem can notably be solved by stacking
the :math:`A_i` one of top of another, and stacking the :math:`b_i` and :math:`W_i` in the same manner, and solving
:math:`||A x - b||^2_{W}` This is known as the augmented task. When the matrices :math:`A` are Jacobians this is known
as the augmented Jacobian (which can sometimes be ill-conditioned).
* Hard priority tasks: with hard-priority tasks, the quadratic programming problem for n tasks is defined in a
sequential manner, where the first most important task will be first optimized, and then the subsequent tasks will be
optimized one after the other. Thus, the first task to be optimized is given by:
.. math:: x_1^* &= \arg \min_x ||A_1 x - b_1||^2 \\ \text{subj. to}
& G_1 x \leq h_1 \\
& F_1 x = c_1,
while the second next most important task that would be solved is given by:
.. math:: x_2^* &= \arg \min_x ||A_2 x - b_2||^2 \\ \text{subj. to}
& G_2 x \leq h_2 \\
& F_2 x = c_2 \\
& A_1 x = A_1 x_1^* \\
& G_1 x \leq h_1 \\
& F_1 x = c_1,
until the :math:`n` most important task, given by:
.. math:: x_n^* \arg \min_x ||A_n x - b_n||^2 \\ \text{subj. to}
& A_1 x = A_1 x_1^* \\
& ... \\
& A_{n-1} x = A_{n-1} x_{n-1}^* \\
& G_1 x \leq h_1 \\
& ... \\
& G_n x \leq h_n \\
& F_1 x = c_1 \\
& ... \\
& F_n x = c_n.
By setting the previous :math:`A_{i-1} x = A_{i-1} x_{i-1}^*` as equality constraints, the current solution
:math:`x_i^*` won't change the optimality of all higher priority tasks.
References:
[1] "Quadratic Programming in Python" (https://scaron.info/blog/quadratic-programming-in-python.html), Caron, 2017
[2] "OpenSoT: A whole-body control library for the compliant humanoid robot COMAN", Rocchi et al., 2015
[3] "Robot Control for Dummies: Insights and Examples using OpenSoT", Hoffman et al., 2017
"""
import numpy as np
@@ -45,11 +133,31 @@ class Constraint(object):
"""Return the robot model."""
return self._model
@property
def lower_bound(self):
"""Return the lower bound."""
return self._lower_bound
@property
def upper_bound(self):
"""Return the upper bound."""
return self._upper_bound
@property
def A_eq(self):
"""Return the :math:`A_{eq}` matrix."""
return self._A_eq
@property
def b_eq(self):
"""Return the :math:`b_{eq}` vector"""
return self._b_eq
###########
# Methods #
###########
def compute(self):
def update(self):
pass
#############
@@ -63,4 +171,4 @@ class Constraint(object):
return self.__class__.__name__
def __call__(self):
return self.compute()
return self.update()
pyrobolearn/priorities/constraints/dynamic_constraints.py
+64
View File
@@ -0,0 +1,64 @@
#!/usr/bin/env python
"""Provide the various kinematic constraints used in QP.
References:
[1] "Quadratic Programming in Python" (https://scaron.info/blog/quadratic-programming-in-python.html), Caron, 2017
[2] "OpenSoT: A whole-body control library for the compliant humanoid robot COMAN", Rocchi et al., 2015
[3] "Robot Control for Dummies: Insights and Examples using OpenSoT", Hoffman et al., 2017
"""
import rbdl
import numpy as np
from pyrobolearn.robots.robot import Robot
from pyrobolearn.priorities.constraints.constraint import Constraint
__author__ = "Brian Delhaisse"
__copyright__ = "Copyright 2018, PyRoboLearn"
__credits__ = ["OpenSoT (Enrico Mingo Hoffman and Alessio Rocchi)", "Songyan Xin"]
__license__ = "MIT"
__version__ = "1.0.0"
__maintainer__ = "Brian Delhaisse"
__email__ = "briandelhaisse@gmail.com"
__status__ = "Development"
class DynamicConstraint(Constraint):
r"""Dynamic Constraints
"""
pass
class StaticsStability(DynamicConstraint):
r"""Statics Stability constraint.
'The goal of statics is to determine the relationship between the generalized forces applied to the end-effector
and the generalized forces applied to the joints.' [1]
References:
[1] "Robotics: Modelling, Planning and Control", Siciliano et al., 2010
"""
pass
class TorqueLimits(DynamicConstraint):
r"""Torque limits constraint.
"""
pass
class FrictionCones(DynamicConstraint):
r"""Friction cones constraint (using contact force optimization)
"""
pass
class WrenchLimits(DynamicConstraint):
r"""Wrench Limits (using contact force optimization).
"""
pass
pyrobolearn/priorities/constraints/kinematic_constraints.py
+72
View File
@@ -0,0 +1,72 @@
#!/usr/bin/env python
"""Provide the various kinematic constraints used in QP.
References:
[1] "Quadratic Programming in Python" (https://scaron.info/blog/quadratic-programming-in-python.html), Caron, 2017
[2] "OpenSoT: A whole-body control library for the compliant humanoid robot COMAN", Rocchi et al., 2015
[3] "Robot Control for Dummies: Insights and Examples using OpenSoT", Hoffman et al., 2017
"""
import rbdl
import numpy as np
from pyrobolearn.robots.robot import Robot
from pyrobolearn.priorities.constraints.constraint import Constraint
__author__ = "Brian Delhaisse"
__copyright__ = "Copyright 2018, PyRoboLearn"
__credits__ = ["OpenSoT (Enrico Mingo Hoffman and Alessio Rocchi)", "Songyan Xin"]
__license__ = "MIT"
__version__ = "1.0.0"
__maintainer__ = "Brian Delhaisse"
__email__ = "briandelhaisse@gmail.com"
__status__ = "Development"
class KinematicConstraint(Constraint):
r"""Kinematic constraint
"""
pass
class CartesianPose(KinematicConstraint):
r"""Cartesian Pose constraint.
"""
pass
class CartesianVelocity(KinematicConstraint):
r"""Cartesian velocity constraint.
"""
pass
class CoMVelocity(KinematicConstraint):
r"""Center-of-mass velocity constraint.
"""
pass
class JointLimits(KinematicConstraint):
r"""Joint limits constraint.
"""
pass
class JointVelocityLimits(KinematicConstraint):
r"""Velocity limits constraint.
"""
pass
class SelfCollisionAvoidance(KinematicConstraint):
r"""Self-collision avoidance constraint.
"""
pyrobolearn/priorities/tasks/README.md
+7 -2
View File
@@ -1,10 +1,15 @@
## Tasks
In this folder, we define the most common objective functions (aka "tasks") used in robotics for priority tasks. Several of them were provided in [1].
In this folder, we define the most common objective functions (aka "tasks") used in robotics for priority tasks.
Several of them were provided in [1, 2].
Tasks include cartesian CoM tracking, cartesian end-effector position tracking, postural positioning, and others.
References:
1. "OpenSoT: A whole-body control library for the compliant humanoid robot COMAN" ([code](https://opensot.wixsite.com/opensot), [slides](https://docs.google.com/presentation/d/1kwJsAnVi_3ADtqFSTP8wq3JOGLcvDV_ypcEEjPHnCEA), [tutorial video](https://www.youtube.com/watch?v=yFon-ZDdSyg), [old code](https://github.com/songcheng/OpenSoT)), Rocchi et al., 2015
1. "OpenSoT: A whole-body control library for the compliant humanoid robot COMAN" (
[code](https://opensot.wixsite.com/opensot),
[slides](https://docs.google.com/presentation/d/1kwJsAnVi_3ADtqFSTP8wq3JOGLcvDV_ypcEEjPHnCEA),
[tutorial video](https://www.youtube.com/watch?v=yFon-ZDdSyg),
[old code](https://github.com/songcheng/OpenSoT)), Rocchi et al., 2015
2. "Robot Control for Dummies: Insights and Examples using OpenSoT", Hoffman et al., 2017
pyrobolearn/priorities/tasks/dynamic_tasks.py
+73
View File
@@ -0,0 +1,73 @@
#!/usr/bin/env python
"""Provide the various dynamic tasks (i.e. objective functions) used in QP.
References:
[1] "Quadratic Programming in Python" (https://scaron.info/blog/quadratic-programming-in-python.html), Caron, 2017
[2] "OpenSoT: A whole-body control library for the compliant humanoid robot COMAN", Rocchi et al., 2015
[3] "Robot Control for Dummies: Insights and Examples using OpenSoT", Hoffman et al., 2017
"""
import rbdl
import numpy as np
from pyrobolearn.robots.robot import Robot
from pyrobolearn.priorities.tasks.task import Task
__author__ = "Brian Delhaisse"
__copyright__ = "Copyright 2018, PyRoboLearn"
__credits__ = ["OpenSoT (Enrico Mingo Hoffman and Alessio Rocchi)", "Songyan Xin"]
__license__ = "MIT"
__version__ = "1.0.0"
__maintainer__ = "Brian Delhaisse"
__email__ = "briandelhaisse@gmail.com"
__status__ = "Development"
class DynamicTask(Task):
r"""Dynamic Task.
"""
pass
class MinAcceleration(DynamicTask):
r"""Min acceleration task.
"""
pass
class MinEffort(DynamicTask):
r"""Min effort task.
"""
pass
class Admittance(DynamicTask):
r"""Admittance task.
"""
pass
class ForceManipulability(DynamicTask):
r"""Force Manipulability task.
"""
pass
class CentroidalDynamics(DynamicTask):
r"""Centroidal dynamics task (u
"""
pass
class Wrench(DynamicTask):
r"""Wrench dynamic task.
"""
pass
pyrobolearn/priorities/tasks/kinematic_tasks.py
+90
View File
@@ -0,0 +1,90 @@
#!/usr/bin/env python
"""Provide the various kinematic tasks (i.e. objective functions) used in QP.
References:
[1] "Quadratic Programming in Python" (https://scaron.info/blog/quadratic-programming-in-python.html), Caron, 2017
[2] "OpenSoT: A whole-body control library for the compliant humanoid robot COMAN", Rocchi et al., 2015
[3] "Robot Control for Dummies: Insights and Examples using OpenSoT", Hoffman et al., 2017
"""
import rbdl
import numpy as np
from pyrobolearn.robots.robot import Robot
from pyrobolearn.priorities.tasks.task import Task
__author__ = "Brian Delhaisse"
__copyright__ = "Copyright 2018, PyRoboLearn"
__credits__ = ["OpenSoT (Enrico Mingo Hoffman and Alessio Rocchi)", "Songyan Xin"]
__license__ = "MIT"
__version__ = "1.0.0"
__maintainer__ = "Brian Delhaisse"
__email__ = "briandelhaisse@gmail.com"
__status__ = "Development"
class KinematicTask(Task):
r"""Kinematic Task
"""
pass
class BasePosition(KinematicTask):
r"""Base Position Task
"""
pass
class BaseOrientation(KinematicTask):
r"""Base Orientation Task
"""
pass
class BasePose(KinematicTask):
r"""Base Pose Task
"""
pass
class Postural(KinematicTask):
r"""Postural kinematic task
While optimizing, the robot can get to weird configurations. This postural kinematic task tries to keep the
robot's kinematic configuration close to the given default joint positions. This task is usually put at the end of
the stack of tasks, once all the other tasks have been fulfilled.
"""
pass
class Cartesian(KinematicTask):
r"""Cartesian kinematic task
"""
pass
class CoM(KinematicTask):
r"""Center of Mass task
"""
pass
class VelocityManipulability(KinematicTask):
r"""Velocity manipulability task
"""
pass
class MinVelocity(KinematicTask):
r"""Min velocity task
"""
pass
pyrobolearn/priorities/tasks/task.py
+120 -7
View File
@@ -1,5 +1,93 @@
#!/usr/bin/env python
"""Provide the various tasks (i.e. objective functions) used in QP.
r"""Provide the various tasks (i.e. objective functions) used in QP.
The tasks presented here represents the quadratic objective functions used in quadratic programming (QP).
A quadratic program (QP) is written in standard form [1] as:
.. math::
x^* &= \arg \min_x \frac{1}{2} x^T Q x + p^T x \\ \text{subj. to}
& Gx \leq h \\
& Fx = c
where :math:`x` is the vector being optimized (in robotics, it can be joint positions, velocities, torques, ...),
"the matrix :math:`Q` and vector :math:`p` are used to define any quadratic objective function of these variables,
while the matrix-vector couples :math:`(G,h)` and :math:`(F,c)` respectively define inequality and equality
constraints" [1]. Inequality constraints can include the lower bounds and upper bounds of :math`x` by setting
:math:`G` to be the identity matrix or minus this one, and :math:`h` to be the upper or lower bounds.
For instance, the quadratic objective function :math:`||Ax - b||^2_{W}` (where :math:`W` is a weight matrix) is given
in the standard form as:
.. math:: ||Ax - b||^2_{W} = (Ax - b)^\top W (Ax - b) = x^\top A^\top W A x - 2 b^\top W A x + b^\top W b
where the last term :math:`b^\top W b` can be removed as it does not depend on the variables we are optimizing (i.e.
:math:`x`). We thus have :math:`Q = A^\top W A` a symmetric matrix and :math:`p = -2 b^\top W A`.
Many control problems in robotics can be formulated as a quadratic programming problem.
For instance, let's assume that we want to optimize the joint velocities :math:`\dot{q}` given the end-effector's
desired position and velocity in task space. We can define the quadratic problem as:
.. math:: || J(q) \dot{q} - \dot{x} ) ||^2
where using a PD reference, :math:`\dot{x} = \dot{x}_d + K (x_d - x)`, where :math:`x_d` and :math:`x` are the desired
and current end-effector's position respectively, and :math:`\dot{x}_d` is the desired velocity.
* Soft priority tasks: with soft-priority tasks, the quadratic programming problem being minimized for n such tasks
is given by:
.. math::
x^* &= \arg \min_x ||A_1 x - b_1||^2_{W_1} + ||A_2 x - b_2 ||^2_{W_2} + ... + ||A_n x - b_n ||^2_{W_n} \\
\text{subj. to} & Gx \leq h \\
& Fx = c
Often, the weight matrices :math:`W_i` are just scalars :math:`w_i`. This problem can notably be solved by stacking
the :math:`A_i` one of top of another, and stacking the :math:`b_i` and :math:`W_i` in the same manner, and solving
:math:`||A x - b||^2_{W}` This is known as the augmented task. When the matrices :math:`A` are Jacobians this is known
as the augmented Jacobian (which can sometimes be ill-conditioned).
* Hard priority tasks: with hard-priority tasks, the quadratic programming problem for n tasks is defined in a
sequential manner, where the first most important task will be first optimized, and then the subsequent tasks will be
optimized one after the other. Thus, the first task to be optimized is given by:
.. math:: x_1^* &= \arg \min_x ||A_1 x - b_1||^2 \\ \text{subj. to}
& G_1 x \leq h_1 \\
& F_1 x = c_1,
while the second next most important task that would be solved is given by:
.. math:: x_2^* &= \arg \min_x ||A_2 x - b_2||^2 \\ \text{subj. to}
& G_2 x \leq h_2 \\
& F_2 x = c_2 \\
& A_1 x = A_1 x_1^* \\
& G_1 x \leq h_1 \\
& F_1 x = c_1,
until the :math:`n` most important task, given by:
.. math:: x_n^* \arg \min_x ||A_n x - b_n||^2 \\ \text{subj. to}
& A_1 x = A_1 x_1^* \\
& ... \\
& A_{n-1} x = A_{n-1} x_{n-1}^* \\
& G_1 x \leq h_1 \\
& ... \\
& G_n x \leq h_n \\
& F_1 x = c_1 \\
& ... \\
& F_n x = c_n.
By setting the previous :math:`A_{i-1} x = A_{i-1} x_{i-1}^*` as equality constraints, the current solution
:math:`x_i^*` won't change the optimality of all higher priority tasks.
References:
[1] "Quadratic Programming in Python" (https://scaron.info/blog/quadratic-programming-in-python.html), Caron, 2017
[2] "OpenSoT: A whole-body control library for the compliant humanoid robot COMAN", Rocchi et al., 2015
[3] "Robot Control for Dummies: Insights and Examples using OpenSoT", Hoffman et al., 2017
"""
import numpy as np
@@ -46,6 +134,9 @@ class Task(object):
self.weight = weight
self._constraints = []
self._A = None
self._b = None
##############
# Properties #
##############
@@ -86,12 +177,22 @@ class Task(object):
"""Return the constraints."""
return self._constraints
@property
def A(self):
"""Return A matrix used in QP."""
return self._A
@property
def b(self):
"""Return b vector used in QP."""
return self._b
###########
# Methods #
###########
def _compute(self): # update
"""Compute the task.
def _update(self):
"""Update the task.
Returns:
np.array: A matrix used in QP.
@@ -99,15 +200,15 @@ class Task(object):
"""
pass
def compute(self): # update
def update(self):
if self.tasks:
for hard_task in self.tasks:
results = [soft_task.compute() for soft_task in hard_task]
results = [soft_task.update() for soft_task in hard_task]
As = np.vstack([result[0] for result in results])
bs = np.vstack([result[1] for result in results])
# TODO: continue for hard priority tasks
return As, bs
return self._compute()
return self._update()
#############
# Operators #
@@ -134,7 +235,7 @@ class Task(object):
return self.__repr__()
def __call__(self):
return self.compute()
return self.update()
def __add__(self, other): # TODO: check when other has some tasks
"""Add a soft priority task."""
@@ -174,6 +275,18 @@ class Task(object):
def __rmul__(self, other):
self.__mul__(other)
def __getitem__(self, key):
"""Get the corresponding task.
Examples:
>>> task1 = Task(weight=1)
>>> task2 = Task(weight=2)
>>> task = Task(tasks=[[task1, task2], [task1]])
>>> task2 == task[0,1] # get the second priority task in the first hard task, i.e. it will return task2
True
"""
pass
# Tests
if __name__ == '__main__':