update priorities and add examples

pyrobolearn/__init__.py

@@ -91,7 +91,7 @@ from . import algos
 # import experiments
 
 # import priority tasks
-# from . import priorities
+from . import priorities
 
 
 # Meta-information about the package

pyrobolearn/priorities/README.rst

@@ -1,8 +1,6 @@
 Priority Tasks
 ==============
 
-THIS IS UNDER CONSTRUCTION
-
 In this folder, you will find the code for priority "tasks". The "tasks" defined here are different from the tasks 
 defined in the ``pyrobolearn/tasks`` folder which defines robot learning tasks. The tasks defined here can be more seen
 as "constraints"; for instance, the constraint for the robot to maintain its balance (i.e. have its center of mass 

pyrobolearn/priorities/attractor_point.py

@@ -0,0 +1,72 @@
+#!/usr/bin/env python
+"""Attractor point using impedance control with RRBot.
+
+Try to move the end-effector using the mouse, and see what happens. This example use priority tasks and constraints,
+which are optimized using Quadratic Programming (QP).
+"""
+
+import numpy as np
+import time
+import pyrobolearn as prl
+
+
+# Create simulator
+sim = prl.simulators.Bullet()
+
+# create world
+world = prl.worlds.BasicWorld(sim)
+
+# create robot
+robot = world.load_robot(prl.robots.RRBot)
+robot.disable_motor()           # disable motors; comment the `robot.set_joint_torques(torques)` to see what happens
+robot.print_info()
+robot.change_transparency()
+
+# define useful variables for impedance control
+link_id = robot.end_effectors[0]  # the link we are interested to
+x_des = robot.get_link_world_positions(link_id)       # desired cartesian position
+wrt_link_id = None
+
+# gains
+K = 100 * np.identity(3)
+D = 6 * np.sqrt(K)
+
+# draw a sphere at the desired location
+world.load_visual_sphere(position=x_des, radius=0.1, color=(0, 1, 0, 0.5))
+
+# create task
+model = prl.priorities.models.RobotModelInterface(robot)
+cartesian_task = prl.priorities.tasks.torque.CartesianImpedanceControlTask(model, distal_link=link_id,
+                                                                           base_link=wrt_link_id,
+                                                                           desired_position=x_des, kp_position=100,
+                                                                           kd_linear=60)
+postural_task = prl.priorities.tasks.torque.JointImpedanceControlTask(model, q_desired=[0., 0.],
+                                                                      kp=10)
+# task = cartesian_task
+# task = cartesian_task / postural_task
+task = cartesian_task + 0.05 * postural_task
+print("\nTask: \n{}\n".format(task))
+solver = prl.priorities.solvers.QPTaskSolver(task=task)
+
+# run simulation
+times = []
+for t in prl.count():
+
+    # update task
+    task.update(update_model=True)
+
+    # solve task
+    start = time.time()
+    torques = solver.solve()
+    end = time.time()
+    times.append(end - start)
+
+    if (t+1) % 1000 == 0:
+        print("solving time: avg={}, std={}".format(np.mean(times), np.std(times)))
+        times = []
+
+    # set joint torques
+    robot.set_joint_torques(torques)
+
+    # step in simulation
+    world.step(sleep_dt=sim.dt)

pyrobolearn/priorities/constraints/acceleration/dynamic_feasibility.py

@@ -1,7 +1,7 @@
 #!/usr/bin/env python
 r"""Provide the dynamic feasibility constraint.
 
-The equality joint acceleration constraint is given by:
+The equality dynamic feasibility constraint is given by:
 
 .. math:: H(q) \ddot{q} + N(q, \dot{q}) = \sum_i J_i^T F_i
 

pyrobolearn/priorities/constraints/constraint.py

@@ -551,7 +551,8 @@ class Constraint(object):
             for constraint in self.constraints:
                 constraint.update()
         else:
-            self._update()
+            if self._enabled:  # update only if enabled
+                self._update()
 
     #############
     # Operators #

pyrobolearn/priorities/constraints/force/contact.py

@@ -91,6 +91,12 @@ class ContactConstraint(LowerUnilateralConstraint, ForceConstraint):
                             "{}".format(type(contacts)))
         self._contacts = contacts
 
+        # enable / disable the constraint based on the number of contact links
+        if len(contacts) == 0:
+            self.disable()
+        else:
+            self.enable()
+
     ###########
     # Methods #
     ###########

pyrobolearn/priorities/constraints/force/friction.py

@@ -164,6 +164,12 @@ class FrictionPyramidConstraint(UpperUnilateralConstraint, ForceConstraint):
                             "{}".format(type(contacts)))
         self._contacts = contacts
 
+        # enable / disable the constraint based on the number of contact links
+        if len(contacts) == 0:
+            self.disable()
+        else:
+            self.enable()
+
     ###########
     # Methods #
     ###########
@@ -172,8 +178,8 @@ class FrictionPyramidConstraint(UpperUnilateralConstraint, ForceConstraint):
         """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 = get_matrix_from_quaternion(self.model.get_orientation(contact)).T
             rot = block_diag((rot, rot))
-            self._A_ineq[i*4:(i+1)*4, i*6:(i+1)*6] = self._friction_matrix.dot()
+            self._A_ineq[i*4:(i+1)*4, i*6:(i+1)*6] = self._friction_matrix.dot(rot)
 
         self._b_upper_bound = np.zeros(4 * len(self.contacts))

pyrobolearn/priorities/constraints/velocity/cartesian_velocity.py

@@ -67,9 +67,9 @@ class CartesianVelocityConstraint(BilateralConstraint, JointVelocityConstraint):
             linear_velocity_bounds (tuple[2 * np.array[float[3]]], np.array[float[3]], None): If tuple, it is the
               lower and upper bounds on the linear velocity. If np.array, then the lower and upper bound would be set
               to (-linear_velocity_bounds, linear_velocity_bounds). If None, it will not be considered.
-            angular_velocity_bounds (tuple[2 * np.array[float[3]]], np.array[float[3]], None): If tuple, it is the lower and
-              upper bounds on the angular velocity. If np.array, then the lower and upper bound would be set to
-              (-angular_velocity_bounds, angular_velocity_bounds). If None, it will not be considered.
+            angular_velocity_bounds (tuple[2 * np.array[float[3]]], np.array[float[3]], None): If tuple, it is the
+              lower and upper bounds on the angular velocity. If np.array, then the lower and upper bound would be set
+              to (-angular_velocity_bounds, angular_velocity_bounds). If None, it will not be considered.
         """
         super(CartesianVelocityConstraint, self).__init__(model)
 

pyrobolearn/priorities/constraints/velocity/convex_hull.py

@@ -1,16 +1,31 @@
 #!/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:
 
-The implementation of this class is inspired by [1] (which is licensed under the LGPLv2).
+.. math:: A_{CH} J_{CoM} \dot{q} \leq b_{CH}
+
+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."
+
+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}`.
+
+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.
+
+The implementation of this class is inspired by [1] (which is licensed under the LGPLv2), but we use the
+`scipy.spatial.ConvexHull` class [2].
 
 References:
     - [1] "OpenSoT: A whole-body control library for the compliant humanoid robot COMAN", Rocchi et al., 2015
+    - [2] ConvexHull: https://docs.scipy.org/doc/scipy-0.19.0/reference/generated/scipy.spatial.ConvexHull.html
 """
 
 import numpy as np
+from scipy.spatial import ConvexHull
 
-from pyrobolearn.priorities.constraints.constraint import UnilateralConstraint, JointVelocityConstraint
+from pyrobolearn.priorities.constraints.constraint import UpperUnilateralConstraint, JointVelocityConstraint
 
 
 __author__ = "Brian Delhaisse"
@@ -23,10 +38,116 @@ __email__ = "briandelhaisse@gmail.com"
 __status__ = "Development"
 
 
-class ConvexHullConstraint(UnilateralConstraint, JointVelocityConstraint):
+class ConvexHullConstraint(UpperUnilateralConstraint, JointVelocityConstraint):
     r"""Convex Hull constraint.
 
+    From the documentation of the framework of [1]: "this constraint implements a constraint of the type:
+
+    .. math:: A_{CH} J_{CoM} \dot{q} \leq b_{CH}
+
+    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."
+
+    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}`.
+
+    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.
+
+    The implementation of this class is inspired by [1] (which is licensed under the LGPLv2), but we use the
+    `scipy.spatial.ConvexHull` class [2].
+
+    References:
+        - [1] "OpenSoT: A whole-body control library for the compliant humanoid robot COMAN", Rocchi et al., 2015
+        - [2] ConvexHull: https://docs.scipy.org/doc/scipy-0.19.0/reference/generated/scipy.spatial.ConvexHull.html
     """
 
-    def __init__(self, model):
+    def __init__(self, model, points=[], ticks=20):
+        r"""
+        Initialize the constraint.
+
+        Args:
+            model (ModelInterface): model interface.
+            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.
+        """
         super(ConvexHullConstraint, self).__init__(model)
+
+        self.ticks = ticks
+        self._cnt = 0
+        self._hull = None
+
+        self.points = points
+
+    ##############
+    # Properties #
+    ##############
+
+    @property
+    def ticks(self):
+        """Return the number of ticks to sleep before the next update."""
+        return self._ticks
+
+    @ticks.setter
+    def ticks(self, ticks):
+        """Set the number of ticks to sleep before the next update."""
+        if not isinstance(ticks, int):
+            raise TypeError("Expecting the given 'ticks' to be a int, but got instead: {}".format(type(ticks)))
+        if ticks < 1:
+            raise ValueError("Expecting the given 'ticks' to be bigger or equal to 1, but got: {}".format(ticks))
+        self._ticks = ticks
+
+    @property
+    def points(self):
+        """Return the list of 3D points."""
+        return self._points
+
+    @points.setter
+    def points(self, points):
+        """Set the list of 3D points."""
+        if not isinstance(points, (list, tuple, np.ndarray)):
+            raise TypeError("Expecting the given 'points' to be a list of 3D points.")
+        if isinstance(points, np.ndarray):
+            points = points.reshape(-1, 3)  # (M,3)
+        self._points = points
+
+    @property
+    def hull(self):
+        """Return the convex hull instance."""
+        return self._hull
+
+    @property
+    def vertices(self):
+        """Return the list of vertices that forms the convex hull."""
+        if self._hull is not None:
+            return self._hull.vertices
+
+    ###########
+    # Methods #
+    ###########
+
+    def _update(self):
+        """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
+
+            # convex hull equations
+            A = hull.equations[:, :-1]
+            b = 1 * hull.equations[:, -1]
+
+            # get jacobian
+            jacobian = self.model.get_com_jacobian(full=False)  # shape: (3,N)
+
+            # constraint matrix and vector
+            self._A_ineq = A.dot(jacobian)  # (F,N)
+            self._b_upper_bound = b  # (F,)
+
+            # reset counter
+            self._cnt = 0
+
+        # update counter
+        self._cnt += 1

pyrobolearn/priorities/constraints/velocity/dynamics.py

@@ -8,6 +8,8 @@ References:
     - [1] "OpenSoT: A whole-body control library for the compliant humanoid robot COMAN", Rocchi et al., 2015
 """
 
+# TODO: finish to implement this class
+
 import numpy as np
 
 from pyrobolearn.priorities.constraints.constraint import Constraint
@@ -26,6 +28,32 @@ __status__ = "Development"
 class DynamicsConstraint(Constraint):
     r"""Dynamics constraint.
 
+    From the documentation of the framework of [1]: "the DynamicsConstraint class implements constraints on joint
+    velocities due to dynamics feasibility.
+
+    The constraint is written as:
+
+    .. math:: u_{min} \leq (M/dT) dq \leq u_{max}
+
+    with:
+
+    .. math::
+
+        u_{min} = \tau_{min} dT - N(q, \dot{q}) dT + M \dot{q} - dT J_c^\top F_c \\
+        u_{max} = \tau_{max} dT - N(q, \dot{q}) dT + M \dot{q} - dT J_c^\top F_c \\\\
+        N(q, \dot{q}) = C(q, \dot{q}) \dot{q} + g(q) \\
+        J_c = [J_{c,1} \cdot J_{c,N}]^\top \\
+        F_c = [F_{c,1} \cdot J_{c,N}]^\top
+
+    where :math:`\dot{q}` is the velocity in the previous step, :math:`J_c` is the Jacobian of all the contacts (here,
+    we consider these Jacobians from the base link to the force/torque sensor frames), :math:`F_c` are the contact
+    forces (at the force/torque sensor frames transformed in the base link)."
+
+
+    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):

pyrobolearn/priorities/ik.py

@@ -1,4 +1,9 @@
+#!/usr/bin/env python
+"""Inverse kinematics with the Kuka robot where the goal is to follow a moving sphere.
 
+The inverse kinematics is performed using priority tasks and constraints, which are optimized using Quadratic
+Programming (QP).
+"""
 
 import numpy as np
 import time
@@ -53,7 +58,6 @@ for t in prl.count():
     cartesian_task.desired_position = sphere.position
     task.update(update_model=True)
 
-    q = robot.get_joint_positions()
     start = time.time()
     dq = solver.solve()
     end = time.time()
@@ -64,6 +68,7 @@ for t in prl.count():
         times = []
 
     # set joint positions
+    q = robot.get_joint_positions()
     q = q[q_idx] + dq * sim.dt
     robot.set_joint_positions(q, joint_ids=joint_ids)
 

pyrobolearn/priorities/models/model.py

@@ -302,6 +302,22 @@ class ModelInterface(object):
         """
         pass
 
+    def get_velocity(self, link, wrt_link=None, point=(0., 0., 0.)):
+        r"""
+        Compute the linear and angular velocity of a link, given by :math:`v = [\dot{p}, \omega]`.
+
+        Args:
+            link (int, str): unique link id, or name.
+            wrt_link (int, str, None): the other link id, or name. If None, returns the velocities wrt to the world,
+              and if -1 wrt to the base.
+            point (np.array[float[3]]): position of the point in link's local frame.
+
+        Returns:
+            np.array[float[6]]: The resulting 6D velocity vector where the first three elements are the linear
+                velocity and the last three are the angular velocity expressed in the global world reference frame.
+        """
+        pass
+
     def get_velocity_twist(self, link):
         r"""
         Compute the angular and linear velocity of a link, given by :math:`v = [\omega, \dot{p}]`.

pyrobolearn/priorities/models/robot_model.py

@@ -337,6 +337,25 @@ class RobotModelInterface(ModelInterface):
             return self.model.get_link_world_orientations(link)
         return self.model.get_link_orientations(link, wrt_link_id=self.get_link_id(wrt_link))
 
+    def get_velocity(self, link, wrt_link=None, point=(0., 0., 0.)):  # TODO: use point
+        r"""
+        Compute the linear and angular velocity of a link, given by :math:`v = [\dot{p}, \omega]`.
+
+        Args:
+            link (int, str): unique link id, or name.
+            wrt_link (int, str, None): the other link id, or name. If None, returns the velocities wrt to the world,
+              and if -1 wrt to the base.
+            point (np.array[float[3]]): position of the point in link's local frame.
+
+        Returns:
+            np.array[float[6]]: The resulting 6D velocity vector where the first three elements are the linear
+                velocity and the last three are the angular velocity expressed in the global world reference frame.
+        """
+        link = self.get_link_id(link)
+        if wrt_link is None:
+            return self.model.get_link_world_velocities(link)
+        return self.model.get_link_velocities(link, wrt_link_id=self.get_link_id(wrt_link))
+
     def get_velocity_twist(self, link, point=(0., 0., 0.)):  # TODO: use point
         r"""
         Compute the angular and linear velocity of a link, given by :math:`v = [\omega, \dot{p}]`.
@@ -512,7 +531,7 @@ class RobotModelInterface(ModelInterface):
             return self._states['H']
 
         # compute, cache, and return it
-        inertia = self.model.get_inertia_matrix(q=q)
+        inertia = self.model.get_inertia_matrix()
         self._states['H'] = inertia
         return inertia
 

pyrobolearn/priorities/tasks/__init__.py

@@ -1,6 +1,7 @@
 
 # import task
 from .task import *
+from .task_from_constraint import TaskFromConstraint
 
 # import velocity tasks
 from . import velocity

pyrobolearn/priorities/tasks/acceleration/cartesian.py

@@ -491,14 +491,14 @@ class CartesianAccelerationTask(JointAccelerationTask):
         """
         return self.x_desired, self.dx_desired, self.ddx_desired
 
-    def _update(self):
+    def _update(self, x=None):
         """
         Update the task by computing the A matrix and b vector that will be used by the task solver.
         """
         x = self.model.get_pose(link=self.distal_link, wrt_link=self.base_link)
         self._A = self.model.get_jacobian(link=self.distal_link, wrt_link=self.base_link,
                                           point=self.local_position)  # shape: (6,N)
-        vel = self.model.get_velocity(link=self.distal_link)
+        vel = self.model.get_velocity(link=self.distal_link, wrt_link=self.base_link)
         jdotqdot = self.model.compute_JdotQdot(link=self.distal_link)
         # b = - \dot{J} \dot{q} + (a_d + K_d (v_d - v) + K_p e)
         b = -jdotqdot + self.desired_acceleration

pyrobolearn/priorities/tasks/acceleration/com.py

@@ -314,7 +314,7 @@ class CoMAccelerationTask(JointAccelerationTask):
         """
         return self.x_desired, self.dx_desired, self.ddx_desired
 
-    def _update(self):
+    def _update(self, x=None):
         """
         Update the task by computing the A matrix and b vector that will be used by the task solver.
         """

pyrobolearn/priorities/tasks/acceleration/contact.py

@@ -81,7 +81,7 @@ class ContactAccelerationTask(JointAccelerationTask):
               square matrix). You can specify only the diagonal elements if you wish. If None, by default it will be
               set to the identity matrix.
             weight (float, np.array[float[6,6]]): weight scalar or matrix associated to the task.
-            constraints (list of Constraint): list of constraints associated with the task.
+            constraints (list[Constraint]): list of constraints associated with the task.
         """
         super(ContactAccelerationTask, self).__init__(model=model, weight=weight, constraints=constraints)
 
@@ -133,7 +133,7 @@ class ContactAccelerationTask(JointAccelerationTask):
     # Methods #
     ###########
 
-    def _update(self):
+    def _update(self, x=None):
         """
         Update the task by computing the A matrix and b vector that will be used by the task solver.
         """

pyrobolearn/priorities/tasks/acceleration/dynamic_feasibility.py

@@ -0,0 +1,111 @@
+#!/usr/bin/env python
+r"""Provide the dynamic feasibility task (which is based on the dynamic feasibility constraint).
+
+The equality joint acceleration constraint is given by:
+
+.. math:: H(q) \ddot{q} + N(q, \dot{q}) = \sum_i J_i^T F_i
+
+where :math:`H(q)` is the joint space inertia matrix, :math:`\ddot{q}` are the joint accelerations being optimized,
+:math:`N(q, \dot{q})` is the vector of force terms that account for the Coriolis and centrifugal forces, gravity,
+and any other forces acting on the system other than the contact forces given by :math:`\sum_i J_i^T F_i` (where
+each :math:`J_i` is a Jacobian matrix and :math:`F_i` is a wrench vector at the contact link :math:`i`).
+
+This formulation can be rewritten as an equality constraint math:`A_{eq} x = b_{eq}` in QP, with
+:math:`x = \ddot{q}`, :math:`A_{eq} = H(q)`, and :math:`b_{eq} = \sum_i J_i^T F_i - N(q, \dot{q})`.
+
+The implementation of this class is inspired by [1] (which is licensed under the LGPLv2).
+
+References:
+    - [1] "OpenSoT: A whole-body control library for the compliant humanoid robot COMAN", Rocchi et al., 2015
+"""
+
+from pyrobolearn.priorities.tasks import JointAccelerationTask, TaskFromConstraint
+from pyrobolearn.priorities.constraints.acceleration.dynamic_feasibility import DynamicFeasibilityConstraint
+
+
+__author__ = "Brian Delhaisse"
+__copyright__ = "Copyright 2019, PyRoboLearn"
+__credits__ = ["Brian Delhaisse"]
+__license__ = "GNU GPLv3"
+__version__ = "1.0.0"
+__maintainer__ = "Brian Delhaisse"
+__email__ = "briandelhaisse@gmail.com"
+__status__ = "Development"
+
+
+class DynamicFeasibilityTask(JointAccelerationTask):
+    r"""Dynamic Feasibility Task
+
+    The dynamic feasibility constraint tries to enforce the joint space dynamic equation of motion given by
+    :math:`H(q) \ddot{q} + N(q, \dot{q}) = \sum_i J_i^T F_i`. This is a softer version of the corresponding equality
+    constraint (see `priorities/constraints/acceleration/dynamic_feasibility.py`).
+
+    The task minimizes:
+
+    .. math:: || H(q) \ddot{q} - (\sum_i J_i^T F_i - N(q, \dot{q})) ||^2,
+
+    where :math:`H(q)` is the joint space inertia matrix, :math:`\ddot{q}` are the joint accelerations being optimized,
+    :math:`N(q, \dot{q})` is the vector of force terms that account for the Coriolis and centrifugal forces, gravity,
+    and any other forces acting on the system other than the contact forces given by :math:`\sum_i J_i^T F_i` (where
+    each :math:`J_i` is a Jacobian matrix and :math:`F_i` is a wrench vector at the contact link :math:`i`).
+
+    This is equivalent to the QP objective function :math:`||Ax - b||^2`, by setting :math:`A=H(q)`,
+    :math:`x=\ddot{q}`, and :math:`b = \sum_i J_i^T F_i - N(q, \dot{q})`.
+
+    Compared to the constraint, this task can be violated during the optimization. The user can set the weight to
+    specify how much this task can be violated.
+    """
+
+    def __init__(self, model, contact_links=[], wrenches=[], weight=1., constraints=[]):
+        r"""
+        Initialize the constraint.
+
+        Args:
+            model (ModelInterface): model interface.
+            contact_links (list[str], list[int], None): list of unique contact link names or ids.
+            wrenches (list[np.array[float[6]]], None): list of associated wrenches applied to the contact links. It
+              must have the same size as the number of contact links.
+            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.
+        """
+        super(DynamicFeasibilityTask, self).__init__(model=model, weight=weight, constraints=constraints)
+
+        # set inner task based on constraint
+        self._constraint = DynamicFeasibilityConstraint(model=model, contact_links=contact_links, wrenches=wrenches)
+        self._task = TaskFromConstraint(self._constraint)
+
+        # first update
+        self.update()
+
+    ##############
+    # Properties #
+    ##############
+
+    @property
+    def contact_links(self):
+        """Get the contact links."""
+        return self._constraint.contact_links
+
+    @contact_links.setter
+    def contact_links(self, contacts):
+        """Set the contact links."""
+        self._constraint.contact_links = contacts
+
+    @property
+    def wrenches(self):
+        """Get the wrenches."""
+        return self._constraint.wrenches
+
+    @wrenches.setter
+    def wrenches(self, wrenches):
+        """Set the wrenches."""
+        self._constraint.wrenches = wrenches
+
+    ###########
+    # Methods #
+    ###########
+
+    def _update(self, x=None):
+        """Update the equality constraint."""
+        self._A = self._task.A
+        self._b = self._task.b

pyrobolearn/priorities/tasks/acceleration/postural.py

@@ -245,7 +245,7 @@ class PosturalAccelerationTask(JointAccelerationTask):
         """
         return self.x_desired, self.dx_desired, self.ddx_desired
 
-    def _update(self):
+    def _update(self, x=None):
         """
         Update the task by computing the A matrix and b vector that will be used by the task solver.
         """

pyrobolearn/priorities/tasks/force/__init__.py

@@ -3,4 +3,6 @@ from .com import CoMForceTask
 
 from .floating_base import FloatingBaseForceTask
 
+# from .manipulability import ForceManipulabilityTask
+
 from .wrench import WrenchTask

pyrobolearn/priorities/tasks/force/com.py

@@ -1,6 +1,20 @@
 #!/usr/bin/env python
 r"""Provide the center of mass force task.
 
+From the documentation of the framework of [1]: "The CoM task computes the wrenches at the contact, in world frame,
+in order to realize a certain acceleration and variation of angular momentum at the CoM considering the Centroidal
+Dynamics":
+
+.. math::
+
+    m * \ddot{r} = \sum_i f_i + mg \\
+    \dot{L} = \sum_i p_i \times f_i + \tau,
+
+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
+the CoM position, :math:`g` is the gravity vector, :math:`L` is the angular momentum around the CoM, :math:`p` is
+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 implementation of this class is inspired by [1] (which is licensed under the LGPLv2).
 
@@ -8,9 +22,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.tasks import Task
+from pyrobolearn.priorities.tasks import ForceTask
 
 
 __author__ = "Brian Delhaisse"
@@ -23,17 +39,108 @@ __email__ = "briandelhaisse@gmail.com"
 __status__ = "Development"
 
 
-class CoMForceTask(Task):
+class CoMForceTask(ForceTask):
     r"""CoM Force Task
 
+    From the documentation of the framework of [1]: "The CoM task computes the wrenches at the contact, in world frame,
+    in order to realize a certain acceleration and variation of angular momentum at the CoM considering the Centroidal
+    Dynamics":
+
+    .. math::
+
+        m * \ddot{r} = \sum_i f_i + mg \\
+        \dot{L} = \sum_i p_i \times f_i + \tau,
+
+    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
+    the CoM position, :math:`g` is the gravity vector, :math:`L` is the angular momentum around the CoM, :math:`p` is
+    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 implementation of this class is inspired by [1] (which is licensed under the LGPLv2).
+
+    References:
+        - [1] "OpenSoT: A whole-body control library for the compliant humanoid robot COMAN", Rocchi et al., 2015
     """
 
-    def __init__(self, model, constraints=[]):
+    def __init__(self, model, contact_links=[], wrenches=[], weight=1., constraints=[]):
         """
         Initialize the task.
 
         Args:
-            model (ModelInterface): model interface
-            constraints (list of Constraint): list of constraints associated with the task.
+            model (ModelInterface): model interface.
+            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.
+            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.
         """
-        super(CoMForceTask, self).__init__(model=model, constraints=constraints)
+        super(CoMForceTask, self).__init__(model=model, weight=weight, constraints=constraints)
+
+        # set variables
+        self.contact_links = contact_links
+        self.wrenches = wrenches
+
+        # first update
+        self.update()
+
+    ##############
+    # Properties #
+    ##############
+
+    @property
+    def contact_links(self):
+        """Get the contact links."""
+        return self._contact_links
+
+    @contact_links.setter
+    def contact_links(self, contacts):
+        """Set the contact links."""
+        if contacts is None:
+            contacts = []
+        elif not isinstance(contacts, (list, tuple, np.ndarray)):
+            raise TypeError("Expecting the given 'contact_links' to be a list of names/ids, but got instead: "
+                            "{}".format(type(contacts)))
+        self._contact_links = contacts
+
+        # enable / disable the tasks based on the number of contact links
+        if len(contacts) == 0:
+            self.disable()
+        else:
+            self.enable()
+
+    @property
+    def wrenches(self):
+        """Get the wrenches."""
+        return self._wrenches
+
+    @wrenches.setter
+    def wrenches(self, wrenches):
+        """Set the wrenches."""
+        if wrenches is None:
+            wrenches = []
+        elif not isinstance(wrenches, (list, tuple, np.ndarray)):
+            raise TypeError("Expecting the given 'wrenches' to be a list of wrench vectors, but got instead: "
+                            "{}".format(type(wrenches)))
+        if isinstance(wrenches, np.ndarray) and wrenches.ndim == 1:
+            wrenches = wrenches.reshape(-1, 6)
+        self._wrenches = wrenches
+
+    ###########
+    # Methods #
+    ###########
+
+    def _update(self, x=None):
+        """
+        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()
+
+        angular_momentum = A_G[:3, :3]
+
+        raise NotImplementedError
+
+
+

pyrobolearn/priorities/tasks/force/floating_base.py

@@ -1,6 +1,15 @@
 #!/usr/bin/env python
 r"""Provide the floating base force task.
 
+From [1]: "this implements a task which maps forces acting on the floating base virtual chain to contacts".
+
+.. math:: || J(q)[:,:6]^\top w - \tau ||^2,
+
+where :math:`w \in \mathbb{R}^{6N_c}` are the wrench vector being optimized (with :math:`N_c` being the number of
+contacts), :math:`J(q) = [J(q)_1^\top \cdot J(q)_{N_c}^\top]^\top \in \mathbb{R}^{6N_c \times 6 + N}` are the
+concatenated jacobians, and :math:`\tau` are the torques applied on the floating base.
+
+Note that this task assumes the robot has a floating base.
 
 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.tasks import Task
+from pyrobolearn.priorities.tasks import ForceTask
 
 
 __author__ = "Brian Delhaisse"
@@ -23,17 +32,102 @@ __email__ = "briandelhaisse@gmail.com"
 __status__ = "Development"
 
 
-class FloatingBaseForceTask(Task):
+class FloatingBaseForceTask(ForceTask):
     r"""Floating base Force Task
 
+    From [1]: "this implements a task which maps forces acting on the floating base virtual chain to contacts".
+
+    .. math:: || J(q)[:,:6]^\top w - \tau ||^2,
+
+    where :math:`w \in \mathbb{R}^{6N_c}` are the wrench vector being optimized (with :math:`N_c` being the number of
+    contacts), :math:`J(q) = [J(q)_1^\top \cdot J(q)_{N_c}^\top]^\top \in \mathbb{R}^{6N_c \times 6 + N}` are the
+    concatenated jacobians, and :math:`\tau` are the torques applied on the floating base.
+
+    Note that this task assumes the robot has a floating base.
+
+    The implementation of this class is inspired by [1] (which is licensed under the LGPLv2).
+
+    References:
+        - [1] "OpenSoT: A whole-body control library for the compliant humanoid robot COMAN", Rocchi et al., 2015
     """
 
-    def __init__(self, model, constraints=[]):
+    def __init__(self, model, contact_links, floating_base_torque=0., weight=1., constraints=[]):
         """
         Initialize the task.
 
         Args:
-            model (ModelInterface): model interface
-            constraints (list of Constraint): list of constraints associated with the task.
+            model (ModelInterface): model interface.
+            contact_links (list[str], list[int]): list of unique contact link names or ids.
+            floating_base_torque (float, np.array[float[6]]): external torque applied on the floating base.
+            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.
         """
-        super(FloatingBaseForceTask, self).__init__(model=model, constraints=constraints)
+        super(FloatingBaseForceTask, self).__init__(model=model, weight=weight, constraints=constraints)
+
+        # check if model has floating base
+        if not self.model.has_floating_base():
+            raise ValueError("Expecting the given robotic 'model' to have a floating base, but it seems this is not "
+                             "the case...")
+
+        # set variables
+        self.contact_links = contact_links
+        self.floating_base_torque = floating_base_torque
+
+        # first update
+        self.update()
+
+    ##############
+    # Properties #
+    ##############
+
+    @property
+    def contact_links(self):
+        """Get the contact links."""
+        return self._contact_links
+
+    @contact_links.setter
+    def contact_links(self, contacts):
+        """Set the contact links."""
+        if contacts is None:
+            contacts = []
+        elif not isinstance(contacts, (list, tuple, np.ndarray)):
+            raise TypeError("Expecting the given 'contact_links' to be a list of names/ids, but got instead: "
+                            "{}".format(type(contacts)))
+        self._contact_links = contacts
+
+        # enable / disable the tasks based on the number of contact links
+        if len(contacts) == 0:
+            self.disable()
+        else:
+            self.enable()
+
+    @property
+    def floating_base_torque(self):
+        """Get the floating base torques."""
+        return self._floating_base_torque
+
+    @floating_base_torque.setter
+    def floating_base_torque(self, torque):
+        """Set the floating base torque."""
+        if not isinstance(torque, (int, float)):
+            if not isinstance(torque, (np.ndarray, list, tuple)):
+                raise TypeError("Expecting the given 'floating_base_torque' to be an int, float, list/tuple/np.array "
+                                "of float, but instead got: {}".format(type(torque)))
+            torque = np.asarray(torque).reshape(-1)
+            if len(torque) != 6:
+                raise ValueError("Expecting the given 'floating_base_torque' to be of size 6, but got a size of: "
+                                 "{}".format(len(torque)))
+        self._floating_base_torque = torque
+
+    ###########
+    # Methods #
+    ###########
+
+    def _update(self, x=None):
+        """
+        Update the task by computing the A matrix and b vector that will be used by the task solver.
+        """
+        jacobians = [self.model.get_jacobian(self.model.get_link_id(link))[:6, :6] for link in self.contact_links]
+        jacobians = np.concatenate(jacobians)  # shape (6*N_c,6)
+        self._A = jacobians.T  # shape (6,6*N_c)
+        self._b = self.floating_base_torque  # shape (6,)

pyrobolearn/priorities/tasks/force/manipulability.py

@@ -1,13 +1,17 @@
 #!/usr/bin/env python
 r"""Provide the force manipulability task.
 
-References:
-    - [1] "OpenSoT: A whole-body control library for the compliant humanoid robot COMAN", Rocchi et al., 2015
+ The manipulability task implements a tasks that tries to maximize the force manipulability measure given in [1]:
+
+.. math:: w(q) = \sqrt( \det( (J(q) W J(q)^\top)^{-1} ) )
+
+where :math:`W` is a constant weight matrix, :math:`q` are the joint positions, and :math:`J(q)` is the jacobian.
+The gradient of :math:`w` is then computed and projected using the gradient projection method [2].
 """
 
 import numpy as np
 
-from pyrobolearn.priorities.tasks import Task
+from pyrobolearn.priorities.tasks import ForceTask
 
 
 __author__ = "Brian Delhaisse"
@@ -20,7 +24,7 @@ __email__ = "briandelhaisse@gmail.com"
 __status__ = "Development"
 
 
-class ForceManipulabilityTask(Task):
+class ForceManipulabilityTask(ForceTask):
     r"""Force Manipulability Task
 
     The manipulability task implements a tasks that tries to maximize the force manipulability measure given in [1]:
@@ -35,12 +39,13 @@ class ForceManipulabilityTask(Task):
         - [2] "OpenSoT: A whole-body control library for the compliant humanoid robot COMAN", Rocchi et al., 2015
     """
 
-    def __init__(self, model, constraints=[]):
+    def __init__(self, model, weight=1., constraints=[]):
         """
         Initialize the task.
 
         Args:
             model (ModelInterface): model interface.
-            constraints (list of Constraint): list of constraints associated with the task.
+            weight (float, np.array[float[6,6]], np.array[float[3,3]]): weight scalar or matrix associated to the task.
+            constraints (list[Constraint]): list of constraints associated with the task.
         """
-        super(ForceManipulabilityTask, self).__init__(model=model, constraints=constraints)
+        super(ForceManipulabilityTask, self).__init__(model=model, weight=weight, constraints=constraints)

pyrobolearn/priorities/tasks/force/wrench.py

@@ -1,6 +1,15 @@
 #!/usr/bin/env python
 r"""Provide the wrench task.
 
+The wrench task tries to generate a wrench near the desired one by minimizing:
+
+.. math:: || w - k (w_{des} - w_t) ||^2
+
+where :math:`w = [f \tau] \in \mathbb{R}^6` is the wrench vector being optimized, :math:`k` is a proportional gain,
+:math:`w_{des}` is the desired wrench vector, and :math:`w_t` is the current wrench vector.
+
+The above formulation is equivalent to the QP objective function :math:`||Ax - b||^2`, by setting
+:math:`A = I`, :math:`x = w`, and :math:`b = k (w_{des} - w_t)`.
 
 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.tasks import Task
+from pyrobolearn.priorities.tasks import ForceTask
 
 
 __author__ = "Brian Delhaisse"
@@ -23,19 +32,110 @@ __email__ = "briandelhaisse@gmail.com"
 __status__ = "Development"
 
 
-class WrenchTask(Task):
+class WrenchTask(ForceTask):  # TODO: improve this class by considering only forces or torques + using links
     r"""Wrench Task
 
-    The wrench task
+    The wrench task tries to generate a wrench near the desired one by minimizing:
 
+    .. math:: || w - k (w_{des} - w_t) ||^2
+
+    where :math:`w = [f \tau] \in \mathbb{R}^6` is the wrench vector being optimized, :math:`k` is a proportional gain,
+    :math:`w_{des}` is the desired wrench vector, and :math:`w_t` is the current wrench vector.
+
+    The above formulation is equivalent to the QP objective function :math:`||Ax - b||^2`, by setting
+    :math:`A = I`, :math:`x = w`, and :math:`b = k (w_{des} - w_t)`.
+
+    The implementation of this class is inspired by [1] (which is licensed under the LGPLv2).
+
+    References:
+        - [1] "OpenSoT: A whole-body control library for the compliant humanoid robot COMAN", Rocchi et al., 2015
     """
 
-    def __init__(self, model, constraints=[]):
+    def __init__(self, model, desired_wrenches, wrenches, kp=1., weight=1., constraints=[]):
         """
         Initialize the task.
 
         Args:
-            model (ModelInterface): model interface
-            constraints (list of Constraint): list of constraints associated with the task.
+            model (ModelInterface): model interface.
+            desired_wrenches (list[np.array[float[6]]]): list of desired wrenches.
+            wrenches (list[np.array[float[6]]]): list of current wrenches that are usually read from F/T sensors. This
+              has to be of the same size as the desired wrenches.
+            weight (float, np.array[float[M*6,M*6]]): weight scalar or matrix associated to the task.
+            constraints (list[Constraint]): list of constraints associated with the task.
         """
-        super(WrenchTask, self).__init__(model=model, constraints=constraints)
+        super(WrenchTask, self).__init__(model=model, weight=weight, constraints=constraints)
+
+        # set variables
+        self.desired_wrenches = desired_wrenches
+        self.wrenches = wrenches
+        self.kp = kp
+
+        # first update
+        self.update()
+
+    ##############
+    # Properties #
+    ##############
+    
+    @property
+    def desired_wrenches(self):
+        """Get the desired wrenches."""
+        return self._desired_wrenches
+    
+    @desired_wrenches.setter
+    def desired_wrenches(self, wrenches):
+        """Set the desired wrenches."""
+        if not isinstance(wrenches, (list, tuple, np.ndarray)):
+            raise TypeError("Expecting the given 'desired_wrenches' to be a tuple/list of np.array, or a np.array, "
+                            "but got instead: {}".format(type(wrenches)))
+        self._desired_wrenches = np.asarray(wrenches).reshape(-1)  # (N*6,) or (N*3,)
+
+        # enable / disable the tasks based on the number of contact links
+        if len(self._desired_wrenches) == 0:
+            self.disable()
+        else:
+            self.enable()
+            # set A matrix
+            self._A = np.identity(len(self._desired_wrenches))
+
+    @property
+    def wrenches(self):
+        """Get the current wrenches."""
+        return self._wrenches
+
+    @wrenches.setter
+    def wrenches(self, wrenches):
+        """Set the current wrenches."""
+        if not isinstance(wrenches, (list, tuple, np.ndarray)):
+            raise TypeError("Expecting the given 'desired_wrenches' to be a tuple/list of np.array, or a np.array, "
+                            "but got instead: {}".format(type(wrenches)))
+        self._wrenches = np.asarray(wrenches).reshape(-1)  # (N*6,) or (N*3,)
+
+    @property
+    def kp(self):
+        """Return the proportional gain."""
+        return self._kp
+
+    @kp.setter
+    def kp(self, kp):
+        """Set the proportional gain."""
+        if kp is None:
+            kp = 1.
+        if not isinstance(kp, (float, int, np.ndarray)):
+            raise TypeError("Expecting the given proportional gain kp to be an int, float, np.array, instead "
+                            "got: {}".format(type(kp)))
+        x_size = len(self.desired_wrenches)
+        if isinstance(kp, np.ndarray) and kp.shape != (x_size, x_size):
+            raise ValueError("Expecting the given proportional gain matrix kp to be of shape {}, but instead "
+                             "got shape: {}".format((x_size, x_size), kp.shape))
+        self._kp = kp
+
+    ###########
+    # Methods #
+    ###########
+
+    def _update(self, x=None):
+        """
+        Update the task by computing the A matrix and b vector that will be used by the task solver.
+        """
+        self._b = self._kp * (self._desired_wrenches - self._wrenches)

pyrobolearn/priorities/tasks/task.py

@@ -861,19 +861,23 @@ class Task(object):
         WAx = np.dot(W, Ax)
         return Ax.T.dot(WAx) - 2 * b.T.dot(WAx) + c.T.dot(x) + b.T.dot(W).dot(b)
 
-    def _update(self):
+    def _update(self, x=None):
         """Update the task.
 
         Compute the A matrix and b vector that will be used by the task solver. This has to be implemented in the
         child classes.
+
+        Args:
+            x (np.array[float], None): variables that are being optimized.
         """
         pass
 
-    def update(self, update_model=False):
+    def update(self, x=None, update_model=False):
         """
         Compute the A matrix and b vector that will be used by the task solver.
 
         Args:
+            x (np.array[float], None): variables that are being optimized.
             update_model (bool): if True, it will update the model before updating each task.
         """
         # update model if specified
@@ -884,9 +888,10 @@ class Task(object):
         if self.is_stack_of_tasks():  # if stack of tasks, update each task
             for hard_task in self.tasks:
                 for soft_task in hard_task:
-                    soft_task.update(update_model=False)
+                    soft_task.update(x=x, update_model=False)
         else:  # if one task, update it
-            self._update()
+            if self._enabled:  # update only if enabled
+                self._update(x=x)
 
         # update the constraints
         for constraint in self.constraints:
@@ -1096,7 +1101,7 @@ class KinematicTask(Task):
     pass
 
 
-class JointVelocityTask(Task):
+class JointVelocityTask(KinematicTask):
     r"""Joint Velocity Task
 
     Joint velocity tasks are tasks that optimize joint velocities :math:`\dot{q}`.
@@ -1112,7 +1117,7 @@ class DynamicTask(Task):
     pass
 
 
-class JointAccelerationTask(Task):
+class JointAccelerationTask(DynamicTask):
     r"""Joint Acceleration Task
 
     Joint acceleration tasks are tasks that optimize joint accelerations :math:`\ddot{q}`.
@@ -1120,7 +1125,7 @@ class JointAccelerationTask(Task):
     pass
 
 
-class JointTorqueTask(Task):
+class JointTorqueTask(DynamicTask):
     r"""Joint Torque Task
 
     Joint torque tasks are tasks that optimize joint torques :math:`\tau`.
@@ -1128,6 +1133,17 @@ class JointTorqueTask(Task):
     pass
 
 
+class ForceTask(DynamicTask):
+    r"""Force Task
+
+    Force tasks are tasks that optimize the cartesian forces (wrenches) :math:`F`. They can be used for instance to
+    optimize the contact wrenches. By optimizing these ones with the joint accelerations :math:`\ddot{q}`, the
+    necessary torques :math:`\tau` to apply to the robot can be computed using the joint space dynamic equation of
+    motion: :math:`\tau = H \ddot{q} + C(q,\dot{q})\dot{q} + g(q) - J^\top F`.
+    """
+    pass
+
+
 # Tests
 if __name__ == '__main__':
 

pyrobolearn/priorities/tasks/task_from_constraint.py

@@ -67,7 +67,7 @@ class TaskFromConstraint(Task):
     # Methods #
     ###########
 
-    def _update(self):
+    def _update(self, x=None):
         """
         Update the task by computing the A matrix and b vector that will be used by the task solver.
         """

pyrobolearn/priorities/tasks/torque/cartesian_impedance_control.py

@@ -1,6 +1,30 @@
 #!/usr/bin/env python
 r"""Provide the cartesian impedance control task.
 
+The cartesian impedance control task optimizes the joint torques such that it applies the necessary torques to
+move a distal link with respect to a base:
+
+.. math:: || J(q) H(q)^{-1} \tau - J(q) H(q)^{-1} J(q)^\top f ||^2 = || J(q) H(q)^{-1} (\tau - J(q)^\top f) ||^2
+
+where :math:`J(q) \in \mathbb{R}^{6 \times N}` is the Jacobian matrix, :math:`H(q) \in \mathbb{R}^{N \times N}` is
+the joint inertia matrix, :math:`\tau \in \mathbb{R}^N` are the torques being optimized, and
+:math:`f \in \mathbb{R}^6` is the desired wrench computed from:
+
+.. math:: f = K_p e + K_d (\dot{x}_d - \dot{x})
+
+where :math:`K_p` and :math:`K_d` are the stiffness and damping gains, :math:`e \in \mathbb{R}^{6}` is the error
+which is the concatenation of the position error given by :math:`e_{p} = (x_d - x)` (with :math:`x_d` being the
+desired pose, and :math:`x` the current pose), and the orientation error given by (if expressed as quaternions
+:math:`o = {s, v}` where :math:`s` is the real scalar part, and :math:`v` is the vector part)
+:math:`e_{o} = s v_d - s_d v - v_d \cross v`, and :math:`\dot{x}_d \in \mathbb{R}^{6}` is the desired cartesian
+velocity for the distal link with respect to the base link.
+
+The above optimization problem is equivalent to the QP objective function :math:`||Ax - b||^2`, by setting
+:math:`A = J(q) H(q)^{-1}`, :math:`x = \tau`, and :math:`b = J(q) H(q)^{-1} J(q)^\top f`.
+
+Note that :math:`||J(q) H(q)^{-1} (\tau - J(q)^\top f)||^2  \leq  ||J(q) H(q)^{-1}|| ||\tau - J(q)^\top f||^2`.
+
+.. seealso:: `tasks/velocity/cartesian.py`
 
 The implementation of this class is inspired by [1] (which is licensed under the LGPLv2).
 
@@ -28,7 +52,7 @@ class CartesianImpedanceControlTask(JointTorqueTask):
     r"""Cartesian Impedance Control Task
 
     The cartesian impedance control task optimizes the joint torques such that it applies the necessary torques to
-    move a distal link with respect to a bas e
+    move a distal link with respect to a base:
 
     .. math:: || J(q) H(q)^{-1} \tau - J(q) H(q)^{-1} J(q)^\top f ||^2 = || J(q) H(q)^{-1} (\tau - J(q)^\top f) ||^2
 
@@ -51,10 +75,17 @@ class CartesianImpedanceControlTask(JointTorqueTask):
     Note that :math:`||J(q) H(q)^{-1} (\tau - J(q)^\top f)||^2  \leq  ||J(q) H(q)^{-1}|| ||\tau - J(q)^\top f||^2`.
 
     .. seealso:: `tasks/velocity/cartesian.py`
+
+
+    The implementation of this class is inspired by [1] (which is licensed under the LGPLv2).
+
+    References:
+        - [1] "OpenSoT: A whole-body control library for the compliant humanoid robot COMAN", Rocchi et al., 2015
     """
 
-    def __init__(self, model, distal_link, base_link=None, local_position=(0, 0, 0), x_desired=None,
-                 dx_desired=None, kp=1., kd=1., weight=1., constraints=[]):
+    def __init__(self, model, distal_link, base_link=None, local_position=(0, 0, 0), desired_position=None,
+                 desired_orientation=None, desired_linear_velocity=None, desired_angular_velocity=None,
+                 kp_position=1., kp_orientation=1., kd_linear=1., kd_angular=1., weight=1., constraints=[]):
         """
         Initialize the task.
 
@@ -62,13 +93,23 @@ class CartesianImpedanceControlTask(JointTorqueTask):
             model (ModelInterface): model interface.
             distal_link (int, str): distal link id or name.
             base_link (int, str, None): base link id or name. If None, it will be the world.
-            local_position (np.array[3]): local position on the distal link.
-            x_desired (np.array[7], None): desired cartesian pose of distal link wrt the base.
-            dx_desired (np.array[6], None): desired cartesian velocity of distal link wrt the base.
-            kp (float, np.array[6,6]): stiffness gain.
-            kd (float, np.array[6,6]): damping gain.
-            weight (float, np.array[6,6]): weight scalar or matrix associated to the task.
-            constraints (list of Constraint): list of constraints associated with the task.
+            local_position (np.array[float[3]]): local position on the distal link.
+            desired_position (np.array[float[3]], None): desired position of distal link wrt the base. If None, it
+              will not be taken into account.
+            desired_orientation (np.array[float[4]], None): desired orientation (expressed as quaternion [x,y,z,w]) of
+              distal link wrt the base. If None, it will not be taken into account.
+            desired_linear_velocity (np.array[float[3]], None): desired linear velocity of distal link wrt the base.
+              If None, it will be set to zero.
+            desired_angular_velocity (np.array[float[3]], None): desired angular velocity of distal link wrt the base.
+              If None, it will be set to zero.
+            kp_position (float, np.array[float[3,3]]): position stiffness gain.
+            kp_orientation (float, np.array[float[3,3]]): orientation stiffness gain.
+            kd_linear (float, np.array[float[3,3]]): linear velocity damping gain.
+            kd_angular (float, np.array[float[3,3]]): angular velocity damping gain.
+            kp (float, np.array[float[6,6]]): stiffness gain.
+            kd (float, np.array[float[6,6]]): damping gain.
+            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.
         """
         super(CartesianImpedanceControlTask, self).__init__(model=model, weight=weight, constraints=constraints)
 
@@ -76,12 +117,18 @@ class CartesianImpedanceControlTask(JointTorqueTask):
         self.distal_link = self.model.get_link_id(distal_link)
         self.base_link = self.model.get_link_id(base_link) if base_link is not None else base_link
         self.local_position = local_position
-        self.kp = kp
-        self.kd = kd
+
+        # gains
+        self.kp_position = kp_position
+        self.kp_orientation = kp_orientation
+        self.kd_linear = kd_linear
+        self.kd_angular = kd_angular
 
         # define desired references
-        self.x_desired = x_desired
-        self.dx_desired = dx_desired
+        self.desired_position = desired_position
+        self.desired_orientation = desired_orientation
+        self.desired_linear_velocity = desired_linear_velocity
+        self.desired_angular_velocity = desired_angular_velocity
 
         # first update
         self.update()
@@ -90,72 +137,210 @@ class CartesianImpedanceControlTask(JointTorqueTask):
     # Properties #
     ##############
 
+    @property
+    def desired_position(self):
+        """Get the desired cartesian position for the distal link wrt the base."""
+        return self._des_pos
+
+    @desired_position.setter
+    def desired_position(self, position):
+        """Set the desired cartesian position for the distal link wrt the base."""
+        if position is not None:
+            if not isinstance(position, (np.ndarray, list, tuple)):
+                raise TypeError("Expecting the given desired position to be a np.array, instead got: "
+                                "{}".format(type(position)))
+            position = np.asarray(position)
+            if len(position) != 3:
+                raise ValueError("Expecting the given desired position array to be of length 3, but instead got: "
+                                 "{}".format(len(position)))
+        self._des_pos = position
+
+    @property
+    def desired_orientation(self):
+        """Get the desired cartesian orientation (expressed as a quaternion [x,y,z,w]) for the distal link wrt the
+        base."""
+        return self._des_quat
+
+    @desired_orientation.setter
+    def desired_orientation(self, orientation):
+        """Set the desired cartesian orientation (expressed as a quaternion [x,y,z,w]) for the distal link wrt the
+        base."""
+        if orientation is not None:
+            if not isinstance(orientation, (np.ndarray, list, tuple)):
+                raise TypeError("Expecting the given desired orientation to be a np.array, instead got: "
+                                "{}".format(type(orientation)))
+            orientation = np.asarray(orientation)
+            if len(orientation) != 4:
+                raise ValueError(
+                    "Expecting the given desired orientation array to be of length 4, but instead got: "
+                    "{}".format(len(orientation)))
+        self._des_quat = orientation
+
+    @property
+    def desired_linear_velocity(self):
+        """Get the desired cartesian linear velocity of the distal link wrt the base."""
+        return self._des_lin_vel
+
+    @desired_linear_velocity.setter
+    def desired_linear_velocity(self, velocity):
+        """Set the desired cartesian linear velocity of the distal link wrt the base."""
+        if velocity is None:
+            velocity = np.zeros(3)
+        elif not isinstance(velocity, (np.ndarray, list, tuple)):
+            raise TypeError("Expecting the given desired linear velocity to be a np.array, instead got: "
+                            "{}".format(type(velocity)))
+        velocity = np.asarray(velocity)
+        if len(velocity) != 3:
+            raise ValueError("Expecting the given desired linear velocity array to be of length 3, but instead "
+                             "got: {}".format(len(velocity)))
+        self._des_lin_vel = velocity
+
+    @property
+    def desired_angular_velocity(self):
+        """Get the desired cartesian angular velocity of the distal link wrt the base."""
+        return self._des_ang_vel
+
+    @desired_angular_velocity.setter
+    def desired_angular_velocity(self, velocity):
+        """Set the desired cartesian angular velocity of the distal link wrt the base."""
+        if velocity is None:
+            velocity = np.zeros(3)
+        elif not isinstance(velocity, (np.ndarray, list, tuple)):
+            raise TypeError("Expecting the given desired angular velocity to be a np.array, instead got: "
+                            "{}".format(type(velocity)))
+        velocity = np.asarray(velocity)
+        if len(velocity) != 3:
+            raise ValueError("Expecting the given desired angular velocity array to be of length 3, but instead "
+                             "got: {}".format(len(velocity)))
+        self._des_ang_vel = velocity
+
+    @property
+    def desired_velocity(self):
+        """Return the linear and angular velocity."""
+        return np.concatenate((self._des_lin_vel, self._des_ang_vel))
+
     @property
     def x_desired(self):
         """Get the desired cartesian pose for the distal link wrt to the base."""
-        return self._x_d
+        position = self.desired_position
+        orientation = self.desired_orientation
+        if position is not None:
+            if orientation is not None:
+                return np.concatenate((position, orientation))
+            return position
+        return orientation
 
     @x_desired.setter
     def x_desired(self, x_d):
-        """Get the desired cartesian pose for the distal link wrt to the base."""
-        if x_d is None:
-            x_d = np.array([0.] * 6 + [1.])
-        if not isinstance(x_d, np.ndarray):
-            raise TypeError("Expecting the given desired pose to be a np.array, instead got: {}".format(type(x_d)))
-        if len(x_d) != 7:
-            raise ValueError("Expecting the given desired pose array to be of length 7 (3 for the position, and 4 "
-                             "for the orientation expressed as a quaternion [x,y,z,w]), instead got a length of: "
-                             "{}".format(len(x_d)))
-        self._x_d = x_d
+        """Set the desired cartesian pose for the distal link wrt to the base."""
+        if x_d is not None:
+            if not isinstance(x_d, (np.ndarray, list, tuple)):
+                raise TypeError(
+                    "Expecting the given desired pose to be a np.array, instead got: {}".format(type(x_d)))
+            x_d = np.asarray(x_d)
+            if len(x_d) == 3:  # only position is provided
+                x_d = np.concatenate((x_d, np.array([0., 0., 0., 1.])))
+            elif len(x_d) == 4:  # only orientation is provided
+                x_d = np.concatenate((np.zeros(3), x_d))
+            if len(x_d) != 7:
+                raise ValueError("Expecting the given desired pose array to be of length 7 (3 for the position, "
+                                 "and 4 for the orientation expressed as a quaternion [x,y,z,w]), instead got a "
+                                 "length of: {}".format(len(x_d)))
+            self._des_pos = x_d[:3]
+            self._des_quat = x_d[3:]
 
     @property
     def dx_desired(self):
         """Get the desired cartesian velocity for the distal link wrt to the base."""
-        return self._dx_d
+        return np.concatenate((self._des_lin_vel, self._des_ang_vel))
 
     @dx_desired.setter
     def dx_desired(self, dx_d):
         """Set the desired cartesian velocity for the distal link wrt to the base."""
-        if dx_d is None:
-            dx_d = np.zeros(6)
-        if not isinstance(dx_d, np.ndarray):
-            raise TypeError("Expecting the given desired velocity to be a np.array, instead got: {}".format(type(dx_d)))
-        if len(dx_d) != 7:
-            raise ValueError("Expecting the given desired velocity array to be of length 6 (3 for the linear and 3 "
-                             "for the angular part), instead got a length of: {}".format(len(dx_d)))
-        self._dx_d = dx_d
+        if dx_d is not None:
+            if not isinstance(dx_d, (np.ndarray, list, tuple)):
+                raise TypeError("Expecting the given desired velocity to be a np.array, instead got: "
+                                "{}".format(type(dx_d)))
+            dx_d = np.asarray(dx_d)
+            if len(dx_d) == 3:  # assume that it is the linear velocity
+                dx_d = np.concatenate((dx_d, np.zeros(3)))
+            if len(dx_d) != 6:
+                raise ValueError("Expecting the given desired velocity array to be of length 6 (3 for the linear "
+                                 "and 3 for the angular part), instead got a length of: {}".format(len(dx_d)))
+            self._des_lin_vel = dx_d[:3]
+            self._des_ang_vel = dx_d[3:]
 
     @property
-    def kp(self):
-        """Return the stiffness gain."""
-        return self._kp
+    def kp_position(self):
+        """Return the position stiffness gain."""
+        return self._kp_pos
 
-    @kp.setter
-    def kp(self, kp):
-        """Set the stiffness gain."""
+    @kp_position.setter
+    def kp_position(self, kp):
+        """Set the position stiffness gain."""
+        if kp is None:
+            kp = 1.
         if not isinstance(kp, (float, int, np.ndarray)):
-            raise TypeError("Expecting the given stiffness gain kp to be an int, float, np.array, instead got: "
-                            "{}".format(type(kp)))
-        if isinstance(kp, np.ndarray) and kp.shape != (6, 6):
-            raise ValueError("Expecting the given stiffness gain matrix kp to be of shape {}, but instead got "
-                             "shape: {}".format((self.x_size, self.x_size), kp.shape))
-        self._kp = kp
+            raise TypeError("Expecting the given position stiffness gain kp to be an int, float, np.array, instead "
+                            "got: {}".format(type(kp)))
+        if isinstance(kp, np.ndarray) and kp.shape != (3, 3):
+            raise ValueError("Expecting the given position stiffness gain matrix kp to be of shape {}, but instead "
+                             "got shape: {}".format((3, 3), kp.shape))
+        self._kp_pos = kp
 
     @property
-    def kd(self):
-        """Return the damping gain."""
-        return self._kd
+    def kp_orientation(self):
+        """Return the orientation stiffness gain."""
+        return self._kp_quat
 
-    @kd.setter
-    def kd(self, kd):
-        """Set the damping gain."""
+    @kp_orientation.setter
+    def kp_orientation(self, kp):
+        """Set the orientation stiffness gain."""
+        if kp is None:
+            kp = 1.
+        if not isinstance(kp, (float, int, np.ndarray)):
+            raise TypeError("Expecting the given orientation stiffness gain kp to be an int, float, np.array, "
+                            "instead got: {}".format(type(kp)))
+        if isinstance(kp, np.ndarray) and kp.shape != (3, 3):
+            raise ValueError("Expecting the given orientation stiffness gain matrix kp to be of shape {}, but "
+                             "instead got shape: {}".format((3, 3), kp.shape))
+        self._kp_quat = kp
+
+    @property
+    def kd_linear(self):
+        """Return the linear velocity damping gain."""
+        return self._kd_lin
+
+    @kd_linear.setter
+    def kd_linear(self, kd):
+        """Set the linear velocity damping gain."""
+        if kd is None:
+            kd = 1.
         if not isinstance(kd, (float, int, np.ndarray)):
-            raise TypeError("Expecting the given damping gain kd to be an int, float, np.array, instead got: "
-                            "{}".format(type(kd)))
-        if isinstance(kd, np.ndarray) and kd.shape != (6, 6):
-            raise ValueError("Expecting the given damping gain matrix kd to be of shape {}, but instead got "
-                             "shape: {}".format((6, 6), kd.shape))
-        self._kd = kd
+            raise TypeError("Expecting the given linear velocity damping gain kd to be an int, float, np.array, "
+                            "instead got: {}".format(type(kd)))
+        if isinstance(kd, np.ndarray) and kd.shape != (3, 3):
+            raise ValueError("Expecting the given linear velocity damping gain matrix kd to be of shape {}, but "
+                             "instead got shape: {}".format((3, 3), kd.shape))
+        self._kd_lin = kd
+
+    @property
+    def kd_angular(self):
+        """Return the angular velocity damping gain."""
+        return self._kd_ang
+
+    @kd_angular.setter
+    def kd_angular(self, kd):
+        """Set the angular velocity damping gain."""
+        if kd is None:
+            kd = 1.
+        if not isinstance(kd, (float, int, np.ndarray)):
+            raise TypeError("Expecting the given angular velocity damping gain kd to be an int, float, np.array, "
+                            "instead got: {}".format(type(kd)))
+        if isinstance(kd, np.ndarray) and kd.shape != (3, 3):
+            raise ValueError("Expecting the given angular velocity damping gain matrix kd to be of shape {}, but "
+                             "instead got shape: {}".format((3, 3), kd.shape))
+        self._kd_ang = kd
 
     ###########
     # Methods #
@@ -165,9 +350,10 @@ class CartesianImpedanceControlTask(JointTorqueTask):
         """Set the desired references.
 
         Args:
-            x_des (np.array[7], None): desired cartesian pose (position and quaternion [x,y,z,w]) of distal link wrt
-                the base.
-            dx_des (np.array[6], None): desired cartesian velocity of distal link wrt the base.
+            x_des (np.array[float[7]], None): desired cartesian pose (position and quaternion [x,y,z,w]) of distal 
+              link wrt the base. If None, it will let the initial desired pose unchanged.
+            dx_des (np.array[float[6]], None): desired cartesian velocity of distal link wrt the base. If None,
+              it will let the initial desired accelerations unchanged.
         """
         self.x_desired = x_des
         self.dx_desired = dx_des
@@ -176,31 +362,46 @@ class CartesianImpedanceControlTask(JointTorqueTask):
         """Return the desired references.
 
         Returns:
-            np.array[7]: desired cartesian pose (position and quaternion [x,y,z,w]) of distal link wrt the base.
-            np.array[6]: desired cartesian velocity of distal link wrt the base.
+            np.array[float[7]]: desired cartesian pose (position and quaternion [x,y,z,w]) of distal link wrt the base.
+            np.array[float[6]]: desired cartesian velocity of distal link wrt the base.
         """
         return self.x_desired, self.dx_desired
 
-    def _update(self):
+    def _update(self, x=None):
         """
         Update the task by computing the A matrix and b vector that will be used by the task solver.
         """
         # get useful variables
-        x = self.model.get_link_pose_wrt(self.distal_link, self.base_link)
-        dx = self.model.get_link_velocity(self.distal_link, self.base_link)
-        J = self.model.get_jacobian(self.distal_link, self.base_link, self.local_position)  # shape: (6,N)
+        x = self.model.get_pose(link=self.distal_link, wrt_link=self.base_link)  # (7,)
+        dx = self.model.get_velocity(link=self.distal_link, wrt_link=self.base_link)  # (6,)
+        jac = self.model.get_jacobian(link=self.distal_link, wrt_link=self.base_link,
+                                      point=self.local_position)  # shape: (6,N)
         H = self.model.get_inertia_matrix()  # shape: (N,N)
+        H_inv = np.linalg.inv(H)
 
-        # compute A matrix
-        self._A = J.dot(np.linalg.inv(H))  # shape: (6,N)
+        if self._des_quat is None:  # only position and/or velocities
+            if self._des_pos is None:  # only velocities
+                force = np.concatenate((np.dot(self.kd_linear, (self._des_lin_vel - dx[:3])),
+                                        np.dot(self.kd_angular, (self._des_ang_vel - dx[3:]))))
+            else:  # only position
+                jac = jac[:3]
+                position = np.dot(self.kp_position, (self._des_pos - x[:3]))
+                lin_vel = np.dot(self.kd_linear, (self._des_lin_vel - dx[:3]))
+                force = position + lin_vel
+        elif self._des_pos is None:  # only orientation
+            jac = jac[3:]
+            orientation = np.dot(self.kp_orientation, quaternion_error(quat_des=self._des_quat, quat_cur=x[3:]))
+            ang_vel = np.dot(self.kd_angular, (self._des_ang_vel - dx[3:]))
+            force = orientation + ang_vel
+        else:  # both
+            # compute position/orientation error
+            position = np.dot(self.kp_position, (self._des_pos - x[:3]))
+            orientation = np.dot(self.kp_orientation, quaternion_error(quat_des=self._des_quat, quat_cur=x[3:]))
+            # compute velocities
+            lin_vel = np.dot(self.kd_linear, (self._des_lin_vel - dx[:3]))
+            ang_vel = np.dot(self.kd_angular, (self._des_ang_vel - dx[3:]))
+            force = np.concatenate((position + lin_vel, orientation + ang_vel))
 
-        # compute position/orientation error
-        position_error = (self._x_d[:3] - x[:3])
-        orientation_error = quaternion_error(quat_des=self._x_d[3:], quat_cur=x[3:])
-        error = np.concatenate((position_error, orientation_error))
-
-        # compute wrench
-        f = np.dot(self.kp, error) + np.dot(self.kd, (self._dx_d - dx))  # shape: (6,)
-
-        # compute b vector
-        self._b = self._A.dot(J.T).dot(f)
+        # compute A matrix and b vector
+        self._A = jac.dot(H_inv)
+        self._b = self._A.dot(jac.T.dot(force))

pyrobolearn/priorities/tasks/torque/joint_impedance_control.py

@@ -1,11 +1,26 @@
 #!/usr/bin/env python
 r"""Provide the joint impedance control task.
 
+The joint impedance control task minimizes the specified torques given as a PD control from the desired joint
+positions and velocities. That it, it minimizes:
+
+.. math:: || \tau - (K_p (q_d - q) + K_d (\dot{q}_d - \dot{q})) ||^2
+
+where :math:`\tau` are the torques being optimized, :math:`K_p` and :math:`K_d` are the stiffness and damping
+gains respectively, :math:`q` and :math:`\dot{q}` are the joint positions and velocities, and the subscript
+:math:`d` means 'desired'.
+
+The above formulation is equivalent to the QP objective function :math:`||Ax - b||^2`, by setting
+:math:`A = I` (where :math:`I` is the identity matrix), :math:`x = \tau`, and
+:math:`b = K_p (q_d - q) + K_d (\dot{q}_d - \dot{q})`.
+
+From [1], "note that "if used in the null-space, it realizes the null-space stiffness as described in [2]".
 
 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
+    - [2] "Cartesian Impedance Control of Redundant and Flexible-Joint Robots", Ott, 2008
 """
 
 import numpy as np
@@ -39,13 +54,14 @@ class JointImpedanceControlTask(JointTorqueTask):
     :math:`A = I` (where :math:`I` is the identity matrix), :math:`x = \tau`, and
     :math:`b = K_p (q_d - q) + K_d (\dot{q}_d - \dot{q})`.
 
-    From [1], "if used in the null-space, it realizes the null-space stiffness as described in [1]".
+    From [1], "if used in the null-space, it realizes the null-space stiffness as described in [3]".
 
     .. seealso:: `tasks/velocity/postural.py`
 
     References:
         - [1] OpenSoT framework
-        - [2] "Cartesian Impedance Control of Redundant and Flexible-Joint Robots", Ott, 2008
+        - [2] "OpenSoT: A whole-body control library for the compliant humanoid robot COMAN", Rocchi et al., 2015
+        - [3] "Cartesian Impedance Control of Redundant and Flexible-Joint Robots", Ott, 2008
     """
 
     def __init__(self, model, q_desired=None, dq_desired=None, kp=1., kd=1., weight=1., constraints=[]):
@@ -54,14 +70,14 @@ class JointImpedanceControlTask(JointTorqueTask):
 
         Args:
             model (ModelInterface): model interface.
-            q_desired (np.array[N], None): desired joint positions, where :math:`N` is the number of DoFs. If None,
-                it will be set to 0.
-            dq_desired (np.array[N], None): desired joint velocities, where :math:`N` is the number of DoFs. If None,
-                it will be set to 0.
-            kp (float, np.array[N,N]): stiffness gain.
-            kd (float, np.array[N,N]): damping gain.
-            weight (float, np.array[N,N]): weight scalar or matrix associated to the task.
-            constraints (list of Constraint): list of constraints associated with the task.
+            q_desired (np.array[float[N]], None): desired joint positions, where :math:`N` is the number of DoFs. If 
+              None, it will be set to 0.
+            dq_desired (np.array[float[N]], None): desired joint velocities, where :math:`N` is the number of DoFs. If 
+              None, it will be set to 0.
+            kp (float, np.array[float[N,N]]): stiffness gain.
+            kd (float, np.array[float[N,N]]): damping gain.
+            weight (float, np.array[float[N,N]]): weight scalar or matrix associated to the task.
+            constraints (list[Constraint]): list of constraints associated with the task.
         """
         super(JointImpedanceControlTask, self).__init__(model=model, weight=weight, constraints=constraints)
 
@@ -70,8 +86,8 @@ class JointImpedanceControlTask(JointTorqueTask):
         self.kd = kd
 
         # define desired references
-        self.x_desired = q_desired
-        self.dx_desired = dq_desired
+        self.q_desired = q_desired
+        self.dq_desired = dq_desired
 
         # first update
         self.update()
@@ -80,6 +96,44 @@ class JointImpedanceControlTask(JointTorqueTask):
     # Properties #
     ##############
 
+    @property
+    def q_desired(self):
+        """Get the desired joint positions."""
+        return self._q_d
+
+    @q_desired.setter
+    def q_desired(self, q_d):
+        """Set the desired joint positions."""
+        if q_d is None:
+            q_d = np.zeros(self.x_size)
+        elif not isinstance(q_d, (np.ndarray, list, tuple)):
+            raise TypeError("Expecting the given desired joint positions to be an instance of np.array, instead got: "
+                            "{}".format(type(q_d)))
+        q_d = np.asarray(q_d)
+        if len(q_d) != self.x_size:
+            raise ValueError("Expecting the length of the given desired joint positions (={}) to be the same as the "
+                             "number of DoFs (={})".format(len(q_d), self.x_size))
+        self._q_d = q_d
+
+    @property
+    def dq_desired(self):
+        """Get the desired joint velocities."""
+        return self._dq_d
+
+    @dq_desired.setter
+    def dq_desired(self, dq_d):
+        """Set the desired joint velocities."""
+        if dq_d is None:
+            dq_d = np.zeros(self.x_size)
+        elif not isinstance(dq_d, (np.ndarray, list, tuple)):
+            raise TypeError("Expecting the given desired joint velocities to be an instance of np.array, instead got: "
+                            "{}".format(type(dq_d)))
+        dq_d = np.asarray(dq_d)
+        if len(dq_d) != self.x_size:
+            raise ValueError("Expecting the length of the given desired joint velocities (={}) to be the same as the "
+                             "number of DoFs (={})".format(len(dq_d), self.x_size))
+        self._dq_d = dq_d
+
     @property
     def x_desired(self):
         """Get the desired joint positions."""
@@ -88,15 +142,7 @@ class JointImpedanceControlTask(JointTorqueTask):
     @x_desired.setter
     def x_desired(self, q_d):
         """Set the desired joint positions."""
-        if q_d is None:
-            q_d = np.zeros(self.x_size)
-        if not isinstance(q_d, np.ndarray):
-            raise TypeError("Expecting the given desired joint positions to be an instance of np.array, instead got: "
-                            "{}".format(type(q_d)))
-        if len(q_d) != self.x_size:
-            raise ValueError("Expecting the length of the given desired joint positions (={}) to be the same as the "
-                             "number of DoFs (={})".format(len(q_d), self.x_size))
-        self._q_d = q_d
+        self.q_desired = q_d
 
     @property
     def dx_desired(self):
@@ -106,15 +152,7 @@ class JointImpedanceControlTask(JointTorqueTask):
     @dx_desired.setter
     def dx_desired(self, dq_d):
         """Set the desired joint velocities."""
-        if dq_d is None:
-            dq_d = np.zeros(self.x_size)
-        if not isinstance(dq_d, np.ndarray):
-            raise TypeError("Expecting the given desired joint velocities to be an instance of np.array, instead got: "
-                            "{}".format(type(dq_d)))
-        if len(dq_d) != self.x_size:
-            raise ValueError("Expecting the length of the given desired joint velocities (={}) to be the same as the "
-                             "number of DoFs (={})".format(len(dq_d), self.x_size))
-        self._dq_d = dq_d
+        self.dq_desired = dq_d
 
     @property
     def kp(self):
@@ -156,10 +194,10 @@ class JointImpedanceControlTask(JointTorqueTask):
         """Set the desired references.
 
         Args:
-            x_des (np.array[N], None): desired joint positions, where :math:`N` is the number of DoFs. If None,
-                it will be set to 0.
-            dx_des (np.array[N], None): desired joint velocities, where :math:`N` is the number of DoFs. If None,
-                it will be set to 0.
+            x_des (np.array[float[N]], None): desired joint positions, where :math:`N` is the number of DoFs. If None,
+              it will be set to 0.
+            dx_des (np.array[float[N]], None): desired joint velocities, where :math:`N` is the number of DoFs. If None,
+              it will be set to 0.
         """
         self.x_desired = x_des
         self.dx_desired = dx_des
@@ -168,12 +206,12 @@ class JointImpedanceControlTask(JointTorqueTask):
         """Return the desired references.
 
         Returns:
-            np.array[N]: desired joint positions.
-            np.array[N]: desired joint velocities.
+            np.array[float[N]]: desired joint positions.
+            np.array[float[N]]: desired joint velocities.
         """
         return self.x_desired, self.dx_desired
 
-    def _update(self):
+    def _update(self, x=None):
         """
         Update the task by computing the A matrix and b vector that will be used by the task solver.
         """

pyrobolearn/priorities/tasks/torque/minimum_torque.py

@@ -0,0 +1,49 @@
+#!/usr/bin/env python
+r"""Provide the minimum torque task.
+
+The minimum torque task minimizes the joint torques, that is it minimizes:
+
+.. math:: ||\tau||^2,
+
+which is equivalent to the QP objective function :math:`||Ax - b||^2`, by setting :math:`A=I`, :math:`x=\tau`,
+and :math:`b=0`.
+"""
+
+from pyrobolearn.priorities.tasks import JointTorqueTask
+
+
+__author__ = "Brian Delhaisse"
+__copyright__ = "Copyright 2019, PyRoboLearn"
+__credits__ = ["Brian Delhaisse"]
+__license__ = "GNU GPLv3"
+__version__ = "1.0.0"
+__maintainer__ = "Brian Delhaisse"
+__email__ = "briandelhaisse@gmail.com"
+__status__ = "Development"
+
+
+class MinTorqueTask(JointTorqueTask):
+    r"""Minimum Torque Task
+
+    The minimum torque task minimizes the joint torques, that is it minimizes:
+
+    .. math:: ||\tau||^2,
+
+    which is equivalent to the QP objective function :math:`||Ax - b||^2`, by setting :math:`A=I`, :math:`x=\tau`,
+    and :math:`b=0`.
+    """
+
+    def __init__(self, model, weight=1., constraints=[]):
+        """
+        Initialize the task.
+
+        Args:
+            model (ModelInterface): model interface.
+            weight (float, np.array[float[N,N]]): weight scalar or matrix associated to the task.
+            constraints (list[Constraint]): list of constraints associated with the task.
+        """
+        # the variables A and b are initialized by default to be A=I and b=0
+        super(MinTorqueTask, self).__init__(model=model, weight=weight, constraints=constraints)
+
+        # first update
+        self.update()

pyrobolearn/priorities/tasks/velocity/__init__.py

@@ -7,17 +7,17 @@ from .com import CoMTask
 
 from .contact import ContactTask
 
-from .gaze import GazeTask
+# from .gaze import GazeTask
 
 from .interaction import InteractionTask
 
 from .linear_momentum import LinearMomentumTask
 
-from .manipulability import ManipulabilityTask
+# from .manipulability import ManipulabilityTask
 
 from .minimum_acceleration import MinAccelerationTask
 
-from .minimum_effort import MinEffortTask
+# from .minimum_effort import MinEffortTask
 
 from .minimum_velocity import MinVelocityTask
 
@@ -25,8 +25,8 @@ from .momentum import CentroidalMomentumTask
 
 from .postural import PosturalTask
 
-from .pure_rolling import PureRollingTask
+# from .pure_rolling import PureRollingTask
 
-from .rigid_rotation import RigidRotationTask
+# from .rigid_rotation import RigidRotationTask
 
-from .unicycle import UnicycleTask
+# from .unicycle import UnicycleTask

pyrobolearn/priorities/tasks/velocity/angular_momentum.py

@@ -206,7 +206,7 @@ class AngularMomentumTask(JointVelocityTask):
         """
         return self.x_desired
 
-    def _update(self):
+    def _update(self, x=None):
         """
         Update the task by computing the A matrix and b vector that will be used by the task solver.
         """

pyrobolearn/priorities/tasks/velocity/cartesian.py

@@ -316,7 +316,7 @@ class CartesianTask(JointVelocityTask):
         """
         return self.x_desired, self.dx_desired
 
-    def _update(self):
+    def _update(self, x=None):
         """
         Update the task by computing the A matrix and b vector that will be used by the task solver.
         """

pyrobolearn/priorities/tasks/velocity/com.py

@@ -182,7 +182,7 @@ class CoMTask(JointVelocityTask):
         """
         return self.x_desired, self.dx_desired
 
-    def _update(self):
+    def _update(self, x=None):
         """
         Update the task by computing the A matrix and b vector that will be used by the task solver.
         """

pyrobolearn/priorities/tasks/velocity/contact.py

@@ -73,7 +73,7 @@ class ContactTask(JointVelocityTask):
               square matrix). You can specify only the diagonal elements if you wish. If None, by default it will be
               set to the identity matrix.
             weight (float, np.array[float[6,6]]): weight scalar or matrix associated to the task.
-            constraints (list of Constraint): list of constraints associated with the task.
+            constraints (list[Constraint]): list of constraints associated with the task.
         """
         super(ContactTask, self).__init__(model=model, weight=weight, constraints=constraints)
 
@@ -125,7 +125,7 @@ class ContactTask(JointVelocityTask):
     # Methods #
     ###########
 
-    def _update(self):
+    def _update(self, x=None):
         """
         Update the task by computing the A matrix and b vector that will be used by the task solver.
         """

pyrobolearn/priorities/tasks/velocity/gaze.py

@@ -47,10 +47,10 @@ class GazeTask(JointVelocityTask):
 
         Args:
             model (ModelInterface): model interface.
-            weight (float, np.array[2,2]): weight scalar or matrix associated to the task.
-            constraints (list of Constraint): list of constraints associated with the task.
+            weight (float, np.array[float[2,2]]): weight scalar or matrix associated to the task.
+            constraints (list[Constraint]): list of constraints associated with the task.
         """
-        super(GazeTask, self).__init__(model=model, constraints=constraints)
+        super(GazeTask, self).__init__(model=model, weight=weight, constraints=constraints)
 
         # self.cartesian_task = CartesianTask(self.model, distal_link=distal_link, weight=weight)
 

pyrobolearn/priorities/tasks/velocity/interaction.py

@@ -8,8 +8,6 @@ References:
     - [1] "OpenSoT: A whole-body control library for the compliant humanoid robot COMAN", Rocchi et al., 2015
 """
 
-# TODO: finish to implement this class
-
 import numpy as np
 
 from pyrobolearn.priorities.tasks import JointVelocityTask
@@ -29,12 +27,12 @@ __status__ = "Development"
 class InteractionTask(JointVelocityTask):
     r"""Interaction Task
 
-    From the documentation of the framework of [1], "The Interaction class implements an Admittance based force
+    From the documentation of the framework of [1]: "the `InteractionTask` class implements an admittance based force
     control using the admittance law:
 
     .. math::
 
-        dx = K_p * (w_d - w) \\
+        dx = K_p (w_d - w) \\
         x_d = x + dx
 
     where :math:`w_d \in \mathbb{R}^6` is the desired wrench in some base_link frame, :math:`w` is the measured wrench
@@ -52,7 +50,7 @@ class InteractionTask(JointVelocityTask):
         - [1] "OpenSoT: A whole-body control library for the compliant humanoid robot COMAN", Rocchi et al., 2015
     """
 
-    def __init__(self, model, distal_link, base_link=-1, desired_wrench=0., weight=1., constraints=[]):
+    def __init__(self, model, distal_link, base_link=None, desired_wrench=0., kp=1., weight=1., constraints=[]):
         """
         Initialize the task.
 
@@ -60,20 +58,101 @@ class InteractionTask(JointVelocityTask):
             model (ModelInterface): model interface.
             distal_link (int, str): distal link id or name.
             base_link (int, str, None): base link id or name. If None, it will be the base root link.
-            desired_wrench (float, np.array[float[6]]): desired wrench.
+            desired_wrench (float, np.array[float[6]]): desired wrench (force and torque) in the base link of reference
+              frame.
+            kp (float, np.array[float[6,6]]): proportional gain = compliance matrix.
             weight (float, np.array[float[6,6]]): weight scalar or matrix associated to the task.
-            constraints (list of Constraint): list of constraints associated with the task.
+            constraints (list[Constraint]): list of constraints associated with the task.
         """
         super(InteractionTask, self).__init__(model=model, weight=weight, constraints=constraints)
 
+        # set variables
+        self.distal_link = self.model.get_link_id(distal_link)
+        self.base_link = self.model.get_link_id(base_link) if base_link is not None else base_link
+        
+        self.desired_wrench = desired_wrench
+        self.wrench = None  # measured wrench
+        self.kp = kp
+
         # create sub-task
         self._task = CartesianTask(model, distal_link=distal_link, base_link=base_link, weight=weight)
 
-        raise NotImplementedError("This class has not been implemented yet.")
+    ##############
+    # Properties #
+    ##############
 
-    def _update(self):
+    @property
+    def desired_wrench(self):
+        """Get the desired wrench."""
+        return self._desired_wrench
+
+    @desired_wrench.setter
+    def desired_wrench(self, wrench):
+        """Set the desired wrench."""
+        if wrench is None:
+            wrench = np.zeros(6)
+        elif isinstance(wrench, (float, int)):
+            wrench = wrench * np.ones(6)
+        if not isinstance(wrench, (list, tuple, np.ndarray)):
+            raise TypeError("Expecting the given 'desired_wrench' to be a tuple/list of np.array, or a np.array, "
+                            "but got instead: {}".format(type(wrench)))
+        self._desired_wrench = np.asarray(wrench).reshape(-1)  # (N*6,) or (N*3,)
+
+    @property
+    def wrench(self):
+        """Get the current wrench."""
+        return self._wrench
+
+    @wrench.setter
+    def wrench(self, wrench):
+        """Set the current wrench expressed in the base link of reference frame."""
+        if wrench is not None:
+            if not isinstance(wrench, (list, tuple, np.ndarray)):
+                raise TypeError("Expecting the given 'desired_wrench' to be a tuple/list of np.array, or a np.array, "
+                                "but got instead: {}".format(type(wrench)))
+            wrench = np.asarray(wrench).reshape(-1)  # (6,) or (3,)
+        self._wrench = wrench
+
+        # enable / disable the tasks based on if the wrench was provided or not
+        if self._wrench is None:
+            self.disable()
+        else:
+            self.enable()
+
+    @property
+    def kp(self):
+        """Return the proportional gain / compliance matrix."""
+        return self._kp
+
+    @kp.setter
+    def kp(self, kp):
+        """Set the proportional gain / compliance matrix."""
+        if kp is None:
+            kp = 1.
+        if not isinstance(kp, (float, int, np.ndarray)):
+            raise TypeError("Expecting the given compliance matrix gain kp to be an int, float, np.array, instead "
+                            "got: {}".format(type(kp)))
+        if isinstance(kp, np.ndarray) and kp.shape != (6, 6):
+            raise ValueError("Expecting the given compliance matrix gain kp to be of shape {}, but instead "
+                             "got shape: {}".format((6, 6), kp.shape))
+        self._kp = kp
+
+    ###########
+    # Methods #
+    ###########
+    
+    def _update(self, x=None):
         """
         Update the task by computing the A matrix and b vector that will be used by the task solver.
         """
+        x = self.model.get_pose(link=self.distal_link, wrt_link=self.base_link)
+        dx = np.dot(self.kp, (self.desired_wrench - self.wrench))
+
+        # update cartesian task
+        self._task.set_desired_references(x_des=x, dx_des=dx)
+        self._task.update()
         self._A = self._task.A
         self._b = self._task.b
+
+        # set wrench to None
+        self._wrench = None

pyrobolearn/priorities/tasks/velocity/linear_momentum.py

@@ -203,7 +203,7 @@ class LinearMomentumTask(JointVelocityTask):
         """
         return self.x_desired
 
-    def _update(self):
+    def _update(self, x=None):
         """
         Update the task by computing the A matrix and b vector that will be used by the task solver.
         """

pyrobolearn/priorities/tasks/velocity/manipulability.py

@@ -1,6 +1,22 @@
 #!/usr/bin/env python
 r"""Provide the manipulability task.
 
+The manipulability task implements a task that tries to maximize the manipulability measure given in [1]:
+
+.. math:: w(q) = \sqrt{ \det( J(q) W J(q)^\top ) }
+
+where :math:`W` is a constant weight matrix, :math:`q` are the joint positions, and :math:`J(q)` is the jacobian.
+The gradient of :math:`w` is then computed and projected using the gradient projection method [2].
+
+The quadratic cost being minimized is:
+
+.. math:: ||\dot{q} - \dot{q}_0||^2
+
+where :math:`\dot{q}` are the joint velocities being optimized,
+:math:`\dot{q}_0 = k_0 \left( \frac{\partial w(q)}{\partial q} \right)^\top` where :math:`k_0 > 0` and
+:math:`w(q)` is an objective function of the joint variables, where in this case, the manipulability measure is
+given by :math:`w(q) = \sqrt{\det( J(q) J^\top(q) )}`. By maximizing this measure, we move away from singularities.
+
 
 The implementation of this class is inspired by [1] (which is licensed under the LGPLv2).
 
@@ -49,14 +65,15 @@ class ManipulabilityTask(JointVelocityTask):
         - [2] "OpenSoT: A whole-body control library for the compliant humanoid robot COMAN", Rocchi et al., 2015
     """
 
-    def __init__(self, model, constraints=[]):
+    def __init__(self, model, weight=1., constraints=[]):
         """
         Initialize the task.
 
         Args:
             model (ModelInterface): model interface
-            constraints (list of Constraint): list of constraints associated with the task.
+            weight (float, np.array[float[N,N]]): weight scalar or matrix associated to the task.
+            constraints (list[Constraint]): list of constraints associated with the task.
         """
-        super(ManipulabilityTask, self).__init__(model=model, constraints=constraints)
+        super(ManipulabilityTask, self).__init__(model=model, weight=weight, constraints=constraints)
 
         raise NotImplementedError("This class has not been implemented yet.")

pyrobolearn/priorities/tasks/velocity/minimum_acceleration.py

@@ -68,7 +68,7 @@ class MinAccelerationTask(JointVelocityTask):
     # Methods #
     ###########
 
-    def _update(self):
+    def _update(self, x=None):
         """
         Update the task by computing the A matrix and b vector that will be used by the task solver.
         """

pyrobolearn/priorities/tasks/velocity/minimum_effort.py

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

pyrobolearn/priorities/tasks/velocity/momentum.py

@@ -245,7 +245,7 @@ class CentroidalMomentumTask(JointVelocityTask):
         """
         return self.x_desired
 
-    def _update(self):
+    def _update(self, x=None):
         """
         Update the task by computing the A matrix and b vector that will be used by the task solver.
         """

pyrobolearn/priorities/tasks/velocity/postural.py

@@ -182,7 +182,7 @@ class PosturalTask(JointVelocityTask):
         """
         return self.x_desired, self.dx_desired
 
-    def _update(self):
+    def _update(self, x=None):
         """
         Update the task by computing the A matrix and b vector that will be used by the task solver.
         """

pyrobolearn/priorities/tasks/velocity/pure_rolling.py

@@ -54,8 +54,8 @@ class PureRollingTask(JointVelocityTask):
 
         Args:
             model (ModelInterface): model interface.
-            weight (float, np.array[3,3]): weight scalar or matrix associated to the task.
-            constraints (list of Constraint): list of constraints associated with the task.
+            weight (float, np.array[float[3,3]]): weight scalar or matrix associated to the task.
+            constraints (list[Constraint]): list of constraints associated with the task.
         """
         super(PureRollingTask, self).__init__(model=model, weight=weight, constraints=constraints)
 

pyrobolearn/priorities/tasks/velocity/rigid_rotation.py

@@ -38,8 +38,8 @@ class RigidRotationTask(JointVelocityTask):
 
         Args:
             model (ModelInterface): model interface.
-            weight (float, np.array[N,N]): weight scalar or matrix associated to the task.
-            constraints (list of Constraint): list of constraints associated with the task.
+            weight (float, np.array[float[N,N]]): weight scalar or matrix associated to the task.
+            constraints (list[Constraint]): list of constraints associated with the task.
         """
         super(RigidRotationTask, self).__init__(model=model, weight=weight, constraints=constraints)
 

pyrobolearn/priorities/tasks/velocity/unicycle.py

@@ -41,8 +41,8 @@ class UnicycleTask(Task):
 
         Args:
             model (ModelInterface): model interface
-            weight (float, np.array[N,N]): weight scalar or matrix associated to the task.
-            constraints (list of Constraint): list of constraints associated with the task.
+            weight (float, np.array[float[N,N]]): weight scalar or matrix associated to the task.
+            constraints (list[Constraint]): list of constraints associated with the task.
         """
         super(UnicycleTask, self).__init__(model=model, weight=weight, constraints=constraints)