diff --git a/pyrobolearn/priorities/constraints/acceleration/__init__.py b/pyrobolearn/priorities/constraints/acceleration/__init__.py index e69de29..f4a6b54 100644 --- a/pyrobolearn/priorities/constraints/acceleration/__init__.py +++ b/pyrobolearn/priorities/constraints/acceleration/__init__.py @@ -0,0 +1,4 @@ + +# joint acceleration constraints + +from .dynamic_feasibility import DynamicFeasibilityConstraint diff --git a/pyrobolearn/priorities/constraints/acceleration/dynamic_feasibility.py b/pyrobolearn/priorities/constraints/acceleration/dynamic_feasibility.py index e0d7eea..86e06f5 100644 --- a/pyrobolearn/priorities/constraints/acceleration/dynamic_feasibility.py +++ b/pyrobolearn/priorities/constraints/acceleration/dynamic_feasibility.py @@ -1,6 +1,17 @@ #!/usr/bin/env python r"""Provide the dynamic feasibility constraint. +The equality joint acceleration constraint is given by: + +.. math:: H(q) \ddot{q} + N(q, \dot{q}) = \sum_i J_i^T F_i + +where :math:`H(q)` is the joint space inertia matrix, :math:`\ddot{q}` are the joint accelerations being optimized, +:math:`N(q, \dot{q})` is the vector of force terms that account for the Coriolis and centrifugal forces, gravity, +and any other forces acting on the system other than the contact forces given by :math:`\sum_i J_i^T F_i` (where +each :math:`J_i` is a Jacobian matrix and :math:`F_i` is a wrench vector at the contact link :math:`i`). + +This formulation can be rewritten as an equality constraint math:`A_{eq} x = b_{eq}` in QP, with +:math:`x = \ddot{q}`, :math:`A_{eq} = H(q)`, and :math:`b_{eq} = \sum_i J_i^T F_i - N(q, \dot{q})`. The implementation of this class is inspired by [1] (which is licensed under the LGPLv2). @@ -10,7 +21,7 @@ References: import numpy as np -from pyrobolearn.priorities.constraints.constraint import Constraint +from pyrobolearn.priorities.constraints.constraint import EqualityConstraint, JointAccelerationConstraint __author__ = "Brian Delhaisse" @@ -23,10 +34,99 @@ __email__ = "briandelhaisse@gmail.com" __status__ = "Development" -class DynamicFeasibilityConstraint(Constraint): +class DynamicFeasibilityConstraint(EqualityConstraint, JointAccelerationConstraint): r"""Dynamic Feasibility Constraint + The equality joint acceleration constraint is given by: + + .. math:: H(q) \ddot{q} + N(q, \dot{q}) = \sum_i J_i^T F_i + + where :math:`H(q)` is the joint space inertia matrix, :math:`\ddot{q}` are the joint accelerations being optimized, + :math:`N(q, \dot{q})` is the vector of force terms that account for the Coriolis and centrifugal forces, gravity, + and any other forces acting on the system other than the contact forces given by :math:`\sum_i J_i^T F_i` (where + each :math:`J_i` is a Jacobian matrix and :math:`F_i` is a wrench vector at the contact link :math:`i`). + + This formulation can be rewritten as an equality constraint math:`A_{eq} x = b_{eq}` in QP, with + :math:`x = \ddot{q}`, :math:`A_{eq} = H(q)`, and :math:`b_{eq} = \sum_i J_i^T F_i - N(q, \dot{q})`. + + The implementation of this class is inspired by [1] (which is licensed under the LGPLv2). + + References: + - [1] "OpenSoT: A whole-body control library for the compliant humanoid robot COMAN", Rocchi et al., 2015 """ - def __init__(self, model): + def __init__(self, model, contact_links=[], wrenches=[]): + r""" + Initialize the constraint. + + Args: + model (ModelInterface): model interface. + contact_links (list[str], list[int], None): list of unique contact link names or ids. + wrenches (list[np.array[float[6]]], None): list of associated wrenches applied to the contact links. It + must have the same size as the number of contact links. + """ super(DynamicFeasibilityConstraint, self).__init__(model) + + # set variables + self.contact_links = contact_links + self.wrenches = wrenches + + # first update + self.update() + + ############## + # Properties # + ############## + + @property + def contact_links(self): + """Get the contact links.""" + return self._contact_links + + @contact_links.setter + def contact_links(self, contacts): + """Set the contact links.""" + if contacts is None: + contacts = [] + elif not isinstance(contacts, (list, tuple, np.ndarray)): + raise TypeError("Expecting the given 'contact_links' to be a list of names/ids, but got instead: " + "{}".format(type(contacts))) + self._contact_links = contacts + + @property + def wrenches(self): + """Get the wrenches.""" + return self._wrenches + + @wrenches.setter + def wrenches(self, wrenches): + """Set the wrenches.""" + if wrenches is None: + wrenches = [] + elif not isinstance(wrenches, (list, tuple, np.ndarray)): + raise TypeError("Expecting the given 'wrenches' to be a list of wrench vectors, but got instead: " + "{}".format(type(wrenches))) + if isinstance(wrenches, np.ndarray) and wrenches.ndim == 1: + wrenches = wrenches.reshape(-1, 6) + self._wrenches = wrenches + + ########### + # Methods # + ########### + + def _update(self): + """Update the equality constraint.""" + # get dynamics + H = self.model.get_inertia_matrix() + N = self.model.compute_nonlinear_term() + + # compute torques due to contact wrenches + tau = 0 + for link, wrench in zip(self.contact_links, self.wrenches): + link = self.model.get_link_id(link) + jacobian = self.model.get_jacobian(link=link) + tau += jacobian.T.dot(wrench) + + # equality constraints + self._A_eq = H + self._b_eq = tau - N diff --git a/pyrobolearn/priorities/constraints/constraint.py b/pyrobolearn/priorities/constraints/constraint.py index 1bbf315..10a6470 100644 --- a/pyrobolearn/priorities/constraints/constraint.py +++ b/pyrobolearn/priorities/constraints/constraint.py @@ -161,9 +161,9 @@ class Constraint(object): Initialize the Constraint. Args: - constraints (list[Constraint], None): inner constraints. By providing a list of constraints, they can be - combined easily. model (ModelInterface): robotic model interface. + constraints (list[Constraint], None): inner constraints. By providing a list of constraints, they can be + combined easily. """ self.constraints = constraints self.model = model @@ -194,6 +194,14 @@ class Constraint(object): "{}".format(model)) self._model = model + @property + def x_size(self): + """Return the number of variables being optimized.""" + # return self._x_size + if self.model is not None: + return self.model.num_actuated_joints + return 0 + @property def constraints(self): """Return the dict of inner constraints.""" @@ -492,14 +500,19 @@ class Constraint(object): else: raise TypeError("The given type of constraint is not currently supported.") + def _update(self): + """Update the constraint variables. This has to be implemented in the child classes.""" + pass + def update(self): r""" Update the various constraint matrices and vectors: :math:`A_{eq}, b_{eq}, A_{ineq}, b_l, b_u, ...`. - - Args: - x (np.array[float[N]]): current optimization variables values. """ - pass + if self.constraints: + for constraint in self.constraints: + constraint.update() + else: + self._update() ############# # Operators # @@ -512,14 +525,11 @@ class Constraint(object): """Return a string describing the class.""" return self.__class__.__name__ - def __call__(self, x): + def __call__(self): """ - Update the constraint (i.e. update the various constraint matrices and vectors. - - Args: - x (np.array[float[N]]): current optimization variable values. + Update the constraint (i.e. update the various constraint matrices and vectors). """ - return self.update(x) + return self.update() def __len__(self): """ @@ -626,3 +636,13 @@ class JointAccelerationConstraint(DynamicConstraint): class JointEffortConstraint(DynamicConstraint): r"""Joint effort constraint.""" pass + + +class JointTorqueConstraint(DynamicConstraint): + r"""Joint torque constraint.""" + pass + + +class JointForceConstraint(DynamicConstraint): + r"""Joint Force constraint.""" + pass diff --git a/pyrobolearn/priorities/constraints/constraint_from_task.py b/pyrobolearn/priorities/constraints/constraint_from_task.py new file mode 100644 index 0000000..c824538 --- /dev/null +++ b/pyrobolearn/priorities/constraints/constraint_from_task.py @@ -0,0 +1,76 @@ +#!/usr/bin/env python +r"""Transform a given equality constraint into a task. + +An equality constraint specified by :math:`Fx = k` is transformed to a soft task :math:`||Ax - b||^2`, where +:math:`A = F` and :math:`b = k`. This allows for the equality constraint to be lightly violated; by specifying the +weight :math:`W` we can specify how much the constraint should be satisfied. +""" + +import pyrobolearn as prl +from pyrobolearn.priorities.constraints import EqualityConstraint + + +__author__ = "Brian Delhaisse" +__copyright__ = "Copyright 2019, PyRoboLearn" +__credits__ = ["Brian Delhaisse"] +__license__ = "GNU GPLv3" +__version__ = "1.0.0" +__maintainer__ = "Brian Delhaisse" +__email__ = "briandelhaisse@gmail.com" +__status__ = "Development" + + +class ConstraintFromTask(EqualityConstraint): + r"""Equality Constraint from Task + + A soft task :math:`||Ax - b||^2` is transformed to an equality constraint specified by :math:`Fx = k`, where + :math:`F = A` and :math:`k = b`. This allows a priori for the task to not be violated, however this might results + in an impossible task to solve. + """ + + def __init__(self, task): + """ + Initialize the task. + + Args: + task (Task): task. + """ + # set task + self.task = task + + # call superclass + super(ConstraintFromTask, self).__init__(model=task.model) + + # first update + self.update() + + ############## + # Properties # + ############## + + @property + def task(self): + """Get the task.""" + return self._task + + @task.setter + def task(self, task): + """Set the task.""" + if not isinstance(task, prl.priorities.tasks.Task): + raise TypeError("Expecting the given 'task' to be an instance of `Task`, but instead got: {}".format(task)) + self._task = task + + ########### + # Methods # + ########### + + def _update(self): + """ + Update the task by computing the A matrix and b vector that will be used by the task solver. + """ + # update task + self.task.update() + + # update A_eq and b_eq + self._A_eq = self.task.A + self._b_eq = self.task.b diff --git a/pyrobolearn/priorities/constraints/force/__init__.py b/pyrobolearn/priorities/constraints/force/__init__.py index e69de29..b6d6adb 100644 --- a/pyrobolearn/priorities/constraints/force/__init__.py +++ b/pyrobolearn/priorities/constraints/force/__init__.py @@ -0,0 +1,2 @@ + +# cartesian force constraints diff --git a/pyrobolearn/priorities/constraints/torque/__init__.py b/pyrobolearn/priorities/constraints/torque/__init__.py index e69de29..7be02cd 100644 --- a/pyrobolearn/priorities/constraints/torque/__init__.py +++ b/pyrobolearn/priorities/constraints/torque/__init__.py @@ -0,0 +1,6 @@ + +# joint torque constraints + +from .joint_limits import JointLimitsConstraint + +from .torque_limits import JointTorqueLimitsConstraint diff --git a/pyrobolearn/priorities/constraints/torque/joint_limits.py b/pyrobolearn/priorities/constraints/torque/joint_limits.py index 0fbb563..0358bb3 100644 --- a/pyrobolearn/priorities/constraints/torque/joint_limits.py +++ b/pyrobolearn/priorities/constraints/torque/joint_limits.py @@ -10,7 +10,7 @@ References: import numpy as np -from pyrobolearn.priorities.constraints.constraint import Constraint +from pyrobolearn.priorities.constraints.constraint import BoundConstraint, JointTorqueConstraint __author__ = "Brian Delhaisse" @@ -23,10 +23,151 @@ __email__ = "briandelhaisse@gmail.com" __status__ = "Development" -class JointLimitsConstraint(Constraint): +class JointLimitsConstraint(BoundConstraint, JointTorqueConstraint): r"""Joint Limits constraint. + This provides bounds/limits on the joint torques: + + .. math:: k_p (q_{lb} - q) - k_d \dot{q} \leq \tau \leq k_p (q_{ub} - q) - k_d \dot{q} + + where :math:`q_{lb}, q_{ub}` are the lower and upper joint position limits, :math:`kp` and :math:`kd` are the + position and velocity gains respectively, :math:`q, \dot{q}` are the current joint positions and velocities, and + :math:`\tau` are the torques that are being optimized. + + The implementation of this class is inspired by [1] (which is licensed under the LGPLv2). + + References: + - [1] "OpenSoT: A whole-body control library for the compliant humanoid robot COMAN", Rocchi et al., 2015 """ - def __init__(self, model): + def __init__(self, model, q_lower_bound=None, q_upper_bound=None, kp=15000., kd=1000.): + r""" + Initialize the constraint. + + Args: + model (ModelInterface): model interface. + q_lower_bound (np.array[float[N]], None): joint position lower limits. If None, it will take the lower + joint limits specified in the model. Note that if the lower limits are equal to the upper limits, they + will be set to -10 and 10 by default. + q_upper_bound (np.array[float[N]], None): joint position upper limits. If None, it will take the upper + joint limits specified in the model. Note that if the upper limits are equal to the lower limits, they + will be set to -10 and 10 by default. + kp (float, np.array[float[N]]): position gain(s). + kd (float, np.array[float[N]]): velocity gain(s). + """ super(JointLimitsConstraint, self).__init__(model) + + # set gains + self.kp = kp + self.kd = kd + + # set variables + if q_lower_bound is None or q_upper_bound is None: + q_lb, q_ub = self.model.get_joint_limits() + if q_lower_bound is None: + q_lower_bound = q_lb + if q_upper_bound is None: + q_upper_bound = q_ub + if np.allclose(q_lower_bound, q_upper_bound): + print("WARNING: the joint position lower and upper limits are the same, by default they will be set " + "to -10 and 10.") + q_lower_bound = -10. * np.ones(len(q_lower_bound)) + q_upper_bound = 10. * np.ones(len(q_upper_bound)) + + self.q_lower_bounds = q_lower_bound + self.q_upper_bounds = q_upper_bound + + # first update + self.update() + + ############## + # Properties # + ############## + + @property + def q_lower_bounds(self): + """Get the lower joint position limits.""" + return self._q_lb + + @q_lower_bounds.setter + def q_lower_bounds(self, q_lb): + """Set the lower joint position limits.""" + if q_lb is None: + q_lb = self.model.get_joint_limits()[0] + if not isinstance(q_lb, np.ndarray): + raise TypeError("Expecting the given lower joint position limits to be a np.array, instead got: " + "{}".format(q_lb)) + if len(q_lb) != self.x_size: + raise ValueError("Expecting the length of the lower joint position limits (={}) to be the same length as " + "the number of variables being optimized =({}).".format(len(q_lb), self.x_size)) + self._q_lb = q_lb + + @property + def q_upper_bounds(self): + """Get the upper joint position limits.""" + return self._q_ub + + @q_upper_bounds.setter + def q_upper_bounds(self, q_ub): + """Set the upper joint position limits.""" + if q_ub is None: + q_ub = self.model.get_joint_limits()[1] + if not isinstance(q_ub, np.ndarray): + raise TypeError("Expecting the given upper joint position limits to be a np.array, instead got: " + "{}".format(q_ub)) + if len(q_ub) != self.x_size: + raise ValueError("Expecting the length of the upper joint position limits (={}) to be the same length as " + "the number of variables being optimized =({}).".format(len(q_ub), self.x_size)) + self._q_ub = q_ub + + @property + def kp(self): + """Return the position gain.""" + return self._kp + + @kp.setter + def kp(self, kp): + """Set the position gain.""" + if not isinstance(kp, (float, int, np.ndarray)): + raise TypeError("Expecting the given position gain kp to be an int, float, np.array, instead got: " + "{}".format(type(kp))) + if isinstance(kp, np.ndarray) and len(kp) != self.x_size: + raise ValueError("Expecting the given position gain matrix kp to be of length {}, but instead got " + "a length of: {}".format(self.x_size, len(kp))) + if np.any(kp < 0): + raise ValueError("The position gain(s) should all be bigger or equal than 0, but found some negative " + "gains") + self._kp = kp + + @property + def kd(self): + """Return the linear velocity gain.""" + return self._kd + + @kd.setter + def kd(self, kd): + """Set the linear velocity gain.""" + if not isinstance(kd, (float, int, np.ndarray)): + raise TypeError("Expecting the given velocity gain kd to be an int, float, np.array, instead got: " + "{}".format(type(kd))) + if isinstance(kd, np.ndarray) and len(kd) != self.x_size: + raise ValueError("Expecting the given velocity gain matrix kd to be of length {}, but instead got " + "a length of: {}".format(self.x_size, len(kd))) + if np.any(kd < 0): + raise ValueError("The velocity gain(s) should all be bigger or equal than 0, but found some negative " + "gains") + self._kd = kd + + ########### + # Methods # + ########### + + def _update(self): + r""" + Update the lower and upper bounds. + """ + q = self.model.get_joint_positions() + dq = self.model.get_joint_velocities() + self._lower_bound = (self._q_lb - q) - self.kd * dq + self._upper_bound = (self._q_ub - q) - self.kd * dq + diff --git a/pyrobolearn/priorities/constraints/torque/torque_limits.py b/pyrobolearn/priorities/constraints/torque/torque_limits.py index 2b6ac5e..66edca0 100644 --- a/pyrobolearn/priorities/constraints/torque/torque_limits.py +++ b/pyrobolearn/priorities/constraints/torque/torque_limits.py @@ -1,6 +1,17 @@ #!/usr/bin/env python r"""Provide the torque limits constraint. +This provides bounds/limits on the joint torques: + +.. math:: \tau_{lb} \leq \tau \leq \tau_{ub} + +where :math:`(\tau_{lb}, \tau_{ub})` are the lower and upper bound on the joint torques, and :math:`\tau` are the +joint torques being optimized. + +This formulation can be rewritten as the inequality constraint math:`lb \leq x \leq ub` in QP, with +:math:`lb = \tau_{lb}`, :math:`ub = \tau_{ub}`, and :math:`x = \tau`. This can also be rewritten as :math:`Gx \leq h`, +with :math:`G = [-I, I]^\top` and :math:`h = [-\tau_{lb}^\top, \tau_{ub}^\top]^\top` where :math:`I` is the square +identity matrix. The implementation of this class is inspired by [1] (which is licensed under the LGPLv2). @@ -10,7 +21,7 @@ References: import numpy as np -from pyrobolearn.priorities.constraints.constraint import Constraint +from pyrobolearn.priorities.constraints.constraint import BoundConstraint, JointTorqueConstraint __author__ = "Brian Delhaisse" @@ -23,10 +34,108 @@ __email__ = "briandelhaisse@gmail.com" __status__ = "Development" -class TorqueLimitsConstraint(Constraint): +class JointTorqueLimitsConstraint(BoundConstraint, JointTorqueConstraint): r"""Torque Limits constraint. + This provides bounds/limits on the joint torques: + + .. math:: \tau_{lb} \leq \tau \leq \tau_{ub} + + where :math:`(\tau_{lb}, \tau_{ub})` are the lower and upper bound on the joint torques, and + :math:`\tau` are the joint torques being optimized. + + This formulation can be rewritten as the inequality constraint math:`lb \leq x \leq ub` in QP, with + :math:`lb = \tau_{lb}`, :math:`ub = \tau_{ub}`, and :math:`x = \tau`. This can also be rewritten as + :math:`Gx \leq h`, with :math:`G = [-I, I]^\top` and :math:`h = [-\tau_{lb}^\top, \tau_{ub}^\top]^\top` + where :math:`I` is the square identity matrix. + + The implementation of this class is inspired by [1] (which is licensed under the LGPLv2). + + References: + - [1] "OpenSoT: A whole-body control library for the compliant humanoid robot COMAN", Rocchi et al., 2015 """ - def __init__(self, model): - super(TorqueLimitsConstraint, self).__init__(model) + def __init__(self, model, torque_lower_bound=None, torque_upper_bound=None): + """ + Initialize the constraint. + + Args: + model (ModelInterface): model interface. + torque_lower_bound (np.array[float[N]], None): joint velocity lower limits. If None, it will take the lower + joint limits specified in the model. Note that if the lower limits are equal to the upper limits, they + will be set to -100 and 100 by default. + torque_upper_bound (np.array[float[N]], None): joint velocity upper limits. If None, it will take the upper + joint limits specified in the model. Note that if the upper limits are equal to the lower limits, they + will be set to -100 and 100 by default. + """ + super(JointTorqueLimitsConstraint, self).__init__(model) + + # set variables + if torque_lower_bound is None or torque_upper_bound is None: + tau_lb, tau_ub = self.model.get_joint_velocity_limits() + if torque_lower_bound is None: + torque_lower_bound = tau_lb + if torque_upper_bound is None: + torque_upper_bound = tau_ub + if np.allclose(torque_lower_bound, torque_upper_bound): + print("WARNING: the joint velocity lower and upper limits are the same, by default they will be set " + "to -10 and 10.") + torque_lower_bound = -10. * np.ones(len(torque_lower_bound)) + torque_upper_bound = 10. * np.ones(len(torque_upper_bound)) + + self.torque_lower_bounds = torque_lower_bound + self.torque_upper_bounds = torque_upper_bound + + # first update + self.update() + + ############## + # Properties # + ############## + + @property + def torque_lower_bounds(self): + """Get the lower joint velocity limits.""" + return self._tau_lb + + @torque_lower_bounds.setter + def torque_lower_bounds(self, torque_lb): + """Set the lower joint velocity limits.""" + if torque_lb is None: + torque_lb = self.model.get_joint_limits()[0] + if not isinstance(torque_lb, np.ndarray): + raise TypeError("Expecting the given lower joint velocity limits to be a np.array, instead got: " + "{}".format(torque_lb)) + if len(torque_lb) != self.x_size: + raise ValueError("Expecting the length of the lower joint velocity limits (={}) to be the same length as " + "the number of variables being optimized =({}).".format(len(torque_lb), self.x_size)) + self._tau_lb = torque_lb + + @property + def torque_upper_bounds(self): + """Get the upper joint velocity limits.""" + return self._tau_ub + + @torque_upper_bounds.setter + def torque_upper_bounds(self, torque_ub): + """Set the upper joint velocity limits.""" + if torque_ub is None: + torque_ub = self.model.get_joint_limits()[1] + if not isinstance(torque_ub, np.ndarray): + raise TypeError("Expecting the given upper joint velocity limits to be a np.array, instead got: " + "{}".format(torque_ub)) + if len(torque_ub) != self.x_size: + raise ValueError("Expecting the length of the upper joint velocity limits (={}) to be the same length as " + "the number of variables being optimized =({}).".format(len(torque_ub), self.x_size)) + self._tau_ub = torque_ub + + ########### + # Methods # + ########### + + def _update(self): + r""" + Update the lower and upper bounds. + """ + self._lower_bound = self._tau_lb + self._upper_bound = self._tau_ub diff --git a/pyrobolearn/priorities/constraints/velocity/__init__.py b/pyrobolearn/priorities/constraints/velocity/__init__.py index 31efa0d..7c6678f 100644 --- a/pyrobolearn/priorities/constraints/velocity/__init__.py +++ b/pyrobolearn/priorities/constraints/velocity/__init__.py @@ -1,4 +1,6 @@ +# joint velocity constraints + from .joint_limits import JointPositionLimitsConstraint from .velocity_limits import JointVelocityLimitsConstraint diff --git a/pyrobolearn/priorities/constraints/velocity/cartesian_position.py b/pyrobolearn/priorities/constraints/velocity/cartesian_position.py index 83a9eed..02db84d 100644 --- a/pyrobolearn/priorities/constraints/velocity/cartesian_position.py +++ b/pyrobolearn/priorities/constraints/velocity/cartesian_position.py @@ -1,6 +1,17 @@ #!/usr/bin/env python r"""Provide the Cartesian Position constraint. +The bilateral inequality cartesian position constraint is given by: + +.. math:: x_{lb} \leq x + J(q) \dot{q} * dt \leq x_{ub} + +where :math:`x_{lb}, x_{ub}` are the lower and upper bound on the cartesian positions of a given distal link, +:math:`x` is the current cartesian position of the distal link wrt the base link, :math:`\dot{q}` are the joint +velocities being optimized, :math:`J(q)` is the Jacobian from the base to the distal link, and :math:`dt` is the +integration time step. + +This formulation can be rewritten as a bilateral inequality constraint :math:`b_l \leq A_{ineq} x \leq b_u` in QP, +with :math:`x = \dot{q}`, :math:`A_{ineq} = J(q) * dt`, :math:`b_l = (x_{lb} - x)` and :math:`b_u = (x_{ub} - x)`. The implementation of this class is inspired by [1] (which is licensed under the LGPLv2). @@ -10,7 +21,7 @@ References: import numpy as np -from pyrobolearn.priorities.constraints.constraint import Constraint +from pyrobolearn.priorities.constraints.constraint import BilateralConstraint, JointVelocityConstraint __author__ = "Brian Delhaisse" @@ -23,10 +34,128 @@ __email__ = "briandelhaisse@gmail.com" __status__ = "Development" -class CartesianPositionConstraint(Constraint): +class CartesianPositionConstraint(BilateralConstraint, JointVelocityConstraint): r"""Cartesian Position constraint. + The bilateral inequality cartesian position constraint is given by: + + .. math:: x_{lb} \leq x + J(q) \dot{q} * dt \leq x_{ub} + + where :math:`x_{lb}, x_{ub}` are the lower and upper bound on the cartesian positions of a given distal link, + :math:`x` is the current cartesian position of the distal link wrt the base link, :math:`\dot{q}` are the joint + velocities being optimized, :math:`J(q)` is the Jacobian from the base to the distal link, and :math:`dt` is the + integration time step. + + This formulation can be rewritten as a bilateral inequality constraint :math:`b_l \leq A_{ineq} x \leq b_u` in QP, + with :math:`x = \dot{q}`, :math:`A_{ineq} = J(q) * dt`, :math:`b_l = (x_{lb} - x)` and :math:`b_u = (x_{ub} - x)`. + + The implementation of this class is inspired by [1] (which is licensed under the LGPLv2). + + References: + - [1] "OpenSoT: A whole-body control library for the compliant humanoid robot COMAN", Rocchi et al., 2015 """ - def __init__(self, model): + def __init__(self, model, dt, distal_link, base_link=None, local_position=(0, 0, 0), x_lower_bound=None, + x_upper_bound=None): + r""" + Initialize the constraint. + + Args: + model (ModelInterface): model interface. + dt (float): integration time step: use to compute :math:`x = x + v dt`. + distal_link (int, str): distal link id or name. + base_link (int, str, None): base link id or name. If None, it will be the world. + local_position (np.array[float[3]]): local position on the distal link. + x_lower_bound (np.array[float[3]], None): the lower bound on the cartesian position of the distal link wrt + the base link. If None, it will not be considered. + x_upper_bound (np.array[float[3]], None): the upper bound on the cartesian position of the distal link wrt + the base link. If None, it will not be considered. + """ super(CartesianPositionConstraint, self).__init__(model) + + # set time + self.dt = dt + + # define variables + self.distal_link = self.model.get_link_id(distal_link) + self.base_link = self.model.get_link_id(base_link) if base_link is not None else base_link + self.local_position = local_position + + self.position_lower_bound = x_lower_bound + self.position_upper_bound = x_upper_bound + + # first update + self.update() + + ############## + # Properties # + ############## + + @property + def dt(self): + """Return the integration time step.""" + return self._dt + + @dt.setter + def dt(self, dt): + """Set the integration time step.""" + if not isinstance(dt, (float, int)): + raise TypeError("Expecting the integration time step `dt` to be a float or int.") + if dt <= 0: + raise ValueError("Expecting the integration time step `dt` to be bigger than 0.") + self._dt = float(dt) + + @property + def position_lower_bound(self): + """Get the lower bound on the cartesian position of the distal link wrt the base link.""" + return self._x_lower_bound + + @position_lower_bound.setter + def position_lower_bound(self, bound): + """Set the lower bound on the cartesian position of the distal link wrt the base link.""" + if bound is not None: + if not isinstance(bound, (list, tuple, np.ndarray)): + raise TypeError("Expecting the given lower cartesian position bound to be a np.array, instead got: " + "{}".format(bound)) + bound = np.asarray(bound) + if len(bound) != 3: + raise ValueError("Expecting the length of the lower bound to be 3, but got instead a length of: " + "{}.".format(len(bound))) + self._x_lower_bound = bound + + @property + def position_upper_bound(self): + """Get the upper bound on the cartesian position of the distal link wrt the base link.""" + return self._x_upper_bound + + @position_upper_bound.setter + def position_upper_bound(self, bound): + """Set the upper bound on the cartesian position of the distal link wrt the base link.""" + if bound is not None: + if not isinstance(bound, (list, tuple, np.ndarray)): + raise TypeError("Expecting the given upper cartesian position bound to be a np.array, instead got: " + "{}".format(bound)) + bound = np.asarray(bound) + if len(bound) != 3: + raise ValueError("Expecting the length of the upper bound to be 3, but got instead a length of: " + "{}.".format(len(bound))) + self._x_upper_bound = bound + + ########### + # Methods # + ########### + + def _update(self): + """Update the inequality matrix and vectors.""" + if self._x_lower_bound is None and self._x_upper_bound is None: + raise ValueError("Expecting at least the lower or upper bounds of the cartesian position to be " + "specified, but instead got None for both of them.") + + self._A_ineq = self.model.get_jacobian(link=self.distal_link, wrt_link=self.base_link, + point=self.local_position)[:3] * self.dt + x = self.model.get_position(link=self.distal_link, wrt_link=self.base_link) + + if self._x_lower_bound is not None: + self._b_lower_bound = self._x_lower_bound - x + if self._x_upper_bound is not None: + self._b_upper_bound = self._x_upper_bound - x diff --git a/pyrobolearn/priorities/constraints/velocity/cartesian_velocity.py b/pyrobolearn/priorities/constraints/velocity/cartesian_velocity.py index 4d2c1b9..20c4f1d 100644 --- a/pyrobolearn/priorities/constraints/velocity/cartesian_velocity.py +++ b/pyrobolearn/priorities/constraints/velocity/cartesian_velocity.py @@ -1,6 +1,17 @@ #!/usr/bin/env python r"""Provide the cartesian velocity constraint. +The bilateral inequality cartesian velocity constraint is given by: + +.. math:: v_{lb} \leq J(q) \dot{q} \leq v_{ub} + +where :math:`v_{lb}, v_{ub}` are the lower and upper bound on the cartesian velocities of a given distal link, +:math:`\dot{q}` are the joint velocities being optimized, and :math:`J(q)` is the Jacobian from the base to the +distal link. + +This formulation can be rewritten as a bilateral inequality constraint :math:`b_l \leq A_{ineq} x \leq b_u` in QP, +with :math:`x = \dot{q}`, :math:`A_{ineq} = J(q)`, :math:`b_l = v_{lb}` and :math:`b_u = v_{ub}`. + The implementation of this class is inspired by [1] (which is licensed under the LGPLv2). @@ -10,7 +21,7 @@ References: import numpy as np -from pyrobolearn.priorities.constraints.constraint import Constraint +from pyrobolearn.priorities.constraints.constraint import BilateralConstraint, JointVelocityConstraint __author__ = "Brian Delhaisse" @@ -23,10 +34,141 @@ __email__ = "briandelhaisse@gmail.com" __status__ = "Development" -class CartesianVelocityConstraint(Constraint): +class CartesianVelocityConstraint(BilateralConstraint, JointVelocityConstraint): r"""Cartesian Velocity constraint. + The bilateral inequality cartesian velocity constraint is given by: + + .. math:: v_{lb} \leq J(q) \dot{q} \leq v_{ub} + + where :math:`v_{lb}, v_{ub}` are the lower and upper bound on the cartesian velocities of a given distal link, + :math:`\dot{q}` are the joint velocities being optimized, and :math:`J(q)` is the Jacobian from the base to the + distal link. + + This formulation can be rewritten as a bilateral inequality constraint :math:`b_l \leq A_{ineq} x \leq b_u` in QP, + with :math:`x = \dot{q}`, :math:`A_{ineq} = J(q)`, :math:`b_l = v_{lb}` and :math:`b_u = v_{ub}`. + + The implementation of this class is inspired by [1] (which is licensed under the LGPLv2). + + References: + - [1] "OpenSoT: A whole-body control library for the compliant humanoid robot COMAN", Rocchi et al., 2015 """ - def __init__(self, model): + def __init__(self, model, distal_link, base_link=None, local_position=(0, 0, 0), linear_velocity_bounds=None, + angular_velocity_bounds=None): + r""" + Initialize the constraint. + + Args: + model (ModelInterface): model interface. + distal_link (int, str): distal link id or name. + base_link (int, str, None): base link id or name. If None, it will be the world. + local_position (np.array[float[3]]): local position on the distal link. + linear_velocity_bounds (tuple[2 * np.array[float[3]]], np.array[float[3]], None): If tuple, it is the + lower and upper bounds on the linear velocity. If np.array, then the lower and upper bound would be set + to (-linear_velocity_bounds, linear_velocity_bounds). If None, it will not be considered. + angular_velocity_bounds (tuple[2 * np.array[float[3]]], np.array[float[3]], None): If tuple, it is the lower and + upper bounds on the angular velocity. If np.array, then the lower and upper bound would be set to + (-angular_velocity_bounds, angular_velocity_bounds). If None, it will not be considered. + """ super(CartesianVelocityConstraint, self).__init__(model) + + # define variables + self.distal_link = self.model.get_link_id(distal_link) + self.base_link = self.model.get_link_id(base_link) if base_link is not None else base_link + self.local_position = local_position + + self.linear_velocity_bounds = linear_velocity_bounds + self.angular_velocity_bounds = angular_velocity_bounds + + # first update + self.update() + + ############## + # Properties # + ############## + + @property + def linear_velocity_bounds(self): + """Get the cartesian linear velocity bounds of the distal link wrt the base.""" + return self._lin_vel_bounds + + @linear_velocity_bounds.setter + def linear_velocity_bounds(self, bounds): + """Set the cartesian linear velocity bounds of the distal link wrt the base.""" + if bounds is not None: + if isinstance(bounds, tuple): + if len(bounds) != 2: + raise ValueError("Expecting the bounds to be a tuple of length 2, but got a length of " + "{}".format(len(bounds))) + for bound in bounds: + if not isinstance(bound, np.ndarray): + raise TypeError("Expecting the given bound to be a np.array, but got instead: " + "{}".format(type(bound))) + if len(bound) != 3: + raise ValueError("Expecting the given bound to be of length 3, but instead got a length of " + "{}".format(len(bound))) + elif isinstance(bounds, np.ndarray): + if len(bounds) == 3: + bounds = (-bounds, bounds) + elif len(bounds) == 6: + bounds = (bounds[:3], bounds[3:]) + else: + raise TypeError("Expecting the given bounds to be a tuple of np.array, a np.array, or None, but " + "instead got: {}".format(type(bounds))) + self._lin_vel_bounds = bounds + + @property + def angular_velocity_bounds(self): + """Get the cartesian angular velocity bounds of the distal link wrt the base.""" + return self._ang_vel_bounds + + @angular_velocity_bounds.setter + def angular_velocity_bounds(self, bounds): + """Set the cartesian angular velocity bounds of the distal link wrt the base.""" + if bounds is not None: + if isinstance(bounds, tuple): + if len(bounds) != 2: + raise ValueError("Expecting the bounds to be a tuple of length 2, but got a length of " + "{}".format(len(bounds))) + for bound in bounds: + if not isinstance(bound, np.ndarray): + raise TypeError("Expecting the given bound to be a np.array, but got instead: " + "{}".format(type(bound))) + if len(bound) != 3: + raise ValueError("Expecting the given bound to be of length 3, but instead got a length of " + "{}".format(len(bound))) + elif isinstance(bounds, np.ndarray): + if len(bounds) == 3: + bounds = (-bounds, bounds) + elif len(bounds) == 6: + bounds = (bounds[:3], bounds[3:]) + else: + raise TypeError("Expecting the given bounds to be a tuple of np.array, a np.array, or None, but " + "instead got: {}".format(type(bounds))) + self._ang_vel_bounds = bounds + + ########### + # Methods # + ########### + + def _update(self): + """Update the inequality matrix and vectors.""" + if self.linear_velocity_bounds is None and self.angular_velocity_bounds is None: + raise ValueError("Expecting at least the linear or angular velocity bounds to be specified, but instead " + "got None for both of them.") + + self._A_ineq = self.model.get_jacobian(link=self.distal_link, wrt_link=self.base_link, + point=self.local_position) + + if self.linear_velocity_bounds is None: + self._b_lower_bound, self._b_upper_bound = self.angular_velocity_bounds + self._A_ineq = self._A_ineq[3:] + elif self.angular_velocity_bounds is None: + self._b_lower_bound, self._b_upper_bound = self.linear_velocity_bounds + self._A_ineq = self._A_ineq[:3] + else: + b_lin_low, b_lin_up = self.linear_velocity_bounds + b_ang_low, b_ang_up = self.angular_velocity_bounds + self._b_lower_bound = np.concatenate((b_lin_low, b_ang_low)) + self._b_upper_bound = np.concatenate((b_lin_up, b_ang_up)) diff --git a/pyrobolearn/priorities/constraints/velocity/joint_limits.py b/pyrobolearn/priorities/constraints/velocity/joint_limits.py index dadfa03..f392d2f 100644 --- a/pyrobolearn/priorities/constraints/velocity/joint_limits.py +++ b/pyrobolearn/priorities/constraints/velocity/joint_limits.py @@ -1,6 +1,18 @@ #!/usr/bin/env python r"""Provide the joint position limits constraint. +This provides bounds/limits on the joint positions, which are given by: + +.. math:: q_{lb} \leq q + \dot{q} dt \leq q_{ub} + +where :math:`(q_{lb}, q_{ub})` are the lower and upper bound of the joint positions respectively, :math:`q` are +the current joint positions, :math:`\dot{q}` are the joint velocities being optimized, and :math:`dt` is the +integration time step. + +This formulation can be rewritten as the inequality constraint math:`lb \leq x \leq ub` in QP, with +:math:`lb = (q_{lb} - q) / dt`, :math:`ub = (q_{ub} - q) / dt`, and :math:`x = \dot{q}`. This can +also be rewritten as :math:`Gx \leq h`, with :math:`G = [-dt*I, dt*I]^\top` and +:math:`h = [(q - q_{lb})^\top, (q_{ub} - q)^\top]^\top` where :math:`I` is the square identity matrix. The implementation of this class is inspired by [1] (which is licensed under the LGPLv2). @@ -34,31 +46,117 @@ class JointPositionLimitsConstraint(BoundConstraint, JointVelocityConstraint): the current joint positions, :math:`\dot{q}` are the joint velocities being optimized, and :math:`dt` is the integration time step. - This formulation can be rewritten as the inequality constraint :math:`Gx \leq h` used in QP, with - :math:`G = [-dt*I, dt*I]^\top` and :math:`h = [(q - q_{lb})^\top, (q_{ub} - q)^\top]^\top` where :math:`I` is the - square identity matrix. + This formulation can be rewritten as the inequality constraint math:`lb \leq x \leq ub` in QP, with + :math:`lb = (q_{lb} - q) / dt`, :math:`ub = (q_{ub} - q) / dt`, and :math:`x = \dot{q}`. This can + also be rewritten as :math:`Gx \leq h`, with :math:`G = [-dt*I, dt*I]^\top` and + :math:`h = [(q - q_{lb})^\top, (q_{ub} - q)^\top]^\top` where :math:`I` is the square identity matrix. + + The implementation of this class is inspired by [1] (which is licensed under the LGPLv2). + + References: + - [1] "OpenSoT: A whole-body control library for the compliant humanoid robot COMAN", Rocchi et al., 2015 """ - def __init__(self, model, dt): + def __init__(self, model, dt, q_lower_bound=None, q_upper_bound=None): r""" Initialize the constraint. Args: model (ModelInterface): model interface. dt (float): integration time step: use to compute :math:`q = q + \dot{q} dt`. + q_lower_bound (np.array[float[N]], None): joint position lower limits. If None, it will take the lower + joint limits specified in the model. Note that if the lower limits are equal to the upper limits, they + will be set to -10 and 10 by default. + q_upper_bound (np.array[float[N]], None): joint position upper limits. If None, it will take the upper + joint limits specified in the model. Note that if the upper limits are equal to the lower limits, they + will be set to -10 and 10 by default. """ super(JointPositionLimitsConstraint, self).__init__(model) + # set time self.dt = dt - bounds = self.model.get_joint_bounds() - self._lower_bound = bounds[0] - self._upper_bound = bounds[1] + # set variables + if q_lower_bound is None or q_upper_bound is None: + q_lb, q_ub = self.model.get_joint_limits() + if q_lower_bound is None: + q_lower_bound = q_lb + if q_upper_bound is None: + q_upper_bound = q_ub + if np.allclose(q_lower_bound, q_upper_bound): + print("WARNING: the joint position lower and upper limits are the same, by default they will be set " + "to -10 and 10.") + q_lower_bound = -10. * np.ones(len(q_lower_bound)) + q_upper_bound = 10. * np.ones(len(q_upper_bound)) - def update(self): + self.q_lower_bounds = q_lower_bound + self.q_upper_bounds = q_upper_bound + + # first update + self.update() + + ############## + # Properties # + ############## + + @property + def dt(self): + """Return the integration time step.""" + return self._dt + + @dt.setter + def dt(self, dt): + """Set the integration time step.""" + if not isinstance(dt, (float, int)): + raise TypeError("Expecting the integration time step `dt` to be a float or int.") + if dt <= 0: + raise ValueError("Expecting the integration time step `dt` to be bigger than 0.") + self._dt = float(dt) + + @property + def q_lower_bounds(self): + """Get the lower joint position limits.""" + return self._q_lb + + @q_lower_bounds.setter + def q_lower_bounds(self, q_lb): + """Set the lower joint position limits.""" + if q_lb is None: + q_lb = self.model.get_joint_limits()[0] + if not isinstance(q_lb, np.ndarray): + raise TypeError("Expecting the given lower joint position limits to be a np.array, instead got: " + "{}".format(q_lb)) + if len(q_lb) != self.x_size: + raise ValueError("Expecting the length of the lower joint position limits (={}) to be the same length as " + "the number of variables being optimized =({}).".format(len(q_lb), self.x_size)) + self._q_lb = q_lb + + @property + def q_upper_bounds(self): + """Get the upper joint position limits.""" + return self._q_ub + + @q_upper_bounds.setter + def q_upper_bounds(self, q_ub): + """Set the upper joint position limits.""" + if q_ub is None: + q_ub = self.model.get_joint_limits()[1] + if not isinstance(q_ub, np.ndarray): + raise TypeError("Expecting the given upper joint position limits to be a np.array, instead got: " + "{}".format(q_ub)) + if len(q_ub) != self.x_size: + raise ValueError("Expecting the length of the upper joint position limits (={}) to be the same length as " + "the number of variables being optimized =({}).".format(len(q_ub), self.x_size)) + self._q_ub = q_ub + + ########### + # Methods # + ########### + + def _update(self): r""" - Update the bounds. + Update the lower and upper bounds. """ q = self.model.get_joint_positions() - self.lower_bound = 0 - self.upper_bound = 0 + self._lower_bound = (self._q_lb - q) / self.dt + self._upper_bound = (self._q_ub - q) / self.dt diff --git a/pyrobolearn/priorities/constraints/velocity/joint_velocity.py b/pyrobolearn/priorities/constraints/velocity/joint_velocity.py index 1b7ee87..f295c52 100644 --- a/pyrobolearn/priorities/constraints/velocity/joint_velocity.py +++ b/pyrobolearn/priorities/constraints/velocity/joint_velocity.py @@ -1,6 +1,19 @@ #!/usr/bin/env python r"""Provide the differential kinematics constraint. +This provides the joint velocity constraints which is given by: + +.. math:: J(q) \dot{q} = v + +where :math:`J(q)` is the jacobian from a base link to a distal link, :math:`\dot{q}` are the joint velocities +being optimized, and :math:`v` is the imposed cartesian velocity imposed on the distal link. + +This formulation can be rewritten as the inequality constraint :math:`A_{eq} x = b_{eq}` used in QP, with +:math:`A_{eq} = J(q)`, :math:`x = \dot{q}`, and :math:`b_{eq} = v`. + + +The implementation of this class is inspired by [1] (which is licensed under the LGPLv2). + References: - [1] "OpenSoT: A whole-body control library for the compliant humanoid robot COMAN", Rocchi et al., 2015 """ @@ -32,9 +45,15 @@ class DifferentialKinematicsConstraint(EqualityConstraint, JointVelocityConstrai This formulation can be rewritten as the inequality constraint :math:`A_{eq} x = b_{eq}` used in QP, with :math:`A_{eq} = J(q)`, :math:`x = \dot{q}`, and :math:`b_{eq} = v`. + + The implementation of this class is inspired by [1] (which is licensed under the LGPLv2). + + References: + - [1] "OpenSoT: A whole-body control library for the compliant humanoid robot COMAN", Rocchi et al., 2015 """ - def __init__(self, model, distal_link, base_link=None, velocity=None): + def __init__(self, model, distal_link, base_link=None, local_position=(0., 0., 0.), linear_velocity=None, + angular_velocity=None): r""" Initialize the constraint. @@ -42,13 +61,82 @@ class DifferentialKinematicsConstraint(EqualityConstraint, JointVelocityConstrai model (ModelInterface): model interface. distal_link (int, str): distal link id or name. base_link (int, str, None): base link id or name. If None, it will be the world. - velocity (np.array[6], None): imposed velocity. If None, it will be set to 0. + local_position (np.array[float[3]]): local position on the distal link. + linear_velocity (np.array[float[3]], None): imposed linear velocity. If None, it will not be considered. + angular_velocity (np.array[float[3]], None): imposed angular velocity. If None, it will not be considered. """ super(DifferentialKinematicsConstraint, self).__init__(model) - raise NotImplementedError - def update(self): + # define variables + self.distal_link = self.model.get_link_id(distal_link) + self.base_link = self.model.get_link_id(base_link) if base_link is not None else base_link + self.local_position = local_position + + # set velocities + self.linear_velocity = linear_velocity + self.angular_velocity = angular_velocity + + # first update + self.update() + + ############## + # Properties # + ############## + + @property + def linear_velocity(self): + """Get the cartesian linear velocity of the distal link wrt the base.""" + return self._lin_vel + + @linear_velocity.setter + def linear_velocity(self, velocity): + """Set the cartesian linear velocity of the distal link wrt the base.""" + if velocity is not None: + if not isinstance(velocity, (np.ndarray, list, tuple)): + raise TypeError("Expecting the given linear velocity to be a np.array, instead got: " + "{}".format(type(velocity))) + velocity = np.asarray(velocity) + if len(velocity) != 3: + raise ValueError("Expecting the given linear velocity array to be of length 3, but instead " + "got: {}".format(len(velocity))) + self._lin_vel = velocity + + @property + def angular_velocity(self): + """Get the cartesian angular velocity of the distal link wrt the base.""" + return self._ang_vel + + @angular_velocity.setter + def angular_velocity(self, velocity): + """Set the cartesian angular velocity of the distal link wrt the base.""" + if velocity is not None: + if not isinstance(velocity, (np.ndarray, list, tuple)): + raise TypeError("Expecting the given angular velocity to be a np.array, instead got: " + "{}".format(type(velocity))) + velocity = np.asarray(velocity) + if len(velocity) != 3: + raise ValueError("Expecting the given angular velocity array to be of length 3, but instead " + "got: {}".format(len(velocity))) + self._ang_vel = velocity + + ########### + # Methods # + ########### + + def _update(self): r""" - Update the bounds. + Update the constraint by computing :math:`A_{eq}` and :math:`b_{eq}`. """ - raise NotImplementedError + if self._lin_vel is None and self._ang_vel is None: + raise ValueError("Expecting at least the linear or angular velocity to be specified, but none are " + "provided.") + self._A_eq = self.model.get_jacobian(link=self.distal_link, wrt_link=self.base_link, point=self.local_position) + + if self._lin_vel is None: + self._A_eq = self._A_eq[3:] + self._b_eq = self._ang_vel + elif self._ang_vel is None: + self._A_eq = self._A_eq[:3] + self._b_eq = self._lin_vel + else: + self._b_eq = np.concatenate((self._lin_vel, self._ang_vel)) diff --git a/pyrobolearn/priorities/constraints/velocity/velocity_limits.py b/pyrobolearn/priorities/constraints/velocity/velocity_limits.py index ea1d890..e7c62b9 100644 --- a/pyrobolearn/priorities/constraints/velocity/velocity_limits.py +++ b/pyrobolearn/priorities/constraints/velocity/velocity_limits.py @@ -1,6 +1,17 @@ #!/usr/bin/env python r"""Provide the velocity limits constraint. +This provides bounds/limits on the joint velocities + +.. math:: \dot{q}_{lb} \leq \dot{q} \leq \dot{q}_{ub} + +where :math:`(\dot{q}_{lb}, \dot{q}_{ub})` are the lower and upper bound on the joint velocities, and +:math:`\dot{q}` are the joint velocities being optimized. + +This formulation can be rewritten as the inequality constraint math:`lb \leq x \leq ub` in QP, with +:math:`lb = \dot{q}_{lb}`, :math:`ub = \dot{q}_{ub}`, and :math:`x = \dot{q}`. This can also be rewritten as +:math:`Gx \leq h`, with :math:`G = [-I, I]^\top` and :math:`h = [-\dot{q}_{lb}^\top, \dot{q}_{ub}^\top]^\top` +where :math:`I` is the square identity matrix. The implementation of this class is inspired by [1] (which is licensed under the LGPLv2). @@ -26,30 +37,105 @@ __status__ = "Development" class JointVelocityLimitsConstraint(BoundConstraint, JointVelocityConstraint): r"""Joint velocity limits constraint. - This provides bounds/limits on the joint velocities + This provides bounds/limits on the joint velocities: .. math:: \dot{q}_{lb} \leq \dot{q} \leq \dot{q}_{ub} where :math:`(\dot{q}_{lb}, \dot{q}_{ub})` are the lower and upper bound on the joint velocities, and :math:`\dot{q}` are the joint velocities being optimized. - This formulation can be rewritten as the inequality constraint :math:`Gx \leq h` used in QP, with - :math:`G = [-I, I]^\top` and :math:`h = [-q_{lb}^\top, q_{ub}^\top]^\top` where :math:`I` is the square identity - matrix. + This formulation can be rewritten as the inequality constraint math:`lb \leq x \leq ub` in QP, with + :math:`lb = \dot{q}_{lb}`, :math:`ub = \dot{q}_{ub}`, and :math:`x = \dot{q}`. This can also be rewritten as + :math:`Gx \leq h`, with :math:`G = [-I, I]^\top` and :math:`h = [-\dot{q}_{lb}^\top, \dot{q}_{ub}^\top]^\top` + where :math:`I` is the square identity matrix. + + The implementation of this class is inspired by [1] (which is licensed under the LGPLv2). + + References: + - [1] "OpenSoT: A whole-body control library for the compliant humanoid robot COMAN", Rocchi et al., 2015 """ - def __init__(self, model): + def __init__(self, model, dq_lower_bound=None, dq_upper_bound=None): """ Initialize the constraint. Args: model (ModelInterface): model interface. + dq_lower_bound (np.array[float[N]], None): joint velocity lower limits. If None, it will take the lower + joint limits specified in the model. Note that if the lower limits are equal to the upper limits, they + will be set to -10 and 10 by default. + dq_upper_bound (np.array[float[N]], None): joint velocity upper limits. If None, it will take the upper + joint limits specified in the model. Note that if the upper limits are equal to the lower limits, they + will be set to -10 and 10 by default. """ super(JointVelocityLimitsConstraint, self).__init__(model) - bounds = self.model.get_joint_velocity_bounds() - self.lower_bound = bounds[0] - self.upper_bound = bounds[1] + # set variables + if dq_lower_bound is None or dq_upper_bound is None: + dq_lb, dq_ub = self.model.get_joint_velocity_limits() + if dq_lower_bound is None: + dq_lower_bound = dq_lb + if dq_upper_bound is None: + dq_upper_bound = dq_ub + if np.allclose(dq_lower_bound, dq_upper_bound): + print("WARNING: the joint velocity lower and upper limits are the same, by default they will be set " + "to -10 and 10.") + dq_lower_bound = -10. * np.ones(len(dq_lower_bound)) + dq_upper_bound = 10. * np.ones(len(dq_upper_bound)) - def update(self): - pass + self.dq_lower_bounds = dq_lower_bound + self.dq_upper_bounds = dq_upper_bound + + # first update + self.update() + + ############## + # Properties # + ############## + + @property + def dq_lower_bounds(self): + """Get the lower joint velocity limits.""" + return self._dq_lb + + @dq_lower_bounds.setter + def dq_lower_bounds(self, dq_lb): + """Set the lower joint velocity limits.""" + if dq_lb is None: + dq_lb = self.model.get_joint_limits()[0] + if not isinstance(dq_lb, np.ndarray): + raise TypeError("Expecting the given lower joint velocity limits to be a np.array, instead got: " + "{}".format(dq_lb)) + if len(dq_lb) != self.x_size: + raise ValueError("Expecting the length of the lower joint velocity limits (={}) to be the same length as " + "the number of variables being optimized =({}).".format(len(dq_lb), self.x_size)) + self._dq_lb = dq_lb + + @property + def dq_upper_bounds(self): + """Get the upper joint velocity limits.""" + return self._dq_ub + + @dq_upper_bounds.setter + def dq_upper_bounds(self, dq_ub): + """Set the upper joint velocity limits.""" + if dq_ub is None: + dq_ub = self.model.get_joint_limits()[1] + if not isinstance(dq_ub, np.ndarray): + raise TypeError("Expecting the given upper joint velocity limits to be a np.array, instead got: " + "{}".format(dq_ub)) + if len(dq_ub) != self.x_size: + raise ValueError("Expecting the length of the upper joint velocity limits (={}) to be the same length as " + "the number of variables being optimized =({}).".format(len(dq_ub), self.x_size)) + self._dq_ub = dq_ub + + ########### + # Methods # + ########### + + def _update(self): + r""" + Update the lower and upper bounds. + """ + self._lower_bound = self._dq_lb + self._upper_bound = self._dq_ub diff --git a/pyrobolearn/priorities/models/model.py b/pyrobolearn/priorities/models/model.py index c3e5f61..689e61f 100644 --- a/pyrobolearn/priorities/models/model.py +++ b/pyrobolearn/priorities/models/model.py @@ -119,6 +119,26 @@ class ModelInterface(object): """ pass + def get_joint_limits(self): + r""" + Return the joint limits. + + Returns: + np.array[float[N]]: lower joint position limits. + np.array[float[N]]: upper joint position limits. + """ + pass + + def get_joint_velocity_limits(self): + r""" + Return the joint velocity limits. + + Returns: + np.array[float[N]]: lower joint velocity limits. + np.array[float[N]]: upper joint velocity limits. + """ + pass + def get_joint_positions(self): """ Get the joint positions. @@ -239,18 +259,49 @@ class ModelInterface(object): """ pass - def get_pose(self, link): + def get_pose(self, link, wrt_link=None, point=(0., 0., 0.)): """ - Return the pose of the specified link. + Return the pose of the specified link with respect to another link. Args: link (int, str): unique link id, or name. + wrt_link (int, str, None): the other link id, or name. If None, returns the pose wrt to the world, and + if -1 wrt to the base. + point (np.array[float[3]]): position of the point in link's local frame. Returns: np.array[float[7]]: pose (position and quaternion expressed as [x,y,z,w]) """ pass + def get_position(self, link, wrt_link=None): + """ + Return the position of the specified link with respect to another link. + + Args: + link (int, str): unique link id, or name. + wrt_link (int, str, None): the other link id, or name. If None, returns the position wrt to the world, + and if -1 wrt to the base. + + Returns: + np.array[float[3]]: position + """ + pass + + def get_orientation(self, link, wrt_link=None): + """ + Return the orientation of the specified link with respect to another link. + + Args: + link (int, str): unique link id, or name. + wrt_link (int, str, None): the other link id, or name. If None, returns the orientation wrt to the world, + and if -1 wrt to the base. + + Returns: + np.array[float[4]]: orientation (expressed as a quaternion [x,y,z,w]) + """ + pass + def get_velocity_twist(self, link): r""" Compute the angular and linear velocity of a link, given by :math:`v = [\omega, \dot{p}]`. diff --git a/pyrobolearn/priorities/models/robot_model.py b/pyrobolearn/priorities/models/robot_model.py index 6b819d6..9ae00a8 100644 --- a/pyrobolearn/priorities/models/robot_model.py +++ b/pyrobolearn/priorities/models/robot_model.py @@ -68,12 +68,12 @@ class RobotModelInterface(ModelInterface): - If fixed base, this is equal to the number of actuated joints. - If floating base, this is equal to the number of actuated joints + 6 DoFs for the base. """ - return self.robot.num_dofs + return self.model.num_dofs @property def num_actuated_joints(self): """Return the number of actuated joints.""" - return self.robot.num_actuated_joints + return self.model.num_actuated_joints ########### # Methods # @@ -120,6 +120,27 @@ class RobotModelInterface(ModelInterface): """ return -1 + def get_joint_limits(self): + r""" + Return the joint limits. + + Returns: + np.array[float[N]]: lower joint position limits. + np.array[float[N]]: upper joint position limits. + """ + return self.model.get_joint_limits() + + def get_joint_velocity_limits(self): + r""" + Return the joint velocity limits. + + Returns: + np.array[float[N]]: lower joint velocity limits. + np.array[float[N]]: upper joint velocity limits. + """ + dq = self.model.get_joint_max_velocities() + return -dq, dq + def get_joint_positions(self): """ Get the joint positions. @@ -127,7 +148,7 @@ class RobotModelInterface(ModelInterface): Returns: np.array[float[N]]: the joint positions. """ - return self.robot.get_joint_positions() + return self.model.get_joint_positions() def get_joint_velocities(self): """ @@ -136,7 +157,7 @@ class RobotModelInterface(ModelInterface): Returns: np.array[float[N]]: the joint positions. """ - return self.robot.get_joint_velocities() + return self.model.get_joint_velocities() def get_joint_accelerations(self): """ @@ -145,7 +166,7 @@ class RobotModelInterface(ModelInterface): Returns: np.array[float[N]]: the joint positions. """ - return self.robot.get_joint_accelerations() + return self.model.get_joint_accelerations() def get_com_position(self): """ @@ -266,11 +287,11 @@ class RobotModelInterface(ModelInterface): def get_pose(self, link, wrt_link=None, point=(0., 0., 0.)): # TODO: use point """ - Return the pose of the specified link. + Return the pose of the specified link with respect to another link. Args: link (int, str): unique link id, or name. - wrt_link (int, str, None): the other link id, or name. If None, returns the position wrt to the world, and + wrt_link (int, str, None): the other link id, or name. If None, returns the pose wrt to the world, and if -1 wrt to the base. point (np.array[float[3]]): position of the point in link's local frame. @@ -282,6 +303,40 @@ class RobotModelInterface(ModelInterface): return self.model.get_link_world_poses(link) return self.model.get_link_poses(link, self.get_link_id(wrt_link)) + def get_position(self, link, wrt_link=None): # TODO: use point + """ + Return the position of the specified link with respect to another link. + + Args: + link (int, str): unique link id, or name. + wrt_link (int, str, None): the other link id, or name. If None, returns the position wrt to the world, + and if -1 wrt to the base. + + Returns: + np.array[float[3]]: position + """ + link = self.get_link_id(link) + if wrt_link is None: + return self.model.get_link_world_positions(link) + return self.model.get_link_positions(link, wrt_link_id=self.get_link_id(wrt_link)) + + def get_orientation(self, link, wrt_link=None): # TODO: use point + """ + Return the orientation of the specified link with respect to another link. + + Args: + link (int, str): unique link id, or name. + wrt_link (int, str, None): the other link id, or name. If None, returns the orientation wrt to the world, + and if -1 wrt to the base. + + Returns: + np.array[float[4]]: orientation (expressed as a quaternion [x,y,z,w]) + """ + link = self.get_link_id(link) + if wrt_link is None: + return self.model.get_link_world_orientations(link) + return self.model.get_link_orientations(link, wrt_link_id=self.get_link_id(wrt_link)) + def get_velocity_twist(self, link, point=(0., 0., 0.)): # TODO: use point r""" Compute the angular and linear velocity of a link, given by :math:`v = [\omega, \dot{p}]`. @@ -417,6 +472,17 @@ class RobotModelInterface(ModelInterface): # return \dot{J}(q) \dot{q} = \dot{v} - J(q) \ddot{q} return acc - jacobian.dot(ddq) + def compute_com_JdotQdot(self): + r""" + Compute :math:`\dot{J}_{CoM}(q) \dot{q}` from the centroidal momentum matrix. + + Returns: + np.array[float[6]]: the matrix multiplication of the first derivative of the CoM Jacobian with the joint + velocities. + """ + A_G, dA_G_dq = self.get_centroidal_dynamics() + return dA_G_dq / self.get_mass() + def compute_relative_JdotQdot(self, target_link, base_link): r""" Compute the relative :math:`\dot{J}(q) \dot{q}`, which appears in @@ -496,6 +562,17 @@ class RobotModelInterface(ModelInterface): """ return self.model.get_centroidal_momentum_matrix() + def get_centroidal_dynamics(self): + r""" + Return the centroidal momentum matrix :math:`A_G` and its derivative multiplied by the joint velocities + :math:`\dot{A}_G \dot{q}`. + + Returns: + np.array[float[6,6+N]]: the centroidal momentum matrix :math:`A_G` + np.array[float[6]]: the centroidal dynamics velocity-dependent bias vector :math:`\dot{A}_G \dot{q}` + """ + return self.model.get_centroidal_dynamics() + def update(self, q=None, dq=None, ddq=None, update_model=False): """Update: move to the next step.""" self._states = dict() diff --git a/pyrobolearn/priorities/solvers/ipopt_task_solver.py b/pyrobolearn/priorities/solvers/ipopt_task_solver.py new file mode 100644 index 0000000..e69de29 diff --git a/pyrobolearn/priorities/solvers/nlp_task_solver.py b/pyrobolearn/priorities/solvers/nlp_task_solver.py index 5a5e1f5..5642dac 100644 --- a/pyrobolearn/priorities/solvers/nlp_task_solver.py +++ b/pyrobolearn/priorities/solvers/nlp_task_solver.py @@ -55,10 +55,4 @@ class NLPTaskSolver(TaskSolver): def solve(self): """Solve the priority task.""" - if self.task.tasks: - for soft_task in self.task.tasks: - As = np.vstack([np.dot(np.sqrt(task.weight), task.A) for task in soft_task]) - bs = np.vstack([np.dot(np.sqrt(task.weight), task.b) for task in soft_task]) - # x = self.solver.optimize(P=As.T.dot(As), q=-bs.T.dot(), G=, h=, A=, b=) - else: - pass + pass diff --git a/pyrobolearn/priorities/solvers/scipy_task_solver.py b/pyrobolearn/priorities/solvers/scipy_task_solver.py new file mode 100644 index 0000000..e69de29 diff --git a/pyrobolearn/priorities/tasks/acceleration/cartesian.py b/pyrobolearn/priorities/tasks/acceleration/cartesian.py index 93433b2..dd9b752 100644 --- a/pyrobolearn/priorities/tasks/acceleration/cartesian.py +++ b/pyrobolearn/priorities/tasks/acceleration/cartesian.py @@ -1,6 +1,58 @@ #!/usr/bin/env python r"""Provide the Cartesian acceleration task. +The Cartesian acceleration task tries to impose a desired pose, velocity and acceleration profiles for a distal +link with respect to a base link, or world frame. + +Before presenting the optimization problem, here is a small reminder. The acceleration is the time derivative of +the velocity, i.e. :math:`a = \frac{dv}{dt}` where the cartesian velocities are related to joint velocities by +:math:`v = J(q) \dot{q}` where :math:`J(q)` is the Jacobian, thus deriving that expression wrt time gives us: + +.. math:: a = \frac{d}{dt} v = \frac{d}{dt} J(q) \dot{q} = J(q) \ddot{q} + \dot{J}(q) \dot{q}. + +Now, we can formulate our minimization problem as: + +.. math:: || J(q) \ddot{q} + \dot{J} \dot{q} - (a_d + K_d (v_d - v) + K_p e) ||^2, + +where :math:`\ddot{q}` are the joint accelerations being optimized, :math:`a_d` are the desired cartesian +accelerations, :math:`v_d = [v_d^\top, \omega_d^\top]^\top` are the desired cartesian velocities, :math:`v` are the +current cartesian velocities of the distal link wrt the base, :math:`J(q) \in \mathbb{R}^{6 \times N}` is the +Jacobian taken from the base to the distal link, :math:`K_p` and :math:`K_d` are the stiffness and damping gains +respectively, :math:`e \in \mathbb{R}^{6}` is the error which is the concatenation of the position error given by +:math:`e_{p} = (x_d - x)` (with :math:`x_d` being the desired position, and :math:`x` the current position), and the +orientation error given by (if expressed as quaternions :math:`o = {s, v}` where :math:`s` is the real scalar part, +and :math:`v` is the vector part) :math:`e_{o} = s v_d - s_d v - v_d \cross v`, and :math:`\dot{x}_d` is the +desired cartesian velocity for the distal link with respect to the base link. + + +The above formulation is equivalent to the QP objective function :math:`||Ax - b||^2`, by setting +:math:`A = J(q)`, :math:`x = \ddot{q}`, and :math:`b = - \dot{J} \dot{q} + (a_d + K_d (v_d - v) + K_p e)`. + +This task can, for instance, be used for foot pose tracking when this one is not in contact with the ground. If +the foot is in contact, we switch to a foot damping task which can be achieved by setting +:math:`a_d = v_d = e = 0` and thus we are trying to solve :math:`||J(q) \ddot{q} - \dot{J} \dot{q} - K_d v_d||^2`. + + +Inverse dynamics +---------------- + +Once the optimal joint accelerations :math:`\ddot{q}^*` have been computed, we can use inverse dynamics to +compute the corresponding torques to apply on the joints. This is given by: + +.. math:: \tau = H(q) \ddot{q} + N(q,\dot{q)} + +where :math:`H(q)` is the inertia joint matrix, and N(q, \dot{q}) is a vector force that accounts for all the +other non-linear forces acting on the system (Coriolis, centrifugal, gravity, external forces, friction, etc.). + + +Important notes: + +- You don't have to specify the whole pose, you can also only specify the position or orientation. +- You can also only specify the desired cartesian accelerations by setting `kp` and `kd` to zero; you don't have + neither to provide the desired cartesian velocities, position or orientation. + + +.. seealso:: `tasks/velocity/cartesian.py` and `tasks/torque/cartesian_impedance_control.py` The implementation of this class is inspired by [1] (which is licensed under the LGPLv2). @@ -8,11 +60,10 @@ References: - [1] "OpenSoT: A whole-body control library for the compliant humanoid robot COMAN", Rocchi et al., 2015 """ -# TODO: finish to implement this - import numpy as np from pyrobolearn.priorities.tasks import JointAccelerationTask +from pyrobolearn.utils.transformation import quaternion_error __author__ = "Brian Delhaisse" @@ -31,22 +82,29 @@ class CartesianAccelerationTask(JointAccelerationTask): The Cartesian acceleration task tries to impose a desired pose, velocity and acceleration profiles for a distal link with respect to a base link, or world frame. - Before presenting the optimization problem, a small reminder. The acceleration is the time derivative of the - velocity, i.e. :math:`a = \frac{dv}{dt}` where the cartesian velocities are related to joint velocities by + Before presenting the optimization problem, here is a small reminder. The acceleration is the time derivative of + the velocity, i.e. :math:`a = \frac{dv}{dt}` where the cartesian velocities are related to joint velocities by :math:`v = J(q) \dot{q}` where :math:`J(q)` is the Jacobian, thus deriving that expression wrt time gives us: .. math:: a = \frac{d}{dt} v = \frac{d}{dt} J(q) \dot{q} = J(q) \ddot{q} + \dot{J}(q) \dot{q}. Now, we can formulate our minimization problem as: - .. math:: || J(q) \ddot{q} - \dot{J} \dot{q} - (a_d + K_d (v_d - v) + K_p e) ||^2, + .. math:: || J(q) \ddot{q} + \dot{J} \dot{q} - (a_d + K_d (v_d - v) + K_p e) ||^2, - where :math:`\ddot{q}` are the joint accelerations being optimized, :math:`a_d` is the desired cartesian - acceleration, :math:`v_d = [\omega_d^\top, v` is the desired cartesian velocity, ... + where :math:`\ddot{q}` are the joint accelerations being optimized, :math:`a_d` are the desired cartesian + accelerations, :math:`v_d = [v_d^\top, \omega_d^\top]^\top` are the desired cartesian velocities, :math:`v` are the + current cartesian velocities of the distal link wrt the base, :math:`J(q) \in \mathbb{R}^{6 \times N}` is the + Jacobian taken from the base to the distal link, :math:`K_p` and :math:`K_d` are the stiffness and damping gains + respectively, :math:`e \in \mathbb{R}^{6}` is the error which is the concatenation of the position error given by + :math:`e_{p} = (x_d - x)` (with :math:`x_d` being the desired pose, and :math:`x` the current pose), and the + orientation error given by (if expressed as quaternions :math:`o = {s, v}` where :math:`s` is the real scalar part, + and :math:`v` is the vector part) :math:`e_{o} = s v_d - s_d v - v_d \cross v`, and :math:`\dot{x}_d` is the + desired cartesian velocity for the distal link with respect to the base link. The above formulation is equivalent to the QP objective function :math:`||Ax - b||^2`, by setting - :math:`A = J(q)`, :math:`x = \ddot{q}`, and :math:`b = - \dot{J} \dot{q} - (a_d + K_d (v_d - v) + K_p e)`. + :math:`A = J(q)`, :math:`x = \ddot{q}`, and :math:`b = - \dot{J} \dot{q} + (a_d + K_d (v_d - v) + K_p e)`. This task can, for instance, be used for foot pose tracking when this one is not in contact with the ground. If the foot is in contact, we switch to a foot damping task which can be achieved by setting @@ -62,14 +120,21 @@ class CartesianAccelerationTask(JointAccelerationTask): .. math:: \tau = H(q) \ddot{q} + N(q,\dot{q)} where :math:`H(q)` is the inertia joint matrix, and N(q, \dot{q}) is a vector force that accounts for all the - other forces acting on the system (Coriolis, centrifugal, gravity, external forces, friction, etc.). - + other non-linear forces acting on the system (Coriolis, centrifugal, gravity, external forces, friction, etc.). .. seealso:: `tasks/velocity/cartesian.py` and `tasks/torque/cartesian_impedance_control.py` + + + The implementation of this class is inspired by [1] (which is licensed under the LGPLv2). + + References: + - [1] "OpenSoT: A whole-body control library for the compliant humanoid robot COMAN", Rocchi et al., 2015 """ - def __init__(self, model, distal_link, base_link=None, local_position=(0, 0, 0), x_desired=None, - dx_desired=None, kp=1., kd=1., weight=1., constraints=[]): + def __init__(self, model, distal_link, base_link=None, local_position=(0, 0, 0), desired_position=None, + desired_orientation=None, desired_linear_velocity=None, desired_angular_velocity=None, + desired_linear_acceleration=None, desired_angular_acceleration=None, + kp_position=1., kp_orientation=1., kd_linear=1., kd_angular=1., weight=1., constraints=[]): """ Initialize the task. @@ -77,26 +142,393 @@ class CartesianAccelerationTask(JointAccelerationTask): model (ModelInterface): model interface. distal_link (int, str): distal link id or name. base_link (int, str, None): base link id or name. If None, it will be the world. - local_position (np.array[3]): local position on the distal link. - x_desired (np.array[7], None): desired cartesian pose of distal link wrt the base. - dx_desired (np.array[6], None): desired cartesian velocity of distal link wrt the base. - kp (float, np.array[6,6]): stiffness gain. - weight (float, np.array[6,6]): weight scalar or matrix associated to the task. - constraints (list of Constraint): list of constraints associated with the task. + local_position (np.array[float[3]]): local position on the distal link. + desired_position (np.array[float[3]], None): desired position of distal link wrt the base. If None, it + will not be taken into account. + desired_orientation (np.array[float[4]], None): desired orientation (expressed as quaternion [x,y,z,w]) of + distal link wrt the base. If None, it will not be taken into account. + desired_linear_velocity (np.array[float[3]], None): desired linear velocity of distal link wrt the base. + If None, it will be set to zero. + desired_angular_velocity (np.array[float[3]], None): desired angular velocity of distal link wrt the base. + If None, it will be set to zero. + desired_linear_acceleration (np.array[float[3]], None): desired linear acceleration of distal link wrt + the base. If None, it will be set to zero. + desired_angular_acceleration (np.array[float[3]], None): desired angular acceleration of distal link wrt + the base. If None, it will be set to zero. + kp_position (float, np.array[float[3,3]]): position stiffness gain. + kp_orientation (float, np.array[float[3,3]]): orientation stiffness gain. + kd_linear (float, np.array[float[3,3]]): linear velocity damping gain. + kd_angular (float, np.array[float[3,3]]): angular velocity damping gain. + weight (float, np.array[float[6,6]], np.array[float[3,3]]): weight scalar or matrix associated to the task. + constraints (list[Constraint]): list of constraints associated with the task. """ super(CartesianAccelerationTask, self).__init__(model=model, weight=weight, constraints=constraints) - # set variables - self.distal_link = distal_link - self.base_link = base_link + # define variables + self.distal_link = self.model.get_link_id(distal_link) + self.base_link = self.model.get_link_id(base_link) if base_link is not None else base_link + self.local_position = local_position - # pose = (position, quaternion) - self.desired_pose = np.array([0]*6 + 1) - self.current_pose = np.array([0]*6 + 1) + if base_link is not None: + raise NotImplementedError("Currently, the base_link can only be set to the world (None).") - # velocity = (angular, linear) - self.desired_velocity = np.zeros(6) - self.current_velocity = np.zeros(6) + # gains + self.kp_position = kp_position + self.kp_orientation = kp_orientation + self.kd_linear = kd_linear + self.kd_angular = kd_angular - # acceleration = (angular, linear) - self.desired_acceleration = np.zeros(6) + # define desired references + self.desired_position = desired_position + self.desired_orientation = desired_orientation + self.desired_linear_velocity = desired_linear_velocity + self.desired_angular_velocity = desired_angular_velocity + self.desired_linear_acceleration = desired_linear_acceleration + self.desired_angular_acceleration = desired_angular_acceleration + + # first update + self.update() + + ############## + # Properties # + ############## + + @property + def desired_position(self): + """Get the desired cartesian position for the distal link wrt the base.""" + return self._des_pos + + @desired_position.setter + def desired_position(self, position): + """Set the desired cartesian position for the distal link wrt the base.""" + if position is not None: + if not isinstance(position, (np.ndarray, list, tuple)): + raise TypeError("Expecting the given desired position to be a np.array, instead got: " + "{}".format(type(position))) + position = np.asarray(position) + if len(position) != 3: + raise ValueError("Expecting the given desired position array to be of length 3, but instead got: " + "{}".format(len(position))) + self._des_pos = position + + @property + def desired_orientation(self): + """Get the desired cartesian orientation (expressed as a quaternion [x,y,z,w]) for the distal link wrt the + base.""" + return self._des_quat + + @desired_orientation.setter + def desired_orientation(self, orientation): + """Set the desired cartesian orientation (expressed as a quaternion [x,y,z,w]) for the distal link wrt the + base.""" + if orientation is not None: + if not isinstance(orientation, (np.ndarray, list, tuple)): + raise TypeError("Expecting the given desired orientation to be a np.array, instead got: " + "{}".format(type(orientation))) + orientation = np.asarray(orientation) + if len(orientation) != 4: + raise ValueError( + "Expecting the given desired orientation array to be of length 4, but instead got: " + "{}".format(len(orientation))) + self._des_quat = orientation + + @property + def desired_linear_velocity(self): + """Get the desired cartesian linear velocity of the distal link wrt the base.""" + return self._des_lin_vel + + @desired_linear_velocity.setter + def desired_linear_velocity(self, velocity): + """Set the desired cartesian linear velocity of the distal link wrt the base.""" + if velocity is None: + velocity = np.zeros(3) + elif not isinstance(velocity, (np.ndarray, list, tuple)): + raise TypeError("Expecting the given desired linear velocity to be a np.array, instead got: " + "{}".format(type(velocity))) + velocity = np.asarray(velocity) + if len(velocity) != 3: + raise ValueError("Expecting the given desired linear velocity array to be of length 3, but instead " + "got: {}".format(len(velocity))) + self._des_lin_vel = velocity + + @property + def desired_angular_velocity(self): + """Get the desired cartesian angular velocity of the distal link wrt the base.""" + return self._des_ang_vel + + @desired_angular_velocity.setter + def desired_angular_velocity(self, velocity): + """Set the desired cartesian angular velocity of the distal link wrt the base.""" + if velocity is None: + velocity = np.zeros(3) + elif not isinstance(velocity, (np.ndarray, list, tuple)): + raise TypeError("Expecting the given desired angular velocity to be a np.array, instead got: " + "{}".format(type(velocity))) + velocity = np.asarray(velocity) + if len(velocity) != 3: + raise ValueError("Expecting the given desired angular velocity array to be of length 3, but instead " + "got: {}".format(len(velocity))) + self._des_ang_vel = velocity + + @property + def desired_velocity(self): + """Return the linear and angular velocity.""" + return np.concatenate((self._des_lin_vel, self._des_ang_vel)) + + @property + def desired_linear_acceleration(self): + """Get the desired cartesian linear acceleration of the distal link wrt the base.""" + return self._des_lin_acc + + @desired_linear_acceleration.setter + def desired_linear_acceleration(self, acceleration): + """Set the desired cartesian linear acceleration of the distal link wrt the base.""" + if acceleration is None: + acceleration = np.zeros(3) + elif not isinstance(acceleration, (np.ndarray, list, tuple)): + raise TypeError("Expecting the given desired linear acceleration to be a np.array, instead got: " + "{}".format(type(acceleration))) + acceleration = np.asarray(acceleration) + if len(acceleration) != 3: + raise ValueError("Expecting the given desired linear acceleration array to be of length 3, but instead " + "got: {}".format(len(acceleration))) + self._des_lin_acc = acceleration + + @property + def desired_angular_acceleration(self): + """Get the desired cartesian angular acceleration of the distal link wrt the base.""" + return self._des_ang_acc + + @desired_angular_acceleration.setter + def desired_angular_acceleration(self, acceleration): + """Set the desired cartesian angular acceleration of the distal link wrt the base.""" + if acceleration is None: + acceleration = np.zeros(3) + elif not isinstance(acceleration, (np.ndarray, list, tuple)): + raise TypeError("Expecting the given desired angular acceleration to be a np.array, instead got: " + "{}".format(type(acceleration))) + acceleration = np.asarray(acceleration) + if len(acceleration) != 3: + raise ValueError("Expecting the given desired angular acceleration array to be of length 3, but instead " + "got: {}".format(len(acceleration))) + self._des_ang_acc = acceleration + + @property + def desired_acceleration(self): + """Return the linear and angular acceleration.""" + return np.concatenate((self._des_lin_acc, self._des_ang_acc)) + + @property + def x_desired(self): + """Get the desired cartesian pose for the distal link wrt to the base.""" + position = self.desired_position + orientation = self.desired_orientation + if position is not None: + if orientation is not None: + return np.concatenate((position, orientation)) + return position + return orientation + + @x_desired.setter + def x_desired(self, x_d): + """Set the desired cartesian pose for the distal link wrt to the base.""" + if x_d is not None: + if not isinstance(x_d, (np.ndarray, list, tuple)): + raise TypeError( + "Expecting the given desired pose to be a np.array, instead got: {}".format(type(x_d))) + x_d = np.asarray(x_d) + if len(x_d) == 3: # only position is provided + x_d = np.concatenate((x_d, np.array([0., 0., 0., 1.]))) + elif len(x_d) == 4: # only orientation is provided + x_d = np.concatenate((np.zeros(3), x_d)) + if len(x_d) != 7: + raise ValueError("Expecting the given desired pose array to be of length 7 (3 for the position, " + "and 4 for the orientation expressed as a quaternion [x,y,z,w]), instead got a " + "length of: {}".format(len(x_d))) + self._des_pos = x_d[:3] + self._des_quat = x_d[3:] + + @property + def dx_desired(self): + """Get the desired cartesian velocity for the distal link wrt to the base.""" + return np.concatenate((self._des_lin_vel, self._des_ang_vel)) + + @dx_desired.setter + def dx_desired(self, dx_d): + """Set the desired cartesian velocity for the distal link wrt to the base.""" + if dx_d is not None: + if not isinstance(dx_d, (np.ndarray, list, tuple)): + raise TypeError("Expecting the given desired velocity to be a np.array, instead got: " + "{}".format(type(dx_d))) + dx_d = np.asarray(dx_d) + if len(dx_d) == 3: # assume that it is the linear velocity + dx_d = np.concatenate((dx_d, np.zeros(3))) + if len(dx_d) != 6: + raise ValueError("Expecting the given desired velocity array to be of length 6 (3 for the linear " + "and 3 for the angular part), instead got a length of: {}".format(len(dx_d))) + self._des_lin_vel = dx_d[:3] + self._des_ang_vel = dx_d[3:] + + @property + def ddx_desired(self): + """Get the desired cartesian acceleration for the distal link wrt to the base.""" + return np.concatenate((self._des_lin_acc, self._des_ang_acc)) + + @ddx_desired.setter + def ddx_desired(self, ddx_d): + """Set the desired cartesian acceleration for the distal link wrt to the base.""" + if ddx_d is not None: + if not isinstance(ddx_d, (np.ndarray, list, tuple)): + raise TypeError("Expecting the given desired acceleration to be a np.array, instead got: " + "{}".format(type(ddx_d))) + ddx_d = np.asarray(ddx_d) + if len(ddx_d) == 3: # assume that it is the linear acceleration + ddx_d = np.concatenate((ddx_d, np.zeros(3))) + if len(ddx_d) != 6: + raise ValueError("Expecting the given desired acceleration array to be of length 6 (3 for the linear " + "and 3 for the angular part), instead got a length of: {}".format(len(ddx_d))) + self._des_lin_acc = ddx_d[:3] + self._des_ang_acc = ddx_d[3:] + + @property + def kp_position(self): + """Return the position stiffness gain.""" + return self._kp_pos + + @kp_position.setter + def kp_position(self, kp): + """Set the position stiffness gain.""" + if kp is None: + kp = 1. + if not isinstance(kp, (float, int, np.ndarray)): + raise TypeError("Expecting the given position stiffness gain kp to be an int, float, np.array, instead " + "got: {}".format(type(kp))) + if isinstance(kp, np.ndarray) and kp.shape != (3, 3): + raise ValueError("Expecting the given position stiffness gain matrix kp to be of shape {}, but instead " + "got shape: {}".format((3, 3), kp.shape)) + self._kp_pos = kp + + @property + def kp_orientation(self): + """Return the orientation stiffness gain.""" + return self._kp_quat + + @kp_orientation.setter + def kp_orientation(self, kp): + """Set the orientation stiffness gain.""" + if kp is None: + kp = 1. + if not isinstance(kp, (float, int, np.ndarray)): + raise TypeError("Expecting the given orientation stiffness gain kp to be an int, float, np.array, " + "instead got: {}".format(type(kp))) + if isinstance(kp, np.ndarray) and kp.shape != (3, 3): + raise ValueError("Expecting the given orientation stiffness gain matrix kp to be of shape {}, but " + "instead got shape: {}".format((3, 3), kp.shape)) + self._kp_quat = kp + + @property + def kd_linear(self): + """Return the linear velocity damping gain.""" + return self._kd_lin + + @kd_linear.setter + def kd_linear(self, kd): + """Set the linear velocity damping gain.""" + if kd is None: + kd = 1. + if not isinstance(kd, (float, int, np.ndarray)): + raise TypeError("Expecting the given linear velocity damping gain kd to be an int, float, np.array, " + "instead got: {}".format(type(kd))) + if isinstance(kd, np.ndarray) and kd.shape != (3, 3): + raise ValueError("Expecting the given linear velocity damping gain matrix kd to be of shape {}, but " + "instead got shape: {}".format((3, 3), kd.shape)) + self._kd_lin = kd + + @property + def kd_angular(self): + """Return the angular velocity damping gain.""" + return self._kd_ang + + @kd_angular.setter + def kd_angular(self, kd): + """Set the angular velocity damping gain.""" + if kd is None: + kd = 1. + if not isinstance(kd, (float, int, np.ndarray)): + raise TypeError("Expecting the given angular velocity damping gain kd to be an int, float, np.array, " + "instead got: {}".format(type(kd))) + if isinstance(kd, np.ndarray) and kd.shape != (3, 3): + raise ValueError("Expecting the given angular velocity damping gain matrix kd to be of shape {}, but " + "instead got shape: {}".format((3, 3), kd.shape)) + self._kd_ang = kd + + ########### + # Methods # + ########### + + def set_desired_references(self, x_des, dx_des=None, ddx_des=None, *args, **kwargs): + """Set the desired references. + + Args: + x_des (np.array[float[7]], None): desired cartesian pose (position and quaternion [x,y,z,w]) of distal + link wrt the base. If None, it will let the initial desired pose unchanged. + dx_des (np.array[float[6]], None): desired cartesian velocity of distal link wrt the base. If None, it + will let the initial desired velocities unchanged. + ddx_des (np.array[float[6]], None): desired cartesian acceleration of distal link wrt the base. If None, + it will let the initial desired accelerations unchanged. + """ + self.x_desired = x_des + self.dx_desired = dx_des + self.ddx_desired = ddx_des + + def get_desired_references(self): + """Return the desired references. + + Returns: + np.array[float[7]]: desired cartesian pose (position and quaternion [x,y,z,w]) of distal link wrt the base. + np.array[float[6]]: desired cartesian velocity of distal link wrt the base. + np.array[float[6]]: desired cartesian acceleration of distal link wrt the base. + """ + return self.x_desired, self.dx_desired, self.ddx_desired + + def _update(self): + """ + Update the task by computing the A matrix and b vector that will be used by the task solver. + """ + x = self.model.get_pose(link=self.distal_link, wrt_link=self.base_link) + self._A = self.model.get_jacobian(link=self.distal_link, wrt_link=self.base_link, + point=self.local_position) # shape: (6,N) + vel = self.model.get_velocity(link=self.distal_link) + jdotqdot = self.model.compute_JdotQdot(link=self.distal_link) + # b = - \dot{J} \dot{q} + (a_d + K_d (v_d - v) + K_p e) + b = -jdotqdot + self.desired_acceleration + + if self._des_quat is None: # only position and/or velocities + if self._des_pos is None: # only velocities + self._b = b + np.concatenate((np.dot(self.kd_linear, (self._des_lin_vel - vel[:3])), + np.dot(self.kd_angular, (self._des_ang_vel - vel[3:])))) + else: # only position + self._A = self._A[:3] + # compute position error + error = (self._des_pos - x[:3]) + # compute b vector + lin_vel = np.dot(self.kd_linear, (self._des_lin_vel - vel[:3])) + self._b = b[:3] + np.dot(self.kp_position, error) + lin_vel + elif self._des_pos is None: # only orientation + self._A = self._A[3:] + # compute orientation error + error = quaternion_error(quat_des=self._des_quat, quat_cur=x[3:]) + # compute b vector + ang_vel = np.dot(self.kd_angular, (self._des_ang_vel - vel[3:])) + self._b = b[3:] + np.dot(self.kp_orientation, error) + ang_vel + else: # both + # compute position/orientation error + position_error = (self._des_pos - x[:3]) + orientation_error = quaternion_error(quat_des=self._des_quat, quat_cur=x[3:]) + + # compute b vector + lin_vel = np.dot(self.kd_linear, (self._des_lin_vel - vel[:3])) + ang_vel = np.dot(self.kd_angular, (self._des_ang_vel - vel[3:])) + b_lin = np.dot(self.kp_position, position_error) + lin_vel + b_ang = np.dot(self.kp_orientation, orientation_error) + ang_vel + self._b = b + np.concatenate((b_lin, b_ang)) diff --git a/pyrobolearn/priorities/tasks/acceleration/com.py b/pyrobolearn/priorities/tasks/acceleration/com.py index 8f5c7e9..bb2320a 100644 --- a/pyrobolearn/priorities/tasks/acceleration/com.py +++ b/pyrobolearn/priorities/tasks/acceleration/com.py @@ -1,6 +1,56 @@ #!/usr/bin/env python r"""Provide the Cartesian CoM acceleration task. +The CoM task tries to impose a desired position of the CoM with respect to the world frame. + +.. math:: ||J_{CoM} \dot{q} - (K_p (x_d - x) + \dot{x}_d)||^2 + +where :math:`J_{CoM}` is the CoM Jacobian, :math:`\dot{q}` are the joint velocities being optimized, :math:`K_p` +is the stiffness gain, :math:`x_d` and :math:`x` are the desired and current cartesian CoM position +respectively, and :math:`\dot{x}_d` is the desired linear velocity of the CoM. + +This is equivalent to the QP objective function :math:`||Ax - b||^2`, by setting :math:`A=J_{CoM}`, +:math:`x=\dot{q}`, and :math:`b = K_p (x_d - x) + \dot{x}_d`. + +Note that you can only specify the center of mass linear velocity if you wish. + +The CoM acceleration task tries to impose a desired pose, velocity and acceleration profiles for the CoM with respect +to the world frame. + +Before presenting the optimization problem, here is a small reminder. The acceleration is the time derivative of +the velocity, i.e. :math:`a = \frac{dv}{dt}` where the cartesian velocities are related to joint velocities by +:math:`v_{CoM} = J_{CoM}(q) \dot{q}` where :math:`J_{CoM}(q)` is the CoM Jacobian, thus deriving that expression wrt +time gives us: + +.. math:: a = \frac{d}{dt} v = \frac{d}{dt} J_{CoM}(q) \dot{q} = J_{CoM}(q) \ddot{q} + \dot{J}_{CoM}(q) \dot{q}. + +Now, we can formulate our minimization problem as: + +.. math:: || J_{CoM}(q) \ddot{q} + \dot{J}_{CoM} \dot{q} - (a_d + K_d (v_d - v) + K_p (x_d - x)) ||^2, + +where :math:`\ddot{q}` are the joint accelerations being optimized, :math:`a_d` are the desired cartesian linear +accelerations, :math:`v_d` and :math:`v` are the desired and current cartesian linear velocities, +:math:`J_{CoM}(q) \in \mathbb{R}^{3 \times N}` is the CoM Jacobian, :math:`K_p` and :math:`K_d` are the stiffness and +damping gains respectively, and :math:`x_d` and :math:`x` are the desired and current cartesian CoM position +respectively. + + +The above formulation is equivalent to the QP objective function :math:`||Ax - b||^2`, by setting +:math:`A = J_{CoM}(q)`, :math:`x = \ddot{q}`, and +:math:`b = - \dot{J}_{CoM} \dot{q} + (a_d + K_d (v_d - v) + K_p (x_d - x))`. + + +Inverse dynamics +---------------- + +Once the optimal joint accelerations :math:`\ddot{q}^*` have been computed, we can use inverse dynamics to +compute the corresponding torques to apply on the joints. This is given by: + +.. math:: \tau = H(q) \ddot{q} + N(q,\dot{q)} + +where :math:`H(q)` is the inertia joint matrix, and N(q, \dot{q}) is a vector force that accounts for all the +other non-linear forces acting on the system (Coriolis, centrifugal, gravity, external forces, friction, etc.). + The implementation of this class is inspired by [1] (which is licensed under the LGPLv2). @@ -10,7 +60,7 @@ References: import numpy as np -from pyrobolearn.priorities.tasks import Task +from pyrobolearn.priorities.tasks import JointAccelerationTask __author__ = "Brian Delhaisse" @@ -23,17 +73,257 @@ __email__ = "briandelhaisse@gmail.com" __status__ = "Development" -class CoMAccelerationTask(Task): +class CoMAccelerationTask(JointAccelerationTask): r"""CoM Acceleration Task + The CoM task tries to impose a desired position of the CoM with respect to the world frame. + + .. math:: ||J_{CoM} \dot{q} - (K_p (x_d - x) + \dot{x}_d)||^2 + + where :math:`J_{CoM}` is the CoM Jacobian, :math:`\dot{q}` are the joint velocities being optimized, :math:`K_p` + is the stiffness gain, :math:`x_d` and :math:`x` are the desired and current cartesian CoM position + respectively, and :math:`\dot{x}_d` is the desired linear velocity of the CoM. + + This is equivalent to the QP objective function :math:`||Ax - b||^2`, by setting :math:`A=J_{CoM}`, + :math:`x=\dot{q}`, and :math:`b = K_p (x_d - x) + \dot{x}_d`. + + Note that you can only specify the center of mass linear velocity if you wish. + + The CoM acceleration task tries to impose a desired pose, velocity and acceleration profiles for the CoM wrt the + world frame. + + Before presenting the optimization problem, here is a small reminder. The acceleration is the time derivative of + the velocity, i.e. :math:`a = \frac{dv}{dt}` where the cartesian velocities are related to joint velocities by + :math:`v_{CoM} = J_{CoM}(q) \dot{q}` where :math:`J_{CoM}(q)` is the CoM Jacobian, thus deriving that expression + wrt time gives us: + + .. math:: a = \frac{d}{dt} v = \frac{d}{dt} J_{CoM}(q) \dot{q} = J_{CoM}(q) \ddot{q} + \dot{J}_{CoM}(q) \dot{q}. + + Now, we can formulate our minimization problem as: + + .. math:: || J_{CoM}(q) \ddot{q} + \dot{J}_{CoM} \dot{q} - (a_d + K_d (v_d - v) + K_p (x_d - x)) ||^2, + + where :math:`\ddot{q}` are the joint accelerations being optimized, :math:`a_d` are the desired cartesian linear + accelerations, :math:`v_d` and :math:`v` are the desired and current cartesian linear velocities, + :math:`J_{CoM}(q) \in \mathbb{R}^{3 \times N}` is the CoM Jacobian, :math:`K_p` and :math:`K_d` are the stiffness + and damping gains respectively, and :math:`x_d` and :math:`x` are the desired and current cartesian CoM position + respectively. + + + The above formulation is equivalent to the QP objective function :math:`||Ax - b||^2`, by setting + :math:`A = J_{CoM}(q)`, :math:`x = \ddot{q}`, and + :math:`b = - \dot{J}_{CoM} \dot{q} + (a_d + K_d (v_d - v) + K_p (x_d - x))`. + + + Inverse dynamics + ---------------- + + Once the optimal joint accelerations :math:`\ddot{q}^*` have been computed, we can use inverse dynamics to + compute the corresponding torques to apply on the joints. This is given by: + + .. math:: \tau = H(q) \ddot{q} + N(q,\dot{q)} + + where :math:`H(q)` is the inertia joint matrix, and N(q, \dot{q}) is a vector force that accounts for all the + other non-linear forces acting on the system (Coriolis, centrifugal, gravity, external forces, friction, etc.). + + + The implementation of this class is inspired by [1] (which is licensed under the LGPLv2). + + References: + - [1] "OpenSoT: A whole-body control library for the compliant humanoid robot COMAN", Rocchi et al., 2015 """ - def __init__(self, model, constraints=[]): + def __init__(self, model, desired_position=None, desired_velocity=None, desired_acceleration=None, kp=1., kd=1., + weight=1., constraints=[]): """ Initialize the task. Args: - model (ModelInterface): model interface - constraints (list of Constraint): list of constraints associated with the task. + model (ModelInterface): model interface. + desired_position (np.array[float[3]], None): desired CoM position. If None, it will be set to 0. + desired_velocity (np.array[float[3]], None): desired CoM linear velocity. If None, it will be set to 0. + desired_acceleration (np.array[float[3]], None): desired CoM linear acceleration. If None, it will be set + to 0. + kp (float, np.array[float[3,3]]): position gain. + kd (float, np.array[float[3,3]]): linear velocity gain. + weight (float, np.array[float[3,3]]): weight scalar or matrix associated to the task. + constraints (list[Constraint]): list of constraints associated with the task. """ - super(CoMAccelerationTask, self).__init__(model=model, constraints=constraints) + super(CoMAccelerationTask, self).__init__(model=model, weight=weight, constraints=constraints) + + # define variable + self.kp = kp + self.kd = kd + + # define desired references + self.desired_position = desired_position + self.desired_velocity = desired_velocity + self.desired_acceleration = desired_acceleration + + # first update + self.update() + + ############## + # Properties # + ############## + + @property + def desired_position(self): + """Get the desired CoM position.""" + return self._des_pos + + @desired_position.setter + def desired_position(self, position): + """Set the desired CoM position.""" + if position is not None: + if not isinstance(position, (np.ndarray, list, tuple)): + raise TypeError("Expecting the given desired position to be a np.array, instead got: " + "{}".format(type(position))) + position = np.asarray(position) + if len(position) != 3: + raise ValueError("Expecting the given desired position array to be of length 3, but instead got: " + "{}".format(len(position))) + self._des_pos = position + + @property + def desired_velocity(self): + """Get the desired CoM linear velocity.""" + return self._des_vel + + @desired_velocity.setter + def desired_velocity(self, velocity): + """Set the desired CoM linear velocity.""" + if velocity is None: + velocity = np.zeros(3) + elif not isinstance(velocity, (np.ndarray, list, tuple)): + raise TypeError("Expecting the given desired linear velocity to be a np.array, instead got: " + "{}".format(type(velocity))) + velocity = np.asarray(velocity) + if len(velocity) != 3: + raise ValueError("Expecting the given desired linear velocity array to be of length 3, but instead " + "got: {}".format(len(velocity))) + self._des_vel = velocity + + @property + def desired_acceleration(self): + """Get the desired CoM linear acceleration.""" + return self._des_acc + + @desired_acceleration.setter + def desired_acceleration(self, acceleration): + """Set the desired CoM linear acceleration.""" + if acceleration is None: + acceleration = np.zeros(3) + elif not isinstance(acceleration, (np.ndarray, list, tuple)): + raise TypeError("Expecting the given desired linear acceleration to be a np.array, instead got: " + "{}".format(type(acceleration))) + acceleration = np.asarray(acceleration) + if len(acceleration) != 3: + raise ValueError("Expecting the given desired linear acceleration array to be of length 3, but instead " + "got: {}".format(len(acceleration))) + self._des_acc = acceleration + + @property + def x_desired(self): + """Get the desired CoM position.""" + return self._des_pos + + @x_desired.setter + def x_desired(self, x_d): + """Set the desired CoM position.""" + self.desired_position = x_d + + @property + def dx_desired(self): + """Get the desired CoM linear velocity.""" + return self._des_vel + + @dx_desired.setter + def dx_desired(self, dx_d): + """Set the desired CoM linear velocity.""" + self.desired_velocity = dx_d + + @property + def ddx_desired(self): + """Get the desired CoM linear acceleration.""" + return self._des_acc + + @ddx_desired.setter + def ddx_desired(self, ddx_d): + """Set the desired CoM linear acceleration.""" + self.desired_acceleration = ddx_d + + @property + def kp(self): + """Return the position gain.""" + return self._kp + + @kp.setter + def kp(self, kp): + """Set the position gain.""" + if kp is None: + kp = 1. + if not isinstance(kp, (float, int, np.ndarray)): + raise TypeError("Expecting the given position gain kp to be an int, float, np.array, instead got: " + "{}".format(type(kp))) + if isinstance(kp, np.ndarray) and kp.shape != (3, 3): + raise ValueError("Expecting the given position gain matrix kp to be of shape {}, but instead got " + "shape: {}".format((self.x_size, self.x_size), kp.shape)) + self._kp = kp + + @property + def kd(self): + """Return the linear velocity gain.""" + return self._kd + + @kd.setter + def kd(self, kd): + """Set the linear velocity gain.""" + if kd is None: + kd = 1. + if not isinstance(kd, (float, int, np.ndarray)): + raise TypeError("Expecting the given linear velocity gain kd to be an int, float, np.array, " + "instead got: {}".format(type(kd))) + if isinstance(kd, np.ndarray) and kd.shape != (3, 3): + raise ValueError("Expecting the given linear velocity gain matrix kd to be of shape {}, but " + "instead got shape: {}".format((3, 3), kd.shape)) + self._kd = kd + + ########### + # Methods # + ########### + + def set_desired_references(self, x_des, dx_des=None, ddx_des=None, *args, **kwargs): + """Set the desired references. + + Args: + x_des (np.array[float[3]], None): desired CoM position. If None, it will be set to 0. + dx_des (np.array[float[3]], None): desired CoM linear velocity. If None, it will be set to 0. + ddx_des (np.array[float[3]], None): desired CoM linear velocity. If None, it will be set to 0. + """ + self.x_desired = x_des + self.dx_desired = dx_des + self.ddx_desired = ddx_des + + def get_desired_references(self): + """Return the desired references. + + Returns: + np.array[float[3]]: desired CoM position. + np.array[float[3]]: desired CoM linear velocity. + """ + return self.x_desired, self.dx_desired, self.ddx_desired + + def _update(self): + """ + Update the task by computing the A matrix and b vector that will be used by the task solver. + """ + + self._A = self.model.get_com_jacobian(full=False) # shape: (3, N) + jdotqdot = self.model.compute_com_JdotQdot()[:3] # shape: (3,) + vel = self.model.get_com_velocity() # shape: (3,) + self._b = -jdotqdot + self._des_acc + np.dot(self.kd, (self._des_vel - vel)) # shape: (3,) + + if self._des_pos is not None: + x = self.model.get_com_position() + self._b = self._b + np.dot(self.kp, (self._des_pos - x)) # shape: (3,) diff --git a/pyrobolearn/priorities/tasks/acceleration/contact.py b/pyrobolearn/priorities/tasks/acceleration/contact.py index 7dda6c9..b12128a 100644 --- a/pyrobolearn/priorities/tasks/acceleration/contact.py +++ b/pyrobolearn/priorities/tasks/acceleration/contact.py @@ -1,6 +1,23 @@ #!/usr/bin/env python r"""Provide the contact acceleration task. +The contact acceleration task tries to minimize the dynamic movement of a contact link: + +.. math:: || C (J_c(q) \ddot{q} + \dot{J}_c(q) \dot{q}) ||^2, + +where :math:`C \in \mathbb{R}^{6 \times 6}` is the contact matrix (=a diagonal selector matrix where the entries +are 1 for cartesian velocities that we wish to minimize such that the link doesn't move, and 0 for cartesian +velocities that are free to change), :math:`J_c(q)` is the contact Jacobian (the Jacobian from the world frame to +the contact point expressed in the distal link frame (i.e. the link which is in contact)), +:math:`\ddot{q}, \dot{q}` are respectively the joint accelerations being optimized and the joint velocities. +Note that because the jacobian is expressed in the distal link frame, the entries of the contact matrix specify +the accelerations/velocities in that particular frame. For instance, [0,0,1,0,0,0] means that the z +linear acceleration/velocity (where z is the z axis of the distal link frame) should be fixed. + +This is equivalent to the QP objective function :math:`||Ax - b||^2`, by setting :math:`A = C J_c(q)`, +:math:`x = \ddot{q}`, and :math:`b = - C \dot{J}_c(q) \dot{q}`. + +This task is useful for instance if we want to keep the feet in contact with the ground. The implementation of this class is inspired by [1] (which is licensed under the LGPLv2). @@ -9,8 +26,10 @@ References: """ import numpy as np +from scipy.linalg import block_diag -from pyrobolearn.priorities.tasks import Task +from pyrobolearn.priorities.tasks import JointAccelerationTask +from pyrobolearn.utils.transformation import get_matrix_from_quaternion __author__ = "Brian Delhaisse" @@ -23,17 +42,112 @@ __email__ = "briandelhaisse@gmail.com" __status__ = "Development" -class ContactAccelerationTask(Task): +class ContactAccelerationTask(JointAccelerationTask): r"""Contact Acceleration Task + The contact acceleration task tries to minimize the dynamic movement of a contact link: + + .. math:: || C (J_c(q) \ddot{q} + \dot{J}_c(q) \dot{q}) ||^2, + + where :math:`C \in \mathbb{R}^{6 \times 6}` is the contact matrix (=a diagonal selector matrix where the entries + are 1 for cartesian velocities that we wish to minimize such that the link doesn't move, and 0 for cartesian + velocities that are free to change), :math:`J_c(q)` is the contact Jacobian (the Jacobian from the world frame to + the contact point expressed in the distal link frame (i.e. the link which is in contact)), + :math:`\ddot{q}, \dot{q}` are respectively the joint accelerations being optimized and the joint velocities. + Note that because the jacobian is expressed in the distal link frame, the entries of the contact matrix specify + the accelerations/velocities in that particular frame. For instance, [0,0,1,0,0,0] means that the z + linear acceleration/velocity (where z is the z axis of the distal link frame) should be fixed. + + This is equivalent to the QP objective function :math:`||Ax - b||^2`, by setting :math:`A = C J_c(q)`, + :math:`x = \ddot{q}`, and :math:`b = - C \dot{J}_c(q) \dot{q}`. + + This task is useful for instance if we want to keep the feet in contact with the ground. + + + The implementation of this class is inspired by [1] (which is licensed under the LGPLv2). + + References: + - [1] "OpenSoT: A whole-body control library for the compliant humanoid robot COMAN", Rocchi et al., 2015 """ - def __init__(self, model, constraints=[]): + def __init__(self, model, distal_link, contact_matrix=1., weight=1., constraints=[]): """ Initialize the task. Args: - model (ModelInterface): model interface + model (ModelInterface): model interface. + distal_link (int, str): distal link id or name. + contact_matrix (np.array[float[6]], np.array[float[6,6]], None): contact selector matrix (=a diagonal + square matrix). You can specify only the diagonal elements if you wish. If None, by default it will be + set to the identity matrix. + weight (float, np.array[float[6,6]]): weight scalar or matrix associated to the task. constraints (list of Constraint): list of constraints associated with the task. """ - super(ContactAccelerationTask, self).__init__(model=model, constraints=constraints) + super(ContactAccelerationTask, self).__init__(model=model, weight=weight, constraints=constraints) + + # set variable + self.distal_link = self.model.get_link_id(distal_link) + self.contact_matrix = contact_matrix + + # set QP vector + self._b = np.zeros(6) + + # first update + self.update() + + ############## + # Properties # + ############## + + @property + def contact_matrix(self): + """Return the contact selector matrix.""" + return self._contact_matrix + + @contact_matrix.setter + def contact_matrix(self, matrix): + """Set the contact selector matrix.""" + if matrix is None: + matrix = np.identity(6) + + # check contact matrix type + if not isinstance(matrix, (int, float, np.ndarray)): + raise TypeError("Expecting the given contact matrix to be an int, float, or diagonal np.array, instead " + "got: {}".format(type(matrix))) + + # if numpy array, check its shape and make sure it is a diagonal matrix + if isinstance(matrix, np.ndarray): + if matrix.shape == (6,): + matrix = np.diag(matrix) + elif matrix.shape != (6, 6): + raise ValueError("Expecting the given contact matrix to be of shape (6,6), instead got a shape of: " + "{}".format(type(matrix))) + else: + # make sure the contact matrix is a diagonal matrix + matrix = np.diag(np.diag(matrix)) + + # set the contact matrix + self._contact_matrix = matrix + + ########### + # Methods # + ########### + + def _update(self): + """ + Update the task by computing the A matrix and b vector that will be used by the task solver. + """ + # get jacobians expressed in the world frame + jacobian = self.model.get_jacobian(link=self.distal_link) + jdotqdot = self.model.compute_JdotQdot(link=self.distal_link) + + # express jacobians in the distal/contact link frame + orientation = self.model.get_orientation(link=self.distal_link) # shape: (4,) + rot = get_matrix_from_quaternion(orientation).T # shape: (3,3) + rot = block_diag(rot, rot) # shape: (6,6) + jacobian = rot.dot(jacobian) + jdotqdot = rot.dot(jdotqdot) + + # set A and b + self._A = np.dot(self.contact_matrix, jacobian) + self._b = np.dot(self.contact_matrix, jdotqdot) diff --git a/pyrobolearn/priorities/tasks/acceleration/dynamic_feasibility.py b/pyrobolearn/priorities/tasks/acceleration/dynamic_feasibility.py new file mode 100644 index 0000000..e69de29 diff --git a/pyrobolearn/priorities/tasks/acceleration/postural.py b/pyrobolearn/priorities/tasks/acceleration/postural.py index 5a60f15..c09e29b 100644 --- a/pyrobolearn/priorities/tasks/acceleration/postural.py +++ b/pyrobolearn/priorities/tasks/acceleration/postural.py @@ -1,6 +1,18 @@ #!/usr/bin/env python r"""Provide the postural acceleration task. +The postural task tries to bring the robot to a reference posture; that is, it minimizes the joint accelerations +such that it gets close to the specified posture (given by the desired joint positions, velocities, and +accelerations): + +.. math:: || \ddot{q} - (\ddot{q}_d + K_d (\dot{q}_d - \dot{q}) + K_p (q_d - q)) ||^2, + +where :math:`\ddot{q}, \dot{q}, q` are respectively the joint accelerations being optimized, joint velocities and +positions, :math:`K_p` and :math:`K_d` are the position and velocity gains respectively, and the subscript +:math:`d` means "desired". + +This is equivalent to the QP objective function :math:`||Ax - b||^2`, by setting :math:`A=I`, :math:`x=\dot{q}`, +and :math:`b = \ddot{q}_d + K_d (\dot{q}_d - \dot{q}) + K_p (q_d - q)`. The implementation of this class is inspired by [1] (which is licensed under the LGPLv2). @@ -10,7 +22,7 @@ References: import numpy as np -from pyrobolearn.priorities.tasks import Task +from pyrobolearn.priorities.tasks import JointAccelerationTask __author__ = "Brian Delhaisse" @@ -23,17 +35,224 @@ __email__ = "briandelhaisse@gmail.com" __status__ = "Development" -class PosturalAccelerationTask(Task): +class PosturalAccelerationTask(JointAccelerationTask): r"""Postural Acceleration Task + The postural task tries to bring the robot to a reference posture; that is, it minimizes the joint accelerations + such that it gets close to the specified posture (given by the desired joint positions, velocities, and + accelerations): + + .. math:: || \ddot{q} - (\ddot{q}_d + K_d (\dot{q}_d - \dot{q}) + K_p (q_d - q)) ||^2, + + where :math:`\ddot{q}, \dot{q}, q` are respectively the joint accelerations being optimized, joint velocities and + positions, :math:`K_p` and :math:`K_d` are the position and velocity gains respectively, and the subscript + :math:`d` means "desired". + + This is equivalent to the QP objective function :math:`||Ax - b||^2`, by setting :math:`A=I`, :math:`x=\dot{q}`, + and :math:`b = \ddot{q}_d + K_d (\dot{q}_d - \dot{q}) + K_p (q_d - q)`. + + The implementation of this class is inspired by [1] (which is licensed under the LGPLv2). + + References: + - [1] "OpenSoT: A whole-body control library for the compliant humanoid robot COMAN", Rocchi et al., 2015 + """ - def __init__(self, model, constraints=[]): + def __init__(self, model, q_desired=None, dq_desired=None, ddq_desired=None, kp=1., kd=1., weight=1., + constraints=[]): """ Initialize the task. Args: - model (ModelInterface): model interface - constraints (list of Constraint): list of constraints associated with the task. + model (ModelInterface): model interface. + q_desired (np.array[float[N]], None): desired joint positions, where :math:`N` is the number of DoFs. If + None, it will not be considered. + dq_desired (np.array[float[N]], None): desired joint velocities, where :math:`N` is the number of DoFs. + If None, it will be set to 0. + ddq_desired (np.array[float[N]], None): desired joint accelerations, where :math:`N` is the number of DoFs. + If None, it will be set to 0. + kp (float, np.array[float[N,N]]): position gain(s). + kd (float, np.array[float[N,N]]): velocity gain(s). + weight (float, np.array[float[N,N]]): weight scalar or matrix associated to the task. + constraints (list[Constraint]): list of constraints associated with the task. """ - super(PosturalAccelerationTask, self).__init__(model=model, constraints=constraints) + super(PosturalAccelerationTask, self).__init__(model=model, weight=weight, constraints=constraints) + + # define variables + self.kp = kp + self.kd = kd + + # define desired references + self.q_desired = q_desired + self.dq_desired = dq_desired + self.ddq_desired = ddq_desired + + # first update + self.update() + + ############## + # Properties # + ############## + + @property + def q_desired(self): + """Get the desired joint positions.""" + return self._q_d + + @q_desired.setter + def q_desired(self, q_d): + """Set the desired joint positions.""" + if q_d is not None: + if not isinstance(q_d, (np.ndarray, list, tuple)): + raise TypeError("Expecting the given desired joint positions to be an instance of np.array, instead " + "got: {}".format(type(q_d))) + q_d = np.asarray(q_d) + if len(q_d) != self.x_size: + raise ValueError("Expecting the length of the given desired joint positions (={}) to be the same as " + "the number of DoFs (={})".format(len(q_d), self.x_size)) + self._q_d = q_d + + @property + def dq_desired(self): + """Get the desired joint velocities.""" + return self._dq_d + + @dq_desired.setter + def dq_desired(self, dq_d): + """Set the desired joint velocities.""" + if dq_d is None: + dq_d = np.zeros(self.x_size) + if not isinstance(dq_d, (np.ndarray, list, tuple)): + raise TypeError("Expecting the given desired joint velocities to be an instance of np.array, instead " + "got: {}".format(type(dq_d))) + dq_d = np.asarray(dq_d) + if len(dq_d) != self.x_size: + raise ValueError("Expecting the length of the given desired joint velocities (={}) to be the same as the " + "number of DoFs (={})".format(len(dq_d), self.x_size)) + self._dq_d = dq_d + + @property + def ddq_desired(self): + """Get the desired joint velocities.""" + return self._ddq_d + + @ddq_desired.setter + def ddq_desired(self, ddq_d): + """Set the desired joint velocities.""" + if ddq_d is None: + ddq_d = np.zeros(self.x_size) + if not isinstance(ddq_d, (np.ndarray, list, tuple)): + raise TypeError("Expecting the given desired joint accelerations to be an instance of np.array, instead " + "got: {}".format(type(ddq_d))) + ddq_d = np.asarray(ddq_d) + if len(ddq_d) != self.x_size: + raise ValueError("Expecting the length of the given desired joint accelerations (={}) to be the same as " + "the number of DoFs (={})".format(len(ddq_d), self.x_size)) + self._ddq_d = ddq_d + + @property + def x_desired(self): + """Get the desired joint positions.""" + return self._q_d + + @x_desired.setter + def x_desired(self, q_d): + """Set the desired joint positions.""" + self.q_desired = q_d + + @property + def dx_desired(self): + """Get the desired joint velocities.""" + return self._dq_d + + @dx_desired.setter + def dx_desired(self, dq_d): + """Set the desired joint velocities.""" + self.dq_desired = dq_d + + @property + def ddx_desired(self): + """Get the desired joint accelerations.""" + return self._ddq_d + + @ddx_desired.setter + def ddx_desired(self, ddq_d): + """Set the desired joint accelerations.""" + self.ddq_desired = ddq_d + + @property + def kp(self): + """Return the position gain.""" + return self._kp + + @kp.setter + def kp(self, kp): + """Set the position gain.""" + if kp is None: + kp = 1. + if not isinstance(kp, (float, int, np.ndarray)): + raise TypeError("Expecting the given position gain kp to be an int, float, np.array, instead got: " + "{}".format(type(kp))) + if isinstance(kp, np.ndarray) and kp.shape != (self.x_size, self.x_size): + raise ValueError("Expecting the given position gain matrix kp to be of shape {}, but instead got " + "shape: {}".format((self.x_size, self.x_size), kp.shape)) + self._kp = kp + + @property + def kd(self): + """Return the velocity gain.""" + return self._kd + + @kd.setter + def kd(self, kd): + """Set the velocity gain.""" + if kd is None: + kd = 1. + if not isinstance(kd, (float, int, np.ndarray)): + raise TypeError("Expecting the given velocity gain kd to be an int, float, np.array, instead got: " + "{}".format(type(kd))) + if isinstance(kd, np.ndarray) and kd.shape != (self.x_size, self.x_size): + raise ValueError("Expecting the given velocity gain matrix kd to be of shape {}, but instead got " + "shape: {}".format((self.x_size, self.x_size), kd.shape)) + self._kd = kd + + ########### + # Methods # + ########### + + def set_desired_references(self, x_des, dx_des=None, ddx_des=None, *args, **kwargs): + """Set the desired references. + + Args: + x_des (np.array[float[N]], None): desired joint positions, where :math:`N` is the number of DoFs. If None, + it will be set to 0. + dx_des (np.array[float[N]], None): desired joint velocities, where :math:`N` is the number of DoFs. If + None, it will be set to 0. + ddx_des (np.array[float[N]], None): desired joint accelerations, where :math:`N` is the number of DoFs. If + None, it will be set to 0. + """ + self.x_desired = x_des + self.dx_desired = dx_des + self.ddx_desired = ddx_des + + def get_desired_references(self): + """Return the desired references. + + Returns: + np.array[float[N]]: desired joint positions. + np.array[float[N]]: desired joint velocities. + np.array[float[N]]: desired joint accelerations. + """ + return self.x_desired, self.dx_desired, self.ddx_desired + + def _update(self): + """ + Update the task by computing the A matrix and b vector that will be used by the task solver. + """ + q = self.model.get_joint_positions() + dq = self.model.get_joint_velocities() + self._b = self._ddq_d + np.dot(self.kd, (self._dq_d - dq)) + + # update b vector + if self._q_d is not None: + self._b += np.dot(self.kp, (self._q_d - q)) # shape: (N,) diff --git a/pyrobolearn/priorities/tasks/dynamic_tasks.py b/pyrobolearn/priorities/tasks/dynamic_tasks.py index f645239..350a840 100644 --- a/pyrobolearn/priorities/tasks/dynamic_tasks.py +++ b/pyrobolearn/priorities/tasks/dynamic_tasks.py @@ -1,6 +1,8 @@ #!/usr/bin/env python """Provide the various dynamic tasks (i.e. objective functions) used in QP. +DEPRECATED. + References: [1] "Quadratic Programming in Python" (https://scaron.info/blog/quadratic-programming-in-python.html), Caron, 2017 [2] "OpenSoT: A whole-body control library for the compliant humanoid robot COMAN", Rocchi et al., 2015 diff --git a/pyrobolearn/priorities/tasks/kinematic_tasks.py b/pyrobolearn/priorities/tasks/kinematic_tasks.py index 7c281a7..aafb77d 100644 --- a/pyrobolearn/priorities/tasks/kinematic_tasks.py +++ b/pyrobolearn/priorities/tasks/kinematic_tasks.py @@ -1,6 +1,8 @@ #!/usr/bin/env python """Provide the various kinematic tasks (i.e. objective functions) used in QP. +DEPRECATED. + References: [1] "Quadratic Programming in Python" (https://scaron.info/blog/quadratic-programming-in-python.html), Caron, 2017 [2] "OpenSoT: A whole-body control library for the compliant humanoid robot COMAN", Rocchi et al., 2015 diff --git a/pyrobolearn/priorities/tasks/task_from_constraint.py b/pyrobolearn/priorities/tasks/task_from_constraint.py new file mode 100644 index 0000000..72966a6 --- /dev/null +++ b/pyrobolearn/priorities/tasks/task_from_constraint.py @@ -0,0 +1,79 @@ +#!/usr/bin/env python +r"""Transform a given equality constraint into a task. + +An equality constraint specified by :math:`Fx = k` is transformed to a soft task :math:`||Ax - b||^2`, where +:math:`A = F` and :math:`b = k`. This allows for the equality constraint to be lightly violated; by specifying the +weight :math:`W` we can specify how much the constraint should be satisfied. +""" + +from pyrobolearn.priorities.tasks import Task +from pyrobolearn.priorities.constraints import EqualityConstraint + + +__author__ = "Brian Delhaisse" +__copyright__ = "Copyright 2019, PyRoboLearn" +__credits__ = ["Brian Delhaisse"] +__license__ = "GNU GPLv3" +__version__ = "1.0.0" +__maintainer__ = "Brian Delhaisse" +__email__ = "briandelhaisse@gmail.com" +__status__ = "Development" + + +class TaskFromConstraint(Task): + r"""Task From Equality Constraint + + An equality constraint specified by :math:`Fx = k` is transformed to a soft task :math:`||Ax - b||_{W}^2`, where + :math:`A = F` and :math:`b = k`. This allows for the equality constraint to be lightly violated; by specifying the + weight :math:`W` we can specify how much the constraint should be satisfied. + """ + + def __init__(self, constraint, weight=1., constraints=[]): + """ + Initialize the task. + + Args: + constraint (EqualityConstraint): equality constraint. + weight (float, np.array[float[6,6]], np.array[float[3,3]]): weight scalar or matrix associated to the task. + constraints (list[Constraint]): list of constraints associated with the task. + """ + # set equality constraint + self.equality_constraint = constraint + + # call superclass + super(TaskFromConstraint, self).__init__(model=constraint.model, weight=weight, constraints=constraints) + + # first update + self.update() + + ############## + # Properties # + ############## + + @property + def equality_constraint(self): + """Get the equality constraint.""" + return self._constraint + + @equality_constraint.setter + def equality_constraint(self, constraint): + """Set the equality constraint.""" + if not isinstance(constraint, EqualityConstraint): + raise TypeError("Expecting the given 'constraint' to be an instance of `EqualityConstraint`, but " + "instead got: {}".format(constraint)) + self._constraint = constraint + + ########### + # Methods # + ########### + + def _update(self): + """ + Update the task by computing the A matrix and b vector that will be used by the task solver. + """ + # update equality constraint + self.equality_constraint.update() + + # update A and b + self._A = self.equality_constraint.A_eq + self._b = self.equality_constraint.b_eq diff --git a/pyrobolearn/priorities/tasks/velocity/cartesian.py b/pyrobolearn/priorities/tasks/velocity/cartesian.py index bf0c89a..0eace9f 100644 --- a/pyrobolearn/priorities/tasks/velocity/cartesian.py +++ b/pyrobolearn/priorities/tasks/velocity/cartesian.py @@ -1,15 +1,15 @@ #!/usr/bin/env python r"""Provide the cartesian (velocity) task. -The cartesian task tries to impose a desired pose (position and orientation) of a distal link with respect to a -base link or the world frame. The minimization problem is given by: +The cartesian task tries to impose a desired pose (position and orientation) and velocity of a distal link with +respect to a base link or the world frame. The minimization problem is given by: .. math:: || ^bJ_d(q) \dot{q} - (K_p e + \dot{x}_d) ||^2 where :math:`^bJ_d(q) \in \mathbb{R}^{6 \times N}` is the Jacobian taken from the base to the distal link, :math:`\dot{q}` are the joint velocities being optimized, :math:`K_p` is the stiffness gain, :math:`e \in \mathbb{R}^{6}` is the error which is the concatenation of the position error given by -:math:`e_{p} = (x_d - x)` (with :math:`x_d` being the desired pose, and :math:`x` the current pose), and the +:math:`e_{p} = (x_d - x)` (with :math:`x_d` being the desired position, and :math:`x` the current position), and the orientation error given by (if expressed as quaternions :math:`o = {s, v}` where :math:`s` is the real scalar part, and :math:`v` is the vector part) :math:`e_{o} = s v_d - s_d v - v_d \cross v`, and :math:`\dot{x}_d` is the desired cartesian velocity for the distal link with respect to the base link. @@ -48,22 +48,27 @@ __status__ = "Development" class CartesianTask(JointVelocityTask): r"""Cartesian (velocity) Task - The cartesian task tries to impose a desired pose (position and orientation) of a distal link with respect to a - base link or the world frame. The minimization problem is given by: + The cartesian task tries to impose a desired pose (position and orientation) and velocity of a distal link with + respect to a base link or the world frame. The minimization problem is given by: .. math:: || ^bJ_d(q) \dot{q} - (K_p e + \dot{x}_d) ||^2 where :math:`^bJ_d(q) \in \mathbb{R}^{6 \times N}` is the Jacobian taken from the base to the distal link, :math:`\dot{q}` are the joint velocities being optimized, :math:`K_p` is the stiffness gain, :math:`e \in \mathbb{R}^{6}` is the error which is the concatenation of the position error given by - :math:`e_{p} = (x_d - x)` (with :math:`x_d` being the desired pose, and :math:`x` the current pose), and the - orientation error given by (if expressed as quaternions :math:`o = {s, v}` where :math:`s` is the real scalar part, - and :math:`v` is the vector part) :math:`e_{o} = s v_d - s_d v - v_d \cross v`, and :math:`\dot{x}_d` is the + :math:`e_{p} = (x_d - x)` (with :math:`x_d` being the desired position, and :math:`x` the current position), and + the orientation error given by (if expressed as quaternions :math:`o = {s, v}` where :math:`s` is the real scalar + part, and :math:`v` is the vector part) :math:`e_{o} = s v_d - s_d v - v_d \cross v`, and :math:`\dot{x}_d` is the desired cartesian velocity for the distal link with respect to the base link. This is equivalent to the QP objective function :math:`||Ax - b||^2`, by setting :math:`A = ^bJ_d(q)`, :math:`x = \dot{q}`, and :math:`b = K_p e + \dot{x}_d`. + Important notes: + + - You don't have to specify the whole pose, you can also only specify the position or orientation. + - You can also only specify the cartesian velocities without providing the cartesian position or orientation. + The implementation of this class is inspired by [1] (which is licensed under the LGPLv2). References: @@ -91,7 +96,7 @@ class CartesianTask(JointVelocityTask): If None, it will be set to zero. kp_position (float, np.array[float[3,3]]): position stiffness gain. kp_orientation (float, np.array[float[3,3]]): orientation stiffness gain. - weight (float, np.array[float[6,6]]): weight scalar or matrix associated to the task. + weight (float, np.array[float[6,6]], np.array[float[3,3]]): weight scalar or matrix associated to the task. constraints (list[Constraint]): list of constraints associated with the task. """ super(CartesianTask, self).__init__(model=model, weight=weight, constraints=constraints) @@ -304,6 +309,7 @@ class CartesianTask(JointVelocityTask): def get_desired_references(self): """Return the desired references. + Returns: np.array[float[7]]: desired cartesian pose (position and quaternion [x,y,z,w]) of distal link wrt the base. np.array[float[6]]: desired cartesian velocity of distal link wrt the base. diff --git a/pyrobolearn/priorities/tasks/velocity/com.py b/pyrobolearn/priorities/tasks/velocity/com.py index 01a9877..91b8080 100644 --- a/pyrobolearn/priorities/tasks/velocity/com.py +++ b/pyrobolearn/priorities/tasks/velocity/com.py @@ -1,7 +1,7 @@ #!/usr/bin/env python r"""Provide the center of mass velocity task. -The CoM task tries to impose a desired position of the CoM with respect to the world frame. +The CoM task tries to impose a desired position and (linear) velocity of the CoM with respect to the world frame. .. math:: ||J_{CoM} \dot{q} - (K_p (x_d - x) + \dot{x}_d)||^2 @@ -39,7 +39,7 @@ __status__ = "Development" class CoMTask(JointVelocityTask): r"""Center of Mass Velocity Task - The CoM task tries to impose a desired position of the CoM with respect to the world frame. + The CoM task tries to impose a desired position and (linear) velocity of the CoM with respect to the world frame. .. math:: ||J_{CoM} \dot{q} - (K_p (x_d - x) + \dot{x}_d)||^2 @@ -50,6 +50,8 @@ class CoMTask(JointVelocityTask): This is equivalent to the QP objective function :math:`||Ax - b||^2`, by setting :math:`A=J_{CoM}`, :math:`x=\dot{q}`, and :math:`b = K_p (x_d - x) + \dot{x}_d`. + Note that you can only specify the center of mass linear velocity if you wish. + The implementation of this class is inspired by [1] (which is licensed under the LGPLv2). References: diff --git a/pyrobolearn/priorities/tasks/velocity/manipulability.py b/pyrobolearn/priorities/tasks/velocity/manipulability.py index bab1564..5e4a431 100644 --- a/pyrobolearn/priorities/tasks/velocity/manipulability.py +++ b/pyrobolearn/priorities/tasks/velocity/manipulability.py @@ -28,7 +28,7 @@ __status__ = "Development" class ManipulabilityTask(JointVelocityTask): r"""Manipulability Task - The manipulability task implements a tasks that tries to maximize the manipulability measure given in [1]: + The manipulability task implements a task that tries to maximize the manipulability measure given in [1]: .. math:: w(q) = \sqrt{ \det( J(q) W J(q)^\top ) } diff --git a/pyrobolearn/priorities/tasks/velocity/minimum_acceleration.py b/pyrobolearn/priorities/tasks/velocity/minimum_acceleration.py index 1bc878c..eb2d3af 100644 --- a/pyrobolearn/priorities/tasks/velocity/minimum_acceleration.py +++ b/pyrobolearn/priorities/tasks/velocity/minimum_acceleration.py @@ -3,13 +3,12 @@ r"""Provide the minimum acceleration task. The minimum acceleration task tries to minimize the change in velocity, that is, it minimizes: -.. math:: || \dot{q}_t - \dot{q}_{t-1} ||^2 +.. math:: || \dot{q} - \dot{q}_t ||^2 -where :math:`\dot{q}_t` are the current joint velocities being optimized, and :math:`\dot{q}_{t-1}` are the -previous joint velocities. +where :math:`\dot{q}` are the joint velocities being optimized, and :math:`\dot{q}_t` are the current joint velocities. This is equivalent to the QP objective function :math:`||Ax - b||^2`, by setting :math:`A = I`, :math:`x = \dot{q}`, -and :math:`b = \dot{q}_{t-1}`. +and :math:`b = \dot{q}_t`. The implementation of this class is inspired by [1] (which is licensed under the LGPLv2). @@ -38,13 +37,13 @@ class MinAccelerationTask(JointVelocityTask): The minimum acceleration task tries to minimize the change in velocity, that is, it minimizes: - .. math:: || \dot{q}_t - \dot{q}_{t-1} ||^2 + .. math:: || \dot{q} - \dot{q}_t ||^2 - where :math:`\dot{q}_t` are the current joint velocities being optimized, and :math:`\dot{q}_{t-1}` are the - previous joint velocities. + where :math:`\dot{q}` are the joint velocities being optimized, and :math:`\dot{q}_t` are the current joint + velocities. - This is equivalent to the QP objective function :math:`||Ax - b||^2`, by setting :math:`A = I`, :math:`x = \dot{q}`, - and :math:`b = \dot{q}_{t-1}`. + This is equivalent to the QP objective function :math:`||Ax - b||^2`, by setting :math:`A = I`, + :math:`x = \dot{q}`, and :math:`b = \dot{q}_t`. The implementation of this class is inspired by [1] (which is licensed under the LGPLv2). diff --git a/pyrobolearn/priorities/tasks/velocity/minimum_effort.py b/pyrobolearn/priorities/tasks/velocity/minimum_effort.py index b62abb3..2e94b07 100644 --- a/pyrobolearn/priorities/tasks/velocity/minimum_effort.py +++ b/pyrobolearn/priorities/tasks/velocity/minimum_effort.py @@ -8,8 +8,6 @@ References: - [1] "OpenSoT: A whole-body control library for the compliant humanoid robot COMAN", Rocchi et al., 2015 """ -# TODO: finish to implement this - import numpy as np from pyrobolearn.priorities.tasks import JointVelocityTask diff --git a/pyrobolearn/priorities/tasks/velocity/postural.py b/pyrobolearn/priorities/tasks/velocity/postural.py index a864260..186061c 100644 --- a/pyrobolearn/priorities/tasks/velocity/postural.py +++ b/pyrobolearn/priorities/tasks/velocity/postural.py @@ -147,6 +147,8 @@ class PosturalTask(JointVelocityTask): @kp.setter def kp(self, kp): """Set the stiffness gain.""" + if kp is None: + kp = 1. if not isinstance(kp, (float, int, np.ndarray)): raise TypeError("Expecting the given stiffness gain kp to be an int, float, np.array, instead got: " "{}".format(type(kp)))