update priorities

pyrobolearn/priorities/constraints/acceleration/__init__.py

@@ -0,0 +1,4 @@
+
+# joint acceleration constraints
+
+from .dynamic_feasibility import DynamicFeasibilityConstraint

pyrobolearn/priorities/constraints/acceleration/dynamic_feasibility.py

@@ -1,6 +1,17 @@
 #!/usr/bin/env python
 r"""Provide the dynamic feasibility constraint.
 
+The equality joint acceleration constraint is given by:
+
+.. math:: H(q) \ddot{q} + N(q, \dot{q}) = \sum_i J_i^T F_i
+
+where :math:`H(q)` is the joint space inertia matrix, :math:`\ddot{q}` are the joint accelerations being optimized,
+:math:`N(q, \dot{q})` is the vector of force terms that account for the Coriolis and centrifugal forces, gravity,
+and any other forces acting on the system other than the contact forces given by :math:`\sum_i J_i^T F_i` (where
+each :math:`J_i` is a Jacobian matrix and :math:`F_i` is a wrench vector at the contact link :math:`i`).
+
+This formulation can be rewritten as an equality constraint math:`A_{eq} x = b_{eq}` in QP, with
+:math:`x = \ddot{q}`, :math:`A_{eq} = H(q)`, and :math:`b_{eq} = \sum_i J_i^T F_i - N(q, \dot{q})`.
 
 The implementation of this class is inspired by [1] (which is licensed under the LGPLv2).
 
@@ -10,7 +21,7 @@ References:
 
 import numpy as np
 
-from pyrobolearn.priorities.constraints.constraint import Constraint
+from pyrobolearn.priorities.constraints.constraint import EqualityConstraint, JointAccelerationConstraint
 
 
 __author__ = "Brian Delhaisse"
@@ -23,10 +34,99 @@ __email__ = "briandelhaisse@gmail.com"
 __status__ = "Development"
 
 
-class DynamicFeasibilityConstraint(Constraint):
+class DynamicFeasibilityConstraint(EqualityConstraint, JointAccelerationConstraint):
     r"""Dynamic Feasibility Constraint
 
+    The equality joint acceleration constraint is given by:
+
+    .. math:: H(q) \ddot{q} + N(q, \dot{q}) = \sum_i J_i^T F_i
+
+    where :math:`H(q)` is the joint space inertia matrix, :math:`\ddot{q}` are the joint accelerations being optimized,
+    :math:`N(q, \dot{q})` is the vector of force terms that account for the Coriolis and centrifugal forces, gravity,
+    and any other forces acting on the system other than the contact forces given by :math:`\sum_i J_i^T F_i` (where
+    each :math:`J_i` is a Jacobian matrix and :math:`F_i` is a wrench vector at the contact link :math:`i`).
+
+    This formulation can be rewritten as an equality constraint math:`A_{eq} x = b_{eq}` in QP, with
+    :math:`x = \ddot{q}`, :math:`A_{eq} = H(q)`, and :math:`b_{eq} = \sum_i J_i^T F_i - N(q, \dot{q})`.
+
+    The implementation of this class is inspired by [1] (which is licensed under the LGPLv2).
+
+    References:
+        - [1] "OpenSoT: A whole-body control library for the compliant humanoid robot COMAN", Rocchi et al., 2015
     """
 
-    def __init__(self, model):
+    def __init__(self, model, contact_links=[], wrenches=[]):
+        r"""
+        Initialize the constraint.
+
+        Args:
+            model (ModelInterface): model interface.
+            contact_links (list[str], list[int], None): list of unique contact link names or ids.
+            wrenches (list[np.array[float[6]]], None): list of associated wrenches applied to the contact links. It
+              must have the same size as the number of contact links.
+        """
         super(DynamicFeasibilityConstraint, self).__init__(model)
+
+        # set variables
+        self.contact_links = contact_links
+        self.wrenches = wrenches
+
+        # first update
+        self.update()
+
+    ##############
+    # Properties #
+    ##############
+
+    @property
+    def contact_links(self):
+        """Get the contact links."""
+        return self._contact_links
+
+    @contact_links.setter
+    def contact_links(self, contacts):
+        """Set the contact links."""
+        if contacts is None:
+            contacts = []
+        elif not isinstance(contacts, (list, tuple, np.ndarray)):
+            raise TypeError("Expecting the given 'contact_links' to be a list of names/ids, but got instead: "
+                            "{}".format(type(contacts)))
+        self._contact_links = contacts
+
+    @property
+    def wrenches(self):
+        """Get the wrenches."""
+        return self._wrenches
+
+    @wrenches.setter
+    def wrenches(self, wrenches):
+        """Set the wrenches."""
+        if wrenches is None:
+            wrenches = []
+        elif not isinstance(wrenches, (list, tuple, np.ndarray)):
+            raise TypeError("Expecting the given 'wrenches' to be a list of wrench vectors, but got instead: "
+                            "{}".format(type(wrenches)))
+        if isinstance(wrenches, np.ndarray) and wrenches.ndim == 1:
+            wrenches = wrenches.reshape(-1, 6)
+        self._wrenches = wrenches
+
+    ###########
+    # Methods #
+    ###########
+
+    def _update(self):
+        """Update the equality constraint."""
+        # get dynamics
+        H = self.model.get_inertia_matrix()
+        N = self.model.compute_nonlinear_term()
+
+        # compute torques due to contact wrenches
+        tau = 0
+        for link, wrench in zip(self.contact_links, self.wrenches):
+            link = self.model.get_link_id(link)
+            jacobian = self.model.get_jacobian(link=link)
+            tau += jacobian.T.dot(wrench)
+
+        # equality constraints
+        self._A_eq = H
+        self._b_eq = tau - N

pyrobolearn/priorities/constraints/constraint.py

@@ -161,9 +161,9 @@ class Constraint(object):
         Initialize the Constraint.
 
         Args:
-            constraints (list[Constraint], None): inner constraints. By providing a list of constraints, they can be
-                combined easily.
             model (ModelInterface): robotic model interface.
+            constraints (list[Constraint], None): inner constraints. By providing a list of constraints, they can be
+              combined easily.
         """
         self.constraints = constraints
         self.model = model
@@ -194,6 +194,14 @@ class Constraint(object):
                             "{}".format(model))
         self._model = model
 
+    @property
+    def x_size(self):
+        """Return the number of variables being optimized."""
+        # return self._x_size
+        if self.model is not None:
+            return self.model.num_actuated_joints
+        return 0
+
     @property
     def constraints(self):
         """Return the dict of inner constraints."""
@@ -492,14 +500,19 @@ class Constraint(object):
         else:
             raise TypeError("The given type of constraint is not currently supported.")
 
+    def _update(self):
+        """Update the constraint variables. This has to be implemented in the child classes."""
+        pass
+
     def update(self):
         r"""
         Update the various constraint matrices and vectors: :math:`A_{eq}, b_{eq}, A_{ineq}, b_l, b_u, ...`.
-
-        Args:
-            x (np.array[float[N]]): current optimization variables values.
         """
-        pass
+        if self.constraints:
+            for constraint in self.constraints:
+                constraint.update()
+        else:
+            self._update()
 
     #############
     # Operators #
@@ -512,14 +525,11 @@ class Constraint(object):
         """Return a string describing the class."""
         return self.__class__.__name__
 
-    def __call__(self, x):
+    def __call__(self):
         """
-        Update the constraint (i.e. update the various constraint matrices and vectors.
-
-        Args:
-            x (np.array[float[N]]): current optimization variable values.
+        Update the constraint (i.e. update the various constraint matrices and vectors).
         """
-        return self.update(x)
+        return self.update()
 
     def __len__(self):
         """
@@ -626,3 +636,13 @@ class JointAccelerationConstraint(DynamicConstraint):
 class JointEffortConstraint(DynamicConstraint):
     r"""Joint effort constraint."""
     pass
+
+
+class JointTorqueConstraint(DynamicConstraint):
+    r"""Joint torque constraint."""
+    pass
+
+
+class JointForceConstraint(DynamicConstraint):
+    r"""Joint Force constraint."""
+    pass

pyrobolearn/priorities/constraints/constraint_from_task.py

@@ -0,0 +1,76 @@
+#!/usr/bin/env python
+r"""Transform a given equality constraint into a task.
+
+An equality constraint specified by :math:`Fx = k` is transformed to a soft task :math:`||Ax - b||^2`, where
+:math:`A = F` and :math:`b = k`. This allows for the equality constraint to be lightly violated; by specifying the
+weight :math:`W` we can specify how much the constraint should be satisfied.
+"""
+
+import pyrobolearn as prl
+from pyrobolearn.priorities.constraints import EqualityConstraint
+
+
+__author__ = "Brian Delhaisse"
+__copyright__ = "Copyright 2019, PyRoboLearn"
+__credits__ = ["Brian Delhaisse"]
+__license__ = "GNU GPLv3"
+__version__ = "1.0.0"
+__maintainer__ = "Brian Delhaisse"
+__email__ = "briandelhaisse@gmail.com"
+__status__ = "Development"
+
+
+class ConstraintFromTask(EqualityConstraint):
+    r"""Equality Constraint from Task
+
+    A soft task :math:`||Ax - b||^2` is transformed to an equality constraint specified by :math:`Fx = k`, where
+    :math:`F = A` and :math:`k = b`. This allows a priori for the task to not be violated, however this might results
+    in an impossible task to solve.
+    """
+
+    def __init__(self, task):
+        """
+        Initialize the task.
+
+        Args:
+            task (Task): task.
+        """
+        # set task
+        self.task = task
+
+        # call superclass
+        super(ConstraintFromTask, self).__init__(model=task.model)
+
+        # first update
+        self.update()
+
+    ##############
+    # Properties #
+    ##############
+
+    @property
+    def task(self):
+        """Get the task."""
+        return self._task
+
+    @task.setter
+    def task(self, task):
+        """Set the task."""
+        if not isinstance(task, prl.priorities.tasks.Task):
+            raise TypeError("Expecting the given 'task' to be an instance of `Task`, but instead got: {}".format(task))
+        self._task = task
+
+    ###########
+    # Methods #
+    ###########
+
+    def _update(self):
+        """
+        Update the task by computing the A matrix and b vector that will be used by the task solver.
+        """
+        # update task
+        self.task.update()
+
+        # update A_eq and b_eq
+        self._A_eq = self.task.A
+        self._b_eq = self.task.b

pyrobolearn/priorities/constraints/force/__init__.py

@@ -0,0 +1,2 @@
+
+# cartesian force constraints

pyrobolearn/priorities/constraints/torque/__init__.py

@@ -0,0 +1,6 @@
+
+# joint torque constraints
+
+from .joint_limits import JointLimitsConstraint
+
+from .torque_limits import JointTorqueLimitsConstraint

pyrobolearn/priorities/constraints/torque/joint_limits.py

@@ -10,7 +10,7 @@ References:
 
 import numpy as np
 
-from pyrobolearn.priorities.constraints.constraint import Constraint
+from pyrobolearn.priorities.constraints.constraint import BoundConstraint, JointTorqueConstraint
 
 
 __author__ = "Brian Delhaisse"
@@ -23,10 +23,151 @@ __email__ = "briandelhaisse@gmail.com"
 __status__ = "Development"
 
 
-class JointLimitsConstraint(Constraint):
+class JointLimitsConstraint(BoundConstraint, JointTorqueConstraint):
     r"""Joint Limits constraint.
 
+    This provides bounds/limits on the joint torques:
+
+    .. math::  k_p (q_{lb} - q) - k_d \dot{q} \leq \tau \leq k_p (q_{ub} - q) - k_d \dot{q}
+
+    where :math:`q_{lb}, q_{ub}` are the lower and upper joint position limits, :math:`kp` and :math:`kd` are the
+    position and velocity gains respectively, :math:`q, \dot{q}` are the current joint positions and velocities, and
+    :math:`\tau` are the torques that are being optimized.
+
+    The implementation of this class is inspired by [1] (which is licensed under the LGPLv2).
+
+    References:
+        - [1] "OpenSoT: A whole-body control library for the compliant humanoid robot COMAN", Rocchi et al., 2015
     """
 
-    def __init__(self, model):
+    def __init__(self, model, q_lower_bound=None, q_upper_bound=None, kp=15000., kd=1000.):
+        r"""
+        Initialize the constraint.
+
+        Args:
+            model (ModelInterface): model interface.
+            q_lower_bound (np.array[float[N]], None): joint position lower limits. If None, it will take the lower
+              joint limits specified in the model. Note that if the lower limits are equal to the upper limits, they
+              will be set to -10 and 10 by default.
+            q_upper_bound (np.array[float[N]], None): joint position upper limits. If None, it will take the upper
+              joint limits specified in the model. Note that if the upper limits are equal to the lower limits, they
+              will be set to -10 and 10 by default.
+            kp (float, np.array[float[N]]): position gain(s).
+            kd (float, np.array[float[N]]): velocity gain(s).
+        """
         super(JointLimitsConstraint, self).__init__(model)
+
+        # set gains
+        self.kp = kp
+        self.kd = kd
+
+        # set variables
+        if q_lower_bound is None or q_upper_bound is None:
+            q_lb, q_ub = self.model.get_joint_limits()
+            if q_lower_bound is None:
+                q_lower_bound = q_lb
+            if q_upper_bound is None:
+                q_upper_bound = q_ub
+            if np.allclose(q_lower_bound, q_upper_bound):
+                print("WARNING: the joint position lower and upper limits are the same, by default they will be set "
+                      "to -10 and 10.")
+                q_lower_bound = -10. * np.ones(len(q_lower_bound))
+                q_upper_bound = 10. * np.ones(len(q_upper_bound))
+
+        self.q_lower_bounds = q_lower_bound
+        self.q_upper_bounds = q_upper_bound
+
+        # first update
+        self.update()
+
+    ##############
+    # Properties #
+    ##############
+
+    @property
+    def q_lower_bounds(self):
+        """Get the lower joint position limits."""
+        return self._q_lb
+
+    @q_lower_bounds.setter
+    def q_lower_bounds(self, q_lb):
+        """Set the lower joint position limits."""
+        if q_lb is None:
+            q_lb = self.model.get_joint_limits()[0]
+        if not isinstance(q_lb, np.ndarray):
+            raise TypeError("Expecting the given lower joint position limits to be a np.array, instead got: "
+                            "{}".format(q_lb))
+        if len(q_lb) != self.x_size:
+            raise ValueError("Expecting the length of the lower joint position limits (={}) to be the same length as "
+                             "the number of variables being optimized =({}).".format(len(q_lb), self.x_size))
+        self._q_lb = q_lb
+
+    @property
+    def q_upper_bounds(self):
+        """Get the upper joint position limits."""
+        return self._q_ub
+
+    @q_upper_bounds.setter
+    def q_upper_bounds(self, q_ub):
+        """Set the upper joint position limits."""
+        if q_ub is None:
+            q_ub = self.model.get_joint_limits()[1]
+        if not isinstance(q_ub, np.ndarray):
+            raise TypeError("Expecting the given upper joint position limits to be a np.array, instead got: "
+                            "{}".format(q_ub))
+        if len(q_ub) != self.x_size:
+            raise ValueError("Expecting the length of the upper joint position limits (={}) to be the same length as "
+                             "the number of variables being optimized =({}).".format(len(q_ub), self.x_size))
+        self._q_ub = q_ub
+
+    @property
+    def kp(self):
+        """Return the position gain."""
+        return self._kp
+
+    @kp.setter
+    def kp(self, kp):
+        """Set the position gain."""
+        if not isinstance(kp, (float, int, np.ndarray)):
+            raise TypeError("Expecting the given position gain kp to be an int, float, np.array, instead got: "
+                            "{}".format(type(kp)))
+        if isinstance(kp, np.ndarray) and len(kp) != self.x_size:
+            raise ValueError("Expecting the given position gain matrix kp to be of length {}, but instead got "
+                             "a length of: {}".format(self.x_size, len(kp)))
+        if np.any(kp < 0):
+            raise ValueError("The position gain(s) should all be bigger or equal than 0, but found some negative "
+                             "gains")
+        self._kp = kp
+
+    @property
+    def kd(self):
+        """Return the linear velocity gain."""
+        return self._kd
+
+    @kd.setter
+    def kd(self, kd):
+        """Set the linear velocity gain."""
+        if not isinstance(kd, (float, int, np.ndarray)):
+            raise TypeError("Expecting the given velocity gain kd to be an int, float, np.array, instead got: "
+                            "{}".format(type(kd)))
+        if isinstance(kd, np.ndarray) and len(kd) != self.x_size:
+            raise ValueError("Expecting the given velocity gain matrix kd to be of length {}, but instead got "
+                             "a length of: {}".format(self.x_size, len(kd)))
+        if np.any(kd < 0):
+            raise ValueError("The velocity gain(s) should all be bigger or equal than 0, but found some negative "
+                             "gains")
+        self._kd = kd
+
+    ###########
+    # Methods #
+    ###########
+
+    def _update(self):
+        r"""
+        Update the lower and upper bounds.
+        """
+        q = self.model.get_joint_positions()
+        dq = self.model.get_joint_velocities()
+        self._lower_bound = (self._q_lb - q) - self.kd * dq
+        self._upper_bound = (self._q_ub - q) - self.kd * dq
+

pyrobolearn/priorities/constraints/torque/torque_limits.py

@@ -1,6 +1,17 @@
 #!/usr/bin/env python
 r"""Provide the torque limits constraint.
 
+This provides bounds/limits on the joint torques:
+
+.. math::  \tau_{lb} \leq \tau \leq \tau_{ub}
+
+where :math:`(\tau_{lb}, \tau_{ub})` are the lower and upper bound on the joint torques, and :math:`\tau` are the
+joint torques being optimized.
+
+This formulation can be rewritten as the inequality constraint math:`lb \leq x \leq ub` in QP, with
+:math:`lb = \tau_{lb}`, :math:`ub = \tau_{ub}`, and :math:`x = \tau`. This can also be rewritten as :math:`Gx \leq h`,
+with :math:`G = [-I, I]^\top` and :math:`h = [-\tau_{lb}^\top, \tau_{ub}^\top]^\top` where :math:`I` is the square
+identity matrix.
 
 The implementation of this class is inspired by [1] (which is licensed under the LGPLv2).
 
@@ -10,7 +21,7 @@ References:
 
 import numpy as np
 
-from pyrobolearn.priorities.constraints.constraint import Constraint
+from pyrobolearn.priorities.constraints.constraint import BoundConstraint, JointTorqueConstraint
 
 
 __author__ = "Brian Delhaisse"
@@ -23,10 +34,108 @@ __email__ = "briandelhaisse@gmail.com"
 __status__ = "Development"
 
 
-class TorqueLimitsConstraint(Constraint):
+class JointTorqueLimitsConstraint(BoundConstraint, JointTorqueConstraint):
     r"""Torque Limits constraint.
 
+    This provides bounds/limits on the joint torques:
+
+    .. math::  \tau_{lb} \leq \tau \leq \tau_{ub}
+
+    where :math:`(\tau_{lb}, \tau_{ub})` are the lower and upper bound on the joint torques, and
+    :math:`\tau` are the joint torques being optimized.
+
+    This formulation can be rewritten as the inequality constraint math:`lb \leq x \leq ub` in QP, with
+    :math:`lb = \tau_{lb}`, :math:`ub = \tau_{ub}`, and :math:`x = \tau`. This can also be rewritten as
+    :math:`Gx \leq h`, with :math:`G = [-I, I]^\top` and :math:`h = [-\tau_{lb}^\top, \tau_{ub}^\top]^\top`
+    where :math:`I` is the square identity matrix.
+    
+    The implementation of this class is inspired by [1] (which is licensed under the LGPLv2).
+
+    References:
+        - [1] "OpenSoT: A whole-body control library for the compliant humanoid robot COMAN", Rocchi et al., 2015
     """
 
-    def __init__(self, model):
-        super(TorqueLimitsConstraint, self).__init__(model)
+    def __init__(self, model, torque_lower_bound=None, torque_upper_bound=None):
+        """
+        Initialize the constraint.
+
+        Args:
+            model (ModelInterface): model interface.
+            torque_lower_bound (np.array[float[N]], None): joint velocity lower limits. If None, it will take the lower
+              joint limits specified in the model. Note that if the lower limits are equal to the upper limits, they
+              will be set to -100 and 100 by default.
+            torque_upper_bound (np.array[float[N]], None): joint velocity upper limits. If None, it will take the upper
+              joint limits specified in the model. Note that if the upper limits are equal to the lower limits, they
+              will be set to -100 and 100 by default.
+        """
+        super(JointTorqueLimitsConstraint, self).__init__(model)
+
+        # set variables
+        if torque_lower_bound is None or torque_upper_bound is None:
+            tau_lb, tau_ub = self.model.get_joint_velocity_limits()
+            if torque_lower_bound is None:
+                torque_lower_bound = tau_lb
+            if torque_upper_bound is None:
+                torque_upper_bound = tau_ub
+            if np.allclose(torque_lower_bound, torque_upper_bound):
+                print("WARNING: the joint velocity lower and upper limits are the same, by default they will be set "
+                      "to -10 and 10.")
+                torque_lower_bound = -10. * np.ones(len(torque_lower_bound))
+                torque_upper_bound = 10. * np.ones(len(torque_upper_bound))
+
+        self.torque_lower_bounds = torque_lower_bound
+        self.torque_upper_bounds = torque_upper_bound
+
+        # first update
+        self.update()
+
+    ##############
+    # Properties #
+    ##############
+
+    @property
+    def torque_lower_bounds(self):
+        """Get the lower joint velocity limits."""
+        return self._tau_lb
+
+    @torque_lower_bounds.setter
+    def torque_lower_bounds(self, torque_lb):
+        """Set the lower joint velocity limits."""
+        if torque_lb is None:
+            torque_lb = self.model.get_joint_limits()[0]
+        if not isinstance(torque_lb, np.ndarray):
+            raise TypeError("Expecting the given lower joint velocity limits to be a np.array, instead got: "
+                            "{}".format(torque_lb))
+        if len(torque_lb) != self.x_size:
+            raise ValueError("Expecting the length of the lower joint velocity limits (={}) to be the same length as "
+                             "the number of variables being optimized =({}).".format(len(torque_lb), self.x_size))
+        self._tau_lb = torque_lb
+
+    @property
+    def torque_upper_bounds(self):
+        """Get the upper joint velocity limits."""
+        return self._tau_ub
+
+    @torque_upper_bounds.setter
+    def torque_upper_bounds(self, torque_ub):
+        """Set the upper joint velocity limits."""
+        if torque_ub is None:
+            torque_ub = self.model.get_joint_limits()[1]
+        if not isinstance(torque_ub, np.ndarray):
+            raise TypeError("Expecting the given upper joint velocity limits to be a np.array, instead got: "
+                            "{}".format(torque_ub))
+        if len(torque_ub) != self.x_size:
+            raise ValueError("Expecting the length of the upper joint velocity limits (={}) to be the same length as "
+                             "the number of variables being optimized =({}).".format(len(torque_ub), self.x_size))
+        self._tau_ub = torque_ub
+
+    ###########
+    # Methods #
+    ###########
+
+    def _update(self):
+        r"""
+        Update the lower and upper bounds.
+        """
+        self._lower_bound = self._tau_lb
+        self._upper_bound = self._tau_ub

pyrobolearn/priorities/constraints/velocity/__init__.py

@@ -1,4 +1,6 @@
 
+# joint velocity constraints
+
 from .joint_limits import JointPositionLimitsConstraint
 
 from .velocity_limits import JointVelocityLimitsConstraint

pyrobolearn/priorities/constraints/velocity/cartesian_position.py

@@ -1,6 +1,17 @@
 #!/usr/bin/env python
 r"""Provide the Cartesian Position constraint.
 
+The bilateral inequality cartesian position constraint is given by:
+
+.. math:: x_{lb} \leq x + J(q) \dot{q} * dt \leq x_{ub}
+
+where :math:`x_{lb}, x_{ub}` are the lower and upper bound on the cartesian positions of a given distal link,
+:math:`x` is the current cartesian position of the distal link wrt the base link, :math:`\dot{q}` are the joint
+velocities being optimized, :math:`J(q)` is the Jacobian from the base to the distal link, and :math:`dt` is the
+integration time step.
+
+This formulation can be rewritten as a bilateral inequality constraint :math:`b_l \leq A_{ineq} x \leq b_u` in QP,
+with :math:`x = \dot{q}`, :math:`A_{ineq} = J(q) * dt`, :math:`b_l = (x_{lb} - x)` and :math:`b_u = (x_{ub} - x)`.
 
 The implementation of this class is inspired by [1] (which is licensed under the LGPLv2).
 
@@ -10,7 +21,7 @@ References:
 
 import numpy as np
 
-from pyrobolearn.priorities.constraints.constraint import Constraint
+from pyrobolearn.priorities.constraints.constraint import BilateralConstraint, JointVelocityConstraint
 
 
 __author__ = "Brian Delhaisse"
@@ -23,10 +34,128 @@ __email__ = "briandelhaisse@gmail.com"
 __status__ = "Development"
 
 
-class CartesianPositionConstraint(Constraint):
+class CartesianPositionConstraint(BilateralConstraint, JointVelocityConstraint):
     r"""Cartesian Position constraint.
 
+    The bilateral inequality cartesian position constraint is given by:
+
+    .. math:: x_{lb} \leq x + J(q) \dot{q} * dt \leq x_{ub}
+
+    where :math:`x_{lb}, x_{ub}` are the lower and upper bound on the cartesian positions of a given distal link,
+    :math:`x` is the current cartesian position of the distal link wrt the base link, :math:`\dot{q}` are the joint
+    velocities being optimized, :math:`J(q)` is the Jacobian from the base to the distal link, and :math:`dt` is the
+    integration time step.
+
+    This formulation can be rewritten as a bilateral inequality constraint :math:`b_l \leq A_{ineq} x \leq b_u` in QP,
+    with :math:`x = \dot{q}`, :math:`A_{ineq} = J(q) * dt`, :math:`b_l = (x_{lb} - x)` and :math:`b_u = (x_{ub} - x)`.
+
+    The implementation of this class is inspired by [1] (which is licensed under the LGPLv2).
+
+    References:
+        - [1] "OpenSoT: A whole-body control library for the compliant humanoid robot COMAN", Rocchi et al., 2015
     """
 
-    def __init__(self, model):
+    def __init__(self, model, dt, distal_link, base_link=None, local_position=(0, 0, 0), x_lower_bound=None,
+                 x_upper_bound=None):
+        r"""
+        Initialize the constraint.
+
+        Args:
+            model (ModelInterface): model interface.
+            dt (float): integration time step: use to compute :math:`x = x + v dt`.
+            distal_link (int, str): distal link id or name.
+            base_link (int, str, None): base link id or name. If None, it will be the world.
+            local_position (np.array[float[3]]): local position on the distal link.
+            x_lower_bound (np.array[float[3]], None): the lower bound on the cartesian position of the distal link wrt
+              the base link. If None, it will not be considered.
+            x_upper_bound (np.array[float[3]], None): the upper bound on the cartesian position of the distal link wrt
+              the base link. If None, it will not be considered.
+        """
         super(CartesianPositionConstraint, self).__init__(model)
+
+        # set time
+        self.dt = dt
+
+        # define variables
+        self.distal_link = self.model.get_link_id(distal_link)
+        self.base_link = self.model.get_link_id(base_link) if base_link is not None else base_link
+        self.local_position = local_position
+
+        self.position_lower_bound = x_lower_bound
+        self.position_upper_bound = x_upper_bound
+
+        # first update
+        self.update()
+
+    ##############
+    # Properties #
+    ##############
+
+    @property
+    def dt(self):
+        """Return the integration time step."""
+        return self._dt
+
+    @dt.setter
+    def dt(self, dt):
+        """Set the integration time step."""
+        if not isinstance(dt, (float, int)):
+            raise TypeError("Expecting the integration time step `dt` to be a float or int.")
+        if dt <= 0:
+            raise ValueError("Expecting the integration time step `dt` to be bigger than 0.")
+        self._dt = float(dt)
+
+    @property
+    def position_lower_bound(self):
+        """Get the lower bound on the cartesian position of the distal link wrt the base link."""
+        return self._x_lower_bound
+
+    @position_lower_bound.setter
+    def position_lower_bound(self, bound):
+        """Set the lower bound on the cartesian position of the distal link wrt the base link."""
+        if bound is not None:
+            if not isinstance(bound, (list, tuple, np.ndarray)):
+                raise TypeError("Expecting the given lower cartesian position bound to be a np.array, instead got: "
+                                "{}".format(bound))
+            bound = np.asarray(bound)
+            if len(bound) != 3:
+                raise ValueError("Expecting the length of the lower bound to be 3, but got instead a length of: "
+                                 "{}.".format(len(bound)))
+        self._x_lower_bound = bound
+
+    @property
+    def position_upper_bound(self):
+        """Get the upper bound on the cartesian position of the distal link wrt the base link."""
+        return self._x_upper_bound
+
+    @position_upper_bound.setter
+    def position_upper_bound(self, bound):
+        """Set the upper bound on the cartesian position of the distal link wrt the base link."""
+        if bound is not None:
+            if not isinstance(bound, (list, tuple, np.ndarray)):
+                raise TypeError("Expecting the given upper cartesian position bound to be a np.array, instead got: "
+                                "{}".format(bound))
+            bound = np.asarray(bound)
+            if len(bound) != 3:
+                raise ValueError("Expecting the length of the upper bound to be 3, but got instead a length of: "
+                                 "{}.".format(len(bound)))
+        self._x_upper_bound = bound
+
+    ###########
+    # Methods #
+    ###########
+
+    def _update(self):
+        """Update the inequality matrix and vectors."""
+        if self._x_lower_bound is None and self._x_upper_bound is None:
+            raise ValueError("Expecting at least the lower or upper bounds of the cartesian position to be "
+                             "specified, but instead got None for both of them.")
+
+        self._A_ineq = self.model.get_jacobian(link=self.distal_link, wrt_link=self.base_link,
+                                               point=self.local_position)[:3] * self.dt
+        x = self.model.get_position(link=self.distal_link, wrt_link=self.base_link)
+
+        if self._x_lower_bound is not None:
+            self._b_lower_bound = self._x_lower_bound - x
+        if self._x_upper_bound is not None:
+            self._b_upper_bound = self._x_upper_bound - x

pyrobolearn/priorities/constraints/velocity/cartesian_velocity.py

@@ -1,6 +1,17 @@
 #!/usr/bin/env python
 r"""Provide the cartesian velocity constraint.
 
+The bilateral inequality cartesian velocity constraint is given by:
+
+.. math:: v_{lb} \leq J(q) \dot{q} \leq v_{ub}
+
+where :math:`v_{lb}, v_{ub}` are the lower and upper bound on the cartesian velocities of a given distal link,
+:math:`\dot{q}` are the joint velocities being optimized, and :math:`J(q)` is the Jacobian from the base to the
+distal link.
+
+This formulation can be rewritten as a bilateral inequality constraint :math:`b_l \leq A_{ineq} x \leq b_u` in QP,
+with :math:`x = \dot{q}`, :math:`A_{ineq} = J(q)`, :math:`b_l = v_{lb}` and :math:`b_u = v_{ub}`.
+
 
 The implementation of this class is inspired by [1] (which is licensed under the LGPLv2).
 
@@ -10,7 +21,7 @@ References:
 
 import numpy as np
 
-from pyrobolearn.priorities.constraints.constraint import Constraint
+from pyrobolearn.priorities.constraints.constraint import BilateralConstraint, JointVelocityConstraint
 
 
 __author__ = "Brian Delhaisse"
@@ -23,10 +34,141 @@ __email__ = "briandelhaisse@gmail.com"
 __status__ = "Development"
 
 
-class CartesianVelocityConstraint(Constraint):
+class CartesianVelocityConstraint(BilateralConstraint, JointVelocityConstraint):
     r"""Cartesian Velocity constraint.
 
+    The bilateral inequality cartesian velocity constraint is given by:
+
+    .. math:: v_{lb} \leq J(q) \dot{q} \leq v_{ub}
+
+    where :math:`v_{lb}, v_{ub}` are the lower and upper bound on the cartesian velocities of a given distal link,
+    :math:`\dot{q}` are the joint velocities being optimized, and :math:`J(q)` is the Jacobian from the base to the
+    distal link.
+
+    This formulation can be rewritten as a bilateral inequality constraint :math:`b_l \leq A_{ineq} x \leq b_u` in QP,
+    with :math:`x = \dot{q}`, :math:`A_{ineq} = J(q)`, :math:`b_l = v_{lb}` and :math:`b_u = v_{ub}`.
+
+    The implementation of this class is inspired by [1] (which is licensed under the LGPLv2).
+
+    References:
+        - [1] "OpenSoT: A whole-body control library for the compliant humanoid robot COMAN", Rocchi et al., 2015
     """
 
-    def __init__(self, model):
+    def __init__(self, model, distal_link, base_link=None, local_position=(0, 0, 0), linear_velocity_bounds=None,
+                 angular_velocity_bounds=None):
+        r"""
+        Initialize the constraint.
+
+        Args:
+            model (ModelInterface): model interface.
+            distal_link (int, str): distal link id or name.
+            base_link (int, str, None): base link id or name. If None, it will be the world.
+            local_position (np.array[float[3]]): local position on the distal link.
+            linear_velocity_bounds (tuple[2 * np.array[float[3]]], np.array[float[3]], None): If tuple, it is the
+              lower and upper bounds on the linear velocity. If np.array, then the lower and upper bound would be set
+              to (-linear_velocity_bounds, linear_velocity_bounds). If None, it will not be considered.
+            angular_velocity_bounds (tuple[2 * np.array[float[3]]], np.array[float[3]], None): If tuple, it is the lower and
+              upper bounds on the angular velocity. If np.array, then the lower and upper bound would be set to
+              (-angular_velocity_bounds, angular_velocity_bounds). If None, it will not be considered.
+        """
         super(CartesianVelocityConstraint, self).__init__(model)
+
+        # define variables
+        self.distal_link = self.model.get_link_id(distal_link)
+        self.base_link = self.model.get_link_id(base_link) if base_link is not None else base_link
+        self.local_position = local_position
+        
+        self.linear_velocity_bounds = linear_velocity_bounds
+        self.angular_velocity_bounds = angular_velocity_bounds
+        
+        # first update
+        self.update()
+
+    ##############
+    # Properties #
+    ##############
+
+    @property
+    def linear_velocity_bounds(self):
+        """Get the cartesian linear velocity bounds of the distal link wrt the base."""
+        return self._lin_vel_bounds
+
+    @linear_velocity_bounds.setter
+    def linear_velocity_bounds(self, bounds):
+        """Set the cartesian linear velocity bounds of the distal link wrt the base."""
+        if bounds is not None:
+            if isinstance(bounds, tuple):
+                if len(bounds) != 2:
+                    raise ValueError("Expecting the bounds to be a tuple of length 2, but got a length of "
+                                     "{}".format(len(bounds)))
+                for bound in bounds:
+                    if not isinstance(bound, np.ndarray):
+                        raise TypeError("Expecting the given bound to be a np.array, but got instead: "
+                                        "{}".format(type(bound)))
+                    if len(bound) != 3:
+                        raise ValueError("Expecting the given bound to be of length 3, but instead got a length of "
+                                         "{}".format(len(bound)))
+            elif isinstance(bounds, np.ndarray):
+                if len(bounds) == 3:
+                    bounds = (-bounds, bounds)
+                elif len(bounds) == 6:
+                    bounds = (bounds[:3], bounds[3:])
+            else:
+                raise TypeError("Expecting the given bounds to be a tuple of np.array, a np.array, or None, but "
+                                "instead got: {}".format(type(bounds)))
+        self._lin_vel_bounds = bounds
+
+    @property
+    def angular_velocity_bounds(self):
+        """Get the cartesian angular velocity bounds of the distal link wrt the base."""
+        return self._ang_vel_bounds
+
+    @angular_velocity_bounds.setter
+    def angular_velocity_bounds(self, bounds):
+        """Set the cartesian angular velocity bounds of the distal link wrt the base."""
+        if bounds is not None:
+            if isinstance(bounds, tuple):
+                if len(bounds) != 2:
+                    raise ValueError("Expecting the bounds to be a tuple of length 2, but got a length of "
+                                     "{}".format(len(bounds)))
+                for bound in bounds:
+                    if not isinstance(bound, np.ndarray):
+                        raise TypeError("Expecting the given bound to be a np.array, but got instead: "
+                                        "{}".format(type(bound)))
+                    if len(bound) != 3:
+                        raise ValueError("Expecting the given bound to be of length 3, but instead got a length of "
+                                         "{}".format(len(bound)))
+            elif isinstance(bounds, np.ndarray):
+                if len(bounds) == 3:
+                    bounds = (-bounds, bounds)
+                elif len(bounds) == 6:
+                    bounds = (bounds[:3], bounds[3:])
+            else:
+                raise TypeError("Expecting the given bounds to be a tuple of np.array, a np.array, or None, but "
+                                "instead got: {}".format(type(bounds)))
+        self._ang_vel_bounds = bounds
+
+    ###########
+    # Methods #
+    ###########
+
+    def _update(self):
+        """Update the inequality matrix and vectors."""
+        if self.linear_velocity_bounds is None and self.angular_velocity_bounds is None:
+            raise ValueError("Expecting at least the linear or angular velocity bounds to be specified, but instead "
+                             "got None for both of them.")
+
+        self._A_ineq = self.model.get_jacobian(link=self.distal_link, wrt_link=self.base_link,
+                                               point=self.local_position)
+
+        if self.linear_velocity_bounds is None:
+            self._b_lower_bound, self._b_upper_bound = self.angular_velocity_bounds
+            self._A_ineq = self._A_ineq[3:]
+        elif self.angular_velocity_bounds is None:
+            self._b_lower_bound, self._b_upper_bound = self.linear_velocity_bounds
+            self._A_ineq = self._A_ineq[:3]
+        else:
+            b_lin_low, b_lin_up = self.linear_velocity_bounds
+            b_ang_low, b_ang_up = self.angular_velocity_bounds
+            self._b_lower_bound = np.concatenate((b_lin_low, b_ang_low))
+            self._b_upper_bound = np.concatenate((b_lin_up, b_ang_up))

pyrobolearn/priorities/constraints/velocity/joint_limits.py

@@ -1,6 +1,18 @@
 #!/usr/bin/env python
 r"""Provide the joint position limits constraint.
 
+This provides bounds/limits on the joint positions, which are given by:
+
+.. math:: q_{lb} \leq q + \dot{q} dt \leq q_{ub}
+
+where :math:`(q_{lb}, q_{ub})` are the lower and upper bound of the joint positions respectively, :math:`q` are
+the current joint positions, :math:`\dot{q}` are the joint velocities being optimized, and :math:`dt` is the
+integration time step.
+
+This formulation can be rewritten as the inequality constraint math:`lb \leq x \leq ub` in QP, with
+:math:`lb = (q_{lb} - q) / dt`, :math:`ub = (q_{ub} - q) / dt`, and :math:`x = \dot{q}`. This can
+also be rewritten as :math:`Gx \leq h`, with :math:`G = [-dt*I, dt*I]^\top` and
+:math:`h = [(q - q_{lb})^\top, (q_{ub} - q)^\top]^\top` where :math:`I` is the square identity matrix.
 
 The implementation of this class is inspired by [1] (which is licensed under the LGPLv2).
 
@@ -34,31 +46,117 @@ class JointPositionLimitsConstraint(BoundConstraint, JointVelocityConstraint):
     the current joint positions, :math:`\dot{q}` are the joint velocities being optimized, and :math:`dt` is the
     integration time step.
 
-    This formulation can be rewritten as the inequality constraint :math:`Gx \leq h` used in QP, with
-    :math:`G = [-dt*I, dt*I]^\top` and :math:`h = [(q - q_{lb})^\top, (q_{ub} - q)^\top]^\top` where :math:`I` is the
-    square identity matrix.
+    This formulation can be rewritten as the inequality constraint math:`lb \leq x \leq ub` in QP, with
+    :math:`lb = (q_{lb} - q) / dt`, :math:`ub = (q_{ub} - q) / dt`, and :math:`x = \dot{q}`. This can
+    also be rewritten as :math:`Gx \leq h`, with :math:`G = [-dt*I, dt*I]^\top` and
+    :math:`h = [(q - q_{lb})^\top, (q_{ub} - q)^\top]^\top` where :math:`I` is the square identity matrix.
+
+    The implementation of this class is inspired by [1] (which is licensed under the LGPLv2).
+
+    References:
+        - [1] "OpenSoT: A whole-body control library for the compliant humanoid robot COMAN", Rocchi et al., 2015
     """
 
-    def __init__(self, model, dt):
+    def __init__(self, model, dt, q_lower_bound=None, q_upper_bound=None):
         r"""
         Initialize the constraint.
 
         Args:
             model (ModelInterface): model interface.
             dt (float): integration time step: use to compute :math:`q = q + \dot{q} dt`.
+            q_lower_bound (np.array[float[N]], None): joint position lower limits. If None, it will take the lower
+              joint limits specified in the model. Note that if the lower limits are equal to the upper limits, they
+              will be set to -10 and 10 by default.
+            q_upper_bound (np.array[float[N]], None): joint position upper limits. If None, it will take the upper
+              joint limits specified in the model. Note that if the upper limits are equal to the lower limits, they
+              will be set to -10 and 10 by default.
         """
         super(JointPositionLimitsConstraint, self).__init__(model)
 
+        # set time
         self.dt = dt
 
-        bounds = self.model.get_joint_bounds()
-        self._lower_bound = bounds[0]
-        self._upper_bound = bounds[1]
+        # set variables
+        if q_lower_bound is None or q_upper_bound is None:
+            q_lb, q_ub = self.model.get_joint_limits()
+            if q_lower_bound is None:
+                q_lower_bound = q_lb
+            if q_upper_bound is None:
+                q_upper_bound = q_ub
+            if np.allclose(q_lower_bound, q_upper_bound):
+                print("WARNING: the joint position lower and upper limits are the same, by default they will be set "
+                      "to -10 and 10.")
+                q_lower_bound = -10. * np.ones(len(q_lower_bound))
+                q_upper_bound = 10. * np.ones(len(q_upper_bound))
 
-    def update(self):
+        self.q_lower_bounds = q_lower_bound
+        self.q_upper_bounds = q_upper_bound
+
+        # first update
+        self.update()
+
+    ##############
+    # Properties #
+    ##############
+
+    @property
+    def dt(self):
+        """Return the integration time step."""
+        return self._dt
+
+    @dt.setter
+    def dt(self, dt):
+        """Set the integration time step."""
+        if not isinstance(dt, (float, int)):
+            raise TypeError("Expecting the integration time step `dt` to be a float or int.")
+        if dt <= 0:
+            raise ValueError("Expecting the integration time step `dt` to be bigger than 0.")
+        self._dt = float(dt)
+
+    @property
+    def q_lower_bounds(self):
+        """Get the lower joint position limits."""
+        return self._q_lb
+
+    @q_lower_bounds.setter
+    def q_lower_bounds(self, q_lb):
+        """Set the lower joint position limits."""
+        if q_lb is None:
+            q_lb = self.model.get_joint_limits()[0]
+        if not isinstance(q_lb, np.ndarray):
+            raise TypeError("Expecting the given lower joint position limits to be a np.array, instead got: "
+                            "{}".format(q_lb))
+        if len(q_lb) != self.x_size:
+            raise ValueError("Expecting the length of the lower joint position limits (={}) to be the same length as "
+                             "the number of variables being optimized =({}).".format(len(q_lb), self.x_size))
+        self._q_lb = q_lb
+
+    @property
+    def q_upper_bounds(self):
+        """Get the upper joint position limits."""
+        return self._q_ub
+
+    @q_upper_bounds.setter
+    def q_upper_bounds(self, q_ub):
+        """Set the upper joint position limits."""
+        if q_ub is None:
+            q_ub = self.model.get_joint_limits()[1]
+        if not isinstance(q_ub, np.ndarray):
+            raise TypeError("Expecting the given upper joint position limits to be a np.array, instead got: "
+                            "{}".format(q_ub))
+        if len(q_ub) != self.x_size:
+            raise ValueError("Expecting the length of the upper joint position limits (={}) to be the same length as "
+                             "the number of variables being optimized =({}).".format(len(q_ub), self.x_size))
+        self._q_ub = q_ub
+
+    ###########
+    # Methods #
+    ###########
+
+    def _update(self):
         r"""
-        Update the bounds.
+        Update the lower and upper bounds.
         """
         q = self.model.get_joint_positions()
-        self.lower_bound = 0
-        self.upper_bound = 0
+        self._lower_bound = (self._q_lb - q) / self.dt
+        self._upper_bound = (self._q_ub - q) / self.dt

pyrobolearn/priorities/constraints/velocity/joint_velocity.py

@@ -1,6 +1,19 @@
 #!/usr/bin/env python
 r"""Provide the differential kinematics constraint.
 
+This provides the joint velocity constraints which is given by:
+
+.. math:: J(q) \dot{q} = v
+
+where :math:`J(q)` is the jacobian from a base link to a distal link, :math:`\dot{q}` are the joint velocities
+being optimized, and :math:`v` is the imposed cartesian velocity imposed on the distal link.
+
+This formulation can be rewritten as the inequality constraint :math:`A_{eq} x = b_{eq}` used in QP, with
+:math:`A_{eq} = J(q)`, :math:`x = \dot{q}`, and :math:`b_{eq} = v`.
+
+
+The implementation of this class is inspired by [1] (which is licensed under the LGPLv2).
+
 References:
     - [1] "OpenSoT: A whole-body control library for the compliant humanoid robot COMAN", Rocchi et al., 2015
 """
@@ -32,9 +45,15 @@ class DifferentialKinematicsConstraint(EqualityConstraint, JointVelocityConstrai
 
     This formulation can be rewritten as the inequality constraint :math:`A_{eq} x = b_{eq}` used in QP, with
     :math:`A_{eq} = J(q)`, :math:`x = \dot{q}`, and :math:`b_{eq} = v`.
+
+    The implementation of this class is inspired by [1] (which is licensed under the LGPLv2).
+
+    References:
+        - [1] "OpenSoT: A whole-body control library for the compliant humanoid robot COMAN", Rocchi et al., 2015
     """
 
-    def __init__(self, model, distal_link, base_link=None, velocity=None):
+    def __init__(self, model, distal_link, base_link=None, local_position=(0., 0., 0.), linear_velocity=None,
+                 angular_velocity=None):
         r"""
         Initialize the constraint.
 
@@ -42,13 +61,82 @@ class DifferentialKinematicsConstraint(EqualityConstraint, JointVelocityConstrai
             model (ModelInterface): model interface.
             distal_link (int, str): distal link id or name.
             base_link (int, str, None): base link id or name. If None, it will be the world.
-            velocity (np.array[6], None): imposed velocity. If None, it will be set to 0.
+            local_position (np.array[float[3]]): local position on the distal link.
+            linear_velocity (np.array[float[3]], None): imposed linear velocity. If None, it will not be considered.
+            angular_velocity (np.array[float[3]], None): imposed angular velocity. If None, it will not be considered.
         """
         super(DifferentialKinematicsConstraint, self).__init__(model)
-        raise NotImplementedError
 
-    def update(self):
+        # define variables
+        self.distal_link = self.model.get_link_id(distal_link)
+        self.base_link = self.model.get_link_id(base_link) if base_link is not None else base_link
+        self.local_position = local_position
+
+        # set velocities
+        self.linear_velocity = linear_velocity
+        self.angular_velocity = angular_velocity
+
+        # first update
+        self.update()
+        
+    ##############
+    # Properties #
+    ##############
+
+    @property
+    def linear_velocity(self):
+        """Get the cartesian linear velocity of the distal link wrt the base."""
+        return self._lin_vel
+
+    @linear_velocity.setter
+    def linear_velocity(self, velocity):
+        """Set the cartesian linear velocity of the distal link wrt the base."""
+        if velocity is not None:
+            if not isinstance(velocity, (np.ndarray, list, tuple)):
+                raise TypeError("Expecting the given linear velocity to be a np.array, instead got: "
+                                "{}".format(type(velocity)))
+            velocity = np.asarray(velocity)
+            if len(velocity) != 3:
+                raise ValueError("Expecting the given linear velocity array to be of length 3, but instead "
+                                 "got: {}".format(len(velocity)))
+        self._lin_vel = velocity
+
+    @property
+    def angular_velocity(self):
+        """Get the cartesian angular velocity of the distal link wrt the base."""
+        return self._ang_vel
+
+    @angular_velocity.setter
+    def angular_velocity(self, velocity):
+        """Set the cartesian angular velocity of the distal link wrt the base."""
+        if velocity is not None:
+            if not isinstance(velocity, (np.ndarray, list, tuple)):
+                raise TypeError("Expecting the given angular velocity to be a np.array, instead got: "
+                                "{}".format(type(velocity)))
+            velocity = np.asarray(velocity)
+            if len(velocity) != 3:
+                raise ValueError("Expecting the given angular velocity array to be of length 3, but instead "
+                                 "got: {}".format(len(velocity)))
+        self._ang_vel = velocity
+    
+    ###########
+    # Methods #
+    ###########
+
+    def _update(self):
         r"""
-        Update the bounds.
+        Update the constraint by computing :math:`A_{eq}` and :math:`b_{eq}`.
         """
-        raise NotImplementedError
+        if self._lin_vel is None and self._ang_vel is None:
+            raise ValueError("Expecting at least the linear or angular velocity to be specified, but none are "
+                             "provided.")
+        self._A_eq = self.model.get_jacobian(link=self.distal_link, wrt_link=self.base_link, point=self.local_position)
+
+        if self._lin_vel is None:
+            self._A_eq = self._A_eq[3:]
+            self._b_eq = self._ang_vel
+        elif self._ang_vel is None:
+            self._A_eq = self._A_eq[:3]
+            self._b_eq = self._lin_vel
+        else:
+            self._b_eq = np.concatenate((self._lin_vel, self._ang_vel))

pyrobolearn/priorities/constraints/velocity/velocity_limits.py

@@ -1,6 +1,17 @@
 #!/usr/bin/env python
 r"""Provide the velocity limits constraint.
 
+This provides bounds/limits on the joint velocities
+
+.. math::  \dot{q}_{lb} \leq \dot{q} \leq \dot{q}_{ub}
+
+where :math:`(\dot{q}_{lb}, \dot{q}_{ub})` are the lower and upper bound on the joint velocities, and
+:math:`\dot{q}` are the joint velocities being optimized.
+
+This formulation can be rewritten as the inequality constraint math:`lb \leq x \leq ub` in QP, with
+:math:`lb = \dot{q}_{lb}`, :math:`ub = \dot{q}_{ub}`, and :math:`x = \dot{q}`. This can also be rewritten as
+:math:`Gx \leq h`, with :math:`G = [-I, I]^\top` and :math:`h = [-\dot{q}_{lb}^\top, \dot{q}_{ub}^\top]^\top`
+where :math:`I` is the square identity matrix.
 
 The implementation of this class is inspired by [1] (which is licensed under the LGPLv2).
 
@@ -26,30 +37,105 @@ __status__ = "Development"
 class JointVelocityLimitsConstraint(BoundConstraint, JointVelocityConstraint):
     r"""Joint velocity limits constraint.
 
-    This provides bounds/limits on the joint velocities
+    This provides bounds/limits on the joint velocities:
 
     .. math::  \dot{q}_{lb} \leq \dot{q} \leq \dot{q}_{ub}
 
     where :math:`(\dot{q}_{lb}, \dot{q}_{ub})` are the lower and upper bound on the joint velocities, and
     :math:`\dot{q}` are the joint velocities being optimized.
 
-    This formulation can be rewritten as the inequality constraint :math:`Gx \leq h` used in QP, with
-    :math:`G = [-I, I]^\top` and :math:`h = [-q_{lb}^\top, q_{ub}^\top]^\top` where :math:`I` is the square identity
-    matrix.
+    This formulation can be rewritten as the inequality constraint math:`lb \leq x \leq ub` in QP, with
+    :math:`lb = \dot{q}_{lb}`, :math:`ub = \dot{q}_{ub}`, and :math:`x = \dot{q}`. This can also be rewritten as
+    :math:`Gx \leq h`, with :math:`G = [-I, I]^\top` and :math:`h = [-\dot{q}_{lb}^\top, \dot{q}_{ub}^\top]^\top`
+    where :math:`I` is the square identity matrix.
+
+    The implementation of this class is inspired by [1] (which is licensed under the LGPLv2).
+
+    References:
+        - [1] "OpenSoT: A whole-body control library for the compliant humanoid robot COMAN", Rocchi et al., 2015
     """
 
-    def __init__(self, model):
+    def __init__(self, model, dq_lower_bound=None, dq_upper_bound=None):
         """
         Initialize the constraint.
 
         Args:
             model (ModelInterface): model interface.
+            dq_lower_bound (np.array[float[N]], None): joint velocity lower limits. If None, it will take the lower
+              joint limits specified in the model. Note that if the lower limits are equal to the upper limits, they
+              will be set to -10 and 10 by default.
+            dq_upper_bound (np.array[float[N]], None): joint velocity upper limits. If None, it will take the upper
+              joint limits specified in the model. Note that if the upper limits are equal to the lower limits, they
+              will be set to -10 and 10 by default.
         """
         super(JointVelocityLimitsConstraint, self).__init__(model)
 
-        bounds = self.model.get_joint_velocity_bounds()
-        self.lower_bound = bounds[0]
-        self.upper_bound = bounds[1]
+        # set variables
+        if dq_lower_bound is None or dq_upper_bound is None:
+            dq_lb, dq_ub = self.model.get_joint_velocity_limits()
+            if dq_lower_bound is None:
+                dq_lower_bound = dq_lb
+            if dq_upper_bound is None:
+                dq_upper_bound = dq_ub
+            if np.allclose(dq_lower_bound, dq_upper_bound):
+                print("WARNING: the joint velocity lower and upper limits are the same, by default they will be set "
+                      "to -10 and 10.")
+                dq_lower_bound = -10. * np.ones(len(dq_lower_bound))
+                dq_upper_bound = 10. * np.ones(len(dq_upper_bound))
 
-    def update(self):
-        pass
+        self.dq_lower_bounds = dq_lower_bound
+        self.dq_upper_bounds = dq_upper_bound
+
+        # first update
+        self.update()
+
+    ##############
+    # Properties #
+    ##############
+
+    @property
+    def dq_lower_bounds(self):
+        """Get the lower joint velocity limits."""
+        return self._dq_lb
+
+    @dq_lower_bounds.setter
+    def dq_lower_bounds(self, dq_lb):
+        """Set the lower joint velocity limits."""
+        if dq_lb is None:
+            dq_lb = self.model.get_joint_limits()[0]
+        if not isinstance(dq_lb, np.ndarray):
+            raise TypeError("Expecting the given lower joint velocity limits to be a np.array, instead got: "
+                            "{}".format(dq_lb))
+        if len(dq_lb) != self.x_size:
+            raise ValueError("Expecting the length of the lower joint velocity limits (={}) to be the same length as "
+                             "the number of variables being optimized =({}).".format(len(dq_lb), self.x_size))
+        self._dq_lb = dq_lb
+
+    @property
+    def dq_upper_bounds(self):
+        """Get the upper joint velocity limits."""
+        return self._dq_ub
+
+    @dq_upper_bounds.setter
+    def dq_upper_bounds(self, dq_ub):
+        """Set the upper joint velocity limits."""
+        if dq_ub is None:
+            dq_ub = self.model.get_joint_limits()[1]
+        if not isinstance(dq_ub, np.ndarray):
+            raise TypeError("Expecting the given upper joint velocity limits to be a np.array, instead got: "
+                            "{}".format(dq_ub))
+        if len(dq_ub) != self.x_size:
+            raise ValueError("Expecting the length of the upper joint velocity limits (={}) to be the same length as "
+                             "the number of variables being optimized =({}).".format(len(dq_ub), self.x_size))
+        self._dq_ub = dq_ub
+
+    ###########
+    # Methods #
+    ###########
+
+    def _update(self):
+        r"""
+        Update the lower and upper bounds.
+        """
+        self._lower_bound = self._dq_lb
+        self._upper_bound = self._dq_ub

pyrobolearn/priorities/models/model.py

@@ -119,6 +119,26 @@ class ModelInterface(object):
         """
         pass
 
+    def get_joint_limits(self):
+        r"""
+        Return the joint limits.
+
+        Returns:
+            np.array[float[N]]: lower joint position limits.
+            np.array[float[N]]: upper joint position limits.
+        """
+        pass
+
+    def get_joint_velocity_limits(self):
+        r"""
+        Return the joint velocity limits.
+
+        Returns:
+            np.array[float[N]]: lower joint velocity limits.
+            np.array[float[N]]: upper joint velocity limits.
+        """
+        pass
+
     def get_joint_positions(self):
         """
         Get the joint positions.
@@ -239,18 +259,49 @@ class ModelInterface(object):
         """
         pass
 
-    def get_pose(self, link):
+    def get_pose(self, link, wrt_link=None, point=(0., 0., 0.)):
         """
-        Return the pose of the specified link.
+        Return the pose of the specified link with respect to another link.
 
         Args:
             link (int, str): unique link id, or name.
+            wrt_link (int, str, None): the other link id, or name. If None, returns the pose wrt to the world, and
+              if -1 wrt to the base.
+            point (np.array[float[3]]): position of the point in link's local frame.
 
         Returns:
             np.array[float[7]]: pose (position and quaternion expressed as [x,y,z,w])
         """
         pass
 
+    def get_position(self, link, wrt_link=None):
+        """
+        Return the position of the specified link with respect to another link.
+
+        Args:
+            link (int, str): unique link id, or name.
+            wrt_link (int, str, None): the other link id, or name. If None, returns the position wrt to the world,
+              and if -1 wrt to the base.
+
+        Returns:
+            np.array[float[3]]: position
+        """
+        pass
+
+    def get_orientation(self, link, wrt_link=None):
+        """
+        Return the orientation of the specified link with respect to another link.
+
+        Args:
+            link (int, str): unique link id, or name.
+            wrt_link (int, str, None): the other link id, or name. If None, returns the orientation wrt to the world,
+              and if -1 wrt to the base.
+
+        Returns:
+            np.array[float[4]]: orientation (expressed as a quaternion [x,y,z,w])
+        """
+        pass
+
     def get_velocity_twist(self, link):
         r"""
         Compute the angular and linear velocity of a link, given by :math:`v = [\omega, \dot{p}]`.

pyrobolearn/priorities/models/robot_model.py

@@ -68,12 +68,12 @@ class RobotModelInterface(ModelInterface):
         - If fixed base, this is equal to the number of actuated joints.
         - If floating base, this is equal to the number of actuated joints + 6 DoFs for the base.
         """
-        return self.robot.num_dofs
+        return self.model.num_dofs
     
     @property
     def num_actuated_joints(self):
         """Return the number of actuated joints."""
-        return self.robot.num_actuated_joints
+        return self.model.num_actuated_joints
 
     ###########
     # Methods #
@@ -120,6 +120,27 @@ class RobotModelInterface(ModelInterface):
         """
         return -1
 
+    def get_joint_limits(self):
+        r"""
+        Return the joint limits.
+
+        Returns:
+            np.array[float[N]]: lower joint position limits.
+            np.array[float[N]]: upper joint position limits.
+        """
+        return self.model.get_joint_limits()
+    
+    def get_joint_velocity_limits(self):
+        r"""
+        Return the joint velocity limits.
+
+        Returns:
+            np.array[float[N]]: lower joint velocity limits.
+            np.array[float[N]]: upper joint velocity limits.
+        """
+        dq = self.model.get_joint_max_velocities()
+        return -dq, dq
+
     def get_joint_positions(self):
         """
         Get the joint positions.
@@ -127,7 +148,7 @@ class RobotModelInterface(ModelInterface):
         Returns:
             np.array[float[N]]: the joint positions.
         """
-        return self.robot.get_joint_positions()
+        return self.model.get_joint_positions()
 
     def get_joint_velocities(self):
         """
@@ -136,7 +157,7 @@ class RobotModelInterface(ModelInterface):
         Returns:
             np.array[float[N]]: the joint positions.
         """
-        return self.robot.get_joint_velocities()
+        return self.model.get_joint_velocities()
 
     def get_joint_accelerations(self):
         """
@@ -145,7 +166,7 @@ class RobotModelInterface(ModelInterface):
         Returns:
             np.array[float[N]]: the joint positions.
         """
-        return self.robot.get_joint_accelerations()
+        return self.model.get_joint_accelerations()
 
     def get_com_position(self):
         """
@@ -266,11 +287,11 @@ class RobotModelInterface(ModelInterface):
 
     def get_pose(self, link, wrt_link=None, point=(0., 0., 0.)):  # TODO: use point
         """
-        Return the pose of the specified link.
+        Return the pose of the specified link with respect to another link.
 
         Args:
             link (int, str): unique link id, or name.
-            wrt_link (int, str, None): the other link id, or name. If None, returns the position wrt to the world, and
+            wrt_link (int, str, None): the other link id, or name. If None, returns the pose wrt to the world, and
               if -1 wrt to the base.
             point (np.array[float[3]]): position of the point in link's local frame.
 
@@ -282,6 +303,40 @@ class RobotModelInterface(ModelInterface):
             return self.model.get_link_world_poses(link)
         return self.model.get_link_poses(link, self.get_link_id(wrt_link))
 
+    def get_position(self, link, wrt_link=None):  # TODO: use point
+        """
+        Return the position of the specified link with respect to another link.
+
+        Args:
+            link (int, str): unique link id, or name.
+            wrt_link (int, str, None): the other link id, or name. If None, returns the position wrt to the world,
+              and if -1 wrt to the base.
+
+        Returns:
+            np.array[float[3]]: position
+        """
+        link = self.get_link_id(link)
+        if wrt_link is None:
+            return self.model.get_link_world_positions(link)
+        return self.model.get_link_positions(link, wrt_link_id=self.get_link_id(wrt_link))
+
+    def get_orientation(self, link, wrt_link=None):  # TODO: use point
+        """
+        Return the orientation of the specified link with respect to another link.
+
+        Args:
+            link (int, str): unique link id, or name.
+            wrt_link (int, str, None): the other link id, or name. If None, returns the orientation wrt to the world,
+              and if -1 wrt to the base.
+
+        Returns:
+            np.array[float[4]]: orientation (expressed as a quaternion [x,y,z,w])
+        """
+        link = self.get_link_id(link)
+        if wrt_link is None:
+            return self.model.get_link_world_orientations(link)
+        return self.model.get_link_orientations(link, wrt_link_id=self.get_link_id(wrt_link))
+
     def get_velocity_twist(self, link, point=(0., 0., 0.)):  # TODO: use point
         r"""
         Compute the angular and linear velocity of a link, given by :math:`v = [\omega, \dot{p}]`.
@@ -417,6 +472,17 @@ class RobotModelInterface(ModelInterface):
         # return \dot{J}(q) \dot{q} = \dot{v} - J(q) \ddot{q}
         return acc - jacobian.dot(ddq)
 
+    def compute_com_JdotQdot(self):
+        r"""
+        Compute :math:`\dot{J}_{CoM}(q) \dot{q}` from the centroidal momentum matrix.
+
+        Returns:
+            np.array[float[6]]: the matrix multiplication of the first derivative of the CoM Jacobian with the joint
+              velocities.
+        """
+        A_G, dA_G_dq = self.get_centroidal_dynamics()
+        return dA_G_dq / self.get_mass()
+
     def compute_relative_JdotQdot(self, target_link, base_link):
         r"""
         Compute the relative :math:`\dot{J}(q) \dot{q}`, which appears in
@@ -496,6 +562,17 @@ class RobotModelInterface(ModelInterface):
         """
         return self.model.get_centroidal_momentum_matrix()
 
+    def get_centroidal_dynamics(self):
+        r"""
+        Return the centroidal momentum matrix :math:`A_G` and its derivative multiplied by the joint velocities
+        :math:`\dot{A}_G \dot{q}`.
+
+        Returns:
+            np.array[float[6,6+N]]: the centroidal momentum matrix :math:`A_G`
+            np.array[float[6]]: the centroidal dynamics velocity-dependent bias vector :math:`\dot{A}_G \dot{q}`
+        """
+        return self.model.get_centroidal_dynamics()
+
     def update(self, q=None, dq=None, ddq=None, update_model=False):
         """Update: move to the next step."""
         self._states = dict()

pyrobolearn/priorities/solvers/nlp_task_solver.py

@@ -55,10 +55,4 @@ class NLPTaskSolver(TaskSolver):
 
     def solve(self):
         """Solve the priority task."""
-        if self.task.tasks:
-            for soft_task in self.task.tasks:
-                As = np.vstack([np.dot(np.sqrt(task.weight), task.A) for task in soft_task])
-                bs = np.vstack([np.dot(np.sqrt(task.weight), task.b) for task in soft_task])
-                # x = self.solver.optimize(P=As.T.dot(As), q=-bs.T.dot(), G=, h=, A=, b=)
-        else:
-            pass
+        pass

pyrobolearn/priorities/tasks/acceleration/cartesian.py

@@ -1,6 +1,58 @@
 #!/usr/bin/env python
 r"""Provide the Cartesian acceleration task.
 
+The Cartesian acceleration task tries to impose a desired pose, velocity and acceleration profiles for a distal
+link with respect to a base link, or world frame.
+
+Before presenting the optimization problem, here is a small reminder. The acceleration is the time derivative of
+the velocity, i.e. :math:`a = \frac{dv}{dt}` where the cartesian velocities are related to joint velocities by
+:math:`v = J(q) \dot{q}` where :math:`J(q)` is the Jacobian, thus deriving that expression wrt time gives us:
+
+.. math:: a = \frac{d}{dt} v = \frac{d}{dt} J(q) \dot{q} = J(q) \ddot{q} + \dot{J}(q) \dot{q}.
+
+Now, we can formulate our minimization problem as:
+
+.. math:: || J(q) \ddot{q} + \dot{J} \dot{q} - (a_d + K_d (v_d - v) + K_p e) ||^2,
+
+where :math:`\ddot{q}` are the joint accelerations being optimized, :math:`a_d` are the desired cartesian
+accelerations, :math:`v_d = [v_d^\top, \omega_d^\top]^\top` are the desired cartesian velocities, :math:`v` are the
+current cartesian velocities of the distal link wrt the base, :math:`J(q) \in \mathbb{R}^{6 \times N}` is the
+Jacobian taken from the base to the distal link, :math:`K_p` and :math:`K_d` are the stiffness and damping gains
+respectively, :math:`e \in \mathbb{R}^{6}` is the error which is the concatenation of the position error given by
+:math:`e_{p} = (x_d - x)` (with :math:`x_d` being the desired position, and :math:`x` the current position), and the
+orientation error given by (if expressed as quaternions :math:`o = {s, v}` where :math:`s` is the real scalar part,
+and :math:`v` is the vector part) :math:`e_{o} = s v_d - s_d v - v_d \cross v`, and :math:`\dot{x}_d` is the
+desired cartesian velocity for the distal link with respect to the base link.
+
+
+The above formulation is equivalent to the QP objective function :math:`||Ax - b||^2`, by setting
+:math:`A = J(q)`, :math:`x = \ddot{q}`, and :math:`b = - \dot{J} \dot{q} + (a_d + K_d (v_d - v) + K_p e)`.
+
+This task can, for instance, be used for foot pose tracking when this one is not in contact with the ground. If
+the foot is in contact, we switch to a foot damping task which can be achieved by setting
+:math:`a_d = v_d = e = 0` and thus we are trying to solve :math:`||J(q) \ddot{q} - \dot{J} \dot{q} - K_d v_d||^2`.
+
+
+Inverse dynamics
+----------------
+
+Once the optimal joint accelerations :math:`\ddot{q}^*` have been computed, we can use inverse dynamics to
+compute the corresponding torques to apply on the joints. This is given by:
+
+.. math:: \tau = H(q) \ddot{q} + N(q,\dot{q)}
+
+where :math:`H(q)` is the inertia joint matrix, and N(q, \dot{q}) is a vector force that accounts for all the
+other non-linear forces acting on the system (Coriolis, centrifugal, gravity, external forces, friction, etc.).
+
+
+Important notes:
+
+- You don't have to specify the whole pose, you can also only specify the position or orientation.
+- You can also only specify the desired cartesian accelerations by setting `kp` and `kd` to zero; you don't have
+  neither to provide the desired cartesian velocities, position or orientation.
+
+
+.. seealso:: `tasks/velocity/cartesian.py` and `tasks/torque/cartesian_impedance_control.py`
 
 The implementation of this class is inspired by [1] (which is licensed under the LGPLv2).
 
@@ -8,11 +60,10 @@ References:
     - [1] "OpenSoT: A whole-body control library for the compliant humanoid robot COMAN", Rocchi et al., 2015
 """
 
-# TODO: finish to implement this
-
 import numpy as np
 
 from pyrobolearn.priorities.tasks import JointAccelerationTask
+from pyrobolearn.utils.transformation import quaternion_error
 
 
 __author__ = "Brian Delhaisse"
@@ -31,22 +82,29 @@ class CartesianAccelerationTask(JointAccelerationTask):
     The Cartesian acceleration task tries to impose a desired pose, velocity and acceleration profiles for a distal
     link with respect to a base link, or world frame.
 
-    Before presenting the optimization problem, a small reminder. The acceleration is the time derivative of the
-    velocity, i.e. :math:`a = \frac{dv}{dt}` where the cartesian velocities are related to joint velocities by
+    Before presenting the optimization problem, here is a small reminder. The acceleration is the time derivative of
+    the velocity, i.e. :math:`a = \frac{dv}{dt}` where the cartesian velocities are related to joint velocities by
     :math:`v = J(q) \dot{q}` where :math:`J(q)` is the Jacobian, thus deriving that expression wrt time gives us:
 
     .. math:: a = \frac{d}{dt} v = \frac{d}{dt} J(q) \dot{q} = J(q) \ddot{q} + \dot{J}(q) \dot{q}.
 
     Now, we can formulate our minimization problem as:
 
-    .. math:: || J(q) \ddot{q} - \dot{J} \dot{q} - (a_d + K_d (v_d - v) + K_p e) ||^2,
+    .. math:: || J(q) \ddot{q} + \dot{J} \dot{q} - (a_d + K_d (v_d - v) + K_p e) ||^2,
 
-    where :math:`\ddot{q}` are the joint accelerations being optimized, :math:`a_d` is the desired cartesian
-    acceleration, :math:`v_d = [\omega_d^\top, v` is the desired cartesian velocity, ...
+    where :math:`\ddot{q}` are the joint accelerations being optimized, :math:`a_d` are the desired cartesian
+    accelerations, :math:`v_d = [v_d^\top, \omega_d^\top]^\top` are the desired cartesian velocities, :math:`v` are the
+    current cartesian velocities of the distal link wrt the base, :math:`J(q) \in \mathbb{R}^{6 \times N}` is the
+    Jacobian taken from the base to the distal link, :math:`K_p` and :math:`K_d` are the stiffness and damping gains
+    respectively, :math:`e \in \mathbb{R}^{6}` is the error which is the concatenation of the position error given by
+    :math:`e_{p} = (x_d - x)` (with :math:`x_d` being the desired pose, and :math:`x` the current pose), and the
+    orientation error given by (if expressed as quaternions :math:`o = {s, v}` where :math:`s` is the real scalar part,
+    and :math:`v` is the vector part) :math:`e_{o} = s v_d - s_d v - v_d \cross v`, and :math:`\dot{x}_d` is the
+    desired cartesian velocity for the distal link with respect to the base link.
 
 
     The above formulation is equivalent to the QP objective function :math:`||Ax - b||^2`, by setting
-    :math:`A = J(q)`, :math:`x = \ddot{q}`, and :math:`b = - \dot{J} \dot{q} - (a_d + K_d (v_d - v) + K_p e)`.
+    :math:`A = J(q)`, :math:`x = \ddot{q}`, and :math:`b = - \dot{J} \dot{q} + (a_d + K_d (v_d - v) + K_p e)`.
 
     This task can, for instance, be used for foot pose tracking when this one is not in contact with the ground. If
     the foot is in contact, we switch to a foot damping task which can be achieved by setting
@@ -62,14 +120,21 @@ class CartesianAccelerationTask(JointAccelerationTask):
     .. math:: \tau = H(q) \ddot{q} + N(q,\dot{q)}
 
     where :math:`H(q)` is the inertia joint matrix, and N(q, \dot{q}) is a vector force that accounts for all the
-    other forces acting on the system (Coriolis, centrifugal, gravity, external forces, friction, etc.).
-
+    other non-linear forces acting on the system (Coriolis, centrifugal, gravity, external forces, friction, etc.).
 
     .. seealso:: `tasks/velocity/cartesian.py` and `tasks/torque/cartesian_impedance_control.py`
+
+
+    The implementation of this class is inspired by [1] (which is licensed under the LGPLv2).
+
+    References:
+        - [1] "OpenSoT: A whole-body control library for the compliant humanoid robot COMAN", Rocchi et al., 2015
     """
 
-    def __init__(self, model, distal_link, base_link=None, local_position=(0, 0, 0), x_desired=None,
-                 dx_desired=None, kp=1., kd=1., weight=1., constraints=[]):
+    def __init__(self, model, distal_link, base_link=None, local_position=(0, 0, 0), desired_position=None,
+                 desired_orientation=None, desired_linear_velocity=None, desired_angular_velocity=None,
+                 desired_linear_acceleration=None, desired_angular_acceleration=None,
+                 kp_position=1., kp_orientation=1., kd_linear=1., kd_angular=1., weight=1., constraints=[]):
         """
         Initialize the task.
 
@@ -77,26 +142,393 @@ class CartesianAccelerationTask(JointAccelerationTask):
             model (ModelInterface): model interface.
             distal_link (int, str): distal link id or name.
             base_link (int, str, None): base link id or name. If None, it will be the world.
-            local_position (np.array[3]): local position on the distal link.
-            x_desired (np.array[7], None): desired cartesian pose of distal link wrt the base.
-            dx_desired (np.array[6], None): desired cartesian velocity of distal link wrt the base.
-            kp (float, np.array[6,6]): stiffness gain.
-            weight (float, np.array[6,6]): weight scalar or matrix associated to the task.
-            constraints (list of Constraint): list of constraints associated with the task.
+            local_position (np.array[float[3]]): local position on the distal link.
+            desired_position (np.array[float[3]], None): desired position of distal link wrt the base. If None, it
+              will not be taken into account.
+            desired_orientation (np.array[float[4]], None): desired orientation (expressed as quaternion [x,y,z,w]) of
+              distal link wrt the base. If None, it will not be taken into account.
+            desired_linear_velocity (np.array[float[3]], None): desired linear velocity of distal link wrt the base.
+              If None, it will be set to zero.
+            desired_angular_velocity (np.array[float[3]], None): desired angular velocity of distal link wrt the base.
+              If None, it will be set to zero.
+            desired_linear_acceleration (np.array[float[3]], None): desired linear acceleration of distal link wrt
+              the base. If None, it will be set to zero.
+            desired_angular_acceleration (np.array[float[3]], None): desired angular acceleration of distal link wrt
+              the base. If None, it will be set to zero.
+            kp_position (float, np.array[float[3,3]]): position stiffness gain.
+            kp_orientation (float, np.array[float[3,3]]): orientation stiffness gain.
+            kd_linear (float, np.array[float[3,3]]): linear velocity damping gain.
+            kd_angular (float, np.array[float[3,3]]): angular velocity damping gain.
+            weight (float, np.array[float[6,6]], np.array[float[3,3]]): weight scalar or matrix associated to the task.
+            constraints (list[Constraint]): list of constraints associated with the task.
         """
         super(CartesianAccelerationTask, self).__init__(model=model, weight=weight, constraints=constraints)
 
-        # set variables
-        self.distal_link = distal_link
-        self.base_link = base_link
+        # define variables
+        self.distal_link = self.model.get_link_id(distal_link)
+        self.base_link = self.model.get_link_id(base_link) if base_link is not None else base_link
+        self.local_position = local_position
 
-        # pose = (position, quaternion)
-        self.desired_pose = np.array([0]*6 + 1)
-        self.current_pose = np.array([0]*6 + 1)
+        if base_link is not None:
+            raise NotImplementedError("Currently, the base_link can only be set to the world (None).")
 
-        # velocity = (angular, linear)
-        self.desired_velocity = np.zeros(6)
-        self.current_velocity = np.zeros(6)
+        # gains
+        self.kp_position = kp_position
+        self.kp_orientation = kp_orientation
+        self.kd_linear = kd_linear
+        self.kd_angular = kd_angular
 
-        # acceleration = (angular, linear)
-        self.desired_acceleration = np.zeros(6)
+        # define desired references
+        self.desired_position = desired_position
+        self.desired_orientation = desired_orientation
+        self.desired_linear_velocity = desired_linear_velocity
+        self.desired_angular_velocity = desired_angular_velocity
+        self.desired_linear_acceleration = desired_linear_acceleration
+        self.desired_angular_acceleration = desired_angular_acceleration
+
+        # first update
+        self.update()
+
+    ##############
+    # Properties #
+    ##############
+
+    @property
+    def desired_position(self):
+        """Get the desired cartesian position for the distal link wrt the base."""
+        return self._des_pos
+
+    @desired_position.setter
+    def desired_position(self, position):
+        """Set the desired cartesian position for the distal link wrt the base."""
+        if position is not None:
+            if not isinstance(position, (np.ndarray, list, tuple)):
+                raise TypeError("Expecting the given desired position to be a np.array, instead got: "
+                                "{}".format(type(position)))
+            position = np.asarray(position)
+            if len(position) != 3:
+                raise ValueError("Expecting the given desired position array to be of length 3, but instead got: "
+                                 "{}".format(len(position)))
+        self._des_pos = position
+
+    @property
+    def desired_orientation(self):
+        """Get the desired cartesian orientation (expressed as a quaternion [x,y,z,w]) for the distal link wrt the
+        base."""
+        return self._des_quat
+
+    @desired_orientation.setter
+    def desired_orientation(self, orientation):
+        """Set the desired cartesian orientation (expressed as a quaternion [x,y,z,w]) for the distal link wrt the
+        base."""
+        if orientation is not None:
+            if not isinstance(orientation, (np.ndarray, list, tuple)):
+                raise TypeError("Expecting the given desired orientation to be a np.array, instead got: "
+                                "{}".format(type(orientation)))
+            orientation = np.asarray(orientation)
+            if len(orientation) != 4:
+                raise ValueError(
+                    "Expecting the given desired orientation array to be of length 4, but instead got: "
+                    "{}".format(len(orientation)))
+        self._des_quat = orientation
+
+    @property
+    def desired_linear_velocity(self):
+        """Get the desired cartesian linear velocity of the distal link wrt the base."""
+        return self._des_lin_vel
+
+    @desired_linear_velocity.setter
+    def desired_linear_velocity(self, velocity):
+        """Set the desired cartesian linear velocity of the distal link wrt the base."""
+        if velocity is None:
+            velocity = np.zeros(3)
+        elif not isinstance(velocity, (np.ndarray, list, tuple)):
+            raise TypeError("Expecting the given desired linear velocity to be a np.array, instead got: "
+                            "{}".format(type(velocity)))
+        velocity = np.asarray(velocity)
+        if len(velocity) != 3:
+            raise ValueError("Expecting the given desired linear velocity array to be of length 3, but instead "
+                             "got: {}".format(len(velocity)))
+        self._des_lin_vel = velocity
+
+    @property
+    def desired_angular_velocity(self):
+        """Get the desired cartesian angular velocity of the distal link wrt the base."""
+        return self._des_ang_vel
+
+    @desired_angular_velocity.setter
+    def desired_angular_velocity(self, velocity):
+        """Set the desired cartesian angular velocity of the distal link wrt the base."""
+        if velocity is None:
+            velocity = np.zeros(3)
+        elif not isinstance(velocity, (np.ndarray, list, tuple)):
+            raise TypeError("Expecting the given desired angular velocity to be a np.array, instead got: "
+                            "{}".format(type(velocity)))
+        velocity = np.asarray(velocity)
+        if len(velocity) != 3:
+            raise ValueError("Expecting the given desired angular velocity array to be of length 3, but instead "
+                             "got: {}".format(len(velocity)))
+        self._des_ang_vel = velocity
+
+    @property
+    def desired_velocity(self):
+        """Return the linear and angular velocity."""
+        return np.concatenate((self._des_lin_vel, self._des_ang_vel))
+
+    @property
+    def desired_linear_acceleration(self):
+        """Get the desired cartesian linear acceleration of the distal link wrt the base."""
+        return self._des_lin_acc
+
+    @desired_linear_acceleration.setter
+    def desired_linear_acceleration(self, acceleration):
+        """Set the desired cartesian linear acceleration of the distal link wrt the base."""
+        if acceleration is None:
+            acceleration = np.zeros(3)
+        elif not isinstance(acceleration, (np.ndarray, list, tuple)):
+            raise TypeError("Expecting the given desired linear acceleration to be a np.array, instead got: "
+                            "{}".format(type(acceleration)))
+        acceleration = np.asarray(acceleration)
+        if len(acceleration) != 3:
+            raise ValueError("Expecting the given desired linear acceleration array to be of length 3, but instead "
+                             "got: {}".format(len(acceleration)))
+        self._des_lin_acc = acceleration
+
+    @property
+    def desired_angular_acceleration(self):
+        """Get the desired cartesian angular acceleration of the distal link wrt the base."""
+        return self._des_ang_acc
+
+    @desired_angular_acceleration.setter
+    def desired_angular_acceleration(self, acceleration):
+        """Set the desired cartesian angular acceleration of the distal link wrt the base."""
+        if acceleration is None:
+            acceleration = np.zeros(3)
+        elif not isinstance(acceleration, (np.ndarray, list, tuple)):
+            raise TypeError("Expecting the given desired angular acceleration to be a np.array, instead got: "
+                            "{}".format(type(acceleration)))
+        acceleration = np.asarray(acceleration)
+        if len(acceleration) != 3:
+            raise ValueError("Expecting the given desired angular acceleration array to be of length 3, but instead "
+                             "got: {}".format(len(acceleration)))
+        self._des_ang_acc = acceleration
+
+    @property
+    def desired_acceleration(self):
+        """Return the linear and angular acceleration."""
+        return np.concatenate((self._des_lin_acc, self._des_ang_acc))
+
+    @property
+    def x_desired(self):
+        """Get the desired cartesian pose for the distal link wrt to the base."""
+        position = self.desired_position
+        orientation = self.desired_orientation
+        if position is not None:
+            if orientation is not None:
+                return np.concatenate((position, orientation))
+            return position
+        return orientation
+
+    @x_desired.setter
+    def x_desired(self, x_d):
+        """Set the desired cartesian pose for the distal link wrt to the base."""
+        if x_d is not None:
+            if not isinstance(x_d, (np.ndarray, list, tuple)):
+                raise TypeError(
+                    "Expecting the given desired pose to be a np.array, instead got: {}".format(type(x_d)))
+            x_d = np.asarray(x_d)
+            if len(x_d) == 3:  # only position is provided
+                x_d = np.concatenate((x_d, np.array([0., 0., 0., 1.])))
+            elif len(x_d) == 4:  # only orientation is provided
+                x_d = np.concatenate((np.zeros(3), x_d))
+            if len(x_d) != 7:
+                raise ValueError("Expecting the given desired pose array to be of length 7 (3 for the position, "
+                                 "and 4 for the orientation expressed as a quaternion [x,y,z,w]), instead got a "
+                                 "length of: {}".format(len(x_d)))
+            self._des_pos = x_d[:3]
+            self._des_quat = x_d[3:]
+
+    @property
+    def dx_desired(self):
+        """Get the desired cartesian velocity for the distal link wrt to the base."""
+        return np.concatenate((self._des_lin_vel, self._des_ang_vel))
+
+    @dx_desired.setter
+    def dx_desired(self, dx_d):
+        """Set the desired cartesian velocity for the distal link wrt to the base."""
+        if dx_d is not None:
+            if not isinstance(dx_d, (np.ndarray, list, tuple)):
+                raise TypeError("Expecting the given desired velocity to be a np.array, instead got: "
+                                "{}".format(type(dx_d)))
+            dx_d = np.asarray(dx_d)
+            if len(dx_d) == 3:  # assume that it is the linear velocity
+                dx_d = np.concatenate((dx_d, np.zeros(3)))
+            if len(dx_d) != 6:
+                raise ValueError("Expecting the given desired velocity array to be of length 6 (3 for the linear "
+                                 "and 3 for the angular part), instead got a length of: {}".format(len(dx_d)))
+            self._des_lin_vel = dx_d[:3]
+            self._des_ang_vel = dx_d[3:]
+
+    @property
+    def ddx_desired(self):
+        """Get the desired cartesian acceleration for the distal link wrt to the base."""
+        return np.concatenate((self._des_lin_acc, self._des_ang_acc))
+
+    @ddx_desired.setter
+    def ddx_desired(self, ddx_d):
+        """Set the desired cartesian acceleration for the distal link wrt to the base."""
+        if ddx_d is not None:
+            if not isinstance(ddx_d, (np.ndarray, list, tuple)):
+                raise TypeError("Expecting the given desired acceleration to be a np.array, instead got: "
+                                "{}".format(type(ddx_d)))
+            ddx_d = np.asarray(ddx_d)
+            if len(ddx_d) == 3:  # assume that it is the linear acceleration
+                ddx_d = np.concatenate((ddx_d, np.zeros(3)))
+            if len(ddx_d) != 6:
+                raise ValueError("Expecting the given desired acceleration array to be of length 6 (3 for the linear "
+                                 "and 3 for the angular part), instead got a length of: {}".format(len(ddx_d)))
+            self._des_lin_acc = ddx_d[:3]
+            self._des_ang_acc = ddx_d[3:]
+
+    @property
+    def kp_position(self):
+        """Return the position stiffness gain."""
+        return self._kp_pos
+
+    @kp_position.setter
+    def kp_position(self, kp):
+        """Set the position stiffness gain."""
+        if kp is None:
+            kp = 1.
+        if not isinstance(kp, (float, int, np.ndarray)):
+            raise TypeError("Expecting the given position stiffness gain kp to be an int, float, np.array, instead "
+                            "got: {}".format(type(kp)))
+        if isinstance(kp, np.ndarray) and kp.shape != (3, 3):
+            raise ValueError("Expecting the given position stiffness gain matrix kp to be of shape {}, but instead "
+                             "got shape: {}".format((3, 3), kp.shape))
+        self._kp_pos = kp
+
+    @property
+    def kp_orientation(self):
+        """Return the orientation stiffness gain."""
+        return self._kp_quat
+
+    @kp_orientation.setter
+    def kp_orientation(self, kp):
+        """Set the orientation stiffness gain."""
+        if kp is None:
+            kp = 1.
+        if not isinstance(kp, (float, int, np.ndarray)):
+            raise TypeError("Expecting the given orientation stiffness gain kp to be an int, float, np.array, "
+                            "instead got: {}".format(type(kp)))
+        if isinstance(kp, np.ndarray) and kp.shape != (3, 3):
+            raise ValueError("Expecting the given orientation stiffness gain matrix kp to be of shape {}, but "
+                             "instead got shape: {}".format((3, 3), kp.shape))
+        self._kp_quat = kp
+
+    @property
+    def kd_linear(self):
+        """Return the linear velocity damping gain."""
+        return self._kd_lin
+
+    @kd_linear.setter
+    def kd_linear(self, kd):
+        """Set the linear velocity damping gain."""
+        if kd is None:
+            kd = 1.
+        if not isinstance(kd, (float, int, np.ndarray)):
+            raise TypeError("Expecting the given linear velocity damping gain kd to be an int, float, np.array, "
+                            "instead got: {}".format(type(kd)))
+        if isinstance(kd, np.ndarray) and kd.shape != (3, 3):
+            raise ValueError("Expecting the given linear velocity damping gain matrix kd to be of shape {}, but "
+                             "instead got shape: {}".format((3, 3), kd.shape))
+        self._kd_lin = kd
+
+    @property
+    def kd_angular(self):
+        """Return the angular velocity damping gain."""
+        return self._kd_ang
+
+    @kd_angular.setter
+    def kd_angular(self, kd):
+        """Set the angular velocity damping gain."""
+        if kd is None:
+            kd = 1.
+        if not isinstance(kd, (float, int, np.ndarray)):
+            raise TypeError("Expecting the given angular velocity damping gain kd to be an int, float, np.array, "
+                            "instead got: {}".format(type(kd)))
+        if isinstance(kd, np.ndarray) and kd.shape != (3, 3):
+            raise ValueError("Expecting the given angular velocity damping gain matrix kd to be of shape {}, but "
+                             "instead got shape: {}".format((3, 3), kd.shape))
+        self._kd_ang = kd
+
+    ###########
+    # Methods #
+    ###########
+
+    def set_desired_references(self, x_des, dx_des=None, ddx_des=None, *args, **kwargs):
+        """Set the desired references.
+
+        Args:
+            x_des (np.array[float[7]], None): desired cartesian pose (position and quaternion [x,y,z,w]) of distal
+              link wrt the base. If None, it will let the initial desired pose unchanged.
+            dx_des (np.array[float[6]], None): desired cartesian velocity of distal link wrt the base. If None, it
+              will let the initial desired velocities unchanged.
+            ddx_des (np.array[float[6]], None): desired cartesian acceleration of distal link wrt the base. If None,
+              it will let the initial desired accelerations unchanged.
+        """
+        self.x_desired = x_des
+        self.dx_desired = dx_des
+        self.ddx_desired = ddx_des
+
+    def get_desired_references(self):
+        """Return the desired references.
+
+        Returns:
+            np.array[float[7]]: desired cartesian pose (position and quaternion [x,y,z,w]) of distal link wrt the base.
+            np.array[float[6]]: desired cartesian velocity of distal link wrt the base.
+            np.array[float[6]]: desired cartesian acceleration of distal link wrt the base.
+        """
+        return self.x_desired, self.dx_desired, self.ddx_desired
+
+    def _update(self):
+        """
+        Update the task by computing the A matrix and b vector that will be used by the task solver.
+        """
+        x = self.model.get_pose(link=self.distal_link, wrt_link=self.base_link)
+        self._A = self.model.get_jacobian(link=self.distal_link, wrt_link=self.base_link,
+                                          point=self.local_position)  # shape: (6,N)
+        vel = self.model.get_velocity(link=self.distal_link)
+        jdotqdot = self.model.compute_JdotQdot(link=self.distal_link)
+        # b = - \dot{J} \dot{q} + (a_d + K_d (v_d - v) + K_p e)
+        b = -jdotqdot + self.desired_acceleration
+
+        if self._des_quat is None:  # only position and/or velocities
+            if self._des_pos is None:  # only velocities
+                self._b = b + np.concatenate((np.dot(self.kd_linear, (self._des_lin_vel - vel[:3])),
+                                              np.dot(self.kd_angular, (self._des_ang_vel - vel[3:]))))
+            else:  # only position
+                self._A = self._A[:3]
+                # compute position error
+                error = (self._des_pos - x[:3])
+                # compute b vector
+                lin_vel = np.dot(self.kd_linear, (self._des_lin_vel - vel[:3]))
+                self._b = b[:3] + np.dot(self.kp_position, error) + lin_vel
+        elif self._des_pos is None:  # only orientation
+            self._A = self._A[3:]
+            # compute orientation error
+            error = quaternion_error(quat_des=self._des_quat, quat_cur=x[3:])
+            # compute b vector
+            ang_vel = np.dot(self.kd_angular, (self._des_ang_vel - vel[3:]))
+            self._b = b[3:] + np.dot(self.kp_orientation, error) + ang_vel
+        else:  # both
+            # compute position/orientation error
+            position_error = (self._des_pos - x[:3])
+            orientation_error = quaternion_error(quat_des=self._des_quat, quat_cur=x[3:])
+
+            # compute b vector
+            lin_vel = np.dot(self.kd_linear, (self._des_lin_vel - vel[:3]))
+            ang_vel = np.dot(self.kd_angular, (self._des_ang_vel - vel[3:]))
+            b_lin = np.dot(self.kp_position, position_error) + lin_vel
+            b_ang = np.dot(self.kp_orientation, orientation_error) + ang_vel
+            self._b = b + np.concatenate((b_lin, b_ang))

pyrobolearn/priorities/tasks/acceleration/com.py

@@ -1,6 +1,56 @@
 #!/usr/bin/env python
 r"""Provide the Cartesian CoM acceleration task.
 
+The CoM task tries to impose a desired position of the CoM with respect to the world frame.
+
+.. math:: ||J_{CoM} \dot{q} - (K_p (x_d - x) + \dot{x}_d)||^2
+
+where :math:`J_{CoM}` is the CoM Jacobian, :math:`\dot{q}` are the joint velocities being optimized, :math:`K_p`
+is the stiffness gain, :math:`x_d` and :math:`x` are the desired and current cartesian CoM position
+respectively, and :math:`\dot{x}_d` is the desired linear velocity of the CoM.
+
+This is equivalent to the QP objective function :math:`||Ax - b||^2`, by setting :math:`A=J_{CoM}`,
+:math:`x=\dot{q}`, and :math:`b = K_p (x_d - x) + \dot{x}_d`.
+
+Note that you can only specify the center of mass linear velocity if you wish.
+
+The CoM acceleration task tries to impose a desired pose, velocity and acceleration profiles for the CoM with respect
+to the world frame.
+
+Before presenting the optimization problem, here is a small reminder. The acceleration is the time derivative of
+the velocity, i.e. :math:`a = \frac{dv}{dt}` where the cartesian velocities are related to joint velocities by
+:math:`v_{CoM} = J_{CoM}(q) \dot{q}` where :math:`J_{CoM}(q)` is the CoM Jacobian, thus deriving that expression wrt
+time gives us:
+
+.. math:: a = \frac{d}{dt} v = \frac{d}{dt} J_{CoM}(q) \dot{q} = J_{CoM}(q) \ddot{q} + \dot{J}_{CoM}(q) \dot{q}.
+
+Now, we can formulate our minimization problem as:
+
+.. math:: || J_{CoM}(q) \ddot{q} + \dot{J}_{CoM} \dot{q} - (a_d + K_d (v_d - v) + K_p (x_d - x)) ||^2,
+
+where :math:`\ddot{q}` are the joint accelerations being optimized, :math:`a_d` are the desired cartesian linear
+accelerations, :math:`v_d` and :math:`v` are the desired and current cartesian linear velocities,
+:math:`J_{CoM}(q) \in \mathbb{R}^{3 \times N}` is the CoM Jacobian, :math:`K_p` and :math:`K_d` are the stiffness and
+damping gains respectively, and :math:`x_d` and :math:`x` are the desired and current cartesian CoM position
+respectively.
+
+
+The above formulation is equivalent to the QP objective function :math:`||Ax - b||^2`, by setting
+:math:`A = J_{CoM}(q)`, :math:`x = \ddot{q}`, and
+:math:`b = - \dot{J}_{CoM} \dot{q} + (a_d + K_d (v_d - v) + K_p (x_d - x))`.
+
+
+Inverse dynamics
+----------------
+
+Once the optimal joint accelerations :math:`\ddot{q}^*` have been computed, we can use inverse dynamics to
+compute the corresponding torques to apply on the joints. This is given by:
+
+.. math:: \tau = H(q) \ddot{q} + N(q,\dot{q)}
+
+where :math:`H(q)` is the inertia joint matrix, and N(q, \dot{q}) is a vector force that accounts for all the
+other non-linear forces acting on the system (Coriolis, centrifugal, gravity, external forces, friction, etc.).
+
 
 The implementation of this class is inspired by [1] (which is licensed under the LGPLv2).
 
@@ -10,7 +60,7 @@ References:
 
 import numpy as np
 
-from pyrobolearn.priorities.tasks import Task
+from pyrobolearn.priorities.tasks import JointAccelerationTask
 
 
 __author__ = "Brian Delhaisse"
@@ -23,17 +73,257 @@ __email__ = "briandelhaisse@gmail.com"
 __status__ = "Development"
 
 
-class CoMAccelerationTask(Task):
+class CoMAccelerationTask(JointAccelerationTask):
     r"""CoM Acceleration Task
 
+    The CoM task tries to impose a desired position of the CoM with respect to the world frame.
+
+    .. math:: ||J_{CoM} \dot{q} - (K_p (x_d - x) + \dot{x}_d)||^2
+
+    where :math:`J_{CoM}` is the CoM Jacobian, :math:`\dot{q}` are the joint velocities being optimized, :math:`K_p`
+    is the stiffness gain, :math:`x_d` and :math:`x` are the desired and current cartesian CoM position
+    respectively, and :math:`\dot{x}_d` is the desired linear velocity of the CoM.
+
+    This is equivalent to the QP objective function :math:`||Ax - b||^2`, by setting :math:`A=J_{CoM}`,
+    :math:`x=\dot{q}`, and :math:`b = K_p (x_d - x) + \dot{x}_d`.
+
+    Note that you can only specify the center of mass linear velocity if you wish.
+
+    The CoM acceleration task tries to impose a desired pose, velocity and acceleration profiles for the CoM wrt the
+    world frame.
+
+    Before presenting the optimization problem, here is a small reminder. The acceleration is the time derivative of
+    the velocity, i.e. :math:`a = \frac{dv}{dt}` where the cartesian velocities are related to joint velocities by
+    :math:`v_{CoM} = J_{CoM}(q) \dot{q}` where :math:`J_{CoM}(q)` is the CoM Jacobian, thus deriving that expression
+    wrt time gives us:
+
+    .. math:: a = \frac{d}{dt} v = \frac{d}{dt} J_{CoM}(q) \dot{q} = J_{CoM}(q) \ddot{q} + \dot{J}_{CoM}(q) \dot{q}.
+
+    Now, we can formulate our minimization problem as:
+
+    .. math:: || J_{CoM}(q) \ddot{q} + \dot{J}_{CoM} \dot{q} - (a_d + K_d (v_d - v) + K_p (x_d - x)) ||^2,
+
+    where :math:`\ddot{q}` are the joint accelerations being optimized, :math:`a_d` are the desired cartesian linear
+    accelerations, :math:`v_d` and :math:`v` are the desired and current cartesian linear velocities,
+    :math:`J_{CoM}(q) \in \mathbb{R}^{3 \times N}` is the CoM Jacobian, :math:`K_p` and :math:`K_d` are the stiffness
+    and damping gains respectively, and :math:`x_d` and :math:`x` are the desired and current cartesian CoM position
+    respectively.
+
+
+    The above formulation is equivalent to the QP objective function :math:`||Ax - b||^2`, by setting
+    :math:`A = J_{CoM}(q)`, :math:`x = \ddot{q}`, and
+    :math:`b = - \dot{J}_{CoM} \dot{q} + (a_d + K_d (v_d - v) + K_p (x_d - x))`.
+
+
+    Inverse dynamics
+    ----------------
+
+    Once the optimal joint accelerations :math:`\ddot{q}^*` have been computed, we can use inverse dynamics to
+    compute the corresponding torques to apply on the joints. This is given by:
+
+    .. math:: \tau = H(q) \ddot{q} + N(q,\dot{q)}
+
+    where :math:`H(q)` is the inertia joint matrix, and N(q, \dot{q}) is a vector force that accounts for all the
+    other non-linear forces acting on the system (Coriolis, centrifugal, gravity, external forces, friction, etc.).
+
+
+    The implementation of this class is inspired by [1] (which is licensed under the LGPLv2).
+
+    References:
+        - [1] "OpenSoT: A whole-body control library for the compliant humanoid robot COMAN", Rocchi et al., 2015
     """
 
-    def __init__(self, model, constraints=[]):
+    def __init__(self, model, desired_position=None, desired_velocity=None, desired_acceleration=None, kp=1., kd=1.,
+                 weight=1., constraints=[]):
         """
         Initialize the task.
 
         Args:
-            model (ModelInterface): model interface
-            constraints (list of Constraint): list of constraints associated with the task.
+            model (ModelInterface): model interface.
+            desired_position (np.array[float[3]], None): desired CoM position. If None, it will be set to 0.
+            desired_velocity (np.array[float[3]], None): desired CoM linear velocity. If None, it will be set to 0.
+            desired_acceleration (np.array[float[3]], None): desired CoM linear acceleration. If None, it will be set
+              to 0.
+            kp (float, np.array[float[3,3]]): position gain.
+            kd (float, np.array[float[3,3]]): linear velocity gain.
+            weight (float, np.array[float[3,3]]): weight scalar or matrix associated to the task.
+            constraints (list[Constraint]): list of constraints associated with the task.
         """
-        super(CoMAccelerationTask, self).__init__(model=model, constraints=constraints)
+        super(CoMAccelerationTask, self).__init__(model=model, weight=weight, constraints=constraints)
+
+        # define variable
+        self.kp = kp
+        self.kd = kd
+
+        # define desired references
+        self.desired_position = desired_position
+        self.desired_velocity = desired_velocity
+        self.desired_acceleration = desired_acceleration
+
+        # first update
+        self.update()
+
+        ##############
+        # Properties #
+        ##############
+
+    @property
+    def desired_position(self):
+        """Get the desired CoM position."""
+        return self._des_pos
+
+    @desired_position.setter
+    def desired_position(self, position):
+        """Set the desired CoM position."""
+        if position is not None:
+            if not isinstance(position, (np.ndarray, list, tuple)):
+                raise TypeError("Expecting the given desired position to be a np.array, instead got: "
+                                "{}".format(type(position)))
+            position = np.asarray(position)
+            if len(position) != 3:
+                raise ValueError("Expecting the given desired position array to be of length 3, but instead got: "
+                                 "{}".format(len(position)))
+        self._des_pos = position
+
+    @property
+    def desired_velocity(self):
+        """Get the desired CoM linear velocity."""
+        return self._des_vel
+
+    @desired_velocity.setter
+    def desired_velocity(self, velocity):
+        """Set the desired CoM linear velocity."""
+        if velocity is None:
+            velocity = np.zeros(3)
+        elif not isinstance(velocity, (np.ndarray, list, tuple)):
+            raise TypeError("Expecting the given desired linear velocity to be a np.array, instead got: "
+                            "{}".format(type(velocity)))
+        velocity = np.asarray(velocity)
+        if len(velocity) != 3:
+            raise ValueError("Expecting the given desired linear velocity array to be of length 3, but instead "
+                             "got: {}".format(len(velocity)))
+        self._des_vel = velocity
+
+    @property
+    def desired_acceleration(self):
+        """Get the desired CoM linear acceleration."""
+        return self._des_acc
+
+    @desired_acceleration.setter
+    def desired_acceleration(self, acceleration):
+        """Set the desired CoM linear acceleration."""
+        if acceleration is None:
+            acceleration = np.zeros(3)
+        elif not isinstance(acceleration, (np.ndarray, list, tuple)):
+            raise TypeError("Expecting the given desired linear acceleration to be a np.array, instead got: "
+                            "{}".format(type(acceleration)))
+        acceleration = np.asarray(acceleration)
+        if len(acceleration) != 3:
+            raise ValueError("Expecting the given desired linear acceleration array to be of length 3, but instead "
+                             "got: {}".format(len(acceleration)))
+        self._des_acc = acceleration
+
+    @property
+    def x_desired(self):
+        """Get the desired CoM position."""
+        return self._des_pos
+
+    @x_desired.setter
+    def x_desired(self, x_d):
+        """Set the desired CoM position."""
+        self.desired_position = x_d
+
+    @property
+    def dx_desired(self):
+        """Get the desired CoM linear velocity."""
+        return self._des_vel
+
+    @dx_desired.setter
+    def dx_desired(self, dx_d):
+        """Set the desired CoM linear velocity."""
+        self.desired_velocity = dx_d
+
+    @property
+    def ddx_desired(self):
+        """Get the desired CoM linear acceleration."""
+        return self._des_acc
+
+    @ddx_desired.setter
+    def ddx_desired(self, ddx_d):
+        """Set the desired CoM linear acceleration."""
+        self.desired_acceleration = ddx_d
+
+    @property
+    def kp(self):
+        """Return the position gain."""
+        return self._kp
+
+    @kp.setter
+    def kp(self, kp):
+        """Set the position gain."""
+        if kp is None:
+            kp = 1.
+        if not isinstance(kp, (float, int, np.ndarray)):
+            raise TypeError("Expecting the given position gain kp to be an int, float, np.array, instead got: "
+                            "{}".format(type(kp)))
+        if isinstance(kp, np.ndarray) and kp.shape != (3, 3):
+            raise ValueError("Expecting the given position gain matrix kp to be of shape {}, but instead got "
+                             "shape: {}".format((self.x_size, self.x_size), kp.shape))
+        self._kp = kp
+
+    @property
+    def kd(self):
+        """Return the linear velocity gain."""
+        return self._kd
+
+    @kd.setter
+    def kd(self, kd):
+        """Set the linear velocity gain."""
+        if kd is None:
+            kd = 1.
+        if not isinstance(kd, (float, int, np.ndarray)):
+            raise TypeError("Expecting the given linear velocity gain kd to be an int, float, np.array, "
+                            "instead got: {}".format(type(kd)))
+        if isinstance(kd, np.ndarray) and kd.shape != (3, 3):
+            raise ValueError("Expecting the given linear velocity gain matrix kd to be of shape {}, but "
+                             "instead got shape: {}".format((3, 3), kd.shape))
+        self._kd = kd
+
+    ###########
+    # Methods #
+    ###########
+
+    def set_desired_references(self, x_des, dx_des=None, ddx_des=None, *args, **kwargs):
+        """Set the desired references.
+
+        Args:
+            x_des (np.array[float[3]], None): desired CoM position. If None, it will be set to 0.
+            dx_des (np.array[float[3]], None): desired CoM linear velocity. If None, it will be set to 0.
+            ddx_des (np.array[float[3]], None): desired CoM linear velocity. If None, it will be set to 0.
+        """
+        self.x_desired = x_des
+        self.dx_desired = dx_des
+        self.ddx_desired = ddx_des
+
+    def get_desired_references(self):
+        """Return the desired references.
+
+        Returns:
+            np.array[float[3]]: desired CoM position.
+            np.array[float[3]]: desired CoM linear velocity.
+        """
+        return self.x_desired, self.dx_desired, self.ddx_desired
+
+    def _update(self):
+        """
+        Update the task by computing the A matrix and b vector that will be used by the task solver.
+        """
+
+        self._A = self.model.get_com_jacobian(full=False)  # shape: (3, N)
+        jdotqdot = self.model.compute_com_JdotQdot()[:3]  # shape: (3,)
+        vel = self.model.get_com_velocity()  # shape: (3,)
+        self._b = -jdotqdot + self._des_acc + np.dot(self.kd, (self._des_vel - vel))  # shape: (3,)
+
+        if self._des_pos is not None:
+            x = self.model.get_com_position()
+            self._b = self._b + np.dot(self.kp, (self._des_pos - x))  # shape: (3,)

pyrobolearn/priorities/tasks/acceleration/contact.py

@@ -1,6 +1,23 @@
 #!/usr/bin/env python
 r"""Provide the contact acceleration task.
 
+The contact acceleration task tries to minimize the dynamic movement of a contact link:
+
+.. math:: || C (J_c(q) \ddot{q} + \dot{J}_c(q) \dot{q}) ||^2,
+
+where :math:`C \in \mathbb{R}^{6 \times 6}` is the contact matrix (=a diagonal selector matrix where the entries
+are 1 for cartesian velocities that we wish to minimize such that the link doesn't move, and 0 for cartesian
+velocities that are free to change), :math:`J_c(q)` is the contact Jacobian (the Jacobian from the world frame to
+the contact point expressed in the distal link frame (i.e. the link which is in contact)),
+:math:`\ddot{q}, \dot{q}` are respectively the joint accelerations being optimized and the joint velocities.
+Note that because the jacobian is expressed in the distal link frame, the entries of the contact matrix specify
+the accelerations/velocities in that particular frame. For instance, [0,0,1,0,0,0] means that the z
+linear acceleration/velocity (where z is the z axis of the distal link frame) should be fixed.
+
+This is equivalent to the QP objective function :math:`||Ax - b||^2`, by setting :math:`A = C J_c(q)`,
+:math:`x = \ddot{q}`, and :math:`b = - C \dot{J}_c(q) \dot{q}`.
+
+This task is useful for instance if we want to keep the feet in contact with the ground.
 
 The implementation of this class is inspired by [1] (which is licensed under the LGPLv2).
 
@@ -9,8 +26,10 @@ References:
 """
 
 import numpy as np
+from scipy.linalg import block_diag
 
-from pyrobolearn.priorities.tasks import Task
+from pyrobolearn.priorities.tasks import JointAccelerationTask
+from pyrobolearn.utils.transformation import get_matrix_from_quaternion
 
 
 __author__ = "Brian Delhaisse"
@@ -23,17 +42,112 @@ __email__ = "briandelhaisse@gmail.com"
 __status__ = "Development"
 
 
-class ContactAccelerationTask(Task):
+class ContactAccelerationTask(JointAccelerationTask):
     r"""Contact Acceleration Task
 
+    The contact acceleration task tries to minimize the dynamic movement of a contact link:
+
+    .. math:: || C (J_c(q) \ddot{q} + \dot{J}_c(q) \dot{q}) ||^2,
+
+    where :math:`C \in \mathbb{R}^{6 \times 6}` is the contact matrix (=a diagonal selector matrix where the entries
+    are 1 for cartesian velocities that we wish to minimize such that the link doesn't move, and 0 for cartesian
+    velocities that are free to change), :math:`J_c(q)` is the contact Jacobian (the Jacobian from the world frame to
+    the contact point expressed in the distal link frame (i.e. the link which is in contact)),
+    :math:`\ddot{q}, \dot{q}` are respectively the joint accelerations being optimized and the joint velocities.
+    Note that because the jacobian is expressed in the distal link frame, the entries of the contact matrix specify
+    the accelerations/velocities in that particular frame. For instance, [0,0,1,0,0,0] means that the z
+    linear acceleration/velocity (where z is the z axis of the distal link frame) should be fixed.
+
+    This is equivalent to the QP objective function :math:`||Ax - b||^2`, by setting :math:`A = C J_c(q)`,
+    :math:`x = \ddot{q}`, and :math:`b = - C \dot{J}_c(q) \dot{q}`.
+
+    This task is useful for instance if we want to keep the feet in contact with the ground.
+
+
+    The implementation of this class is inspired by [1] (which is licensed under the LGPLv2).
+
+    References:
+        - [1] "OpenSoT: A whole-body control library for the compliant humanoid robot COMAN", Rocchi et al., 2015
     """
 
-    def __init__(self, model, constraints=[]):
+    def __init__(self, model, distal_link, contact_matrix=1., weight=1., constraints=[]):
         """
         Initialize the task.
 
         Args:
-            model (ModelInterface): model interface
+            model (ModelInterface): model interface.
+            distal_link (int, str): distal link id or name.
+            contact_matrix (np.array[float[6]], np.array[float[6,6]], None): contact selector matrix (=a diagonal
+              square matrix). You can specify only the diagonal elements if you wish. If None, by default it will be
+              set to the identity matrix.
+            weight (float, np.array[float[6,6]]): weight scalar or matrix associated to the task.
             constraints (list of Constraint): list of constraints associated with the task.
         """
-        super(ContactAccelerationTask, self).__init__(model=model, constraints=constraints)
+        super(ContactAccelerationTask, self).__init__(model=model, weight=weight, constraints=constraints)
+
+        # set variable
+        self.distal_link = self.model.get_link_id(distal_link)
+        self.contact_matrix = contact_matrix
+
+        # set QP vector
+        self._b = np.zeros(6)
+
+        # first update
+        self.update()
+
+    ##############
+    # Properties #
+    ##############
+
+    @property
+    def contact_matrix(self):
+        """Return the contact selector matrix."""
+        return self._contact_matrix
+
+    @contact_matrix.setter
+    def contact_matrix(self, matrix):
+        """Set the contact selector matrix."""
+        if matrix is None:
+            matrix = np.identity(6)
+
+        # check contact matrix type
+        if not isinstance(matrix, (int, float, np.ndarray)):
+            raise TypeError("Expecting the given contact matrix to be an int, float, or diagonal np.array, instead "
+                            "got: {}".format(type(matrix)))
+
+        # if numpy array, check its shape and make sure it is a diagonal matrix
+        if isinstance(matrix, np.ndarray):
+            if matrix.shape == (6,):
+                matrix = np.diag(matrix)
+            elif matrix.shape != (6, 6):
+                raise ValueError("Expecting the given contact matrix to be of shape (6,6), instead got a shape of: "
+                                 "{}".format(type(matrix)))
+            else:
+                # make sure the contact matrix is a diagonal matrix
+                matrix = np.diag(np.diag(matrix))
+
+        # set the contact matrix
+        self._contact_matrix = matrix
+
+    ###########
+    # Methods #
+    ###########
+
+    def _update(self):
+        """
+        Update the task by computing the A matrix and b vector that will be used by the task solver.
+        """
+        # get jacobians expressed in the world frame
+        jacobian = self.model.get_jacobian(link=self.distal_link)
+        jdotqdot = self.model.compute_JdotQdot(link=self.distal_link)
+
+        # express jacobians in the distal/contact link frame
+        orientation = self.model.get_orientation(link=self.distal_link)  # shape: (4,)
+        rot = get_matrix_from_quaternion(orientation).T  # shape: (3,3)
+        rot = block_diag(rot, rot)  # shape: (6,6)
+        jacobian = rot.dot(jacobian)
+        jdotqdot = rot.dot(jdotqdot)
+
+        # set A and b
+        self._A = np.dot(self.contact_matrix, jacobian)
+        self._b = np.dot(self.contact_matrix, jdotqdot)

pyrobolearn/priorities/tasks/acceleration/postural.py

@@ -1,6 +1,18 @@
 #!/usr/bin/env python
 r"""Provide the postural acceleration task.
 
+The postural task tries to bring the robot to a reference posture; that is, it minimizes the joint accelerations
+such that it gets close to the specified posture (given by the desired joint positions, velocities, and
+accelerations):
+
+.. math:: || \ddot{q} - (\ddot{q}_d + K_d (\dot{q}_d - \dot{q}) + K_p (q_d - q)) ||^2,
+
+where :math:`\ddot{q}, \dot{q}, q` are respectively the joint accelerations being optimized, joint velocities and
+positions, :math:`K_p` and :math:`K_d` are the position and velocity gains respectively, and the subscript
+:math:`d` means "desired".
+
+This is equivalent to the QP objective function :math:`||Ax - b||^2`, by setting :math:`A=I`, :math:`x=\dot{q}`,
+and :math:`b = \ddot{q}_d + K_d (\dot{q}_d - \dot{q}) + K_p (q_d - q)`.
 
 The implementation of this class is inspired by [1] (which is licensed under the LGPLv2).
 
@@ -10,7 +22,7 @@ References:
 
 import numpy as np
 
-from pyrobolearn.priorities.tasks import Task
+from pyrobolearn.priorities.tasks import JointAccelerationTask
 
 
 __author__ = "Brian Delhaisse"
@@ -23,17 +35,224 @@ __email__ = "briandelhaisse@gmail.com"
 __status__ = "Development"
 
 
-class PosturalAccelerationTask(Task):
+class PosturalAccelerationTask(JointAccelerationTask):
     r"""Postural Acceleration Task
 
+    The postural task tries to bring the robot to a reference posture; that is, it minimizes the joint accelerations
+    such that it gets close to the specified posture (given by the desired joint positions, velocities, and
+    accelerations):
+
+    .. math:: || \ddot{q} - (\ddot{q}_d + K_d (\dot{q}_d - \dot{q}) + K_p (q_d - q)) ||^2,
+
+    where :math:`\ddot{q}, \dot{q}, q` are respectively the joint accelerations being optimized, joint velocities and
+    positions, :math:`K_p` and :math:`K_d` are the position and velocity gains respectively, and the subscript
+    :math:`d` means "desired".
+
+    This is equivalent to the QP objective function :math:`||Ax - b||^2`, by setting :math:`A=I`, :math:`x=\dot{q}`,
+    and :math:`b = \ddot{q}_d + K_d (\dot{q}_d - \dot{q}) + K_p (q_d - q)`.
+
+    The implementation of this class is inspired by [1] (which is licensed under the LGPLv2).
+
+    References:
+        - [1] "OpenSoT: A whole-body control library for the compliant humanoid robot COMAN", Rocchi et al., 2015
+
     """
 
-    def __init__(self, model, constraints=[]):
+    def __init__(self, model, q_desired=None, dq_desired=None, ddq_desired=None, kp=1., kd=1., weight=1.,
+                 constraints=[]):
         """
         Initialize the task.
 
         Args:
-            model (ModelInterface): model interface
-            constraints (list of Constraint): list of constraints associated with the task.
+            model (ModelInterface): model interface.
+            q_desired (np.array[float[N]], None): desired joint positions, where :math:`N` is the number of DoFs. If
+              None, it will not be considered.
+            dq_desired (np.array[float[N]], None): desired joint velocities, where :math:`N` is the number of DoFs.
+              If None, it will be set to 0.
+            ddq_desired (np.array[float[N]], None): desired joint accelerations, where :math:`N` is the number of DoFs.
+              If None, it will be set to 0.
+            kp (float, np.array[float[N,N]]): position gain(s).
+            kd (float, np.array[float[N,N]]): velocity gain(s).
+            weight (float, np.array[float[N,N]]): weight scalar or matrix associated to the task.
+            constraints (list[Constraint]): list of constraints associated with the task.
         """
-        super(PosturalAccelerationTask, self).__init__(model=model, constraints=constraints)
+        super(PosturalAccelerationTask, self).__init__(model=model, weight=weight, constraints=constraints)
+
+        # define variables
+        self.kp = kp
+        self.kd = kd
+
+        # define desired references
+        self.q_desired = q_desired
+        self.dq_desired = dq_desired
+        self.ddq_desired = ddq_desired
+
+        # first update
+        self.update()
+
+    ##############
+    # Properties #
+    ##############
+
+    @property
+    def q_desired(self):
+        """Get the desired joint positions."""
+        return self._q_d
+
+    @q_desired.setter
+    def q_desired(self, q_d):
+        """Set the desired joint positions."""
+        if q_d is not None:
+            if not isinstance(q_d, (np.ndarray, list, tuple)):
+                raise TypeError("Expecting the given desired joint positions to be an instance of np.array, instead "
+                                "got: {}".format(type(q_d)))
+            q_d = np.asarray(q_d)
+            if len(q_d) != self.x_size:
+                raise ValueError("Expecting the length of the given desired joint positions (={}) to be the same as "
+                                 "the number of DoFs (={})".format(len(q_d), self.x_size))
+        self._q_d = q_d
+
+    @property
+    def dq_desired(self):
+        """Get the desired joint velocities."""
+        return self._dq_d
+
+    @dq_desired.setter
+    def dq_desired(self, dq_d):
+        """Set the desired joint velocities."""
+        if dq_d is None:
+            dq_d = np.zeros(self.x_size)
+        if not isinstance(dq_d, (np.ndarray, list, tuple)):
+            raise TypeError("Expecting the given desired joint velocities to be an instance of np.array, instead "
+                            "got: {}".format(type(dq_d)))
+        dq_d = np.asarray(dq_d)
+        if len(dq_d) != self.x_size:
+            raise ValueError("Expecting the length of the given desired joint velocities (={}) to be the same as the "
+                             "number of DoFs (={})".format(len(dq_d), self.x_size))
+        self._dq_d = dq_d
+
+    @property
+    def ddq_desired(self):
+        """Get the desired joint velocities."""
+        return self._ddq_d
+
+    @ddq_desired.setter
+    def ddq_desired(self, ddq_d):
+        """Set the desired joint velocities."""
+        if ddq_d is None:
+            ddq_d = np.zeros(self.x_size)
+        if not isinstance(ddq_d, (np.ndarray, list, tuple)):
+            raise TypeError("Expecting the given desired joint accelerations to be an instance of np.array, instead "
+                            "got: {}".format(type(ddq_d)))
+        ddq_d = np.asarray(ddq_d)
+        if len(ddq_d) != self.x_size:
+            raise ValueError("Expecting the length of the given desired joint accelerations (={}) to be the same as "
+                             "the number of DoFs (={})".format(len(ddq_d), self.x_size))
+        self._ddq_d = ddq_d
+
+    @property
+    def x_desired(self):
+        """Get the desired joint positions."""
+        return self._q_d
+
+    @x_desired.setter
+    def x_desired(self, q_d):
+        """Set the desired joint positions."""
+        self.q_desired = q_d
+
+    @property
+    def dx_desired(self):
+        """Get the desired joint velocities."""
+        return self._dq_d
+
+    @dx_desired.setter
+    def dx_desired(self, dq_d):
+        """Set the desired joint velocities."""
+        self.dq_desired = dq_d
+
+    @property
+    def ddx_desired(self):
+        """Get the desired joint accelerations."""
+        return self._ddq_d
+
+    @ddx_desired.setter
+    def ddx_desired(self, ddq_d):
+        """Set the desired joint accelerations."""
+        self.ddq_desired = ddq_d
+
+    @property
+    def kp(self):
+        """Return the position gain."""
+        return self._kp
+
+    @kp.setter
+    def kp(self, kp):
+        """Set the position gain."""
+        if kp is None:
+            kp = 1.
+        if not isinstance(kp, (float, int, np.ndarray)):
+            raise TypeError("Expecting the given position gain kp to be an int, float, np.array, instead got: "
+                            "{}".format(type(kp)))
+        if isinstance(kp, np.ndarray) and kp.shape != (self.x_size, self.x_size):
+            raise ValueError("Expecting the given position gain matrix kp to be of shape {}, but instead got "
+                             "shape: {}".format((self.x_size, self.x_size), kp.shape))
+        self._kp = kp
+
+    @property
+    def kd(self):
+        """Return the velocity gain."""
+        return self._kd
+
+    @kd.setter
+    def kd(self, kd):
+        """Set the velocity gain."""
+        if kd is None:
+            kd = 1.
+        if not isinstance(kd, (float, int, np.ndarray)):
+            raise TypeError("Expecting the given velocity gain kd to be an int, float, np.array, instead got: "
+                            "{}".format(type(kd)))
+        if isinstance(kd, np.ndarray) and kd.shape != (self.x_size, self.x_size):
+            raise ValueError("Expecting the given velocity gain matrix kd to be of shape {}, but instead got "
+                             "shape: {}".format((self.x_size, self.x_size), kd.shape))
+        self._kd = kd
+
+    ###########
+    # Methods #
+    ###########
+
+    def set_desired_references(self, x_des, dx_des=None, ddx_des=None, *args, **kwargs):
+        """Set the desired references.
+
+        Args:
+            x_des (np.array[float[N]], None): desired joint positions, where :math:`N` is the number of DoFs. If None,
+              it will be set to 0.
+            dx_des (np.array[float[N]], None): desired joint velocities, where :math:`N` is the number of DoFs. If
+              None, it will be set to 0.
+            ddx_des (np.array[float[N]], None): desired joint accelerations, where :math:`N` is the number of DoFs. If
+              None, it will be set to 0.
+        """
+        self.x_desired = x_des
+        self.dx_desired = dx_des
+        self.ddx_desired = ddx_des
+
+    def get_desired_references(self):
+        """Return the desired references.
+
+        Returns:
+            np.array[float[N]]: desired joint positions.
+            np.array[float[N]]: desired joint velocities.
+            np.array[float[N]]: desired joint accelerations.
+        """
+        return self.x_desired, self.dx_desired, self.ddx_desired
+
+    def _update(self):
+        """
+        Update the task by computing the A matrix and b vector that will be used by the task solver.
+        """
+        q = self.model.get_joint_positions()
+        dq = self.model.get_joint_velocities()
+        self._b = self._ddq_d + np.dot(self.kd, (self._dq_d - dq))
+
+        # update b vector
+        if self._q_d is not None:
+            self._b += np.dot(self.kp, (self._q_d - q))  # shape: (N,)

pyrobolearn/priorities/tasks/dynamic_tasks.py

@@ -1,6 +1,8 @@
 #!/usr/bin/env python
 """Provide the various dynamic tasks (i.e. objective functions) used in QP.
 
+DEPRECATED.
+
 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

pyrobolearn/priorities/tasks/kinematic_tasks.py

@@ -1,6 +1,8 @@
 #!/usr/bin/env python
 """Provide the various kinematic tasks (i.e. objective functions) used in QP.
 
+DEPRECATED.
+
 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

pyrobolearn/priorities/tasks/task_from_constraint.py

@@ -0,0 +1,79 @@
+#!/usr/bin/env python
+r"""Transform a given equality constraint into a task.
+
+An equality constraint specified by :math:`Fx = k` is transformed to a soft task :math:`||Ax - b||^2`, where
+:math:`A = F` and :math:`b = k`. This allows for the equality constraint to be lightly violated; by specifying the
+weight :math:`W` we can specify how much the constraint should be satisfied.
+"""
+
+from pyrobolearn.priorities.tasks import Task
+from pyrobolearn.priorities.constraints import EqualityConstraint
+
+
+__author__ = "Brian Delhaisse"
+__copyright__ = "Copyright 2019, PyRoboLearn"
+__credits__ = ["Brian Delhaisse"]
+__license__ = "GNU GPLv3"
+__version__ = "1.0.0"
+__maintainer__ = "Brian Delhaisse"
+__email__ = "briandelhaisse@gmail.com"
+__status__ = "Development"
+
+
+class TaskFromConstraint(Task):
+    r"""Task From Equality Constraint
+
+    An equality constraint specified by :math:`Fx = k` is transformed to a soft task :math:`||Ax - b||_{W}^2`, where
+    :math:`A = F` and :math:`b = k`. This allows for the equality constraint to be lightly violated; by specifying the
+    weight :math:`W` we can specify how much the constraint should be satisfied.
+    """
+
+    def __init__(self, constraint, weight=1., constraints=[]):
+        """
+        Initialize the task.
+
+        Args:
+            constraint (EqualityConstraint): equality constraint.
+            weight (float, np.array[float[6,6]], np.array[float[3,3]]): weight scalar or matrix associated to the task.
+            constraints (list[Constraint]): list of constraints associated with the task.
+        """
+        # set equality constraint
+        self.equality_constraint = constraint
+
+        # call superclass
+        super(TaskFromConstraint, self).__init__(model=constraint.model, weight=weight, constraints=constraints)
+
+        # first update
+        self.update()
+
+    ##############
+    # Properties #
+    ##############
+
+    @property
+    def equality_constraint(self):
+        """Get the equality constraint."""
+        return self._constraint
+
+    @equality_constraint.setter
+    def equality_constraint(self, constraint):
+        """Set the equality constraint."""
+        if not isinstance(constraint, EqualityConstraint):
+            raise TypeError("Expecting the given 'constraint' to be an instance of `EqualityConstraint`, but "
+                            "instead got: {}".format(constraint))
+        self._constraint = constraint
+
+    ###########
+    # Methods #
+    ###########
+
+    def _update(self):
+        """
+        Update the task by computing the A matrix and b vector that will be used by the task solver.
+        """
+        # update equality constraint
+        self.equality_constraint.update()
+
+        # update A and b
+        self._A = self.equality_constraint.A_eq
+        self._b = self.equality_constraint.b_eq

pyrobolearn/priorities/tasks/velocity/cartesian.py

@@ -1,15 +1,15 @@
 #!/usr/bin/env python
 r"""Provide the cartesian (velocity) task.
 
-The cartesian task tries to impose a desired pose (position and orientation) of a distal link with respect to a
-base link or the world frame. The minimization problem is given by:
+The cartesian task tries to impose a desired pose (position and orientation) and velocity of a distal link with
+respect to a base link or the world frame. The minimization problem is given by:
 
 .. math:: || ^bJ_d(q) \dot{q} - (K_p e + \dot{x}_d) ||^2
 
 where :math:`^bJ_d(q) \in \mathbb{R}^{6 \times N}` is the Jacobian taken from the base to the distal link,
 :math:`\dot{q}` are the joint velocities being optimized, :math:`K_p` is the stiffness gain,
 :math:`e \in \mathbb{R}^{6}` is the error which is the concatenation of the position error given by
-:math:`e_{p} = (x_d - x)` (with :math:`x_d` being the desired pose, and :math:`x` the current pose), and the
+:math:`e_{p} = (x_d - x)` (with :math:`x_d` being the desired position, and :math:`x` the current position), and the
 orientation error given by (if expressed as quaternions :math:`o = {s, v}` where :math:`s` is the real scalar part,
 and :math:`v` is the vector part) :math:`e_{o} = s v_d - s_d v - v_d \cross v`, and :math:`\dot{x}_d` is the
 desired cartesian velocity for the distal link with respect to the base link.
@@ -48,22 +48,27 @@ __status__ = "Development"
 class CartesianTask(JointVelocityTask):
     r"""Cartesian (velocity) Task
 
-    The cartesian task tries to impose a desired pose (position and orientation) of a distal link with respect to a
-    base link or the world frame. The minimization problem is given by:
+    The cartesian task tries to impose a desired pose (position and orientation) and velocity of a distal link with
+    respect to a base link or the world frame. The minimization problem is given by:
 
     .. math:: || ^bJ_d(q) \dot{q} - (K_p e + \dot{x}_d) ||^2
 
     where :math:`^bJ_d(q) \in \mathbb{R}^{6 \times N}` is the Jacobian taken from the base to the distal link,
     :math:`\dot{q}` are the joint velocities being optimized, :math:`K_p` is the stiffness gain,
     :math:`e \in \mathbb{R}^{6}` is the error which is the concatenation of the position error given by
-    :math:`e_{p} = (x_d - x)` (with :math:`x_d` being the desired pose, and :math:`x` the current pose), and the
-    orientation error given by (if expressed as quaternions :math:`o = {s, v}` where :math:`s` is the real scalar part,
-    and :math:`v` is the vector part) :math:`e_{o} = s v_d - s_d v - v_d \cross v`, and :math:`\dot{x}_d` is the
+    :math:`e_{p} = (x_d - x)` (with :math:`x_d` being the desired position, and :math:`x` the current position), and
+    the orientation error given by (if expressed as quaternions :math:`o = {s, v}` where :math:`s` is the real scalar
+    part, and :math:`v` is the vector part) :math:`e_{o} = s v_d - s_d v - v_d \cross v`, and :math:`\dot{x}_d` is the
     desired cartesian velocity for the distal link with respect to the base link.
 
     This is equivalent to the QP objective function :math:`||Ax - b||^2`, by setting :math:`A = ^bJ_d(q)`,
     :math:`x = \dot{q}`, and :math:`b = K_p e + \dot{x}_d`.
 
+    Important notes:
+
+    - You don't have to specify the whole pose, you can also only specify the position or orientation.
+    - You can also only specify the cartesian velocities without providing the cartesian position or orientation.
+
     The implementation of this class is inspired by [1] (which is licensed under the LGPLv2).
 
     References:
@@ -91,7 +96,7 @@ class CartesianTask(JointVelocityTask):
               If None, it will be set to zero.
             kp_position (float, np.array[float[3,3]]): position stiffness gain.
             kp_orientation (float, np.array[float[3,3]]): orientation stiffness gain.
-            weight (float, np.array[float[6,6]]): weight scalar or matrix associated to the task.
+            weight (float, np.array[float[6,6]], np.array[float[3,3]]): weight scalar or matrix associated to the task.
             constraints (list[Constraint]): list of constraints associated with the task.
         """
         super(CartesianTask, self).__init__(model=model, weight=weight, constraints=constraints)
@@ -304,6 +309,7 @@ class CartesianTask(JointVelocityTask):
 
     def get_desired_references(self):
         """Return the desired references.
+
         Returns:
             np.array[float[7]]: desired cartesian pose (position and quaternion [x,y,z,w]) of distal link wrt the base.
             np.array[float[6]]: desired cartesian velocity of distal link wrt the base.

pyrobolearn/priorities/tasks/velocity/com.py

@@ -1,7 +1,7 @@
 #!/usr/bin/env python
 r"""Provide the center of mass velocity task.
 
-The CoM task tries to impose a desired position of the CoM with respect to the world frame.
+The CoM task tries to impose a desired position and (linear) velocity of the CoM with respect to the world frame.
 
 .. math:: ||J_{CoM} \dot{q} - (K_p (x_d - x) + \dot{x}_d)||^2
 
@@ -39,7 +39,7 @@ __status__ = "Development"
 class CoMTask(JointVelocityTask):
     r"""Center of Mass Velocity Task
 
-    The CoM task tries to impose a desired position of the CoM with respect to the world frame.
+    The CoM task tries to impose a desired position and (linear) velocity of the CoM with respect to the world frame.
 
     .. math:: ||J_{CoM} \dot{q} - (K_p (x_d - x) + \dot{x}_d)||^2
 
@@ -50,6 +50,8 @@ class CoMTask(JointVelocityTask):
     This is equivalent to the QP objective function :math:`||Ax - b||^2`, by setting :math:`A=J_{CoM}`,
     :math:`x=\dot{q}`, and :math:`b = K_p (x_d - x) + \dot{x}_d`.
 
+    Note that you can only specify the center of mass linear velocity if you wish.
+
     The implementation of this class is inspired by [1] (which is licensed under the LGPLv2).
 
     References:

pyrobolearn/priorities/tasks/velocity/manipulability.py

@@ -28,7 +28,7 @@ __status__ = "Development"
 class ManipulabilityTask(JointVelocityTask):
     r"""Manipulability Task
 
-    The manipulability task implements a tasks that tries to maximize the manipulability measure given in [1]:
+    The manipulability task implements a task that tries to maximize the manipulability measure given in [1]:
 
     .. math:: w(q) = \sqrt{ \det( J(q) W J(q)^\top ) }
 

pyrobolearn/priorities/tasks/velocity/minimum_acceleration.py

@@ -3,13 +3,12 @@ r"""Provide the minimum acceleration task.
 
 The minimum acceleration task tries to minimize the change in velocity, that is, it minimizes:
 
-.. math:: || \dot{q}_t - \dot{q}_{t-1} ||^2
+.. math:: || \dot{q} - \dot{q}_t ||^2
 
-where :math:`\dot{q}_t` are the current joint velocities being optimized, and :math:`\dot{q}_{t-1}` are the
-previous joint velocities.
+where :math:`\dot{q}` are the joint velocities being optimized, and :math:`\dot{q}_t` are the current joint velocities.
 
 This is equivalent to the QP objective function :math:`||Ax - b||^2`, by setting :math:`A = I`, :math:`x = \dot{q}`,
-and :math:`b = \dot{q}_{t-1}`.
+and :math:`b = \dot{q}_t`.
 
 
 The implementation of this class is inspired by [1] (which is licensed under the LGPLv2).
@@ -38,13 +37,13 @@ class MinAccelerationTask(JointVelocityTask):
 
     The minimum acceleration task tries to minimize the change in velocity, that is, it minimizes:
 
-    .. math:: || \dot{q}_t - \dot{q}_{t-1} ||^2
+    .. math:: || \dot{q} - \dot{q}_t ||^2
 
-    where :math:`\dot{q}_t` are the current joint velocities being optimized, and :math:`\dot{q}_{t-1}` are the
-    previous joint velocities.
+    where :math:`\dot{q}` are the joint velocities being optimized, and :math:`\dot{q}_t` are the current joint
+    velocities.
 
-    This is equivalent to the QP objective function :math:`||Ax - b||^2`, by setting :math:`A = I`, :math:`x = \dot{q}`,
-    and :math:`b = \dot{q}_{t-1}`.
+    This is equivalent to the QP objective function :math:`||Ax - b||^2`, by setting :math:`A = I`,
+    :math:`x = \dot{q}`, and :math:`b = \dot{q}_t`.
 
 
     The implementation of this class is inspired by [1] (which is licensed under the LGPLv2).

pyrobolearn/priorities/tasks/velocity/minimum_effort.py

@@ -8,8 +8,6 @@ References:
     - [1] "OpenSoT: A whole-body control library for the compliant humanoid robot COMAN", Rocchi et al., 2015
 """
 
-# TODO: finish to implement this
-
 import numpy as np
 
 from pyrobolearn.priorities.tasks import JointVelocityTask

pyrobolearn/priorities/tasks/velocity/postural.py

@@ -147,6 +147,8 @@ class PosturalTask(JointVelocityTask):
     @kp.setter
     def kp(self, kp):
         """Set the stiffness gain."""
+        if kp is None:
+            kp = 1.
         if not isinstance(kp, (float, int, np.ndarray)):
             raise TypeError("Expecting the given stiffness gain kp to be an int, float, np.array, instead got: "
                             "{}".format(type(kp)))