fix bugs in priorities

pyrobolearn/priorities/constraints/velocity/convex_hull.py

@@ -1,15 +1,22 @@
 #!/usr/bin/env python
 r"""Provide the Convex Hull constraint.
 
-From the documentation of the framework of [1]: "this constraint implements a constraint of the type:
+This convex hull constraints make sure that the projected CoM position (in the x-y directions) belongs to the
+convex hull (i.e. support polygon). This can be defined as:
 
-.. math:: A_{CH} J_{CoM} \dot{q} \leq b_{CH}
+.. math:: A_{CH} J_{CoM} \dot{q} dt \leq b_{CH} - A_{CH} x_{CoM} - d
 
-where the number of row for :math:`A_{CH} \in \mathbb{R}^{F \times 3}` and :math:`b_{CH} \in \mathbb{F}` are the
-number of facets :math:`F` in the convex hull."
+where :math:`A_{CH} \in \mathbb{R}^{F \times 2}` and :math:`b_{CH} \in \mathbb{R}^F` are the matrix and vector
+that appears in the convex hull hyperplane equation :math:`A_{CH} x \leq b_{CH}`, where :math:`F` is the number
+of facets in the convex hull, :math:`J_{CoM} \in \mathbb{R}^{2 \times N}` is the truncated CoM Jacobian that only
+accounts for the x and y direction components, :math:`\dot{q}` are the joint velocities being optimized,
+:math:`x_{CoM} \in \mathbb{R}^2` is the current position of the CoM (due to the current joint configuration
+:math:`q`) in the x-y directions, :math:`dt` is the integration time step, and :math:`d` is a safety margin
+distance. Note that :math:`A_{CH}` has also be truncated to only take into account the x-y components (i.e.
+:math:`A_{CH} \in \mathbb{R}^{F \times 2}` and not :math:`A_{CH} \in \mathbb{R}^{F \times 3}`).
 
 This formulation can be rewritten as a upper unilateral inequality constraint :math:`A_{ineq} x \leq b_u` in QP,
-with :math:`x = \dot{q}`, :math:`A_{ineq} = A_{CH} J_{CoM}`, and :math:`b_u = b_{CH}`.
+with :math:`x = \dot{q}`, :math:`A_{ineq} = A_{CH} J_{CoM} dt`, and :math:`b_u = b_{CH} - A_{CH} x_{CoM} - d`.
 
 Note that computing the ConvexHull at each time step can be quite expensive from a computing point of view, as
 such you can specify the number of ticks to sleep before the next computation.
@@ -23,7 +30,7 @@ References:
 """
 
 import numpy as np
-from scipy.spatial import ConvexHull
+import scipy.spatial.qhull as qhull
 
 from pyrobolearn.priorities.constraints.constraint import UpperUnilateralConstraint, JointVelocityConstraint
 
@@ -41,15 +48,22 @@ __status__ = "Development"
 class ConvexHullConstraint(UpperUnilateralConstraint, JointVelocityConstraint):
     r"""Convex Hull constraint.
 
-    From the documentation of the framework of [1]: "this constraint implements a constraint of the type:
+    This convex hull constraints make sure that the projected CoM position (in the x-y directions) belongs to the
+    convex hull (i.e. support polygon). This can be defined as:
 
-    .. math:: A_{CH} J_{CoM} \dot{q} \leq b_{CH}
+    .. math:: A_{CH} J_{CoM} \dot{q} dt \leq b_{CH} - A_{CH} x_{CoM} - d
 
-    where the number of row for :math:`A_{CH} \in \mathbb{R}^{F \times 3}` and :math:`b_{CH} \in \mathbb{F}` are the
-    number of facets :math:`F` in the convex hull."
+    where :math:`A_{CH} \in \mathbb{R}^{F \times 2}` and :math:`b_{CH} \in \mathbb{R}^F` are the matrix and vector
+    that appears in the convex hull hyperplane equation :math:`A_{CH} x \leq b_{CH}`, where :math:`F` is the number
+    of facets in the convex hull, :math:`J_{CoM} \in \mathbb{R}^{2 \times N}` is the truncated CoM Jacobian that only
+    accounts for the x and y direction components, :math:`\dot{q}` are the joint velocities being optimized,
+    :math:`x_{CoM} \in \mathbb{R}^2` is the current position of the CoM (due to the current joint configuration
+    :math:`q`) in the x-y directions, :math:`dt` is the integration time step, and :math:`d` is a safety margin
+    distance. Note that :math:`A_{CH}` has also be truncated to only take into account the x-y components (i.e.
+    :math:`A_{CH} \in \mathbb{R}^{F \times 2}` and not :math:`A_{CH} \in \mathbb{R}^{F \times 3}`).
 
     This formulation can be rewritten as a upper unilateral inequality constraint :math:`A_{ineq} x \leq b_u` in QP,
-    with :math:`x = \dot{q}`, :math:`A_{ineq} = A_{CH} J_{CoM}`, and :math:`b_u = b_{CH}`.
+    with :math:`x = \dot{q}`, :math:`A_{ineq} = A_{CH} J_{CoM} dt`, and :math:`b_u = b_{CH} - A_{CH} x_{CoM} - d`.
 
     Note that computing the ConvexHull at each time step can be quite expensive from a computing point of view, as
     such you can specify the number of ticks to sleep before the next computation.
@@ -62,28 +76,50 @@ class ConvexHullConstraint(UpperUnilateralConstraint, JointVelocityConstraint):
         - [2] ConvexHull: https://docs.scipy.org/doc/scipy-0.19.0/reference/generated/scipy.spatial.ConvexHull.html
     """
 
-    def __init__(self, model, points=[], ticks=20):
+    def __init__(self, model, dt, points=[], ticks=20, safety_margin=0.):
         r"""
         Initialize the constraint.
 
         Args:
             model (ModelInterface): model interface.
+            dt (float): integration time step used to compute :math:`dq = \dot{q} dt`.
             points (list[np.array[float[3]]]): list of 3D contact points. The convex hull will be built using these.
             ticks (ticks): the number of ticks to sleep before updating. Calculating the convex hull can be quite
               computing demanding.
+            safety_margin (float): safety margin distance. This will be removed from the computed b vector.
         """
         super(ConvexHullConstraint, self).__init__(model)
 
+        # set variables
+        self.dt = dt
         self.ticks = ticks
         self._cnt = 0
         self._hull = None
-
+        self.safety_margin = safety_margin
         self.points = points
 
+        # first update
+        self.update()
+        self._cnt = 0
+
     ##############
     # 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 ticks(self):
         """Return the number of ticks to sleep before the next update."""
@@ -112,6 +148,21 @@ class ConvexHullConstraint(UpperUnilateralConstraint, JointVelocityConstraint):
             points = points.reshape(-1, 3)  # (M,3)
         self._points = points
 
+        if len(points) > 2:  # we need at least 3 (different) points to construct the convex hull
+            self.disable()
+        else:
+            self.enable()
+
+    @property
+    def safety_margin(self):
+        """Return the safety margin distance."""
+        return self._margin
+
+    @safety_margin.setter
+    def safety_margin(self, margin):
+        """Set the safety margin distance."""
+        self._margin = float(margin) if margin >= 0 else 0.
+
     @property
     def hull(self):
         """Return the convex hull instance."""
@@ -131,20 +182,27 @@ class ConvexHullConstraint(UpperUnilateralConstraint, JointVelocityConstraint):
         """Update the :math:`A_{ineq}` matrix and the :math:`b_u` vector"""
         # if time to update
         if self._cnt % self._ticks == 0:
-            # convex hull
-            hull = ConvexHull(self._points)  # compute convex hull
-            self._hull = hull
+            # compute convex hull
+            try:
+                self._hull = qhull.ConvexHull(self._points)
+            except qhull.QhullError:
+                print("Not enough different points to compute the convex hull, using the old one.")
 
-            # convex hull equations
-            A = hull.equations[:, :-1]
-            b = 1 * hull.equations[:, -1]
+            if self._hull is not None:
 
-            # get jacobian
-            jacobian = self.model.get_com_jacobian(full=False)  # shape: (3,N)
+                # convex hull equations
+                A = self._hull.equations[:, :-2]  # shape: (F,2) - we only care about x and y (and not z))
+                b = 1 * self._hull.equations[:, -1] - self.safety_margin
 
-            # constraint matrix and vector
-            self._A_ineq = A.dot(jacobian)  # (F,N)
-            self._b_upper_bound = b  # (F,)
+                # get jacobian (note that we only care about x and y (and not z))
+                jacobian = self.model.get_com_jacobian(full=False)[:2]  # shape: (2,N)
+
+                # get current com position
+                x_com = self.model.get_com_position()[:2]  # shape: (2,)
+
+                # constraint matrix and vector
+                self._A_ineq = A.dot(jacobian)  # (F,N)
+                self._b_upper_bound = b - self._margin - A.dot(x_com)  # (F,)
 
             # reset counter
             self._cnt = 0

pyrobolearn/priorities/constraints/velocity/joint_limits.py

@@ -78,7 +78,8 @@ class JointPositionLimitsConstraint(BoundConstraint, JointVelocityConstraint):
 
         # set variables
         if q_lower_bound is None or q_upper_bound is None:
-            q_lb, q_ub = self.model.get_joint_limits()
+            limits = self.model.get_joint_limits()
+            q_lb, q_ub = limits[:, 0], limits[:, 1]
             if q_lower_bound is None:
                 q_lower_bound = q_lb
             if q_upper_bound is None:
@@ -128,7 +129,7 @@ class JointPositionLimitsConstraint(BoundConstraint, JointVelocityConstraint):
                             "{}".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))
+                             "the number of variables being optimized (={}).".format(len(q_lb), self.x_size))
         self._q_lb = q_lb
 
     @property

pyrobolearn/priorities/models/model.py

@@ -124,8 +124,7 @@ class ModelInterface(object):
         Return the joint limits.
 
         Returns:
-            np.array[float[N]]: lower joint position limits.
-            np.array[float[N]]: upper joint position limits.
+            np.array[float[2, N]]: lower and upper joint position limits.
         """
         pass
 
@@ -134,8 +133,7 @@ class ModelInterface(object):
         Return the joint velocity limits.
 
         Returns:
-            np.array[float[N]]: lower joint velocity limits.
-            np.array[float[N]]: upper joint velocity limits.
+            np.array[float[2, N]]: lower and upper joint velocity limits.
         """
         pass
 
@@ -255,7 +253,7 @@ class ModelInterface(object):
             point (np.array[float[3]]): position of the point in link's local frame.
 
         Returns:
-            np.array[float[6,N]]: 6D Jacobian (=concatenation of the angular and linear Jacobian).
+            np.array[float[6,N]]: 6D Jacobian (=concatenation of the linear and angular Jacobian).
         """
         pass
 
@@ -484,6 +482,26 @@ class ModelInterface(object):
         """
         pass
 
+    def get_centroidal_momentum(self):
+        r"""
+        Return the centroidal momentum vector :math:`h_G = A_G \dot{q} \in \mathbb{R}^6`
+
+        Returns:
+            np.array[float[6]]: centroidal momentum vector.
+        """
+        pass
+
+    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}`
+        """
+        pass
+
     def update(self):
         """
         This is to notify the model interface that we moved to the next time step :math:`t \rightarrow t+1`.

pyrobolearn/priorities/models/robot_model.py

@@ -125,8 +125,7 @@ class RobotModelInterface(ModelInterface):
         Return the joint limits.
 
         Returns:
-            np.array[float[N]]: lower joint position limits.
-            np.array[float[N]]: upper joint position limits.
+            np.array[float[2, N]]: lower and upper joint position limits.
         """
         return self.model.get_joint_limits()
     
@@ -135,8 +134,7 @@ class RobotModelInterface(ModelInterface):
         Return the joint velocity limits.
 
         Returns:
-            np.array[float[N]]: lower joint velocity limits.
-            np.array[float[N]]: upper joint velocity limits.
+            np.array[float[2, N]]: lower and upper joint velocity limits.
         """
         dq = self.model.get_joint_max_velocities()
         return -dq, dq
@@ -581,6 +579,15 @@ class RobotModelInterface(ModelInterface):
         """
         return self.model.get_centroidal_momentum_matrix()
 
+    def get_centroidal_momentum(self):
+        r"""
+        Return the centroidal momentum vector :math:`h_G = A_G \dot{q} \in \mathbb{R}^6`
+
+        Returns:
+            np.array[float[6]]: centroidal momentum vector.
+        """
+        return self.model.get_centroidal_momentum()
+
     def get_centroidal_dynamics(self):
         r"""
         Return the centroidal momentum matrix :math:`A_G` and its derivative multiplied by the joint velocities

pyrobolearn/priorities/tasks/force/com.py

@@ -8,7 +8,7 @@ Dynamics":
 .. math::
 
     m * \ddot{r} = \sum_i f_i + mg \\
-    \dot{L} = \sum_i p_i \times f_i + \tau,
+    \dot{L} = \sum_i p_i \times f_i + \tau_i,
 
 where :math:`w = [f \tau] \in \mathbb{R}^6` is the wrench vector composed of a force vector
 :math:`f \in \mathbb{R}^3` and a torque vector :math:`\tau \in \mathbb{R}^3`, :math:`m` is the mass, :math:`r` is
@@ -16,17 +16,52 @@ the CoM position, :math:`g` is the gravity vector, :math:`L` is the angular mome
 the position vector of where the wrench is applied (with respect to the CoM), and the subscript :math:`i` is to
 denote each link where a wrench is applied to it (by contact).
 
+The task can be mathematically described as:
+
+.. math:: || A w - [m (\ddot{r}_{ref} - g)^\top, \dot{L}_{ref}^\top]^\top ||^2
+
+where :math:`A \in \mathbb{R}^{6 \times 6N_c}` (described below in more details), :math:`N_c` is the number of
+contact links, :math:`w = [f_1 \tau1 \cdot f_{N_c} \tau_{N_c}] \in \mathbb{R}^{6N_c}` is the wrench vector being
+optimized, :math:`m \in \mathbb{R}` is the total mass of the robot, :math:`g \in \mathbb{R}^3` is the gravity
+vector, :math:`\ddot{r}_{ref} \in \mathbb{R}^3` is the reference CoM acceleration vector (see below), and
+:math:`\dot{L}_{ref} \in \mathbb{R}^3` is the reference variation of the angular momentum around the CoM (see
+below).
+
+The reference vectors are given by:
+
+.. math::
+
+    \ddot{r}_{ref} = \ddot{r}_{des} + k_d (\dot{r}_{des} - \dot{r}) + k_p (r_{des} - r) \\
+    \dot{L}_{ref} = \dot{L}_{des} + k_d (L_{des} - L)
+
+The matrix :math:`A` is given:
+
+.. math::
+
+    A = \left[ \begin{array}{cc}
+        I_{3 \times 3} & 0_{3 \times 3} \\
+        S(p_1) & I_{3 \times 3}
+        \end{array} \cdot \begin{array}{cc}
+        I_{3 \times 3} & 0_{3 \times 3} \\
+        S(p_{N_c}) & I_{3 \times 3}
+        \end{array} \right]
+
+where :math:`S(p_i)` is the skew-symmetric matrix built from the contact position vector
+:math:`p_i \in \mathbb{R}^3`.
+
+The above formulation is equivalent to the QP objective function :math:`||Ax - b||^2`, by setting
+:math:`A = A`, :math:`x = w`, and :math:`b = [m (\ddot{r}_{ref} - g)^\top, \dot{L}_{ref}^\top]^\top`.
+
 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
 """
 
-# TODO: finish to implement this
-
 import numpy as np
 
 from pyrobolearn.priorities.tasks import ForceTask
+from pyrobolearn.utils.transformation import skew_matrix
 
 
 __author__ = "Brian Delhaisse"
@@ -49,7 +84,7 @@ class CoMForceTask(ForceTask):
     .. math::
 
         m * \ddot{r} = \sum_i f_i + mg \\
-        \dot{L} = \sum_i p_i \times f_i + \tau,
+        \dot{L} = \sum_i p_i \times f_i + \tau_i,
 
     where :math:`w = [f \tau] \in \mathbb{R}^6` is the wrench vector composed of a force vector
     :math:`f \in \mathbb{R}^3` and a torque vector :math:`\tau \in \mathbb{R}^3`, :math:`m` is the mass, :math:`r` is
@@ -57,21 +92,68 @@ class CoMForceTask(ForceTask):
     the position vector of where the wrench is applied (with respect to the CoM), and the subscript :math:`i` is to
     denote each link where a wrench is applied to it (by contact).
 
+    The task can be mathematically described as:
+
+    .. math:: || A w - [m (\ddot{r}_{ref} - g)^\top, \dot{L}_{ref}^\top]^\top ||^2
+
+    where :math:`A \in \mathbb{R}^{6 \times 6N_c}` (described below in more details), :math:`N_c` is the number of
+    contact links, :math:`w = [f_1 \tau1 \cdot f_{N_c} \tau_{N_c}] \in \mathbb{R}^{6N_c}` is the wrench vector being
+    optimized, :math:`m \in \mathbb{R}` is the total mass of the robot, :math:`g \in \mathbb{R}^3` is the gravity
+    vector, :math:`\ddot{r}_{ref} \in \mathbb{R}^3` is the reference CoM acceleration vector (see below), and
+    :math:`\dot{L}_{ref} \in \mathbb{R}^3` is the reference variation of the angular momentum around the CoM (see
+    below).
+
+    The reference vectors are given by:
+
+    .. math::
+
+        \ddot{r}_{ref} = \ddot{r}_{des} + k_d (\dot{r}_{des} - \dot{r}) + k_p (r_{des} - r) \\
+        \dot{L}_{ref} = \dot{L}_{des} + k_d (L_{des} - L)
+
+    The matrix :math:`A` is given:
+
+    .. math::
+
+        A = \left[ \begin{array}{cc}
+            I_{3 \times 3} & 0_{3 \times 3} \\
+            S(p_1) & I_{3 \times 3}
+            \end{array} \cdot \begin{array}{cc}
+            I_{3 \times 3} & 0_{3 \times 3} \\
+            S(p_{N_c}) & I_{3 \times 3}
+            \end{array} \right]
+
+    where :math:`S(p_i)` is the skew-symmetric matrix built from the contact position vector
+    :math:`p_i \in \mathbb{R}^3`.
+
+    The above formulation is equivalent to the QP objective function :math:`||Ax - b||^2`, by setting
+    :math:`A = A`, :math:`x = w`, and :math:`b = [m (\ddot{r}_{ref} - g)^\top, \dot{L}_{ref}^\top]^\top`.
+
     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, contact_links=[], wrenches=[], weight=1., constraints=[]):
+    def __init__(self, model, contact_links=[], desired_acceleration=None, desired_velocity=None,
+                 desired_position=None, desired_variation_angular_momentum=None, desired_angular_momentum=None,
+                 k_velocity=1., k_position=1., k_angular_momentum=1., weight=1., constraints=[]):
         """
         Initialize the task.
 
         Args:
             model (ModelInterface): model interface.
             contact_links (list[str], list[int]): list of unique contact link names or ids.
-            wrenches (list[np.array[float[6]]]): list of associated wrenches applied to the contact links. It
-              must have the same size as the number of contact links.
+            desired_acceleration (np.array[float[3]], None): desired CoM linear acceleration. 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_position (np.array[float[3]], None): desired CoM position. If None, it will not be considered.
+            desired_variation_angular_momentum (np.array[float[3]], None): desired CoM variation angular momentum. If
+              None, it will be set to 0.
+            desired_angular_momentum (np.array[float[3]], None): desired CoM angular momentum. If None, it will be set
+              to 0.
+            k_velocity (float, np.array[float[3,3]]): CoM velocity gain.
+            k_position (float, np.array[float[3,3]]): CoM position gain.
+            k_angular_momentum (float, np.array[float[3,3]]): CoM angular momentum.
             weight (float, np.array[float[6,6]]): weight scalar or matrix associated to the task.
             constraints (list[Constraint]): list of constraints associated with the task.
         """
@@ -79,7 +161,16 @@ class CoMForceTask(ForceTask):
 
         # set variables
         self.contact_links = contact_links
-        self.wrenches = wrenches
+        self.desired_acceleration = desired_acceleration
+        self.desired_velocity = desired_velocity
+        self.desired_position = desired_position
+        self.desired_variation_angular_momentum = desired_variation_angular_momentum
+        self.desired_angular_momentum = desired_angular_momentum
+
+        # set gains
+        self.k_velocity = k_velocity
+        self.k_position = k_position
+        self.k_angular_momentum = k_angular_momentum
 
         # first update
         self.update()
@@ -110,21 +201,152 @@ class CoMForceTask(ForceTask):
             self.enable()
 
     @property
-    def wrenches(self):
-        """Get the wrenches."""
-        return self._wrenches
+    def desired_acceleration(self):
+        """Get the desired CoM linear acceleration."""
+        return self._a_des
 
-    @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
+    @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._a_des = acceleration
+
+    @property
+    def desired_velocity(self):
+        """Get the desired CoM linear velocity."""
+        return self._v_des
+
+    @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._v_des = velocity
+
+    @property
+    def desired_position(self):
+        """Get the desired CoM position."""
+        return self._x_des
+
+    @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._x_des = position
+
+    @property
+    def desired_angular_momentum(self):
+        """Get the desired CoM angular momentum."""
+        return self._l_des
+
+    @desired_angular_momentum.setter
+    def desired_angular_momentum(self, angular_momentum):
+        """Set the desired CoM angular momentum."""
+        if angular_momentum is None:
+            angular_momentum = np.zeros(3)
+        elif not isinstance(angular_momentum, (np.ndarray, list, tuple)):
+            raise TypeError("Expecting the given desired angular momentum to be a np.array, instead got: "
+                            "{}".format(type(angular_momentum)))
+        angular_momentum = np.asarray(angular_momentum)
+        if len(angular_momentum) != 3:
+            raise ValueError("Expecting the given desired angular momentum array to be of length 3, but instead "
+                             "got: {}".format(len(angular_momentum)))
+        self._l_des = angular_momentum
+
+    @property
+    def desired_variation_angular_momentum(self):
+        """Get the desired CoM variation angular momentum."""
+        return self._dl_des
+
+    @desired_variation_angular_momentum.setter
+    def desired_variation_angular_momentum(self, variation_angular_momentum):
+        """Set the desired CoM variation angular momentum."""
+        if variation_angular_momentum is None:
+            variation_angular_momentum = np.zeros(3)
+        elif not isinstance(variation_angular_momentum, (np.ndarray, list, tuple)):
+            raise TypeError("Expecting the given desired variation angular momentum to be a np.array, instead got: "
+                            "{}".format(type(variation_angular_momentum)))
+        variation_angular_momentum = np.asarray(variation_angular_momentum)
+        if len(variation_angular_momentum) != 3:
+            raise ValueError("Expecting the given desired variation angular momentum array to be of length 3, but "
+                             "instead got: {}".format(len(variation_angular_momentum)))
+        self._dl_des = variation_angular_momentum
+
+    @property
+    def k_position(self):
+        """Return the position gain."""
+        return self._kp
+
+    @k_position.setter
+    def k_position(self, k):
+        """Set the position gain."""
+        if k is None:
+            k = 1.
+        if not isinstance(k, (float, int, np.ndarray)):
+            raise TypeError("Expecting the given position gain to be an int, float, np.array, instead "
+                            "got: {}".format(type(k)))
+        if isinstance(k, np.ndarray) and k.shape != (3, 3):
+            raise ValueError("Expecting the given position gain matrix to be of shape {}, but instead "
+                             "got shape: {}".format((3, 3), k.shape))
+        self._kp = k
+
+    @property
+    def k_velocity(self):
+        """Return the velocity gain."""
+        return self._kv
+
+    @k_velocity.setter
+    def k_velocity(self, k):
+        """Set the velocity gain."""
+        if k is None:
+            k = 1.
+        if not isinstance(k, (float, int, np.ndarray)):
+            raise TypeError("Expecting the given velocity gain to be an int, float, np.array, instead "
+                            "got: {}".format(type(k)))
+        if isinstance(k, np.ndarray) and k.shape != (3, 3):
+            raise ValueError("Expecting the given velocity gain matrix to be of shape {}, but instead "
+                             "got shape: {}".format((3, 3), k.shape))
+        self._kv = k
+
+    @property
+    def k_angular_momentum(self):
+        """Return the angular momentum gain."""
+        return self._kl
+
+    @k_angular_momentum.setter
+    def k_angular_momentum(self, k):
+        """Set the angular momentum gain."""
+        if k is None:
+            k = 1.
+        if not isinstance(k, (float, int, np.ndarray)):
+            raise TypeError("Expecting the given angular momentum gain to be an int, float, np.array, instead "
+                            "got: {}".format(type(k)))
+        if isinstance(k, np.ndarray) and k.shape != (3, 3):
+            raise ValueError("Expecting the given angular momentum gain matrix to be of shape {}, but instead "
+                             "got shape: {}".format((3, 3), k.shape))
+        self._kl = k
 
     ###########
     # Methods #
@@ -134,13 +356,27 @@ class CoMForceTask(ForceTask):
         """
         Update the task by computing the A matrix and b vector that will be used by the task solver.
         """
-        x = self.model.get_com_position()
-        dx = self.model.get_com_velocity()
-        A_G = self.model.get_centroidal_momentum_matrix()
+        x = self.model.get_com_position()  # shape: (3,)
+        dx = self.model.get_com_velocity()  # shape: (3,)
+        l = self.model.get_centroidal_momentum()[:3]  # shape: (3,)
 
-        angular_momentum = A_G[:3, :3]
-
-        raise NotImplementedError
+        # compute reference acceleration
+        a_ref = self._a_des + np.dot(self._kv, (self._v_des - dx))  # shape: (3,)
+        if self._x_des is not None:
+            a_ref += np.dot(self._kp, (self._x_des - x))  # shape: (3,)
 
+        # compute reference variation of the angular momentum
+        l_ref = self._dl_des + np.dot(self._kl, (self._l_des - l))  # shape: (3,)
 
+        # compute A matrix
+        As = []
+        for link in self.contact_links:
+            link = self.model.get_link_id(link)
+            contact_pos = self.model.get_position(link)
+            A = np.vstack((np.hstack((np.identity(3), np.zeros(3))),
+                           np.hstack((skew_matrix(contact_pos - x), np.identity(3)))))
+            As.append(A)
 
+        # compute A matrix and b vector
+        self._A = np.concatenate(As, axis=1)  # (6, 6*Nc)
+        self._b = np.concatenate((a_ref, l_ref))  # (6,)

pyrobolearn/priorities/tasks/force/floating_base.py

@@ -11,6 +11,9 @@ concatenated jacobians, and :math:`\tau` are the torques applied on the floating
 
 Note that this task assumes the robot has a floating base.
 
+The above formulation is equivalent to the QP objective function :math:`||Ax - b||^2`, by setting
+:math:`A = J(q)[:,:6]^\top`, :math:`x = w`, and :math:`b = \tau`.
+
 The implementation of this class is inspired by [1] (which is licensed under the LGPLv2).
 
 References:
@@ -45,6 +48,9 @@ class FloatingBaseForceTask(ForceTask):
 
     Note that this task assumes the robot has a floating base.
 
+    The above formulation is equivalent to the QP objective function :math:`||Ax - b||^2`, by setting
+    :math:`A = J(q)[:,:6]^\top`, :math:`x = w`, and :math:`b = \tau`.
+
     The implementation of this class is inspired by [1] (which is licensed under the LGPLv2).
 
     References:

pyrobolearn/priorities/tasks/task.py

@@ -463,6 +463,10 @@ class Task(object):
                 AW = np.concatenate([np.dot(soft_task.A.T, soft_task.weight) for soft_task in hard_task], axis=1)
                 b = np.concatenate([soft_task.b for soft_task in hard_task])
                 c = hard_task[0].c
+                print(hard_task)
+                print(c.shape)
+                print(AW.shape)
+                print(b.shape)
                 ps.append(c - 2 * AW.dot(b))
             return ps
         return self._c - 2 * self._A.T.dot(self.weight).dot(self._b)

pyrobolearn/priorities/tasks/velocity/contact.py

@@ -130,5 +130,5 @@ class ContactTask(JointVelocityTask):
         Update the task by computing the A matrix and b vector that will be used by the task solver.
         """
         # get jacobian expressed in the distal link frame
-        jacobian = self.model.get_jacobian(link=self.distal_link, frame=self.distal_link)
+        jacobian = self.model.get_jacobian(link=self.distal_link, frame=self.distal_link)  # shape: (6,N)
         self._A = np.dot(self.contact_matrix, jacobian)