From ab9a3e18a111b1d45e2e1452b2e08044cc25c450 Mon Sep 17 00:00:00 2001 From: Brian Delhaisse Date: Sun, 14 Apr 2019 16:17:40 +0200 Subject: [PATCH] update priorities: doc + signatures of kinematic and dynamic tasks --- pyrobolearn/priorities/README.md | 50 +++++-- pyrobolearn/priorities/constraints/README.md | 9 +- .../priorities/constraints/constraint.py | 114 +++++++++++++++- .../constraints/dynamic_constraints.py | 64 +++++++++ .../constraints/kinematic_constraints.py | 72 ++++++++++ pyrobolearn/priorities/tasks/README.md | 9 +- pyrobolearn/priorities/tasks/dynamic_tasks.py | 73 ++++++++++ .../priorities/tasks/kinematic_tasks.py | 90 +++++++++++++ pyrobolearn/priorities/tasks/task.py | 127 +++++++++++++++++- 9 files changed, 585 insertions(+), 23 deletions(-) create mode 100644 pyrobolearn/priorities/constraints/dynamic_constraints.py create mode 100644 pyrobolearn/priorities/constraints/kinematic_constraints.py create mode 100644 pyrobolearn/priorities/tasks/dynamic_tasks.py create mode 100644 pyrobolearn/priorities/tasks/kinematic_tasks.py diff --git a/pyrobolearn/priorities/README.md b/pyrobolearn/priorities/README.md index 466d6c5..bf4beba 100644 --- a/pyrobolearn/priorities/README.md +++ b/pyrobolearn/priorities/README.md @@ -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 - diff --git a/pyrobolearn/priorities/constraints/README.md b/pyrobolearn/priorities/constraints/README.md index 9ec7399..88ede2e 100644 --- a/pyrobolearn/priorities/constraints/README.md +++ b/pyrobolearn/priorities/constraints/README.md @@ -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 diff --git a/pyrobolearn/priorities/constraints/constraint.py b/pyrobolearn/priorities/constraints/constraint.py index 65e5509..bf823c7 100644 --- a/pyrobolearn/priorities/constraints/constraint.py +++ b/pyrobolearn/priorities/constraints/constraint.py @@ -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() diff --git a/pyrobolearn/priorities/constraints/dynamic_constraints.py b/pyrobolearn/priorities/constraints/dynamic_constraints.py new file mode 100644 index 0000000..16664e1 --- /dev/null +++ b/pyrobolearn/priorities/constraints/dynamic_constraints.py @@ -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 diff --git a/pyrobolearn/priorities/constraints/kinematic_constraints.py b/pyrobolearn/priorities/constraints/kinematic_constraints.py new file mode 100644 index 0000000..3b4d543 --- /dev/null +++ b/pyrobolearn/priorities/constraints/kinematic_constraints.py @@ -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. + + """ \ No newline at end of file diff --git a/pyrobolearn/priorities/tasks/README.md b/pyrobolearn/priorities/tasks/README.md index 1656230..de08017 100644 --- a/pyrobolearn/priorities/tasks/README.md +++ b/pyrobolearn/priorities/tasks/README.md @@ -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 diff --git a/pyrobolearn/priorities/tasks/dynamic_tasks.py b/pyrobolearn/priorities/tasks/dynamic_tasks.py new file mode 100644 index 0000000..2908d84 --- /dev/null +++ b/pyrobolearn/priorities/tasks/dynamic_tasks.py @@ -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 diff --git a/pyrobolearn/priorities/tasks/kinematic_tasks.py b/pyrobolearn/priorities/tasks/kinematic_tasks.py new file mode 100644 index 0000000..6fac378 --- /dev/null +++ b/pyrobolearn/priorities/tasks/kinematic_tasks.py @@ -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 diff --git a/pyrobolearn/priorities/tasks/task.py b/pyrobolearn/priorities/tasks/task.py index 410ea00..da99e42 100644 --- a/pyrobolearn/priorities/tasks/task.py +++ b/pyrobolearn/priorities/tasks/task.py @@ -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__':