diff --git a/pyrobolearn/__init__.py b/pyrobolearn/__init__.py index e45fb08..e341e01 100644 --- a/pyrobolearn/__init__.py +++ b/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 diff --git a/pyrobolearn/priorities/README.rst b/pyrobolearn/priorities/README.rst index c93e609..6e4154d 100644 --- a/pyrobolearn/priorities/README.rst +++ b/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 diff --git a/pyrobolearn/priorities/attractor_point.py b/pyrobolearn/priorities/attractor_point.py new file mode 100644 index 0000000..b6d584e --- /dev/null +++ b/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) diff --git a/pyrobolearn/priorities/constraints/acceleration/dynamic_feasibility.py b/pyrobolearn/priorities/constraints/acceleration/dynamic_feasibility.py index 86e06f5..f7f4b26 100644 --- a/pyrobolearn/priorities/constraints/acceleration/dynamic_feasibility.py +++ b/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 diff --git a/pyrobolearn/priorities/constraints/constraint.py b/pyrobolearn/priorities/constraints/constraint.py index 829cfd6..cdcecff 100644 --- a/pyrobolearn/priorities/constraints/constraint.py +++ b/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 # diff --git a/pyrobolearn/priorities/constraints/force/contact.py b/pyrobolearn/priorities/constraints/force/contact.py index 1163f82..b54c8fe 100644 --- a/pyrobolearn/priorities/constraints/force/contact.py +++ b/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 # ########### diff --git a/pyrobolearn/priorities/constraints/force/friction.py b/pyrobolearn/priorities/constraints/force/friction.py index 6ed2704..e5b2e55 100644 --- a/pyrobolearn/priorities/constraints/force/friction.py +++ b/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)) diff --git a/pyrobolearn/priorities/constraints/velocity/cartesian_velocity.py b/pyrobolearn/priorities/constraints/velocity/cartesian_velocity.py index 20c4f1d..65e6c28 100644 --- a/pyrobolearn/priorities/constraints/velocity/cartesian_velocity.py +++ b/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) diff --git a/pyrobolearn/priorities/constraints/velocity/convex_hull.py b/pyrobolearn/priorities/constraints/velocity/convex_hull.py index f68fbfb..5970eac 100644 --- a/pyrobolearn/priorities/constraints/velocity/convex_hull.py +++ b/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 diff --git a/pyrobolearn/priorities/constraints/velocity/dynamics.py b/pyrobolearn/priorities/constraints/velocity/dynamics.py index 772d91c..127510b 100644 --- a/pyrobolearn/priorities/constraints/velocity/dynamics.py +++ b/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): diff --git a/pyrobolearn/priorities/ik.py b/pyrobolearn/priorities/ik.py index 2de1f3a..106bfc1 100644 --- a/pyrobolearn/priorities/ik.py +++ b/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) diff --git a/pyrobolearn/priorities/models/model.py b/pyrobolearn/priorities/models/model.py index 689e61f..22dda11 100644 --- a/pyrobolearn/priorities/models/model.py +++ b/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}]`. diff --git a/pyrobolearn/priorities/models/robot_model.py b/pyrobolearn/priorities/models/robot_model.py index 9ae00a8..b27727f 100644 --- a/pyrobolearn/priorities/models/robot_model.py +++ b/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 diff --git a/pyrobolearn/priorities/tasks/__init__.py b/pyrobolearn/priorities/tasks/__init__.py index 7cb8910..18fc76f 100644 --- a/pyrobolearn/priorities/tasks/__init__.py +++ b/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 diff --git a/pyrobolearn/priorities/tasks/acceleration/cartesian.py b/pyrobolearn/priorities/tasks/acceleration/cartesian.py index dd9b752..80002ed 100644 --- a/pyrobolearn/priorities/tasks/acceleration/cartesian.py +++ b/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 diff --git a/pyrobolearn/priorities/tasks/acceleration/com.py b/pyrobolearn/priorities/tasks/acceleration/com.py index bb2320a..f731841 100644 --- a/pyrobolearn/priorities/tasks/acceleration/com.py +++ b/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. """ diff --git a/pyrobolearn/priorities/tasks/acceleration/contact.py b/pyrobolearn/priorities/tasks/acceleration/contact.py index b12128a..15bb1cb 100644 --- a/pyrobolearn/priorities/tasks/acceleration/contact.py +++ b/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. """ diff --git a/pyrobolearn/priorities/tasks/acceleration/dynamic_feasibility.py b/pyrobolearn/priorities/tasks/acceleration/dynamic_feasibility.py index e69de29..a2cf195 100644 --- a/pyrobolearn/priorities/tasks/acceleration/dynamic_feasibility.py +++ b/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 diff --git a/pyrobolearn/priorities/tasks/acceleration/postural.py b/pyrobolearn/priorities/tasks/acceleration/postural.py index c09e29b..1bb2e85 100644 --- a/pyrobolearn/priorities/tasks/acceleration/postural.py +++ 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. """ diff --git a/pyrobolearn/priorities/tasks/force/__init__.py b/pyrobolearn/priorities/tasks/force/__init__.py index 12d0ca4..783985f 100644 --- a/pyrobolearn/priorities/tasks/force/__init__.py +++ b/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 diff --git a/pyrobolearn/priorities/tasks/force/com.py b/pyrobolearn/priorities/tasks/force/com.py index 968c3fe..e43c880 100644 --- a/pyrobolearn/priorities/tasks/force/com.py +++ b/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 + + + diff --git a/pyrobolearn/priorities/tasks/force/floating_base.py b/pyrobolearn/priorities/tasks/force/floating_base.py index 0da4ccd..856107a 100644 --- a/pyrobolearn/priorities/tasks/force/floating_base.py +++ b/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,) diff --git a/pyrobolearn/priorities/tasks/force/manipulability.py b/pyrobolearn/priorities/tasks/force/manipulability.py index 01a66ea..d831bc6 100644 --- a/pyrobolearn/priorities/tasks/force/manipulability.py +++ b/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) diff --git a/pyrobolearn/priorities/tasks/force/wrench.py b/pyrobolearn/priorities/tasks/force/wrench.py index 181f9a7..db34ae7 100644 --- a/pyrobolearn/priorities/tasks/force/wrench.py +++ b/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) diff --git a/pyrobolearn/priorities/tasks/task.py b/pyrobolearn/priorities/tasks/task.py index 65a84ae..6c76fe6 100644 --- a/pyrobolearn/priorities/tasks/task.py +++ b/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__': diff --git a/pyrobolearn/priorities/tasks/task_from_constraint.py b/pyrobolearn/priorities/tasks/task_from_constraint.py index 72966a6..3cbdbbf 100644 --- a/pyrobolearn/priorities/tasks/task_from_constraint.py +++ b/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. """ diff --git a/pyrobolearn/priorities/tasks/torque/cartesian_impedance_control.py b/pyrobolearn/priorities/tasks/torque/cartesian_impedance_control.py index 451c307..c8537b3 100644 --- a/pyrobolearn/priorities/tasks/torque/cartesian_impedance_control.py +++ b/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)) diff --git a/pyrobolearn/priorities/tasks/torque/joint_impedance_control.py b/pyrobolearn/priorities/tasks/torque/joint_impedance_control.py index cacc23b..ff526e6 100644 --- a/pyrobolearn/priorities/tasks/torque/joint_impedance_control.py +++ b/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. """ diff --git a/pyrobolearn/priorities/tasks/torque/minimum_torque.py b/pyrobolearn/priorities/tasks/torque/minimum_torque.py new file mode 100644 index 0000000..232e9e4 --- /dev/null +++ b/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() diff --git a/pyrobolearn/priorities/tasks/velocity/__init__.py b/pyrobolearn/priorities/tasks/velocity/__init__.py index 011519d..c147a03 100644 --- a/pyrobolearn/priorities/tasks/velocity/__init__.py +++ b/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 diff --git a/pyrobolearn/priorities/tasks/velocity/angular_momentum.py b/pyrobolearn/priorities/tasks/velocity/angular_momentum.py index 15a353a..339a9c4 100644 --- a/pyrobolearn/priorities/tasks/velocity/angular_momentum.py +++ b/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. """ diff --git a/pyrobolearn/priorities/tasks/velocity/cartesian.py b/pyrobolearn/priorities/tasks/velocity/cartesian.py index 0eace9f..85ae29f 100644 --- a/pyrobolearn/priorities/tasks/velocity/cartesian.py +++ b/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. """ diff --git a/pyrobolearn/priorities/tasks/velocity/com.py b/pyrobolearn/priorities/tasks/velocity/com.py index 91b8080..b589655 100644 --- a/pyrobolearn/priorities/tasks/velocity/com.py +++ b/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. """ diff --git a/pyrobolearn/priorities/tasks/velocity/contact.py b/pyrobolearn/priorities/tasks/velocity/contact.py index d99eab3..802d073 100644 --- a/pyrobolearn/priorities/tasks/velocity/contact.py +++ b/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. """ diff --git a/pyrobolearn/priorities/tasks/velocity/gaze.py b/pyrobolearn/priorities/tasks/velocity/gaze.py index 7e8a1f8..b6965e7 100644 --- a/pyrobolearn/priorities/tasks/velocity/gaze.py +++ b/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) diff --git a/pyrobolearn/priorities/tasks/velocity/interaction.py b/pyrobolearn/priorities/tasks/velocity/interaction.py index 0666a61..623355c 100644 --- a/pyrobolearn/priorities/tasks/velocity/interaction.py +++ b/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 diff --git a/pyrobolearn/priorities/tasks/velocity/linear_momentum.py b/pyrobolearn/priorities/tasks/velocity/linear_momentum.py index 42be645..8a2617c 100644 --- a/pyrobolearn/priorities/tasks/velocity/linear_momentum.py +++ b/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. """ diff --git a/pyrobolearn/priorities/tasks/velocity/manipulability.py b/pyrobolearn/priorities/tasks/velocity/manipulability.py index 5e4a431..916b24a 100644 --- a/pyrobolearn/priorities/tasks/velocity/manipulability.py +++ b/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.") diff --git a/pyrobolearn/priorities/tasks/velocity/minimum_acceleration.py b/pyrobolearn/priorities/tasks/velocity/minimum_acceleration.py index eb2d3af..3885d8d 100644 --- a/pyrobolearn/priorities/tasks/velocity/minimum_acceleration.py +++ b/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. """ diff --git a/pyrobolearn/priorities/tasks/velocity/minimum_effort.py b/pyrobolearn/priorities/tasks/velocity/minimum_effort.py index 2e94b07..7542baf 100644 --- a/pyrobolearn/priorities/tasks/velocity/minimum_effort.py +++ b/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 diff --git a/pyrobolearn/priorities/tasks/velocity/momentum.py b/pyrobolearn/priorities/tasks/velocity/momentum.py index ee6b284..e92c93c 100644 --- a/pyrobolearn/priorities/tasks/velocity/momentum.py +++ b/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. """ diff --git a/pyrobolearn/priorities/tasks/velocity/postural.py b/pyrobolearn/priorities/tasks/velocity/postural.py index 186061c..7dfbdc2 100644 --- a/pyrobolearn/priorities/tasks/velocity/postural.py +++ b/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. """ diff --git a/pyrobolearn/priorities/tasks/velocity/pure_rolling.py b/pyrobolearn/priorities/tasks/velocity/pure_rolling.py index 9badc1a..22fbcaf 100644 --- a/pyrobolearn/priorities/tasks/velocity/pure_rolling.py +++ b/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) diff --git a/pyrobolearn/priorities/tasks/velocity/rigid_rotation.py b/pyrobolearn/priorities/tasks/velocity/rigid_rotation.py index 9b55cf9..d96ea7a 100644 --- a/pyrobolearn/priorities/tasks/velocity/rigid_rotation.py +++ b/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) diff --git a/pyrobolearn/priorities/tasks/velocity/unicycle.py b/pyrobolearn/priorities/tasks/velocity/unicycle.py index c88955b..6d2ba17 100644 --- a/pyrobolearn/priorities/tasks/velocity/unicycle.py +++ b/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)