diff --git a/pyrobolearn/optimizers/nlopt_optimizer.py b/pyrobolearn/optimizers/nlopt_optimizer.py
index 13be388..18d7c5c 100644
--- a/pyrobolearn/optimizers/nlopt_optimizer.py
+++ b/pyrobolearn/optimizers/nlopt_optimizer.py
@@ -47,9 +47,9 @@ class NLopt(Optimizer):
     - algorithms: https://nlopt.readthedocs.io/en/latest/NLopt_Algorithms/
 
     References:
-        [1] NLopt: https://nlopt.readthedocs.io/en/latest/
-        [2] NLopt with Python: with Python: https://nlopt.readthedocs.io/en/latest/NLopt_Python_Reference/
-        [3] Github repo: https://github.com/stevengj/nlopt
+        - [1] NLopt: https://nlopt.readthedocs.io/en/latest/
+        - [2] NLopt with Python: with Python: https://nlopt.readthedocs.io/en/latest/NLopt_Python_Reference/
+        - [3] Github repo: https://github.com/stevengj/nlopt
     """
 
     def __init__(self, method, submethod=None, seed=None, *args, **kwargs):
diff --git a/pyrobolearn/optimizers/qpsolvers_optimizer.py b/pyrobolearn/optimizers/qpsolvers_optimizer.py
index c8e646a..84e05f6 100644
--- a/pyrobolearn/optimizers/qpsolvers_optimizer.py
+++ b/pyrobolearn/optimizers/qpsolvers_optimizer.py
@@ -140,13 +140,13 @@ class QP(Optimizer):
     # Methods #
     ###########
 
-    def optimize(self, P, q, x0=None, G=None, h=None, A=None, b=None):
+    def optimize(self, Q, p, x0=None, G=None, h=None, A=None, b=None):
         r"""
         Optimize the given quadratic problem.
 
         .. math::
 
-            \min_{x \in \mathbb{R}^N} \frac{1}{2} x^T P x + q^T x
+            \min_{x \in \mathbb{R}^N} \frac{1}{2} x^T Q x + p^T x
 
         subject to
 
@@ -156,9 +156,9 @@ class QP(Optimizer):
             Ax = b
 
         Args:
-            P (np.array[N,N]): matrix used in the QP objective function where `N` is the size of the vector `x` being
+            Q (np.array[N,N]): matrix used in the QP objective function where `N` is the size of the vector `x` being
                 optimized.
-            q (np.array[N]): vector used in the QP objective function where `N` is the size of the vector `x` being
+            p (np.array[N]): vector used in the QP objective function where `N` is the size of the vector `x` being
                 optimized.
             G (np.array[M,N]): matrix used in the inequality constraint, where `M` is the number of inequalities, and
                 `N` is the size of the vector `x` being optimized. Note that if you have lower and upper bounds for
@@ -174,4 +174,4 @@ class QP(Optimizer):
         Returns:
             np.array: QP solution
         """
-        return qpsolvers.solve_qp(P, q, G, h, A, b, solver=self.method, initvals=x0, sym_proj=self.sym_proj)
+        return qpsolvers.solve_qp(Q, p, G, h, A, b, solver=self.method, initvals=x0, sym_proj=self.sym_proj)
diff --git a/pyrobolearn/priorities/constraints/README.rst b/pyrobolearn/priorities/constraints/README.rst
index 17032d4..634e14b 100644
--- a/pyrobolearn/priorities/constraints/README.rst
+++ b/pyrobolearn/priorities/constraints/README.rst
@@ -4,7 +4,13 @@ Constraints
 In this folder, we define the most common inequality and equality optimization constraints used in robotics for 
 priority tasks. Several of them were provided in [1].
 
-Constraints include joint limits, joint velocity limits, collision avoidance, and others.
+Constraints include joint limits, joint velocity limits, collision avoidance, and others. They are separated into 
+4 folders (velocity, acceleration, torque, and cartesian force); one for each optimization variable vector that is 
+being optimized. Note that different type of tasks (and thus constraints) can be combined together; for instance, 
+we can combine acceleration tasks with force tasks. This will create an optimization variable vector 
+:math:`x = [\ddot{q}^\top, F^\top]^\top` which can then be used with the joint space dynamic equation 
+:math:`\tau = H \ddot{q} + C(q,\dot{q})\dot{q} + g(q) - J^\top F` to get the equivalent joint torques to be applied 
+on the robot. 
 
 References:
 
diff --git a/pyrobolearn/priorities/constraints/__init__.py b/pyrobolearn/priorities/constraints/__init__.py
index e924b3b..4e827b5 100644
--- a/pyrobolearn/priorities/constraints/__init__.py
+++ b/pyrobolearn/priorities/constraints/__init__.py
@@ -1,6 +1,7 @@
 
 # import constraint
 from .constraint import *
+from .constraint_from_task import ConstraintFromTask
 
 # import velocity constraints
 from . import velocity
diff --git a/pyrobolearn/priorities/constraints/constraint.py b/pyrobolearn/priorities/constraints/constraint.py
index 10a6470..829cfd6 100644
--- a/pyrobolearn/priorities/constraints/constraint.py
+++ b/pyrobolearn/priorities/constraints/constraint.py
@@ -111,6 +111,7 @@ References:
 
 import numpy as np
 
+import pyrobolearn as prl
 from pyrobolearn.priorities.models import ModelInterface
 
 
@@ -168,6 +169,10 @@ class Constraint(object):
         self.constraints = constraints
         self.model = model
 
+        # if the constraint is enabled or not, by default it is. This allows to dynamically enable and disable
+        # constraints.
+        self._enabled = True
+
         # variables to be set in the corresponding child classes
         self._lower_bound = None
         self._upper_bound = None
@@ -238,6 +243,9 @@ class Constraint(object):
                     raise TypeError("Expecting the {}th inequality constraint to be an instance of `BoundConstraint`,"
                                     " `UnilateralConstraint`, `BilateralConstraint`, but got: "
                                     "{}".format(i, type(constraint)))
+            elif isinstance(constraint, prl.priorities.tasks.Task):
+                constraint = prl.priorities.constraints.ConstraintFromTask(constraint)
+                constraint_dict.setdefault(EqualityConstraint, []).append(constraint)
             else:
                 raise TypeError("Expecting the {}th constraint to be an instance of `EqualityConstraint` or "
                                 "`InequalityConstraint`, but got: {}".format(i, type(constraint)))
@@ -253,7 +261,7 @@ class Constraint(object):
         """
         if self.constraints:
             constraints = self.constraints.get(BoundConstraint, [])
-            return [constraint.lower_bound for constraint in constraints]
+            return [constraint.lower_bound for constraint in constraints if constraint.enabled]
         return self._lower_bound
 
     @property
@@ -265,7 +273,7 @@ class Constraint(object):
         """
         if self.constraints:
             constraints = self.constraints.get(BoundConstraint, [])
-            return [constraint.lower_bound for constraint in constraints]
+            return [constraint.lower_bound for constraint in constraints if constraint.enabled]
         return self._upper_bound
 
     @property
@@ -277,7 +285,7 @@ class Constraint(object):
         """
         if self.constraints:
             constraints = self.constraints.get(EqualityConstraint, [])
-            return [constraint.A_eq for constraint in constraints]
+            return [constraint.A_eq for constraint in constraints if constraint.enabled]
         return self._A_eq
 
     @property
@@ -289,7 +297,7 @@ class Constraint(object):
         """
         if self.constraints:
             constraints = self.constraints.get(EqualityConstraint, [])
-            return [constraint.b_eq for constraint in constraints]
+            return [constraint.b_eq for constraint in constraints if constraint.enabled]
         return self._b_eq
 
     @property
@@ -302,7 +310,8 @@ class Constraint(object):
         if self.constraints:
             unilateral_constraints = self.constraints.get(UnilateralConstraint, [])
             bilateral_constraints = self.constraints.get(BilateralConstraint, [])
-            return [constraint.A_ineq for constraint in unilateral_constraints + bilateral_constraints]
+            constraints = unilateral_constraints + bilateral_constraints
+            return [constraint.A_ineq for constraint in constraints if constraint.enabled]
         return self._A_ineq
 
     @property
@@ -315,7 +324,8 @@ class Constraint(object):
         if self.constraints:
             unilateral_constraints = self.constraints.get(LowerUnilateralConstraint, [])
             bilateral_constraints = self.constraints.get(BilateralConstraint, [])
-            return [constraint.b_lower_bound for constraint in unilateral_constraints + bilateral_constraints]
+            constraints = unilateral_constraints + bilateral_constraints
+            return [constraint.b_lower_bound for constraint in constraints if constraint.enabled]
         return self._b_lower_bound
 
     @property
@@ -328,7 +338,8 @@ class Constraint(object):
         if self.constraints:
             unilateral_constraints = self.constraints.get(UpperUnilateralConstraint, [])
             bilateral_constraints = self.constraints.get(BilateralConstraint, [])
-            return [constraint.b_upper_bound for constraint in unilateral_constraints + bilateral_constraints]
+            constraints = unilateral_constraints + bilateral_constraints
+            return [constraint.b_upper_bound for constraint in constraints if constraint.enabled]
         return self._b_upper_bound
 
     @property
@@ -342,31 +353,35 @@ class Constraint(object):
             results = []
             bound_constraints = self.constraints.get(BoundConstraint, [])
             for constraint in bound_constraints:
-                x_size = len(constraint.lower_bound)
-                results.append(-np.identity(x_size))
-                results.append(np.identity(x_size))
+                if constraint.enabled:
+                    x_size = len(constraint.lower_bound)
+                    results.append(-np.identity(x_size))
+                    results.append(np.identity(x_size))
             bilateral_constraints = self.constraints.get(BilateralConstraint, [])
             for constraint in bilateral_constraints:
-                results.append(-constraint.A_ineq)
-                results.append(constraint.A_ineq)
+                if constraint.enabled:
+                    results.append(-constraint.A_ineq)
+                    results.append(constraint.A_ineq)
             lower_unilateral_constraints = self.constraints.get(LowerUnilateralConstraint, [])
             for constraint in lower_unilateral_constraints:
-                results.append(-constraint.A_ineq)
+                if constraint.enabled:
+                    results.append(-constraint.A_ineq)
             upper_unilateral_constraints = self.constraints.get(UpperUnilateralConstraint, [])
             for constraint in upper_unilateral_constraints:
-                results.append(constraint.A_ineq)
+                if constraint.enabled:
+                    results.append(constraint.A_ineq)
 
             if results:
                 return np.concatenate(results)
         else:
-            if isinstance(self, BoundConstraint):
+            if isinstance(self, BoundConstraint) and self.enabled:
                 x_size = len(self._lower_bound)
                 return np.concatenate((-np.identity(x_size), np.identity(x_size)))
-            elif isinstance(self, BilateralConstraint):
+            elif isinstance(self, BilateralConstraint) and self.enabled:
                 return np.concatenate((-self._A_ineq, self._A_ineq))
-            elif isinstance(self, LowerUnilateralConstraint):
+            elif isinstance(self, LowerUnilateralConstraint) and self.enabled:
                 return -self._A_ineq
-            elif isinstance(self, UpperUnilateralConstraint):
+            elif isinstance(self, UpperUnilateralConstraint) and self.enabled:
                 return self._A_ineq
 
     @property
@@ -380,29 +395,33 @@ class Constraint(object):
             results = []
             bound_constraints = self.constraints.get(BoundConstraint, [])
             for constraint in bound_constraints:
-                results.append(-constraint.lower_bound)
-                results.append(constraint.upper_bound)
+                if constraint.enabled:
+                    results.append(-constraint.lower_bound)
+                    results.append(constraint.upper_bound)
             bilateral_constraints = self.constraints.get(BilateralConstraint, [])
             for constraint in bilateral_constraints:
-                results.append(-constraint.b_lower_bound)
-                results.append(constraint.b_upper_bound)
+                if constraint.enabled:
+                    results.append(-constraint.b_lower_bound)
+                    results.append(constraint.b_upper_bound)
             lower_unilateral_constraints = self.constraints.get(LowerUnilateralConstraint, [])
             for constraint in lower_unilateral_constraints:
-                results.append(-constraint.b_lower_bound)
+                if constraint.enabled:
+                    results.append(-constraint.b_lower_bound)
             upper_unilateral_constraints = self.constraints.get(UpperUnilateralConstraint, [])
             for constraint in upper_unilateral_constraints:
-                results.append(constraint.b_upper_bound)
+                if constraint.enabled:
+                    results.append(constraint.b_upper_bound)
 
             if results:
                 return np.concatenate(results)
         else:
-            if isinstance(self, BoundConstraint):
+            if isinstance(self, BoundConstraint) and self.enabled:
                 return np.concatenate((-self._lower_bound, self._upper_bound))
-            elif isinstance(self, BilateralConstraint):
+            elif isinstance(self, BilateralConstraint) and self.enabled:
                 return np.concatenate((-self._b_lower_bound, self._upper_bound))
-            elif isinstance(self, LowerUnilateralConstraint):
+            elif isinstance(self, LowerUnilateralConstraint) and self.enabled:
                 return -self._b_lower_bound
-            elif isinstance(self, UpperUnilateralConstraint):
+            elif isinstance(self, UpperUnilateralConstraint) and self.enabled:
                 return self._b_upper_bound
 
     @property
@@ -423,6 +442,11 @@ class Constraint(object):
         """
         return self.b_eq
 
+    @property
+    def enabled(self):
+        """Return if the task is enabled or not."""
+        return self._enabled
+
     ##################
     # Static methods #
     ##################
@@ -457,6 +481,18 @@ class Constraint(object):
     # Methods #
     ###########
 
+    def enable(self, enable=True):
+        """Enable the single constraint, or each inner constraint."""
+        if not self.is_single_constraint():
+            for constraint in self.constraints:
+                constraint.enable(enable=enable)
+        else:
+            self._enabled = enable
+
+    def disable(self, disable=True):
+        """Disable the single constraint, or each inner constraint."""
+        self.enable(not disable)
+
     def has_constraints(self):
         """Return True if it has inner constraints."""
         return len(self.constraints) > 0
@@ -477,7 +513,7 @@ class Constraint(object):
             raise TypeError("Expecting the given 'constraint' to be an instance of `Constraint` but got instead: "
                             "{}".format(type(constraint)))
         if not constraint.is_single_constraint():
-            raise ValueError("Expecting to append a single constraint, but the given constraint as a list of "
+            raise ValueError("Expecting to append a single constraint, but the given constraint has a list of "
                              "inner constraints")
 
         if isinstance(constraint, EqualityConstraint):
@@ -497,6 +533,9 @@ class Constraint(object):
             else:
                 raise TypeError("Expecting the inequality constraint to be an instance of `BoundConstraint`, "
                                 "`UnilateralConstraint`, `BilateralConstraint`, but got: {}".format(type(constraint)))
+        elif isinstance(constraint, prl.priorities.tasks.Task):
+            constraint = prl.priorities.constraints.ConstraintFromTask(constraint)
+            self.constraints.setdefault(EqualityConstraint, []).append(constraint)
         else:
             raise TypeError("The given type of constraint is not currently supported.")
 
@@ -643,6 +682,6 @@ class JointTorqueConstraint(DynamicConstraint):
     pass
 
 
-class JointForceConstraint(DynamicConstraint):
-    r"""Joint Force constraint."""
+class ForceConstraint(DynamicConstraint):
+    r"""Force constraint."""
     pass
diff --git a/pyrobolearn/priorities/constraints/force/__init__.py b/pyrobolearn/priorities/constraints/force/__init__.py
index b6d6adb..594f62b 100644
--- a/pyrobolearn/priorities/constraints/force/__init__.py
+++ b/pyrobolearn/priorities/constraints/force/__init__.py
@@ -1,2 +1,12 @@
 
 # cartesian force constraints
+
+from .contact import ContactConstraint
+
+# from .cop import CoPConstraint
+
+from .friction import FrictionPyramidConstraint  # , FrictionConeConstraint
+
+from .wrench_limits import WrenchLimitsConstraint
+
+from .zmp import ZMPConstraint
diff --git a/pyrobolearn/priorities/constraints/force/contact.py b/pyrobolearn/priorities/constraints/force/contact.py
index 43cf25f..1163f82 100644
--- a/pyrobolearn/priorities/constraints/force/contact.py
+++ b/pyrobolearn/priorities/constraints/force/contact.py
@@ -1,17 +1,29 @@
 #!/usr/bin/env python
 r"""Provide the contact (force normal) constraint.
 
+The lower unilateral contact force constraint is given by :math:`0 \leq f^i_n` where :math:`f^i_n` is the normal
+force with respect to the contact surface applied on the link in contact :math:`i` defined in the local frame.
 
-The implementation of this class is inspired by [1] (which is licensed under the LGPLv2).
+The optimization variables are these contact forces :math:`f^i_n` expressed in the world frame, thus they are
+rotated to their local frame. This formulation can be rewritten as a unilateral inequality constraint
+:math:`b_l \leq A_{ineq} x` in QP, with :math:`x = f^w \in \mathbb{R}^{6N_c}` which is the concatenation of all
+the contact force variables (one for each contact point) expressed in the world frame,
+:math:`A_{ineq} = R^l_w \in \mathbb{R}^{6N_c \times 6N_c}` is the block diagonal matrix that rotates the force
+variables expressed in the world frame :math:`w` to their respective local frame :math:`l`, and
+:math:`b_l = [-\infty, -\infty, 0, -\infty, -\infty, -\infty] * N_c`, where :math:`N_c` is the total number of
+contact points.
+
+The implementation of this class is inspired by [1].
 
 References:
     - [1] "Motion Planning and Control of Dynamics Humanoid Locomotion" (PhD thesis), Xin, 2018
 """
 
 import numpy as np
+from scipy.linalg import block_diag
 
-from pyrobolearn.priorities.constraints.constraint import Constraint
-
+from pyrobolearn.priorities.constraints.constraint import LowerUnilateralConstraint, ForceConstraint
+from pyrobolearn.utils.transformation import get_matrix_from_quaternion
 
 __author__ = "Brian Delhaisse"
 __copyright__ = "Copyright 2019, PyRoboLearn"
@@ -23,15 +35,72 @@ __email__ = "briandelhaisse@gmail.com"
 __status__ = "Development"
 
 
-class ContactConstraint(Constraint):
+class ContactConstraint(LowerUnilateralConstraint, ForceConstraint):
     r"""Contact force constraint
 
-    The contact force constraint is given by :math:`0 \leq f^i_n` where :math:`f^i_n` is the normal force with respect
-    to the contact surface applied on the link in contact :math:`i` defined in the local frame.
+    The lower unilateral contact force constraint is given by :math:`0 \leq f^i_n` where :math:`f^i_n` is the normal
+    force with respect to the contact surface applied on the link in contact :math:`i` defined in the local frame.
+
+    The optimization variables are these contact forces :math:`f^i_n` expressed in the world frame, thus they are
+    rotated to their local frame. This formulation can be rewritten as a unilateral inequality constraint
+    :math:`b_l \leq A_{ineq} x` in QP, with :math:`x = f^w \in \mathbb{R}^{6N_c}` which is the concatenation of all
+    the contact force variables (one for each contact point) expressed in the world frame,
+    :math:`A_{ineq} = R^l_w \in \mathbb{R}^{6N_c \times 6N_c}` is the block diagonal matrix that rotates the force
+    variables expressed in the world frame :math:`w` to their respective local frame :math:`l`, and
+    :math:`b_l = [-\infty, -\infty, 0, -\infty, -\infty, -\infty] * N_c`, where :math:`N_c` is the total number of
+    contact points.
+
+    The implementation of this class is inspired by [1].
 
     References:
         - [1] "Motion Planning and Control of Dynamics Humanoid Locomotion" (PhD thesis), Xin, 2018
     """
 
-    def __init__(self, model):
+    def __init__(self, model, contacts=[]):
+        r"""
+        Initialize the constraint.
+
+        Args:
+            model (ModelInterface): model interface.
+            contacts (list[int], list[str]): list of contact links (ids or names).
+        """
         super(ContactConstraint, self).__init__(model)
+
+        # set variables
+        self.contacts = contacts
+        self._vector = -np.infty * np.ones(6)
+        self._vector[2] = 0
+
+        # first update
+        self.update()
+
+    ##############
+    # Properties #
+    ##############
+
+    @property
+    def contacts(self):
+        """Get the list of contact links."""
+        return self._contacts
+
+    @contacts.setter
+    def contacts(self, contacts):
+        """Set the contact links."""
+        if not isinstance(contacts, (list, tuple, np.ndarray)):
+            raise TypeError("Expecting the given 'contacts' to be a list of int/str, but instead got: "
+                            "{}".format(type(contacts)))
+        self._contacts = contacts
+
+    ###########
+    # Methods #
+    ###########
+
+    def _update(self):
+        """Update the lower unilateral inequality matrix and vector."""
+        rotations = []
+        for contact in self.contacts:
+            link = self.model.get_link_id(contact)
+            rot = get_matrix_from_quaternion(self.model.get_orientation(link)).T  # (3,3)
+            rotations.append(block_diag((rot, rot)))  # (6,6)
+        self._A_ineq = block_diag(rotations)  # (M*6,M*6)
+        self._b_lower_bound = np.concatenate([self._vector for _ in self.contacts])  # (M*6,)
diff --git a/pyrobolearn/priorities/constraints/force/friction.py b/pyrobolearn/priorities/constraints/force/friction.py
index bdfa93d..6ed2704 100644
--- a/pyrobolearn/priorities/constraints/force/friction.py
+++ b/pyrobolearn/priorities/constraints/force/friction.py
@@ -10,9 +10,10 @@ References:
 """
 
 import numpy as np
+from scipy.linalg import block_diag
 
-from pyrobolearn.priorities.constraints.constraint import Constraint
-
+from pyrobolearn.priorities.constraints.constraint import UpperUnilateralConstraint, ForceConstraint
+from pyrobolearn.utils.transformation import get_matrix_from_quaternion
 
 __author__ = "Brian Delhaisse"
 __copyright__ = "Copyright 2019, PyRoboLearn"
@@ -25,7 +26,7 @@ __email__ = "briandelhaisse@gmail.com"
 __status__ = "Development"
 
 
-class FrictionConeConstraint(Constraint):
+class FrictionConeConstraint(UpperUnilateralConstraint, ForceConstraint):
     r"""Friction Cone constraint
 
     The friction cone is defined as:
@@ -47,18 +48,28 @@ class FrictionConeConstraint(Constraint):
         - [4] "Motion Planning and Control of Dynamics Humanoid Locomotion" (PhD thesis), Xin, 2018
     """
 
-    def __init__(self, model):
+    def __init__(self, model, mu=0.7):
+        r"""
+        Initialize the constraint.
+
+        Args:
+            model (ModelInterface): model interface.
+            mu (float): friction coefficient.
+        """
         super(FrictionConeConstraint, self).__init__(model)
 
+        # set variable
+        self.mu = mu
 
-class FrictionPyramidConstraint(Constraint):
+
+class FrictionPyramidConstraint(UpperUnilateralConstraint, ForceConstraint):
     r"""Friction Pyramid constraint
 
     The friction pyramid constraint is a linear approximation of the friction cone.
 
     The friction pyramid is defined as:
 
-    .. math:: P^i_s = {(f^i_x, f^i_y, f^i_z) \in \mathbb{R}^3 | f^i_x \leq \mu_i f^i_z, f^i_y \leq \mu_i f^i_z}
+    .. math:: P^i_s = {(f^i_x, f^i_y, f^i_z) \in \mathbb{R}^3 | |f^i_x| \leq \mu_i f^i_z, |f^i_y| \leq \mu_i f^i_z}
 
     where where :math:`i` denotes the ith support/contact, :math:`f^i_s` is the contact spatial force exerted at
     the contact point :math:`C_i`, and :math:`\mu_i` is the static friction coefficient at that contact point.
@@ -69,11 +80,100 @@ class FrictionPyramidConstraint(Constraint):
     This linear approximation is often used as a linear constraint in a quadratic optimization problem along with
     the unilateral constraint :math:`f^i_z \geq 0`.
 
+    QP formulation
+    --------------
+
+    The friction pyramid constraints given by:
+
+    .. math::
+
+        -\mu f^i_z \leq f^i_x \leq \mu f^i_z \\
+        -\mu f^i_z \leq f^i_y \leq \mu f^i_z
+
+    can be rewritten as:
+
+    .. math::
+
+        f^i_x - \mu f^i_z \leq 0 \\
+        -f^i_x - \mu f^i_z \leq 0 \\
+        f^i_y - \mu f^i_z \leq 0 \\
+        -f^i_y - \mu f^i_z \leq 0
+
+    Thus, it can be rewritten as the inequality constraint :math:`G x \leq h` in QP, with:
+
+    .. math::
+
+        G = \left[\begin{array}{cccccc}
+            1 & 0 & -\mu & 0 & 0 & 0  \\
+            -1 & 0 & -\mu & 0 & 0 & 0  \\
+            0 & 1 & -\mu & 0 & 0 & 0  \\
+            0 & -1 & -\mu & 0 & 0 & 0  \\
+        \end{array}\right],
+
+    :math:`x = [f^i_x, f^i_y, f^i_z, n^i_x, n^i_y, n^i_z]` being the optimized variables, and :math:`h = [0,0,0,0]`.
+    Note that the optimized variables are expressed in the world frame, and thus a rotation to express them in their
+    contact local frame is performed beforehand.
+
     References:
         - [1] https://scaron.info/teaching/friction-cones.html
         - [2] "Stability of Surface Contacts for Humanoid Robots: Closed-Form Formulae of the Contact Wrench Cone
             for Rectangular Support Areas", Caron et al., 2015
+        - [3] "Motion Planning and Control of Dynamics Humanoid Locomotion" (PhD thesis), Xin, 2018
+        - [4] "OpenSoT: A whole-body control library for the compliant humanoid robot COMAN", Rocchi et al., 2015
     """
 
-    def __init__(self, model):
-        super(FrictionPyramidConstraint, self).__init__(model)
\ No newline at end of file
+    def __init__(self, model, mu=0.7, contacts=[]):
+        r"""
+        Initialize the constraint.
+
+        Args:
+            model (ModelInterface): model interface.
+            mu (float): friction coefficient.
+            contacts (list[int], list[str]): list of contact link unique id or name.
+        """
+        super(FrictionPyramidConstraint, self).__init__(model)
+
+        # set variable
+        self.mu = mu
+        self._friction_matrix = np.zeros((4, 6))
+        self._friction_matrix[0, 0] = 1
+        self._friction_matrix[1, 0] = -1
+        self._friction_matrix[2, 1] = 1
+        self._friction_matrix[3, 1] = -1
+        self._friction_matrix[:, 2] = -self.mu
+
+        self.contacts = contacts
+
+        # first update
+        self.update()
+
+    ##############
+    # Properties #
+    ##############
+
+    @property
+    def contacts(self):
+        """Get the list of contact links."""
+        return self._contacts
+
+    @contacts.setter
+    def contacts(self, contacts):
+        """Set the contact links."""
+        if not isinstance(contacts, (list, tuple, np.ndarray)):
+            raise TypeError("Expecting the given 'contacts' to be a list of int/str, but instead got: "
+                            "{}".format(type(contacts)))
+        self._contacts = contacts
+
+    ###########
+    # Methods #
+    ###########
+
+    def _update(self):
+        """Update the lower unilateral inequality matrix and vector."""
+        self._A_ineq = np.zeros(4 * len(self.contacts), 6 * len(self.contacts))
+        for i, contact in enumerate(self.contacts):
+            rot = get_matrix_from_quaternion(self.model.get_orientation(self._link)).T
+            rot = block_diag((rot, rot))
+            self._A_ineq[i*4:(i+1)*4, i*6:(i+1)*6] = self._friction_matrix.dot()
+
+        self._b_upper_bound = np.zeros(4 * len(self.contacts))
diff --git a/pyrobolearn/priorities/constraints/force/wrench_limits.py b/pyrobolearn/priorities/constraints/force/wrench_limits.py
index 438393e..1cbd45c 100644
--- a/pyrobolearn/priorities/constraints/force/wrench_limits.py
+++ b/pyrobolearn/priorities/constraints/force/wrench_limits.py
@@ -1,6 +1,17 @@
 #!/usr/bin/env python
 r"""Provide the wrench limits constraint.
 
+This provides bounds/limits on the wrenches:
+
+.. math::  F_{lb} \leq F \leq F_{ub}
+
+where :math:`(F_{lb}, F_{ub})` are the lower and upper bound on the wrenches, and
+:math:`F` is the wrench vector being optimized.
+
+This formulation can be rewritten as the inequality constraint math:`lb \leq x \leq ub` in QP, with
+:math:`lb = F_{lb}`, :math:`ub = F_{ub}`, and :math:`x = F`. This can also be rewritten as :math:`Gx \leq h`,
+with :math:`G = [-I, I]^\top` and :math:`h = [-F_{lb}^\top, F_{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, ForceConstraint
 
 
 __author__ = "Brian Delhaisse"
@@ -23,10 +34,94 @@ __email__ = "briandelhaisse@gmail.com"
 __status__ = "Development"
 
 
-class WrenchLimitsConstraint(Constraint):
+class WrenchLimitsConstraint(BoundConstraint, ForceConstraint):
     r"""Wrench Limits constraint.
 
+     This provides bounds/limits on the wrenches:
+
+    .. math::  F_{lb} \leq F \leq F_{ub}
+
+    where :math:`(F_{lb}, F_{ub})` are the lower and upper bound on the wrenches, and
+    :math:`F` is the wrench vector being optimized.
+
+    This formulation can be rewritten as the inequality constraint math:`lb \leq x \leq ub` in QP, with
+    :math:`lb = F_{lb}`, :math:`ub = F_{ub}`, and :math:`x = F`. This can also be rewritten as :math:`Gx \leq h`,
+    with :math:`G = [-I, I]^\top` and :math:`h = [-F_{lb}^\top, F_{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, bounds):
+        r"""
+        Initialize the constraint.
+
+        Args:
+            model (ModelInterface): model interface.
+            bounds (tuple[2 * np.array[float[M]]], np.array[float[M]]): wrench limits, where `M` is 3 (vector of
+              forces) or 6 (vector of forces and torques). If tuple, it is the lower and upper bounds on the wrenches.
+              If np.array, then the lower and upper bound will be set to (-bounds, bounds).
+        """
         super(WrenchLimitsConstraint, self).__init__(model)
+
+        # set variables
+        self.bounds = bounds
+
+        # first update
+        self.update()
+
+    ##############
+    # Properties #
+    ##############
+
+    @property
+    def bounds(self):
+        """Get the wrench bounds."""
+        return self._bounds
+
+    @bounds.setter
+    def bounds(self, bounds):
+        """Set the wrench bounds."""
+        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 i, bound in enumerate(bounds):
+                    if isinstance(bound, (int, float)):
+                        bound = np.ones(3) * bound
+                        bounds[i] = bound
+                    elif isinstance(bound, np.ndarray) and len(bound) != 3:
+                        raise ValueError("Expecting the given bound to be of length 3, but instead got a length of "
+                                         "{}".format(len(bound)))
+                    else:
+                        raise TypeError("Expecting the given bound to be a np.array, but got instead: "
+                                        "{}".format(type(bound)))
+            elif isinstance(bounds, np.ndarray):
+                bounds = bounds.reshape(-1)
+                if len(bounds) == 3:
+                    bounds = (-bounds, bounds)
+                elif len(bounds) == 6:
+                    bounds = (bounds[:3], bounds[3:])
+                elif len(bounds) == 12:
+                    bounds = (bounds[:6], bounds[6:])
+                else:
+                    raise ValueError("Expecting the given bounds to be of length 3 or 6 but got instead a length of: "
+                                     "{}".format(len(bounds)))
+            elif isinstance(bounds, (int, float)):
+                bounds = (-np.ones(3) * bounds, np.ones(3) * bounds)
+            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._bounds = bounds
+
+    ###########
+    # Methods #
+    ###########
+
+    def _update(self):
+        """Update the lower and upper bounds."""
+        self._b_lower_bound, self._b_upper_bound = self._bounds
diff --git a/pyrobolearn/priorities/constraints/force/zmp.py b/pyrobolearn/priorities/constraints/force/zmp.py
index 3323a65..73c4caf 100644
--- a/pyrobolearn/priorities/constraints/force/zmp.py
+++ b/pyrobolearn/priorities/constraints/force/zmp.py
@@ -1,15 +1,68 @@
 #!/usr/bin/env python
 r"""Provide the Zero-Moment Point constraint.
 
+The ZMP constraints can be expressed as:
+
+.. math::
+
+    d_x^{-} \leq -\frac{n^i_y}{f^i_z} \leq d_x^{+} \\
+    d_y^{-} \leq \frac{n^i_x}{f^i_z} \leq d_y^{+}
+
+which ensures the stability of the foot/ground contact. The :math:`(d_x^{-}, d_x^{+})` and :math:`(d_y^{-}, d_y^{+})`
+defines the size of the sole in the x and y directions respectively. Basically, this means that the ZMP point must be
+inside the convex hull in order to have a static stability. The :math:`n^i` are the contact torques around the contact
+point :math:`i`, and :math:`f` is the contact force at the contact point :math:`i`.
+
+Notes:
+    - the ZMP and CoP are equivalent for horizontal ground surfaces. For irregular ground surfaces they are
+    distinct. [2]
+
+
+QP formulation
+--------------
+
+The ZMP constraints given by:
+
+.. math::
+
+    d_x^{-} \leq -\frac{n^i_y}{f^i_z} \leq d_x^{+} \\
+    d_y^{-} \leq \frac{n^i_x}{f^i_z} \leq d_y^{+}
+
+can be rewritten as:
+
+.. math::
+
+    d_x^{-} f^i_z + n^i_y \leq 0 \\
+    -d_x^{+} f^i_z - n^i_y \leq 0 \\
+    d_y^{-} f^i_z - n^i_x \leq 0 \\
+    -d_y^{+} f^i_z + n^i_x \leq 0
+
+Thus, it can be rewritten as the inequality constraint :math:`G x \leq h` in QP, with:
+
+.. math::
+
+    G = \left[\begin{array}{cccccc}
+        0 & 0 & d_x^{-} & 0 & 1 & 0  \\
+        0 & 0 & -d_x^{+} & 0 & -1 & 0  \\
+        0 & 0 & d_y^{-} & -1 & 0 & 0  \\
+        0 & 0 & -d_y^{+} & 1 & 0 & 0  \\
+    \end{array}\right],
+
+:math:`x = [f^i_x, f^i_y, f^i_z, n^i_x, n^i_y, n^i_z]` being the optimized variables, and :math:`h = [0,0,0,0]`.
 
 References:
     - [1] "Motion Planning and Control of Dynamics Humanoid Locomotion" (PhD thesis), Xin, 2018
+    - [2] "Ground Reference Points in Legged Locomotion: Definitions, Biological Trajectories and Control
+        Implications", Popovic et al., 2005
 """
 
+# TODO: correct this file when there are multiple links
+
 import numpy as np
+from scipy.linalg import block_diag
 
-from pyrobolearn.priorities.constraints.constraint import Constraint
-
+from pyrobolearn.priorities.constraints.constraint import UpperUnilateralConstraint, ForceConstraint
+from pyrobolearn.utils.transformation import get_matrix_from_quaternion
 
 __author__ = "Brian Delhaisse"
 __copyright__ = "Copyright 2019, PyRoboLearn"
@@ -21,7 +74,7 @@ __email__ = "briandelhaisse@gmail.com"
 __status__ = "Development"
 
 
-class ZMPConstraint(Constraint):
+class ZMPConstraint(UpperUnilateralConstraint, ForceConstraint):
     r"""Zero-Moment Point constraint.
 
     "The ZMP is the point on the ground surface about which the horizontal component of the moment of ground
@@ -54,8 +107,8 @@ class ZMPConstraint(Constraint):
 
     .. math::
 
-        d_x^{-} \leq \frac{n^i_y}{f^i_z} \leq d_x^{+} \\
-        d_y^{-} \leq -\frac{n^i_x}{f^i_z} \leq d_y^{+}
+        d_x^{-} \leq -\frac{n^i_y}{f^i_z} \leq d_x^{+} \\
+        d_y^{-} \leq \frac{n^i_x}{f^i_z} \leq d_y^{+}
 
     which ensures the stability of the foot/ground contact. The :math:`(d_x^{-}, d_x^{+})` and
     :math:`(d_y^{-}, d_y^{+})` defines the size of the sole in the x and y directions respectively. Basically,
@@ -69,12 +122,140 @@ class ZMPConstraint(Constraint):
         - the FRI coincides with the ZMP when the foot is stationary. [1]
         - the CMP coincides with the ZMP, when the moment about the CoM is zero. [1]
 
+
+    QP formulation
+    --------------
+
+    The ZMP constraints given by:
+
+    .. math::
+
+        d_x^{-} \leq -\frac{n^i_y}{f^i_z} \leq d_x^{+} \\
+        d_y^{-} \leq \frac{n^i_x}{f^i_z} \leq d_y^{+}
+
+    can be rewritten as:
+
+    .. math::
+
+        d_x^{-} f^i_z + n^i_y \leq 0 \\
+        -d_x^{+} f^i_z - n^i_y \leq 0 \\
+        d_y^{-} f^i_z - n^i_x \leq 0 \\
+        -d_y^{+} f^i_z + n^i_x \leq 0
+
+    Thus, it can be rewritten as the inequality constraint :math:`G x \leq h` in QP, with:
+
+    .. math::
+
+        G = \left[\begin{array}{cccccc}
+            0 & 0 & d_x^{-} & 0 & 1 & 0  \\
+            0 & 0 & -d_x^{+} & 0 & -1 & 0  \\
+            0 & 0 & d_y^{-} & -1 & 0 & 0  \\
+            0 & 0 & -d_y^{+} & 1 & 0 & 0
+        \end{array}\right],
+
+    :math:`x = [f^i_x, f^i_y, f^i_z, n^i_x, n^i_y, n^i_z]` being the optimized variables, and :math:`h = [0,0,0,0]`.
+    Note that the optimized variables are expressed in the world frame, and thus a rotation to express them in their
+    contact local frame is performed beforehand.
+
+
+    The implementation is based from insights provided in [4].
+
     References:
         - [1] "Ground Reference Points in Legged Locomotion: Definitions, Biological Trajectories and Control
         Implications", Popovic et al., 2005
         - [2] "Biped Walking Pattern Generation by using Preview Control of ZMP", Kajita et al., 2003
         - [3] "Exploiting Angular Momentum to Enhance Bipedal Center-of-Mass Control", Hofmann et al., 2009
+        - [4] "Motion Planning and Control of Dynamics Humanoid Locomotion" (PhD thesis), Xin, 2018
     """
 
-    def __init__(self, model):
+    def __init__(self, model, x_bounds, y_bounds, link):  # TODO: correct when multiple links
+        r"""
+        Initialize the constraint.
+
+        Args:
+            model (ModelInterface): model interface.
+            x_bounds (tuple[2*float]): lower and upper bound of the size of the sole in the x direction.
+            y_bounds (tuple[2*float]): lower and upper bound of the size of the sole in the y direction.
+            link (int, str): unique link id or name.
+        """
         super(ZMPConstraint, self).__init__(model)
+
+        # set optimization variables
+        self._b_upper_bound = np.zeros(4)
+        self._zmp_matrix = np.zeros((4, 6))
+        self._zmp_matrix[0, 4] = 1
+        self._zmp_matrix[1, 4] = -1
+        self._zmp_matrix[2, 3] = -1
+        self._zmp_matrix[3, 3] = 1
+        # the rest of A_ineq is set when setting the bounds
+
+        # set variables
+        self.x_bounds = x_bounds
+        self.y_bounds = y_bounds
+        self._link = self.model.get_link_id(link)
+
+        # first update
+        self.update()
+
+    ##############
+    # Properties #
+    ##############
+
+    @property
+    def x_bounds(self):
+        """Get the sole size bounds in the x-direction."""
+        return self._x_bounds
+
+    @x_bounds.setter
+    def x_bounds(self, bounds):
+        """Set the sole size bounds in the x-direction."""
+        if not isinstance(bounds, (tuple, list, np.ndarray)):
+            raise TypeError("Expecting the given 'x_bounds' to be a tuple, list or np.array of 2 float, but got "
+                            "instead: {}".format(bounds))
+        if len(bounds) != 2:
+            raise ValueError("Expecting the given 'x_bounds' to be of size 2, but got a size of "
+                             "{}".format(len(bounds)))
+        self._x_bounds = bounds
+        self._zmp_matrix[0, 2] = bounds[0]
+        self._zmp_matrix[1, 2] = -bounds[1]
+
+    @property
+    def y_bounds(self):
+        """Get the sole size bounds in the y-direction."""
+        return self._y_bounds
+
+    @y_bounds.setter
+    def y_bounds(self, bounds):
+        """Set the sole size bounds in the y-direction."""
+        if not isinstance(bounds, (tuple, list, np.ndarray)):
+            raise TypeError("Expecting the given 'y_bounds' to be a tuple, list or np.array of 2 float, but got "
+                            "instead: {}".format(bounds))
+        if len(bounds) != 2:
+            raise ValueError("Expecting the given 'y_bounds' to be of size 2, but got a size of "
+                             "{}".format(len(bounds)))
+        self._y_bounds = bounds
+        self._zmp_matrix[2, 2] = bounds[0]
+        self._zmp_matrix[3, 2] = -bounds[1]
+
+    # @property
+    # def contacts(self):
+    #     """Get the list of contact links."""
+    #     return self._contacts
+    #
+    # @contacts.setter
+    # def contacts(self, contacts):
+    #     """Set the contact links."""
+    #     if not isinstance(contacts, (list, tuple, np.ndarray)):
+    #         raise TypeError("Expecting the given 'contacts' to be a list of int/str, but instead got: "
+    #                         "{}".format(type(contacts)))
+    #     self._contacts = contacts
+
+    ###########
+    # Methods #
+    ###########
+
+    def _update(self):
+        """Update the upper unilateral inequality matrix and vector."""
+        rot = get_matrix_from_quaternion(self.model.get_orientation(self._link)).T
+        rot = block_diag((rot, rot))
+        self._A_ineq = self._zmp_matrix.dot(rot)  # (4,6)
diff --git a/pyrobolearn/priorities/constraints/torque/joint_limits.py b/pyrobolearn/priorities/constraints/torque/joint_limits.py
index 0358bb3..da3dfa4 100644
--- a/pyrobolearn/priorities/constraints/torque/joint_limits.py
+++ b/pyrobolearn/priorities/constraints/torque/joint_limits.py
@@ -26,7 +26,7 @@ __status__ = "Development"
 class JointLimitsConstraint(BoundConstraint, JointTorqueConstraint):
     r"""Joint Limits constraint.
 
-    This provides bounds/limits on the joint torques:
+    This provides bounds/limits on the joint torques (based on a PD control feedback law):
 
     .. math::  k_p (q_{lb} - q) - k_d \dot{q} \leq \tau \leq k_p (q_{ub} - q) - k_d \dot{q}
 
diff --git a/pyrobolearn/priorities/constraints/velocity/__init__.py b/pyrobolearn/priorities/constraints/velocity/__init__.py
index 7c6678f..f0f193a 100644
--- a/pyrobolearn/priorities/constraints/velocity/__init__.py
+++ b/pyrobolearn/priorities/constraints/velocity/__init__.py
@@ -1,8 +1,22 @@
 
 # joint velocity constraints
 
+# from .capture_point import CapturePointConstraint
+
+from .cartesian_position import CartesianPositionConstraint
+
+from .cartesian_velocity import CartesianVelocityConstraint
+
+from .com_velocity import CoMVelocityConstraint
+
+from .convex_hull import ConvexHullConstraint
+
+# from .dynamics import DynamicsConstraint
+
 from .joint_limits import JointPositionLimitsConstraint
 
-from .velocity_limits import JointVelocityLimitsConstraint
-
 from .joint_velocity import DifferentialKinematicsConstraint
+
+# from .self_collision_avoidance import SelfCollisionAvoidanceConstraint
+
+from .velocity_limits import JointVelocityLimitsConstraint
diff --git a/pyrobolearn/priorities/constraints/velocity/capture_point.py b/pyrobolearn/priorities/constraints/velocity/capture_point.py
index 47b2973..c1c6808 100644
--- a/pyrobolearn/priorities/constraints/velocity/capture_point.py
+++ b/pyrobolearn/priorities/constraints/velocity/capture_point.py
@@ -8,9 +8,11 @@ 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.constraints.constraint import Constraint
+from pyrobolearn.priorities.constraints.constraint import UnilateralConstraint, JointVelocityConstraint
 
 
 __author__ = "Brian Delhaisse"
@@ -23,9 +25,20 @@ __email__ = "briandelhaisse@gmail.com"
 __status__ = "Development"
 
 
-class CapturePointConstraint(Constraint):
+class CapturePointConstraint(UnilateralConstraint, JointVelocityConstraint):
     r"""Capture Point constraint.
 
+    Definition: "For a biped in state :math:`x`, a Capture Point (CP) :math:`P`, is a point on the ground such that
+    if the biped covers :math:`P` (makes its base of support include :math:`P`), either with its stance foot or by
+    stepping to :math:`P` in a single step, and then maintains its Center of Pressure (CoP) to lie on :math:`P`, then
+    there exists a safe feasible trajectory leading to a capture state (i.e. a state in which the kinetic energy of
+    the biped is zero and can remain zero with suitable joint torque (note that the CoM must lie above the CoP in a
+    capture state))." [1] "Intuitively, the CP is the point on the floor onto which the robot has to step to come
+    to a complete rest" [2].
+
+    References:
+        - [1] "Capture Point: A Step toward Humanoid PushRecovery", Pratt et al., 2006
+        - [2] "Bipedal walking control based on Capture Point dynamics", Englsberger et al., 2011
     """
 
     def __init__(self, model):
diff --git a/pyrobolearn/priorities/constraints/velocity/com_velocity.py b/pyrobolearn/priorities/constraints/velocity/com_velocity.py
index 9f6ec51..aeb7ef3 100644
--- a/pyrobolearn/priorities/constraints/velocity/com_velocity.py
+++ b/pyrobolearn/priorities/constraints/velocity/com_velocity.py
@@ -1,6 +1,15 @@
 #!/usr/bin/env python
 r"""Provide the Center of Mass velocity constraint.
 
+The bilateral inequality CoM velocity constraint is given by:
+
+.. math:: v_{lb} \leq J_{CoM}(q) \dot{q} \leq v_{ub}
+
+where :math:`v_{lb}, v_{ub}` are the lower and upper bound of the CoM velocities, :math:`\dot{q}` are the joint
+velocities being optimized, and :math:`J_{CoM}(q)` is the CoM Jacobian.
+
+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_{CoM}(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 +19,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 +32,91 @@ __email__ = "briandelhaisse@gmail.com"
 __status__ = "Development"
 
 
-class CoMVelocityConstraint(Constraint):
-    r"""Center of Mass Velocity constraint.
+class CoMVelocityConstraint(BilateralConstraint, JointVelocityConstraint):
+    r"""Center of Mass (CoM) Velocity constraint.
 
+    The bilateral inequality CoM velocity constraint is given by:
+
+    .. math:: v_{lb} \leq J_{CoM}(q) \dot{q} \leq v_{ub}
+
+    where :math:`v_{lb}, v_{ub}` are the lower and upper bound of the CoM linear velocities, :math:`\dot{q}` are the
+    joint velocities being optimized, and :math:`J_{CoM}(q)` is the CoM Jacobian.
+
+    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_{CoM}(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, velocity_bounds):
+        r"""
+        Initialize the constraint.
+
+        Args:
+            model (ModelInterface): model interface.
+            velocity_bounds (tuple[2 * np.array[float[3]]], np.array[float[3]], float, None): If tuple, it is the
+              lower and upper bounds on the linear velocity. If np.array or float, then the lower and upper bound
+              would be set to (-velocity_bounds, velocity_bounds). If None, it will not be considered.
+        """
         super(CoMVelocityConstraint, self).__init__(model)
+
+        # define variables
+        self.velocity_bounds = velocity_bounds
+
+        # first update
+        self.update()
+
+    ##############
+    # Properties #
+    ##############
+
+    @property
+    def velocity_bounds(self):
+        """Get the CoM linear velocity bounds."""
+        return self._vel_bounds
+
+    @velocity_bounds.setter
+    def velocity_bounds(self, bounds):
+        """Set the CoM linear velocity bounds."""
+        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 i, bound in enumerate(bounds):
+                    if isinstance(bound, (int, float)):
+                        bound = np.ones(3) * bound
+                        bounds[i] = bound
+                    elif isinstance(bound, np.ndarray) and len(bound) != 3:
+                        raise ValueError("Expecting the given bound to be of length 3, but instead got a length of "
+                                         "{}".format(len(bound)))
+                    else:
+                        raise TypeError("Expecting the given bound to be a np.array, but got instead: "
+                                        "{}".format(type(bound)))
+            elif isinstance(bounds, np.ndarray):
+                bounds = bounds.reshape(-1)
+                if len(bounds) == 3:
+                    bounds = (-bounds, bounds)
+                elif len(bounds) == 6:
+                    bounds = (bounds[:3], bounds[3:])
+                else:
+                    raise ValueError("Expecting the given bounds to be of length 3 or 6 but got instead a length of: "
+                                     "{}".format(len(bounds)))
+            elif isinstance(bounds, (int, float)):
+                bounds = (-np.ones(3) * bounds, np.ones(3) * bounds)
+            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._vel_bounds = bounds
+
+    ###########
+    # Methods #
+    ###########
+
+    def _update(self):
+        """Update the inequality matrix and vectors."""
+        self._A_ineq = self.model.get_com_jacobian(full=False)  # (3,N)
+        self._b_lower_bound, self._b_upper_bound = self._vel_bounds
diff --git a/pyrobolearn/priorities/constraints/velocity/convex_hull.py b/pyrobolearn/priorities/constraints/velocity/convex_hull.py
index faab44a..f68fbfb 100644
--- a/pyrobolearn/priorities/constraints/velocity/convex_hull.py
+++ b/pyrobolearn/priorities/constraints/velocity/convex_hull.py
@@ -10,7 +10,7 @@ References:
 
 import numpy as np
 
-from pyrobolearn.priorities.constraints.constraint import Constraint
+from pyrobolearn.priorities.constraints.constraint import UnilateralConstraint, JointVelocityConstraint
 
 
 __author__ = "Brian Delhaisse"
@@ -23,7 +23,7 @@ __email__ = "briandelhaisse@gmail.com"
 __status__ = "Development"
 
 
-class ConvexHullConstraint(Constraint):
+class ConvexHullConstraint(UnilateralConstraint, JointVelocityConstraint):
     r"""Convex Hull constraint.
 
     """
diff --git a/pyrobolearn/priorities/tasks/README.rst b/pyrobolearn/priorities/tasks/README.rst
index a38a6f6..0a74b15 100644
--- a/pyrobolearn/priorities/tasks/README.rst
+++ b/pyrobolearn/priorities/tasks/README.rst
@@ -104,6 +104,15 @@ the identity matrix and :math:`b=0` is the zero/null vector).
 
 Tasks include cartesian CoM tracking, cartesian end-effector position tracking, postural positioning, and others.
 
+
+Tasks are separated into 4 folders (velocity, acceleration, torque, and cartesian force); one for each optimization 
+variable vector that is being optimized. Note that different type of tasks can be combined together; for instance, 
+we can combine acceleration tasks with force tasks. This will create an optimization variable vector 
+:math:`x = [\ddot{q}^\top, F^\top]^\top` which can then be used with the joint space dynamic equation 
+:math:`\tau = H \ddot{q} + C(q,\dot{q})\dot{q} + g(q) - J^\top F` to get the equivalent joint torques to be applied 
+on the robot. 
+
+
 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
diff --git a/pyrobolearn/priorities/tasks/force/wrench.py b/pyrobolearn/priorities/tasks/force/wrench.py
index 85469d0..181f9a7 100644
--- a/pyrobolearn/priorities/tasks/force/wrench.py
+++ b/pyrobolearn/priorities/tasks/force/wrench.py
@@ -26,6 +26,8 @@ __status__ = "Development"
 class WrenchTask(Task):
     r"""Wrench Task
 
+    The wrench task
+
     """
 
     def __init__(self, model, constraints=[]):
diff --git a/pyrobolearn/priorities/tasks/task.py b/pyrobolearn/priorities/tasks/task.py
index 6a58507..65a84ae 100644
--- a/pyrobolearn/priorities/tasks/task.py
+++ b/pyrobolearn/priorities/tasks/task.py
@@ -111,6 +111,7 @@ References:
 import numpy as np
 import copy
 
+import pyrobolearn as prl
 from pyrobolearn.priorities.models import ModelInterface
 from pyrobolearn.priorities.constraints.constraint import Constraint, NullConstraint
 
@@ -125,9 +126,6 @@ __email__ = "briandelhaisse@gmail.com"
 __status__ = "Development"
 
 
-# TODO: take into account constraints
-# TODO: take into account hard priority tasks
-
 class Task(object):
     r"""Task (abstract) class.
 
@@ -163,6 +161,9 @@ class Task(object):
         self.weight = weight
         self.constraints = constraints
 
+        # if the task is enabled or not. A disabled task is the same as setting the weight to 0.
+        self._enabled = True
+
         # check that the task is a valid task or a stack o tasks
         if self.model is None and not self.is_stack_of_tasks():
             raise RuntimeError("Expecting the task to be a valid task or a stack of tasks. You can not instantiate "
@@ -205,8 +206,12 @@ class Task(object):
                 # go through each soft task in the hard task
                 for j, soft_task in enumerate(hard_task):
                     if not isinstance(soft_task, Task):
-                        raise TypeError("The given task positioned at ({}, {}) is not an instance of `Task`, but: "
-                                        "{}".format(i, j, type(soft_task)))
+                        if isinstance(soft_task, Constraint):
+                            # if constraint (must be an equality constraint), try to convert it into a task
+                            tasks[i][j] = prl.priorities.tasks.TaskFromConstraint(soft_task)
+                        else:
+                            raise TypeError("The given task positioned at ({}, {}) is not an instance of `Task`, "
+                                            "but: {}".format(i, j, type(soft_task)))
             else:   # if not, check that the hard task is an instance of Task
                 if not isinstance(hard_task, Task):
                     raise TypeError("Expecting the {}th hard task to be an instance of `Task` or a list of `Task`, "
@@ -458,9 +463,6 @@ 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(c.shape)
-                print(b.shape)
-                print(AW.shape)
                 ps.append(c - 2 * AW.dot(b))
             return ps
         return self._c - 2 * self._A.T.dot(self.weight).dot(self._b)
@@ -655,6 +657,11 @@ class Task(object):
         #     return results[0]
         return results
 
+    @property
+    def enabled(self):
+        """Return if the task is enabled or not."""
+        return self._enabled
+
     ##################
     # Static Methods #
     ##################
@@ -699,6 +706,19 @@ class Task(object):
         """Return True if the current task is a single task."""
         return self.model is not None and not self.is_stack_of_tasks()
 
+    def enable(self, enable=True):
+        """Enable the single task, or each task if stack of tasks."""
+        if self.is_stack_of_tasks():
+            for hard_task in self.tasks:
+                for soft_task in hard_task:
+                    soft_task.enable(enable=enable)
+        else:
+            self._enabled = enable
+
+    def disable(self, disable=True):
+        """Disable the single task, or each task if stack of tasks."""
+        self.enable(not disable)
+
     def add_hard_task(self, task):
         """Add the given hard task.
 
@@ -714,11 +734,14 @@ class Task(object):
 
         constraints = task.constraints
         if task.is_stack_of_tasks():
-            task = task.tasks
+            tasks = task.tasks
         else:
-            task = [[task]]
+            if isinstance(task, Constraint):
+                # if given task is a constraint (must be an equality constraint), try to convert it into a task
+                task = prl.priorities.tasks.TaskFromConstraint(task)
+            tasks = [[task]]
 
-        for hard_task, constraint in zip(task, constraints):
+        for hard_task, constraint in zip(tasks, constraints):
             self.tasks.append(hard_task)
             self.constraints.append(constraint)
 
@@ -747,6 +770,9 @@ class Task(object):
                 for soft_task in hard_task:
                     self.tasks[-1].append(soft_task)
         else:  # task is single task
+            if isinstance(task, Constraint):
+                # if given task is a constraint (must be an equality constraint), try to convert it into a task
+                task = prl.priorities.tasks.TaskFromConstraint(task)
             self.tasks[-1].append(task)
             self._constraints[-1] = self._constraints[-1] + task.constraint
 
@@ -1104,7 +1130,6 @@ class JointTorqueTask(Task):
 
 # Tests
 if __name__ == '__main__':
-    import pyrobolearn as prl
 
     sim = prl.simulators.Bullet(render=False)
     robot = prl.robots.KukaIIWA(sim)
diff --git a/pyrobolearn/robots/legged_robot.py b/pyrobolearn/robots/legged_robot.py
index 935cc5b..44a51f1 100644
--- a/pyrobolearn/robots/legged_robot.py
+++ b/pyrobolearn/robots/legged_robot.py
@@ -253,8 +253,8 @@ class LeggedRobot(Robot):
 
         .. math::
 
-            d_x^{-} \leq \frac{n^i_y}{f^i_z} \leq d_x^{+} \\
-            d_y^{-} \leq -\frac{n^i_x}{f^i_z} \leq d_y^{+}
+            d_x^{-} \leq -\frac{n^i_y}{f^i_z} \leq d_x^{+} \\
+            d_y^{-} \leq \frac{n^i_x}{f^i_z} \leq d_y^{+}
 
         which ensures the stability of the foot/ground contact. The :math:`(d_x^{-}, d_x^{+})` and
         :math:`(d_y^{-}, d_y^{+})` defines the size of the sole in the x and y directions respectively. Basically,
diff --git a/pyrobolearn/robots/robot.py b/pyrobolearn/robots/robot.py
index 5740e00..3499bd7 100644
--- a/pyrobolearn/robots/robot.py
+++ b/pyrobolearn/robots/robot.py
@@ -4281,9 +4281,12 @@ class Robot(ControllableBody):
         # compute centroidal momentum matrix and the dot product between the derivative of this centroidal momentum
         # matrix with the generalized velocities vector
         A_G = X_iG_T.dot(Psi_1.T).dot(U_1).dot(H)  # shape = (6,n+6)
-        A_Gd_dq = X_iG_T.dot(Psi_1.T).dot(U_1).dot(C_dq)  # shape = (6,)
+        dA_G_dq = X_iG_T.dot(Psi_1.T).dot(U_1).dot(C_dq)  # shape = (6,)
 
-        return A_G, A_Gd_dq
+        return A_G, dA_G_dq
+
+    def get_centroidal_momentum_matrix(self, q=None, dq=None, inertia=None):
+        return self.get_centroidal_dynamics(q=q, dq=dq, inertia=inertia)[0]
 
     def get_centroidal_momentum(self, q=None, dq=None, inertia=None):
         r"""
diff --git a/pyrobolearn/utils/transformation.py b/pyrobolearn/utils/transformation.py
index 6757a26..15b48d1 100644
--- a/pyrobolearn/utils/transformation.py
+++ b/pyrobolearn/utils/transformation.py
@@ -10,6 +10,7 @@ References:
 """
 
 import numpy as np
+from scipy.linalg import block_diag
 import quaternion
 # from pyquaternion import Quaternion  # TODO: check API at http://kieranwynn.github.io/pyquaternion
 import sympy
@@ -45,6 +46,19 @@ def min_angle_difference(q1, q2):
     return diff
 
 
+def get_adjoint_from_rotation(rotation_matrix):
+    r"""
+    Get the adjoint matrix from a rotation matrix.
+
+    Args:
+        rotation_matrix (np.array[float[3,3]]): rotation matrix.
+
+    Returns:
+        np.array[float[6,6]]: adjoint matrix.
+    """
+    return block_diag(rotation_matrix, rotation_matrix)
+
+
 def get_homogeneous_transform(position, orientation):
     r"""
     Return the Homogeneous transform matrix given the position vector and the orientation.