From 9cac32be9d449f6e19f29461ab849696f223013c Mon Sep 17 00:00:00 2001 From: Brian Delhaisse Date: Sat, 31 Aug 2019 06:03:34 +0200 Subject: [PATCH] update priorities --- .../priorities/constraints/constraint.py | 166 ++++--- .../constraints/velocity/joint_limits.py | 4 +- pyrobolearn/priorities/ik.py | 71 +++ pyrobolearn/priorities/models/model.py | 18 +- pyrobolearn/priorities/models/robot_model.py | 35 +- .../priorities/solvers/qp_task_solver.py | 52 ++- pyrobolearn/priorities/tasks/task.py | 408 +++++++++++++++--- .../tasks/velocity/angular_momentum.py | 104 ++++- .../priorities/tasks/velocity/cartesian.py | 282 +++++++++--- pyrobolearn/priorities/tasks/velocity/com.py | 113 +++-- .../priorities/tasks/velocity/contact.py | 46 +- .../priorities/tasks/velocity/interaction.py | 14 +- .../tasks/velocity/linear_momentum.py | 97 ++++- .../tasks/velocity/minimum_acceleration.py | 23 +- .../tasks/velocity/minimum_effort.py | 4 +- .../tasks/velocity/minimum_velocity.py | 20 +- .../priorities/tasks/velocity/momentum.py | 156 ++++++- .../priorities/tasks/velocity/postural.py | 105 +++-- 18 files changed, 1372 insertions(+), 346 deletions(-) create mode 100644 pyrobolearn/priorities/ik.py diff --git a/pyrobolearn/priorities/constraints/constraint.py b/pyrobolearn/priorities/constraints/constraint.py index d02daa0..1bbf315 100644 --- a/pyrobolearn/priorities/constraints/constraint.py +++ b/pyrobolearn/priorities/constraints/constraint.py @@ -168,6 +168,15 @@ class Constraint(object): self.constraints = constraints self.model = model + # variables to be set in the corresponding child classes + self._lower_bound = None + self._upper_bound = None + self._A_eq = None + self._b_eq = None + self._A_ineq = None + self._b_lower_bound = None + self._b_upper_bound = None + ############## # Properties # ############## @@ -195,26 +204,28 @@ class Constraint(object): if constraints is None: constraints = dict() elif isinstance(constraints, dict): - constraints = list(constraints.values()) + equality_constraints = constraints.get(EqualityConstraint, []) + inequality_constraints = constraints.get(InequalityConstraint, []) + constraints = equality_constraints + inequality_constraints elif not isinstance(constraints, (list, tuple)): constraints = [constraints] constraint_dict = dict() for i, constraint in enumerate(constraints): - # if not isinstance(constraint, Constraint): - # raise TypeError("Expecting the given {}th constraint to be an instance of `Constraint`, instead " - # "got: {}".format(i, type(constraint))) if isinstance(constraint, EqualityConstraint): constraint_dict.setdefault(EqualityConstraint, []).append(constraint) elif isinstance(constraint, InequalityConstraint): - constraint_dict.setdefault(InequalityConstraint, dict()) - d = constraint_dict[InequalityConstraint] + constraint_dict.setdefault(InequalityConstraint, []).append(constraint) if isinstance(constraint, BoundConstraint): - d.setdefault(BoundConstraint, []).append(constraint) + constraint_dict.setdefault(BoundConstraint, []).append(constraint) elif isinstance(constraint, UnilateralConstraint): - d.setdefault(UnilateralConstraint, []).append(constraint) + constraint_dict.setdefault(UnilateralConstraint, []).append(constraint) + if isinstance(constraint, LowerUnilateralConstraint): + constraint_dict.setdefault(LowerUnilateralConstraint, []).append(constraint) + elif isinstance(constraint, UpperUnilateralConstraint): + constraint_dict.setdefault(UpperUnilateralConstraint, []).append(constraint) elif isinstance(constraint, BilateralConstraint): - d.setdefault(BilateralConstraint, []).append(constraint) + constraint_dict.setdefault(BilateralConstraint, []).append(constraint) else: raise TypeError("Expecting the {}th inequality constraint to be an instance of `BoundConstraint`," " `UnilateralConstraint`, `BilateralConstraint`, but got: " @@ -222,6 +233,7 @@ class Constraint(object): else: raise TypeError("Expecting the {}th constraint to be an instance of `EqualityConstraint` or " "`InequalityConstraint`, but got: {}".format(i, type(constraint))) + self._constraints = constraint_dict @property @@ -232,9 +244,9 @@ class Constraint(object): np.array[float[N]]: lower bound. """ if self.constraints: - constraints = self.constraints.get(InequalityConstraint, {}).get(BoundConstraint, []) + constraints = self.constraints.get(BoundConstraint, []) return [constraint.lower_bound for constraint in constraints] - return self._get_lower_bound() + return self._lower_bound @property def upper_bound(self): @@ -244,9 +256,9 @@ class Constraint(object): np.array[float[N]]: upper bound. """ if self.constraints: - constraints = self.constraints.get(InequalityConstraint, {}).get(BoundConstraint, []) + constraints = self.constraints.get(BoundConstraint, []) return [constraint.lower_bound for constraint in constraints] - return self._get_upper_bound() + return self._upper_bound @property def A_eq(self): @@ -258,7 +270,7 @@ class Constraint(object): if self.constraints: constraints = self.constraints.get(EqualityConstraint, []) return [constraint.A_eq for constraint in constraints] - return self._get_equality_matrix() + return self._A_eq @property def b_eq(self): @@ -269,8 +281,8 @@ class Constraint(object): """ if self.constraints: constraints = self.constraints.get(EqualityConstraint, []) - return [constraint.A_eq for constraint in constraints] - return self._get_equality_matrix() + return [constraint.b_eq for constraint in constraints] + return self._b_eq @property def A_ineq(self): @@ -279,6 +291,10 @@ class Constraint(object): Returns: np.array[float[N,N]]: inequality constraint matrix. """ + if self.constraints: + unilateral_constraints = self.constraints.get(UnilateralConstraint, []) + bilateral_constraints = self.constraints.get(BilateralConstraint, []) + return [constraint.A_ineq for constraint in unilateral_constraints + bilateral_constraints] return self._A_ineq @property @@ -288,6 +304,10 @@ class Constraint(object): Returns: np.array[float[N]]: inequality constraint lower bound vector. """ + if self.constraints: + unilateral_constraints = self.constraints.get(LowerUnilateralConstraint, []) + bilateral_constraints = self.constraints.get(BilateralConstraint, []) + return [constraint.b_lower_bound for constraint in unilateral_constraints + bilateral_constraints] return self._b_lower_bound @property @@ -297,6 +317,10 @@ class Constraint(object): Returns: np.array[float[N]]: inequality constraint upper bound vector. """ + if self.constraints: + unilateral_constraints = self.constraints.get(UpperUnilateralConstraint, []) + bilateral_constraints = self.constraints.get(BilateralConstraint, []) + return [constraint.b_upper_bound for constraint in unilateral_constraints + bilateral_constraints] return self._b_upper_bound @property @@ -306,10 +330,36 @@ class Constraint(object): Returns: np.array[float[N,N]]: inequality constraint matrix. """ - # TODO: improve this - G_lb = [np.identity(len(lb_)) for lb_ in self.lower_bound] - G_ub = [np.identity(len(ub_)) for ub_ in self.upper_bound] - return np.concatenate((G_lb + G_ub,)) + if self.constraints: + results = [] + bound_constraints = self.constraints.get(BoundConstraint, []) + for constraint in bound_constraints: + x_size = len(constraint.lower_bound) + results.append(-np.identity(x_size)) + results.append(np.identity(x_size)) + bilateral_constraints = self.constraints.get(BilateralConstraint, []) + for constraint in bilateral_constraints: + results.append(-constraint.A_ineq) + results.append(constraint.A_ineq) + lower_unilateral_constraints = self.constraints.get(LowerUnilateralConstraint, []) + for constraint in lower_unilateral_constraints: + results.append(-constraint.A_ineq) + upper_unilateral_constraints = self.constraints.get(UpperUnilateralConstraint, []) + for constraint in upper_unilateral_constraints: + results.append(constraint.A_ineq) + + if results: + return np.concatenate(results) + else: + if isinstance(self, BoundConstraint): + x_size = len(self._lower_bound) + return np.concatenate((-np.identity(x_size), np.identity(x_size))) + elif isinstance(self, BilateralConstraint): + return np.concatenate((-self._A_ineq, self._A_ineq)) + elif isinstance(self, LowerUnilateralConstraint): + return -self._A_ineq + elif isinstance(self, UpperUnilateralConstraint): + return self._A_ineq @property def h(self): @@ -318,11 +368,34 @@ class Constraint(object): Returns: np.array[float[N]]: inequality constraint vector. """ - lb = self.lower_bound - ub = self.upper_bound - b_lb = self.b_lower_bound - b_ub = self.b_upper_bound - return np.concatenate(-lb, ub, -b_lb, b_ub) + if self.constraints: + results = [] + bound_constraints = self.constraints.get(BoundConstraint, []) + for constraint in bound_constraints: + results.append(-constraint.lower_bound) + results.append(constraint.upper_bound) + bilateral_constraints = self.constraints.get(BilateralConstraint, []) + for constraint in bilateral_constraints: + results.append(-constraint.b_lower_bound) + results.append(constraint.b_upper_bound) + lower_unilateral_constraints = self.constraints.get(LowerUnilateralConstraint, []) + for constraint in lower_unilateral_constraints: + results.append(-constraint.b_lower_bound) + upper_unilateral_constraints = self.constraints.get(UpperUnilateralConstraint, []) + for constraint in upper_unilateral_constraints: + results.append(constraint.b_upper_bound) + + if results: + return np.concatenate(results) + else: + if isinstance(self, BoundConstraint): + return np.concatenate((-self._lower_bound, self._upper_bound)) + elif isinstance(self, BilateralConstraint): + return np.concatenate((-self._b_lower_bound, self._upper_bound)) + elif isinstance(self, LowerUnilateralConstraint): + return -self._b_lower_bound + elif isinstance(self, UpperUnilateralConstraint): + return self._b_upper_bound @property def F(self): @@ -363,7 +436,8 @@ class Constraint(object): # # @staticmethod # def is_unilateral_constraint(): - # r"""Return True if it is a unilateral constraint: math:`b_l \leq A_{ineq} x` xor :math:`A_{ineq} x \leq b_u`.""" + # r"""Return True if it is a unilateral constraint: math:`b_l \leq A_{ineq} x` xor :math:`A_{ineq} x + # \leq b_u`.""" # return False # # @staticmethod @@ -402,13 +476,16 @@ class Constraint(object): self.constraints.setdefault(EqualityConstraint, []).append(constraint) elif isinstance(constraint, InequalityConstraint): self.constraints.setdefault(InequalityConstraint, dict()) - d = self.constraints[InequalityConstraint] if isinstance(constraint, BoundConstraint): - d.setdefault(BoundConstraint, []).append(constraint) + self.constraints.setdefault(BoundConstraint, []).append(constraint) elif isinstance(constraint, UnilateralConstraint): - d.setdefault(UnilateralConstraint, []).append(constraint) + self.constraints.setdefault(UnilateralConstraint, []).append(constraint) + if isinstance(constraint, LowerUnilateralConstraint): + self.constraints.setdefault(LowerUnilateralConstraint, []).append(constraint) + elif isinstance(constraint, UpperUnilateralConstraint): + self.constraints.setdefault(UpperUnilateralConstraint, []).append(constraint) elif isinstance(constraint, BilateralConstraint): - d.setdefault(BilateralConstraint, []).append(constraint) + self.constraints.setdefault(BilateralConstraint, []).append(constraint) else: raise TypeError("Expecting the inequality constraint to be an instance of `BoundConstraint`, " "`UnilateralConstraint`, `BilateralConstraint`, but got: {}".format(type(constraint))) @@ -424,27 +501,6 @@ class Constraint(object): """ pass - def _get_lower_bound(self): - return [] - - def _get_upper_bound(self): - return [] - - def _get_equality_matrix(self): - return [] - - def _get_equality_vector(self): - return [] - - def _get_inequality_matrix(self): - return [] - - def _get_inequality_lower_bound(self): - return [] - - def _get_inequality_upper_bound(self): - return [] - ############# # Operators # ############# @@ -504,6 +560,14 @@ class Constraint(object): return Constraint() +class NullConstraint(Constraint): + r"""Null constraint. + + This is a dummy constraint which is used to specify that no constraints are used. + """ + pass + + class EqualityConstraint(Constraint): r"""Equality constraint.""" pass diff --git a/pyrobolearn/priorities/constraints/velocity/joint_limits.py b/pyrobolearn/priorities/constraints/velocity/joint_limits.py index 8deb87b..dadfa03 100644 --- a/pyrobolearn/priorities/constraints/velocity/joint_limits.py +++ b/pyrobolearn/priorities/constraints/velocity/joint_limits.py @@ -52,8 +52,8 @@ class JointPositionLimitsConstraint(BoundConstraint, JointVelocityConstraint): self.dt = dt bounds = self.model.get_joint_bounds() - self.lower_bound = bounds[0] - self.upper_bound = bounds[1] + self._lower_bound = bounds[0] + self._upper_bound = bounds[1] def update(self): r""" diff --git a/pyrobolearn/priorities/ik.py b/pyrobolearn/priorities/ik.py new file mode 100644 index 0000000..2de1f3a --- /dev/null +++ b/pyrobolearn/priorities/ik.py @@ -0,0 +1,71 @@ + + +import numpy as np +import time +import pyrobolearn as prl + + +# Create simulator +sim = prl.simulators.Bullet() + +# create world +world = prl.worlds.BasicWorld(sim) + +# create robot +robot = world.load_robot('kuka_iiwa') + +# define useful variables for IK +link_id = robot.get_end_effector_ids(end_effector=0) +joint_ids = robot.joints # actuated joint +wrt_link_id = None # robot.get_link_ids('iiwa_link_1') +q_idx = robot.get_q_indices(joint_ids) + +# create sphere to follow +x_des = np.array([0.5, 0., 1.]) +quat_des = np.array([0., 0., 0., 1.]) +sphere = world.load_visual_sphere(position=x_des, radius=0.05, color=(1, 0, 0, 0.5), return_body=True) + +# create task +model = prl.priorities.models.RobotModelInterface(robot) +cartesian_task = prl.priorities.tasks.velocity.CartesianTask(model, distal_link=link_id, base_link=wrt_link_id, + desired_position=x_des, kp_position=50.) + # desired_orientation=quat_des, kp_orientation=50.) +q_desired = [1.448e-03, 2.790e-01, -2.199e-03, -1.013, 5.948e-04, -1.293, 3.882e-04] +postural_task = prl.priorities.tasks.velocity.PosturalTask(model, q_desired=q_desired, kp=50.) +# task = cartesian_task +# task = postural_task +# task = 1 * cartesian_task + 1 * postural_task +task = cartesian_task / postural_task +print("\nTask: \n{}\n".format(task)) +solver = prl.priorities.solvers.QPTaskSolver(task=task) + + +# define amplitude and angular velocity when moving the sphere +w = 0.01 +r = 0.2 + +# run simulation +times = [] +for t in prl.count(): + # move sphere + sphere.position = np.array([0.5, r * np.cos(w*t + np.pi/2), (1.-r) + r * np.sin(w*t + np.pi/2)]) + # cartesian_task.set_desired_references(desired_position=sphere.position) + cartesian_task.desired_position = sphere.position + task.update(update_model=True) + + q = robot.get_joint_positions() + start = time.time() + dq = solver.solve() + end = time.time() + times.append(end - start) + + if (t+1) % 1000 == 0: + print("solving time: avg={}, std={}".format(np.mean(times), np.std(times))) + times = [] + + # set joint positions + q = q[q_idx] + dq * sim.dt + robot.set_joint_positions(q, joint_ids=joint_ids) + + # step in simulation + world.step(sleep_dt=sim.dt) diff --git a/pyrobolearn/priorities/models/model.py b/pyrobolearn/priorities/models/model.py index 39eb64c..c3e5f61 100644 --- a/pyrobolearn/priorities/models/model.py +++ b/pyrobolearn/priorities/models/model.py @@ -71,6 +71,11 @@ class ModelInterface(object): """Return the number of degrees of freedom.""" raise NotImplementedError + @property + def num_actuated_joints(self): + """Return the number of actuated joints.""" + raise NotImplementedError + ########### # Methods # ########### @@ -212,7 +217,7 @@ class ModelInterface(object): """ pass - def get_jacobian(self, link, wrt_link=None, point=(0., 0., 0.)): + def get_jacobian(self, link, wrt_link=None, frame=None, point=(0., 0., 0.)): r""" Get the 6D Jacobian for a point on a link, that when multiplied with :math:`\dot{q}` gives a 6D vector that has the angular velocity as the first three entries and the linear velocity as the last three entries. @@ -225,6 +230,8 @@ class ModelInterface(object): link (int, str): unique link id, or name. wrt_link (int, str, None): unique link id, or name. If specified, it will take the relative jacobian. If None, the jacobian will be taken with respect to the world frame. + frame (int, str, None): unique link id, or name. If specified, it will express the final jacobian in that + specified frame. point (np.array[float[3]]): position of the point in link's local frame. Returns: @@ -401,6 +408,15 @@ class ModelInterface(object): """ pass + def get_centroidal_momentum_matrix(self): + r""" + Return the centroidal momentum matrix. + + Returns: + np.array[float[6,6+N]]: the centroidal momentum matrix :math:`A_G` + """ + pass + def update(self): """ This is to notify the model interface that we moved to the next time step :math:`t \rightarrow t+1`. diff --git a/pyrobolearn/priorities/models/robot_model.py b/pyrobolearn/priorities/models/robot_model.py index 4e87e3f..6b819d6 100644 --- a/pyrobolearn/priorities/models/robot_model.py +++ b/pyrobolearn/priorities/models/robot_model.py @@ -184,12 +184,15 @@ class RobotModelInterface(ModelInterface): Returns: if full: - np.array[float[6,N]]: CoM Jacobian (concatenation of the angular and linear Jacobian, where N is the + np.array[float[6,N]]: CoM Jacobian (concatenation of the linear and angular Jacobian, where N is the number of DoFs) else: np.array[float[3,N]]: CoM Jacobian (only the linear part) """ - return self.model.get_center_of_mass_jacobian() + jac = self.model.get_center_of_mass_jacobian() + if full: + return jac + return jac[:3] def get_gravity(self): """ @@ -218,7 +221,7 @@ class RobotModelInterface(ModelInterface): """ return self.model.get_link_names(self.model.joints) - def get_jacobian(self, link, wrt_link=None, point=(0., 0., 0.)): + def get_jacobian(self, link, wrt_link=None, frame=None, point=(0., 0., 0.)): r""" Get the 6D Jacobian for a point on a link, that when multiplied with :math:`\dot{q}` gives a 6D vector that has the angular velocity as the first three entries and the linear velocity as the last three entries. @@ -234,6 +237,8 @@ class RobotModelInterface(ModelInterface): link (int): unique link id. wrt_link (int, str, None): unique link id, or name. If specified, it will take the relative jacobian. If None, the jacobian will be taken with respect to the world frame. + frame (int, str, None): unique link id, or name. If specified, it will express the final jacobian in that + specified frame. point (np.array[float[3]], None): the point on the specified link to compute the Jacobian (in link local coordinates around its center of mass). If None, it will use the CoM position (in the link frame). @@ -242,17 +247,21 @@ class RobotModelInterface(ModelInterface): """ link = self.get_link_id(link) wrt_link = None if wrt_link is None else self.get_link_id(wrt_link) + frame = None if frame is None else self.get_link_id(frame) # if the jacobian is already cached, return it - if ('J', link, wrt_link, tuple(point)) in self._states: - return self._states[('J', link, point)] + if ('J', link, wrt_link, frame, tuple(point)) in self._states: + return self._states[('J', link, wrt_link, frame, tuple(point))] # get the jacobian, cache it, and return it if wrt_link is None: jacobian = self.model.get_jacobian(link_id=link, local_position=point) else: jacobian = self.model.get_relative_jacobian(link_id=link, wrt_link_id=wrt_link, local_position=point) - self._states[('J', link, wrt_link, tuple(point))] = jacobian + if frame is not None: + jacobian = self.model.express_jacobian_in_frame(jacobian, link_id=frame) + + self._states[('J', link, wrt_link, frame, tuple(point))] = jacobian return jacobian def get_pose(self, link, wrt_link=None, point=(0., 0., 0.)): # TODO: use point @@ -478,7 +487,17 @@ class RobotModelInterface(ModelInterface): link = self.get_link_id(link) return self.model.get_link_world_accelerations(link) - def update(self, q=None, dq=None, ddq=None): + def get_centroidal_momentum_matrix(self): + r""" + Return the centroidal momentum matrix. + + Returns: + np.array[float[6,6+N]]: the centroidal momentum matrix :math:`A_G` + """ + return self.model.get_centroidal_momentum_matrix() + + def update(self, q=None, dq=None, ddq=None, update_model=False): """Update: move to the next step.""" self._states = dict() - self.model.step() + if update_model: + self.model.step() diff --git a/pyrobolearn/priorities/solvers/qp_task_solver.py b/pyrobolearn/priorities/solvers/qp_task_solver.py index f2b7b2e..57fa7d4 100644 --- a/pyrobolearn/priorities/solvers/qp_task_solver.py +++ b/pyrobolearn/priorities/solvers/qp_task_solver.py @@ -126,7 +126,7 @@ class QPTaskSolver(TaskSolver): The QP task solver uses QP to solve a task or stack of tasks. """ - def __init__(self, task, method='quadprog'): + def __init__(self, task, method='quadprog', epsilon=1.e-8): """ Initialize the task solver. @@ -134,9 +134,12 @@ class QPTaskSolver(TaskSolver): task (Task): Priority tasks. method (str): QP method/library to use. Select between ['cvxopt', 'cvxpy', 'ecos', 'gurobi', 'mosek', 'osqp', 'qpoases', 'quadprog'] + epsilon (float): this small amount is added to the diagonal elements of the quadratic matrix such that it + is positive definite. """ solver = QP(method=method) super(QPTaskSolver, self).__init__(task, solver) + self.epsilon = epsilon ############## # Properties # @@ -163,7 +166,7 @@ class QPTaskSolver(TaskSolver): """Update the priority task; compute the matrices and vectors to be used later in the `solve` method.""" self.task.update() - def solve(self, x0=None): + def solve(self, x0=None, update=False): """Solve the priority task. Args: @@ -172,12 +175,15 @@ class QPTaskSolver(TaskSolver): Returns: np.array[float[N]]: the optimized variables. """ + # update if necessary + if update: + self.update() + # get task objectives and constraints # objectives As = self.task.A - bs = self.task.b - cs = self.task.c - Ws = self.task.W + Qs = self.task.Q + ps = self.task.p # constraints Gs = self.task.G hs = self.task.h @@ -186,23 +192,41 @@ class QPTaskSolver(TaskSolver): # if not a stack of tasks, just transform the task objectives/constraints into lists if not self.task.is_stack_of_tasks(): - As, bs, cs, Ws = [As], [bs], [cs], [Ws] - Gs, hs, Fs, ks = [Gs], [hs], [Fs], [ks] + As, Qs, ps = [As], [Qs], [ps] # solve x_opt, x_opts, x_projs = x0, [], [] for i in range(len(As)): - var = As[i].T.dot(Ws[i]) - Q = var.dot(As[i]) - p = cs[i] - var.dot(bs[i]) - G = np.concatenate(Gs[:i+1]) - h = np.concatenate(hs[:i+1]) - F = np.concatenate(Fs[:i+1] + As[:i]) - k = np.concatenate(ks[:i+1] + x_projs[:i]) + # objectives + Q = Qs[i] + p = ps[i] + + # constraints + G = Gs[:i+1] + G = np.concatenate(G) if len(G) > 0 else None + h = hs[:i+1] + h = np.concatenate(h) if len(h) > 0 else None + F = Fs[:i+1] + As[:i] + F = np.concatenate(F) if len(F) > 0 else None + k = ks[:i+1] + x_projs[:i] + k = np.concatenate(k) if len(k) > 0 else None + + # make sure that Q is PD + diag_Q = np.einsum('ii->i', Q) + diag_Q += self.epsilon + # print("evals(Q): ", np.linalg.eigvals(Q)) + + # print("Q: ", Q.shape) + # print("p: ", p.shape) + # print("G: ", G.shape) + # print("h: ", h.shape) + # print("F: ", F.shape) + # print("k: ", k.shape) # solve (by starting from previous optimized solution) x_opt = self.solver.optimize(Q=Q, p=p, x0=x_opt, G=G, h=h, A=F, b=k) x_opts.append(x_opt) + # print("Loss: {}".format(self.task.loss(x_opt))) # project best solution (will be used later in the stack for constraints) if i < len(As) - 1: diff --git a/pyrobolearn/priorities/tasks/task.py b/pyrobolearn/priorities/tasks/task.py index a88124b..6a58507 100644 --- a/pyrobolearn/priorities/tasks/task.py +++ b/pyrobolearn/priorities/tasks/task.py @@ -109,9 +109,10 @@ References: """ import numpy as np +import copy from pyrobolearn.priorities.models import ModelInterface -from pyrobolearn.priorities.constraints.constraint import Constraint +from pyrobolearn.priorities.constraints.constraint import Constraint, NullConstraint __author__ = "Brian Delhaisse" @@ -152,7 +153,9 @@ class Task(object): have to be weighted together. model (ModelInterface, None): robotic model interface associated to the task. weight (float, np.array[float[M,M]]): weight scalar or matrix associated to the task. - constraints (list[Constraint]): list of constraints associated to the task. + constraints (list[Constraint]): list of constraints associated to the task. If it is a single task, it can + only contains one constraint. If we have a stack of tasks, the list should have the same size as the + number of hard tasks. If not, it will append `NullConstraint`. """ # set and check each given parameter self.tasks = stack_of_tasks @@ -168,10 +171,12 @@ class Task(object): # define task matrix and vector if self.is_single_task(): # set the number of variables to optimize - # self._x_size = self.model.num_dofs - self._A = np.identity(1) # None - self._b = np.zeros(1) # None - self._c = np.zeros(1) # None + x_size = self.x_size + if x_size == 0: + x_size = 1 + self._A = np.identity(x_size) # None + self._b = np.zeros(x_size) # None + self._c = np.zeros(x_size) # None ############## # Properties # @@ -214,6 +219,8 @@ class Task(object): @property def model(self): """Return the model interface.""" + if self.is_stack_of_tasks(): + return self.tasks[0][0].model return self._model @model.setter @@ -255,6 +262,13 @@ class Task(object): """Return the depth of the stack of tasks.""" return len(self.tasks) + @property + def constraint(self): + """Return the single constraint if single task, otherwise return the list of constraints.""" + if len(self._constraints) > 0 and self.is_single_task(): + return self._constraints[0] + return self._constraints + @property def constraints(self): """Return the constraints.""" @@ -278,6 +292,21 @@ class Task(object): raise TypeError("The {}th given constraint is not an instance of `Constraint`, instead got: " "{}".format(i, type(constraint))) + if not isinstance(constraints, list): + raise TypeError("Expecting a list of Constraints.") + + # make the number of hard task match the number of constraints + if self.is_stack_of_tasks(): + if len(self.tasks) > len(constraints): + constraints = constraints + [NullConstraint() for _ in range(len(self.tasks) - len(constraints))] + elif len(self.tasks) < len(constraints): + constraints = constraints[:len(self.tasks)] + else: # single task + if len(constraints) == 0: # create dummy constraint + constraints = [NullConstraint()] + elif len(constraints) > 1: # keep first constraint + constraints = constraints[:1] + # set the constraints associated with the task self._constraints = constraints @@ -286,7 +315,7 @@ class Task(object): """Return the number of variables being optimized.""" # return self._x_size if self.model is not None: - return self.model.num_dofs + return self.model.num_actuated_joints return 0 @property @@ -313,6 +342,19 @@ class Task(object): return [np.concatenate([soft_task.A for soft_task in hard_task]) for hard_task in self.tasks] return self._A + @property + def As(self): + r"""Return the A matrices from :math:`||Ax - b||^2` used in QP, for each task. + + Warnings: this does not concatenate the A matrices. + + Returns: + list[list[np.array[float[M,N]]]]: the A matrices (for each task in the stack of tasks). + """ + if self.is_stack_of_tasks(): + return [[soft_task.A for soft_task in hard_task] for hard_task in self.tasks] + return [[self._A]] + @property def b(self): r"""Return the b vector from :math:`||Ax - b||^2` used in QP. @@ -324,36 +366,118 @@ class Task(object): return [np.concatenate([soft_task.b for soft_task in hard_task]) for hard_task in self.tasks] return self._b + @property + def bs(self): + r"""Return the b vectors from :math:`||Ax - b||^2` used in QP, for each task. + + Warnings: this does not concatenate the b vectors. + + Returns: + list[list[np.array[float[M]]]]: the b vectors (for each task in the stack of tasks). + """ + if self.is_stack_of_tasks(): + return [[soft_task.b for soft_task in hard_task] for hard_task in self.tasks] + return [[self._b]] + @property def c(self): - r"""Return the c vector from :math:`||Ax - b||^2 + c^\top x` used in QP.""" + r"""Return the c vector from :math:`||Ax - b||^2 + c^\top x` used in QP. + + Returns: + np.array[float[M]]: c vector. + """ if self.is_stack_of_tasks(): # if not a single task return [np.concatenate([soft_task.c for soft_task in hard_task]) for hard_task in self.tasks] return self._c + @property + def cs(self): + r"""Return the c vectors from :math:`||Ax - b||^2 + c^\top x` used in QP, for each task. + + Warnings: this does not concatenate the c vectors. + + Returns: + list[list[np.array[float[M]]]]: the c vectors (for each task in the stack of tasks). + """ + if self.is_stack_of_tasks(): + return [[soft_task.c for soft_task in hard_task] for hard_task in self.tasks] + return [[self._c]] + @property def W(self): r"""Return the weights.""" if self.is_stack_of_tasks(): # if not a single task - return [np.concatenate([soft_task.W for soft_task in hard_task]) for hard_task in self.tasks] + return [[soft_task.W for soft_task in hard_task] for hard_task in self.tasks] return self.weight + @property + def Ws(self): + r"""Return the weights for each task. + + Warnings: this does not concatenate the weight matrices/scalars. + + Returns: + list[list[np.array[float[M,M]]]], list[list[float]]: the weights (for each task in the stack of tasks). + """ + if self.is_stack_of_tasks(): # if not a single task + return [[soft_task.W for soft_task in hard_task] for hard_task in self.tasks] + return [[self.weight]] + @property def Q(self): r"""Return the Q matrix :math:`Q = A^\top W A` used in :math:`\frac{1}{2} x^T Q x + p^T x` for QP.""" if self.is_stack_of_tasks(): # if not a single task - return [np.concatenate([soft_task.Q for soft_task in hard_task]) for hard_task in self.tasks] + Qs = [] + for hard_task in self.tasks: + Q_ = np.concatenate([np.dot(np.sqrt(soft_task.weight), soft_task.A) for soft_task in hard_task]) + Qs.append(Q_.T.dot(Q_)) + return Qs return self._A.T.dot(self.weight).dot(self._A) + @property + def Qs(self): + r"""Return the Q matrices :math:`Q = A^\top W A` used in :math:`\frac{1}{2} x^T Q x + p^T x` for QP, for each + task. + + Warnings: this does not concatenate the Q matrices. + + Returns: + list[list[np.array[float[M,M]]]]: Q matrices (for each task in the stack of tasks). + """ + if self.is_stack_of_tasks(): # if not a single task + return [[soft_task.Q for soft_task in hard_task] for hard_task in self.tasks] + return [[self._A.T.dot(self.weight).dot(self._A)]] + @property def p(self): r"""Return the p vector :math:`p = (c - 2 A^\top W b)` used in :math:`\frac{1}{2} x^T Q x + p^T x` for QP.""" if self.is_stack_of_tasks(): # if not a single task - return [np.concatenate([soft_task.p for soft_task in hard_task]) for hard_task in self.tasks] - return self.c - self._A.T.dot(self.weight).dot(self._b) + ps = [] + for hard_task in self.tasks: + AW = np.concatenate([np.dot(soft_task.A.T, soft_task.weight) for soft_task in hard_task], axis=1) + b = np.concatenate([soft_task.b for soft_task in hard_task]) + c = hard_task[0].c + print(c.shape) + print(b.shape) + print(AW.shape) + ps.append(c - 2 * AW.dot(b)) + return ps + return self._c - 2 * self._A.T.dot(self.weight).dot(self._b) - # TODO: constraints + @property + def ps(self): + r"""Return the p vectors :math:`p = (c - 2 A^\top W b)` used in :math:`\frac{1}{2} x^T Q x + p^T x` + for QP, for each task. + + Warnings: this does not concatenate the p vectors. + + Returns: + list[list[np.array[float[M]]]]: the p vectors (for each task in the stack of tasks). + """ + if self.is_stack_of_tasks(): # if not a single task + return [[soft_task.p for soft_task in hard_task] for hard_task in self.tasks] + return [[self._c - self._A.T.dot(self.weight).dot(self._b)]] @property def lower_bound(self): @@ -362,7 +486,11 @@ class Task(object): Returns: np.array[float[N]]: lower bound. """ - results = [constraint.lower_bound for constraint in self.constraints] + results = [] + for constraint in self.constraints: + b = constraint.lower_bound + if b is not None: + results.append(b) if len(results) == 1: return results[0] return results @@ -374,7 +502,11 @@ class Task(object): Returns: np.array[float[N]]: upper bound. """ - results = [constraint.upper_bound for constraint in self.constraints] + results = [] + for constraint in self.constraints: + b = constraint.upper_bound + if b is not None: + results.append(b) if len(results) == 1: return results[0] return results @@ -386,7 +518,11 @@ class Task(object): Returns: np.array[float[N,N]]: equality constraint matrix. """ - results = [constraint.A_eq for constraint in self.constraints] + results = [] + for constraint in self.constraints: + A = constraint.A_eq + if A is not None: + results.append(A) if len(results) == 1: return results[0] return results @@ -398,7 +534,11 @@ class Task(object): Returns: np.array[float[N]]: equality constraint vector. """ - results = [constraint.b_eq for constraint in self.constraints] + results = [] + for constraint in self.constraints: + b = constraint.b_eq + if b is not None: + results.append(b) if len(results) == 1: return results[0] return results @@ -410,7 +550,11 @@ class Task(object): Returns: np.array[float[N,N]]: inequality constraint matrix. """ - results = [constraint.A_ineq for constraint in self.constraints] + results = [] + for constraint in self.constraints: + A = constraint.A_ineq + if A is not None: + results.append(A) if len(results) == 1: return results[0] return results @@ -422,7 +566,11 @@ class Task(object): Returns: np.array[float[N]]: inequality constraint lower bound vector. """ - results = [constraint.b_lower_bound for constraint in self.constraints] + results = [] + for constraint in self.constraints: + b = constraint.b_lower_bound + if b is not None: + results.append(b) if len(results) == 1: return results[0] return results @@ -434,7 +582,11 @@ class Task(object): Returns: np.array[float[N]]: inequality constraint upper bound vector. """ - results = [constraint.b_upper_bound for constraint in self.constraints] + results = [] + for constraint in self.constraints: + b = constraint.b_upper_bound + if b is not None: + results.append(b) if len(results) == 1: return results[0] return results @@ -444,11 +596,15 @@ class Task(object): r"""Return the inequality constraint matrix :math:`G` used in inequality constraints :math:`Gx \leq h` in QP. Returns: - np.array[float[N,N]]: inequality constraint matrix. + list[np.array[float[N,N]]]: list of inequality constraint matrix. """ - results = [constraint.G for constraint in self.constraints] - if len(results) == 1: - return results[0] + results = [] + for constraint in self.constraints: + G = constraint.G + if G is not None: + results.append(G) + # if len(results) == 1: + # return results[0] return results @property @@ -456,11 +612,15 @@ class Task(object): r"""Return the inequality constraint vector :math:`h` used in inequality constraints :math:`Gx \leq h` in QP. Returns: - np.array[float[N]]: inequality constraint vector. + list[np.array[float[N]]]: list of inequality constraint vector. """ - results = [constraint.h for constraint in self.constraints] - if len(results) == 1: - return results[0] + results = [] + for constraint in self.constraints: + h = constraint.h + if h is not None: + results.append(h) + # if len(results) == 1: + # return results[0] return results @property @@ -468,11 +628,15 @@ class Task(object): r"""Return the equality constraint matrix :math:`F` used in equality constraints :math:`Fx = k` in QP. Returns: - np.array[float[N,N]]: equality constraint matrix. + list[np.array[float[N,N]]]: list of equality constraint matrix. """ - results = [constraint.F for constraint in self.constraints] - if len(results) == 1: - return results[0] + results = [] + for constraint in self.constraints: + F = constraint.F + if F is not None: + results.append(F) + # if len(results) == 1: + # return results[0] return results @property @@ -480,11 +644,15 @@ class Task(object): r"""Return the equality constraint vector :math:`c` used in equality constraints :math:`Fx = k` in QP. Returns: - np.array[float[N]]: equality constraint vector. + list[np.array[float[N]]]: list of equality constraint vector. """ - results = [constraint.k for constraint in self.constraints] - if len(results) == 1: - return results[0] + results = [] + for constraint in self.constraints: + k = constraint.k + if k is not None: + results.append(k) + # if len(results) == 1: + # return results[0] return results ################## @@ -543,11 +711,16 @@ class Task(object): if not self.is_stack_of_tasks(): raise ValueError("The current task is not a stack of tasks... This method can not be called for a " "particular task, but only for the `Task` instance.") - if not task.is_stack_of_tasks(): - task = [task] - for t in task: - self.tasks.append(t) - self.constraints.append(t.constraints) + + constraints = task.constraints + if task.is_stack_of_tasks(): + task = task.tasks + else: + task = [[task]] + + for hard_task, constraint in zip(task, constraints): + self.tasks.append(hard_task) + self.constraints.append(constraint) def add_soft_task(self, task): """Add the given soft task. @@ -561,7 +734,21 @@ class Task(object): if not self.is_stack_of_tasks(): raise ValueError("The current task is not a stack of tasks... This method can not be called for a " "particular task, but only for the `Task` instance.") - pass + + if task.is_stack_of_tasks(): + for i, hard_task in enumerate(task.tasks): + if i == 0: + self._constraints[-1] = self._constraints[-1] + task.constraints[0] # combine constraint + else: + self.tasks.append([]) # add new layer in the stack + self._constraints.append(task.constraints[i]) # add new constraint + + # add each soft task in current layer + for soft_task in hard_task: + self.tasks[-1].append(soft_task) + else: # task is single task + self.tasks[-1].append(task) + self._constraints[-1] = self._constraints[-1] + task.constraint def get_num_tasks(self): """Return the total number of tasks.""" @@ -625,9 +812,28 @@ class Task(object): x (np.array[float[N]]): joint variables that are being optimized. Returns: - float: loss value + if single task: + float: loss value + else: + list[float]: loss values for each layer in the stack. """ - return np.sum((self._A.dot(x) - self._b) ** 2) + if self.is_stack_of_tasks(): + losses = [] + # ||Ax - b||_{W}^2 + c^\top x = x^\top A^\top W A x - (2 b^\top W A - c^\top) x + b^\top W b + for hard_A, hard_b, hard_c, hard_W in zip(self.As, self.bs, self.cs, self.Ws): + loss = 0 + for A, b, c, W in zip(hard_A, hard_b, hard_c, hard_W): + Ax = A.dot(x) + WAx = np.dot(W, Ax) + loss += Ax.T.dot(WAx) - 2 * b.T.dot(WAx) + c.T.dot(x) + b.T.dot(W).dot(b) + losses.append(loss) + return losses + + # ||Ax - b||_{W}^2 + c^\top x = x^\top A^\top W A x - (2 b^\top W A - c^\top) x + b^\top W b + A, b, c, W = self._A, self._b, self._c, self._weight + Ax = A.dot(x) + WAx = np.dot(W, Ax) + return Ax.T.dot(WAx) - 2 * b.T.dot(WAx) + c.T.dot(x) + b.T.dot(W).dot(b) def _update(self): """Update the task. @@ -637,15 +843,22 @@ class Task(object): """ pass - def update(self): + def update(self, update_model=False): """ Compute the A matrix and b vector that will be used by the task solver. + + Args: + update_model (bool): if True, it will update the model before updating each task. """ - # if stack of tasks, update each task - if self.is_stack_of_tasks(): + # update model if specified + if update_model: + self.model.update() + + # update tasks + if self.is_stack_of_tasks(): # if stack of tasks, update each task for hard_task in self.tasks: for soft_task in hard_task: - soft_task.update() + soft_task.update(update_model=False) else: # if one task, update it self._update() @@ -653,6 +866,32 @@ class Task(object): for constraint in self.constraints: constraint.update() + def lookfor(self, class_type): + """ + Look for the specified task class type/name in the stack of tasks, and returns it. + + Args: + class_type (type, str): class type or name + + Returns: + Task, None: the corresponding instance of the `Task` class. None if it was not found. + """ + # if string, lowercase it + if isinstance(class_type, str): + class_type = class_type.lower() + + # if stack of tasks + if self.is_stack_of_tasks(): + for hard_task in self.tasks: + for soft_task in hard_task: + if soft_task.__class__ == class_type or soft_task.__class__.__name__.lower() == class_type: + return soft_task + + # else, if single task + else: + if self.__class__ == class_type or self.__class__.__name__.lower() == class_type: + return self + ############# # Operators # ############# @@ -681,7 +920,7 @@ class Task(object): """Update the tasks.""" return self.update() - def __add__(self, other): # TODO: check when other has some tasks + def __add__(self, other): """Add a soft priority task. Examples: @@ -697,23 +936,41 @@ class Task(object): # copy current stack of tasks tasks = list(self.tasks) + constraints = list(self.constraints) # if other = stack of tasks, combine each level of tasks if other.is_stack_of_tasks(): for i in range(len(other.tasks)): if i < len(tasks): tasks[i] = tasks[i] + other.tasks[i] + constraints[i] = constraints[i] + other.constraints[i] else: tasks.append(other.tasks[i]) + constraints.append(other.constraints[i]) else: # else, just append the given other task to the last level - tasks[len(tasks) - 1].append(other) + tasks[-1].append(other) + constraints[-1] = constraints[-1] + other.constraint else: if other.is_stack_of_tasks(): tasks = list(other.tasks) - tasks[len(tasks) - 1].append(self) - else: + constraints = list(other.constraints) + tasks[-1].append(self) + constraints[-1] = constraints[-1] + self.constraint + else: # both are single tasks tasks = [[self, other]] - return Task(stack_of_tasks=tasks) + constraints = self.constraints + other.constraints + return Task(stack_of_tasks=tasks, constraints=constraints) + + def __truediv__(self, other): + """Append a hard priority task to the stack of tasks. + + Examples: + task1 = Task(weight=2) + task2 = Task(weight=3) + task = task1 / task2 + print(task) + """ + return self.__div__(other) def __div__(self, other): """Append a hard priority task to the stack of tasks. @@ -757,20 +1014,31 @@ class Task(object): if not isinstance(other, Constraint): raise TypeError("Expecting 'other' to be an instance of Constraint, instead got: {}".format(type(other))) - # if we have a stack of tasks, insert the constraint for all tasks - if self.is_stack_of_tasks(): - for hard_task in self.tasks: - if isinstance(hard_task, list): - for soft_task in hard_task: - soft_task << other - else: - hard_task << other - else: # if we have one task, append the constraint - self.constraints.append(other) + # add constraint + for i, constraint in enumerate(self.constraints): + self.constraints[i] = constraint + other + + # # if we have a stack of tasks, insert the constraint for all tasks + # if self.is_stack_of_tasks(): + # for hard_task in self.tasks: + # if isinstance(hard_task, list): + # for soft_task in hard_task: + # soft_task << other + # else: + # hard_task << other + # else: # if we have one task, append the constraint + # self.constraints.append(other) def __mul__(self, other): - """Multiply the task by a relative weight scalar or matrix.""" + """Multiply the task by a relative weight scalar or matrix. + + Warnings: this is an inplace operation!! + """ + # task = copy.copy(self) + # task.weight = other * task.weight + # return task self.weight = other + return self def __rmul__(self, other): """Multiply the task by a relative weight""" @@ -836,7 +1104,11 @@ class JointTorqueTask(Task): # Tests if __name__ == '__main__': - model = ModelInterface() + import pyrobolearn as prl + + sim = prl.simulators.Bullet(render=False) + robot = prl.robots.KukaIIWA(sim) + model = prl.priorities.models.RobotModelInterface(robot) task1 = Task(model=model, weight=2) task2 = Task(model=model, weight=3) task = Task(stack_of_tasks=[[task1, task2], [task1]]) @@ -844,7 +1116,11 @@ if __name__ == '__main__': print(task2 == task[0, 1]) - task = 1./2 * task1 + 1./3 * task2 + task3 = 1./2 * task1 + print(task1.weight) + print(task3.weight) + + task = 1./2 * task1 + 1./4 * task2 task = task / task1 print(task) diff --git a/pyrobolearn/priorities/tasks/velocity/angular_momentum.py b/pyrobolearn/priorities/tasks/velocity/angular_momentum.py index a58d243..15a353a 100644 --- a/pyrobolearn/priorities/tasks/velocity/angular_momentum.py +++ b/pyrobolearn/priorities/tasks/velocity/angular_momentum.py @@ -1,12 +1,62 @@ #!/usr/bin/env python r"""Provide the angular momentum task. +The is the angular part of the `CentroidalMomentumTask`. The centroidal momentum task tries to minimize the +difference between the desired and current centroidal moment given by: + +.. math:: ||A_G \dot{q} - h_{G,d}||^2 + +where :math:`A_G \in \mathbb{R}^{6 \times N}` is the centroidal momentum matrix (CMM, see below for description), +:math:`\dot{q}` are the joint velocities being optimized, and :math:`h_{G,d}` is the desired centroidal momentum. + +This is equivalent to the QP objective function :math:`||Ax - b||^2`, by setting :math:`A=A_G`, :math:`x=\dot{q}`, +and :math:`b = h_{G,d}`. + +The centroidal momentum (which is the sum of all body spatial momenta computed wrt the CoM) is given by: + +.. math:: h_G = A_G \dot{q} + +where :math:`h_G = [k_G^\top, l_G^\top]^\top \in \mathbb{R}^6` is the centroidal momentum (the subscript :math:`G` +denotes the CoM) with :math:`k_G \in \mathbb{R}^3` being the angular momentum and :math:`l_G \in \mathbb{R}^3` the +linear part, :math:`\dot{q}` are the joint velocities, and :math:`A_G \in \mathbb{R}^{6 \times N}` (with :math:`N` +is the number of DoFs) is the centroidal momentum matrix (CMM). + +"The CMM is computed from the joint space inertia matrix :math:`H(q)`, given by: + +.. math:: A_G = ^1X_G^\top S_1 H(q) = ^1X_G^\top H_1(q) + +where :math:`^1X_G^\top \in \mathbb{R}^{6 \times 6}` is the spatial transformation matrix that transfers spatial +momentum from the floating base (Body 1) to the CoM (G), :math:`H(q)` is the full joint space inertia matrix, +:math:`H_1 = S_1 H` is the floating base (Body 1) inertia matrix selected using the selector matrix +:math:`S_1 = [1_{6 \times 6}, 0_{6 \times (N-6)}}`. + +The spatial transformation matrix is given by: + +.. math:: + + ^1X_G^\top = \left[ \begin{array}{cc} + ^GR_1 & ^GR_1 S(^1p_G)^\top \\ + 0 & ^GR_1 + \\end{array} \right] + +where :math:`^GR_1` is the rotation matrix of :math:`G` wrt the floating base (Body 1), +:math:`^1p_G = ^1R_0 (^0p_G - ^0p_1)` is the position vector from the floating base (Body 1) origin to the CoM +expressed in the floating base (Body 1) frame, :math:`S(\cdot)` provides the skew symmetric cross product matrix +such that :math:`S(p)v = p \cross v`. Note that the orientation of Frame :math:`G` (CoM) could be selected to be +parallel to the ground inertial (i.e. world) frame then the rotation matrix :math:`^GR_1 = ^0R_1`." [2] + + +The centroidal angular momentum task focuses on the angular part :math:`k_G \in \mathbb{R}^3` in the centroidal +momentum :math:`h_G = [k_G^\top, l_G^\top]^\top \in \mathbb{R}^6`. + The implementation of this class is inspired by [1, 2] (where [1] is licensed under the LGPLv2) References: - [1] "OpenSoT: A whole-body control library for the compliant humanoid robot COMAN", Rocchi et al., 2015 - [2] "Motion Planning and Control of Dynamic Humanoid Locomotion" (PhD thesis), Xin, 2018 + - [3] "Centroidal momentum matrix of a humanoid robot: structure and properties", Orin et al., 2008 + - [4] "Centroidal dynamics of a humanoid robot", Orin et al., 2013 """ import numpy as np @@ -28,7 +78,7 @@ class AngularMomentumTask(JointVelocityTask): r"""CoM Angular Momentum Task The is the angular part of the `CentroidalMomentumTask`. The centroidal momentum task tries to minimize the - difference between the desired and current centroidal linear moment given by: + difference between the desired and current centroidal moment given by: .. math:: ||A_G \dot{q} - h_{G,d}||^2 @@ -71,31 +121,34 @@ class AngularMomentumTask(JointVelocityTask): such that :math:`S(p)v = p \cross v`. Note that the orientation of Frame :math:`G` (CoM) could be selected to be parallel to the ground inertial (i.e. world) frame then the rotation matrix :math:`^GR_1 = ^0R_1`." [3] - The centroidal angular momentum task focuses on the angular part :math:`k_G \in \mathbb{R}^3` in the centroidal momentum :math:`h_G = [k_G^\top, l_G^\top]^\top \in \mathbb{R}^6`. + The implementation of this class is inspired by [3, 4] (where [4] is licensed under the LGPLv2) + References: - [1] "Centroidal momentum matrix of a humanoid robot: structure and properties", Orin et al., 2008 - [2] "Centroidal dynamics of a humanoid robot", Orin et al., 2013 - [3] "Motion Planning and Control of Dynamic Humanoid Locomotion" (PhD thesis), Xin, 2018 + - [4] "OpenSoT: A whole-body control library for the compliant humanoid robot COMAN", Rocchi et al., 2015 """ - def __init__(self, model, k_desired=None, weight=1., constraints=[]): + def __init__(self, model, desired_angular_momentum=None, weight=1., constraints=[]): """ Initialize the task. Args: model (ModelInterface): model interface. - k_desired (np.array[3], None): desired centroidal angular momentum. - weight (float, np.array[3,3]): weight scalar or matrix associated to the task. - constraints (list of Constraint): list of constraints associated with the task. + desired_angular_momentum (np.array[float[3]], None): desired centroidal angular momentum. If None, it + will be set to zero. + 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(AngularMomentumTask, self).__init__(model=model, weight=weight, constraints=constraints) # define desired reference - self.x_desired = k_desired + self.desired_angular_momentum = desired_angular_momentum # first update self.update() @@ -104,23 +157,34 @@ class AngularMomentumTask(JointVelocityTask): # Properties # ############## + @property + def desired_angular_momentum(self): + """Get the desired centroidal angular momentum.""" + return self._des_k + + @desired_angular_momentum.setter + def desired_angular_momentum(self, k_d): + """Set the desired centroidal angular momentum.""" + if k_d is None: + k_d = np.zeros(3) + if not isinstance(k_d, (np.ndarray, list, tuple)): + raise TypeError("Expecting the given desired centroidal angular momentum to be an instance of np.array, " + "instead got: {}".format(type(k_d))) + k_d = np.asarray(k_d) + if len(k_d) != 3: + raise ValueError("Expecting the length of the given desired angular centroidal momentum to be of length " + "3, instead got: {}".format(len(k_d))) + self._des_k = k_d + @property def x_desired(self): """Get the desired centroidal angular momentum.""" - return self._k_desired + return self._des_k @x_desired.setter def x_desired(self, k_d): """Set the desired centroidal angular momentum.""" - if k_d is None: - k_d = np.zeros(3) - if not isinstance(k_d, np.ndarray): - raise TypeError("Expecting the given desired centroidal angular momentum to be an instance of np.array, " - "instead got: {}".format(type(k_d))) - if len(k_d) != 3: - raise ValueError("Expecting the length of the given desired angular centroidal momentum to be of length " - "3, instead got: {}".format(len(k_d))) - self._k_desired = k_d + self.desired_angular_momentum = k_d ########### # Methods # @@ -130,7 +194,7 @@ class AngularMomentumTask(JointVelocityTask): """Set the desired references. Args: - x_des (np.array[3], None): desired centroidal angular momentum. + x_des (np.array[float[3]], None): desired centroidal angular momentum. """ self.x_desired = x_des @@ -138,7 +202,7 @@ class AngularMomentumTask(JointVelocityTask): """Return the desired references. Returns: - np.array[3]: desired centroidal angular momentum. + np.array[float[3]]: desired centroidal angular momentum. """ return self.x_desired @@ -147,4 +211,4 @@ class AngularMomentumTask(JointVelocityTask): Update the task by computing the A matrix and b vector that will be used by the task solver. """ self._A = self.model.get_centroidal_momentum_matrix()[:3] # shape: (3, N) - self._b = self._k_desired # shape: (3,) + self._b = self._des_k # shape: (3,) diff --git a/pyrobolearn/priorities/tasks/velocity/cartesian.py b/pyrobolearn/priorities/tasks/velocity/cartesian.py index 389e92c..bf0c89a 100644 --- a/pyrobolearn/priorities/tasks/velocity/cartesian.py +++ b/pyrobolearn/priorities/tasks/velocity/cartesian.py @@ -1,6 +1,27 @@ #!/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: + +.. 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 +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). @@ -42,10 +63,16 @@ class CartesianTask(JointVelocityTask): 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`. + + 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., weight=1., constraints=[]): + def __init__(self, model, distal_link, base_link=None, local_position=(0, 0, 0), desired_position=None, + desired_orientation=None, desired_linear_velocity=None, desired_angular_velocity=None, + kp_position=1., kp_orientation=1., weight=1., constraints=[]): """ Initialize the task. @@ -54,11 +81,18 @@ class CartesianTask(JointVelocityTask): 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_desired (np.array[float[7]], None): desired cartesian pose of distal link wrt the base. - dx_desired (np.array[float[6]], None): desired cartesian velocity of distal link wrt the base. - kp (float, np.array[float[6,6]]): stiffness gain. + desired_position (np.array[float[3]], None): desired position of distal link wrt the base. If None, it + will not be taken into account. + desired_orientation (np.array[float[4]], None): desired orientation (expressed as quaternion [x,y,z,w]) of + distal link wrt the base. If None, it will not be taken into account. + desired_linear_velocity (np.array[float[3]], None): desired linear velocity of distal link wrt the base. + If None, it will be set to zero. + desired_angular_velocity (np.array[float[3]], None): desired angular velocity of distal link wrt the base. + If None, it will be set to zero. + kp_position (float, np.array[float[3,3]]): position stiffness gain. + kp_orientation (float, np.array[float[3,3]]): orientation stiffness gain. weight (float, np.array[float[6,6]]): weight scalar or matrix associated to the task. - constraints (list of Constraint): list of constraints associated with the task. + constraints (list[Constraint]): list of constraints associated with the task. """ super(CartesianTask, self).__init__(model=model, weight=weight, constraints=constraints) @@ -66,11 +100,16 @@ class CartesianTask(JointVelocityTask): self.distal_link = self.model.get_link_id(distal_link) self.base_link = self.model.get_link_id(base_link) if base_link is not None else base_link self.local_position = local_position - self.kp = kp + + # gains + self.kp_position = kp_position + self.kp_orientation = kp_orientation # define desired references - self.x_desired = x_desired - self.dx_desired = dx_desired + self.desired_position = desired_position + self.desired_orientation = desired_orientation + self.desired_linear_velocity = desired_linear_velocity + self.desired_angular_velocity = desired_angular_velocity # first update self.update() @@ -79,62 +118,173 @@ class CartesianTask(JointVelocityTask): # Properties # ############## + @property + def desired_position(self): + """Get the desired cartesian position for the distal link wrt the base.""" + return self._des_pos + + @desired_position.setter + def desired_position(self, position): + """Set the desired cartesian position for the distal link wrt the base.""" + if position is not None: + if not isinstance(position, (np.ndarray, list, tuple)): + raise TypeError("Expecting the given desired position to be a np.array, instead got: " + "{}".format(type(position))) + position = np.asarray(position) + if len(position) != 3: + raise ValueError("Expecting the given desired position array to be of length 3, but instead got: " + "{}".format(len(position))) + self._des_pos = position + + @property + def desired_orientation(self): + """Get the desired cartesian orientation (expressed as a quaternion [x,y,z,w]) for the distal link wrt the + base.""" + return self._des_quat + + @desired_orientation.setter + def desired_orientation(self, orientation): + """Set the desired cartesian orientation (expressed as a quaternion [x,y,z,w]) for the distal link wrt the + base.""" + if orientation is not None: + if not isinstance(orientation, (np.ndarray, list, tuple)): + raise TypeError("Expecting the given desired orientation to be a np.array, instead got: " + "{}".format(type(orientation))) + orientation = np.asarray(orientation) + if len(orientation) != 4: + raise ValueError( + "Expecting the given desired orientation array to be of length 4, but instead got: " + "{}".format(len(orientation))) + self._des_quat = orientation + + @property + def desired_linear_velocity(self): + """Get the desired cartesian linear velocity of the distal link wrt the base.""" + return self._des_lin_vel + + @desired_linear_velocity.setter + def desired_linear_velocity(self, velocity): + """Set the desired cartesian linear velocity of the distal link wrt the base.""" + if velocity is None: + velocity = np.zeros(3) + elif not isinstance(velocity, (np.ndarray, list, tuple)): + raise TypeError("Expecting the given desired linear velocity to be a np.array, instead got: " + "{}".format(type(velocity))) + velocity = np.asarray(velocity) + if len(velocity) != 3: + raise ValueError("Expecting the given desired linear velocity array to be of length 3, but instead " + "got: {}".format(len(velocity))) + self._des_lin_vel = velocity + + @property + def desired_angular_velocity(self): + """Get the desired cartesian angular velocity of the distal link wrt the base.""" + return self._des_ang_vel + + @desired_angular_velocity.setter + def desired_angular_velocity(self, velocity): + """Set the desired cartesian angular velocity of the distal link wrt the base.""" + if velocity is None: + velocity = np.zeros(3) + elif not isinstance(velocity, (np.ndarray, list, tuple)): + raise TypeError("Expecting the given desired angular velocity to be a np.array, instead got: " + "{}".format(type(velocity))) + velocity = np.asarray(velocity) + if len(velocity) != 3: + raise ValueError("Expecting the given desired angular velocity array to be of length 3, but instead " + "got: {}".format(len(velocity))) + self._des_ang_vel = velocity + + @property + def desired_velocity(self): + """Return the linear and angular velocity.""" + return np.concatenate((self._des_lin_vel, self._des_ang_vel)) + @property def x_desired(self): """Get the desired cartesian pose for the distal link wrt to the base.""" - return self._x_d + position = self.desired_position + orientation = self.desired_orientation + if position is not None: + if orientation is not None: + return np.concatenate((position, orientation)) + return position + return orientation @x_desired.setter def x_desired(self, x_d): - """Get the desired cartesian pose for the distal link wrt to the base.""" - if x_d is None: - x_d = np.array([0.]*6 + [1.]) - if not isinstance(x_d, np.ndarray): - raise TypeError("Expecting the given desired pose to be a np.array, instead got: {}".format(type(x_d))) - if len(x_d) == 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._x_d = x_d + """Set the desired cartesian pose for the distal link wrt to the base.""" + if x_d is not None: + if not isinstance(x_d, (np.ndarray, list, tuple)): + raise TypeError("Expecting the given desired pose to be a np.array, instead got: {}".format(type(x_d))) + x_d = np.asarray(x_d) + if len(x_d) == 3: # only position is provided + x_d = np.concatenate((x_d, np.array([0., 0., 0., 1.]))) + elif len(x_d) == 4: # only orientation is provided + x_d = np.concatenate((np.zeros(3), x_d)) + if len(x_d) != 7: + raise ValueError("Expecting the given desired pose array to be of length 7 (3 for the position, and 4 " + "for the orientation expressed as a quaternion [x,y,z,w]), instead got a length of: " + "{}".format(len(x_d))) + self._des_pos = x_d[:3] + self._des_quat = x_d[3:] @property def dx_desired(self): """Get the desired cartesian velocity for the distal link wrt to the base.""" - return self._dx_d + return np.concatenate((self._des_lin_vel, self._des_ang_vel)) @dx_desired.setter def dx_desired(self, dx_d): """Set the desired cartesian velocity for the distal link wrt to the base.""" - if dx_d is None: - dx_d = np.zeros(6) - if not isinstance(dx_d, np.ndarray): - raise TypeError("Expecting the given desired velocity to be a np.array, instead got: {}".format(type(dx_d))) - if len(dx_d) == 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._dx_d = dx_d + if dx_d is not None: + if not isinstance(dx_d, (np.ndarray, list, tuple)): + raise TypeError("Expecting the given desired velocity to be a np.array, instead got: " + "{}".format(type(dx_d))) + dx_d = np.asarray(dx_d) + if len(dx_d) == 3: # assume that it is the linear velocity + dx_d = np.concatenate((dx_d, np.zeros(3))) + if len(dx_d) != 6: + raise ValueError("Expecting the given desired velocity array to be of length 6 (3 for the linear and " + "3 for the angular part), instead got a length of: {}".format(len(dx_d))) + self._des_lin_vel = dx_d[:3] + self._des_ang_vel = dx_d[3:] @property - def kp(self): - """Return the stiffness gain.""" - return self._kp + def kp_position(self): + """Return the position stiffness gain.""" + return self._kp_pos - @kp.setter - def kp(self, kp): - """Set the stiffness gain.""" + @kp_position.setter + def kp_position(self, kp): + """Set the position stiffness gain.""" + if kp is None: + kp = 1. if not isinstance(kp, (float, int, np.ndarray)): - raise TypeError("Expecting the given stiffness gain kp to be an int, float, np.array, instead got: " - "{}".format(type(kp))) - if isinstance(kp, np.ndarray) and kp.shape != (6, 6): - raise ValueError("Expecting the given stiffness gain matrix kp to be of shape {}, but instead got " - "shape: {}".format((6, 6), kp.shape)) - self._kp = kp + raise TypeError("Expecting the given position stiffness gain kp to be an int, float, np.array, instead " + "got: {}".format(type(kp))) + if isinstance(kp, np.ndarray) and kp.shape != (3, 3): + raise ValueError("Expecting the given position stiffness gain matrix kp to be of shape {}, but instead " + "got shape: {}".format((3, 3), kp.shape)) + self._kp_pos = kp + + @property + def 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 ########### # Methods # @@ -145,15 +295,15 @@ class CartesianTask(JointVelocityTask): Args: x_des (np.array[float[7]], None): desired cartesian pose (position and quaternion [x,y,z,w]) of distal - link wrt the base. - dx_des (np.array[float[6]], None): desired cartesian velocity of distal link wrt the base. + 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. """ self.x_desired = x_des self.dx_desired = dx_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. @@ -165,12 +315,30 @@ class CartesianTask(JointVelocityTask): Update the task by computing the A matrix and b vector that will be used by the task solver. """ x = self.model.get_pose(self.distal_link, self.base_link) - self._A = self.model.get_jacobian(self.distal_link, self.base_link, self.local_position) # shape: (6,N) + self._A = self.model.get_jacobian(link=self.distal_link, wrt_link=self.base_link, + point=self.local_position) # shape: (6,N) - # compute position/orientation error - position_error = (self._x_d[:3] - x[:3]) - orientation_error = quaternion_error(quat_des=self._x_d[3:], quat_cur=x[3:]) - error = np.concatenate((position_error, orientation_error)) + if self._des_quat is None: # only position and/or velocities + if self._des_pos is None: # only velocities + self._b = np.concatenate((self._des_lin_vel, self._des_ang_vel)) + else: # only position + self._A = self._A[:3] + # compute position error + error = (self._des_pos - x[:3]) + # compute b vector + self._b = np.dot(self.kp_position, error) + self._des_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 + self._b = np.dot(self.kp_orientation, error) + self._des_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 - self._b = np.dot(self.kp, error) + self._dx_d # shape: (6,) + # compute b vector + b_position = np.dot(self.kp_position, position_error) + self._des_lin_vel + b_orientation = np.dot(self.kp_orientation, orientation_error) + self._des_ang_vel + self._b = np.concatenate((b_position, b_orientation)) diff --git a/pyrobolearn/priorities/tasks/velocity/com.py b/pyrobolearn/priorities/tasks/velocity/com.py index 12e9186..01a9877 100644 --- a/pyrobolearn/priorities/tasks/velocity/com.py +++ b/pyrobolearn/priorities/tasks/velocity/com.py @@ -1,6 +1,19 @@ #!/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. + +.. 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 implementation of this class is inspired by [1] (which is licensed under the LGPLv2). @@ -36,19 +49,24 @@ 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`. + + 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, x_desired=None, dx_desired=None, kp=1., weight=1., constraints=[]): + def __init__(self, model, desired_position=None, desired_velocity=None, kp=1., weight=1., constraints=[]): """ Initialize the task. Args: model (ModelInterface): model interface. - x_desired (np.array[3], None): desired CoM position. If None, it will be set to 0. - dx_desired (np.array[3], None): desired CoM linear velocity. If None, it will be set to 0. - kp (float, np.array[3,3]): stiffness gain. - weight (float, np.array[3,3]): weight scalar or matrix associated to the task. - constraints (list of Constraint): list of constraints associated with the task. + 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. + kp (float, np.array[float[3,3]]): stiffness 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(CoMTask, self).__init__(model=model, weight=weight, constraints=constraints) @@ -56,8 +74,8 @@ class CoMTask(JointVelocityTask): self.kp = kp # define desired references - self.x_desired = x_desired - self.dx_desired = dx_desired + self.desired_position = desired_position + self.desired_velocity = desired_velocity # first update self.update() @@ -66,41 +84,62 @@ class CoMTask(JointVelocityTask): # 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 x_desired(self): """Get the desired CoM position.""" - return self._x_d + return self._des_pos @x_desired.setter def x_desired(self, x_d): """Set the desired CoM position.""" - if x_d is None: - x_d = np.zeros(3) - if not isinstance(x_d, np.ndarray): - raise TypeError("Expecting the given desired CoM position to be a np.array, instead got: " - "{}".format(type(x_d))) - if len(x_d) != 3: - raise ValueError("Expecting the given desired CoM position array to be of length 3, instead got a length " - "of: {}".format(len(x_d))) - self._x_d = x_d + self.desired_position = x_d @property def dx_desired(self): """Get the desired CoM linear velocity.""" - return self._dx_d + return self._des_vel @dx_desired.setter def dx_desired(self, dx_d): """Set the desired CoM linear velocity.""" - if dx_d is None: - dx_d = np.zeros(3) - if not isinstance(dx_d, np.ndarray): - raise TypeError("Expecting the given desired CoM linear velocity to be a np.array, instead got: " - "{}".format(type(dx_d))) - if len(dx_d) != 3: - raise ValueError("Expecting the given desired CoM linear velocity array to be of length 3, instead got a " - "length of: {}".format(len(dx_d))) - self._dx_d = dx_d + self.desired_velocity = dx_d @property def kp(self): @@ -126,8 +165,8 @@ class CoMTask(JointVelocityTask): """Set the desired references. Args: - x_des (np.array[3], None): desired CoM position. If None, it will be set to 0. - dx_des (np.array[3], None): desired CoM linear velocity. If None, it will be set to 0. + 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. """ self.x_desired = x_des self.dx_desired = dx_des @@ -136,8 +175,8 @@ class CoMTask(JointVelocityTask): """Return the desired references. Returns: - np.array[3]: desired CoM position. - np.array[3]: desired CoM linear velocity. + np.array[float[3]]: desired CoM position. + np.array[float[3]]: desired CoM linear velocity. """ return self.x_desired, self.dx_desired @@ -145,6 +184,10 @@ class CoMTask(JointVelocityTask): """ Update the task by computing the A matrix and b vector that will be used by the task solver. """ - x = self.model.get_com_position() - self._A = self.model.get_com_jacobian() # shape: (3, N) - self._b = np.dot(self.kp, (self._x_d - x)) + self._dx_d # shape: (3,) + + self._A = self.model.get_com_jacobian(full=False) # shape: (3, N) + if self._des_pos is not None: + x = self.model.get_com_position() + self._b = np.dot(self.kp, (self._des_pos - x)) + self._des_vel # shape: (3,) + else: + self._b = self._des_vel # shape: (3,) diff --git a/pyrobolearn/priorities/tasks/velocity/contact.py b/pyrobolearn/priorities/tasks/velocity/contact.py index 43cce6f..d99eab3 100644 --- a/pyrobolearn/priorities/tasks/velocity/contact.py +++ b/pyrobolearn/priorities/tasks/velocity/contact.py @@ -1,6 +1,17 @@ #!/usr/bin/env python r"""Provide the contact task. +The contact task tries to minimize the movement of a contact link: + +.. math:: || C 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), and :math:`\dot{q}` are the joint velocities being optimized. + +This is equivalent to the QP objective function :math:`||Ax - b||^2`, by setting :math:`A = C J_c(q)`, +:math:`x = \dot{q}`, and :math:`b = 0`. The implementation of this class is inspired by [1] (which is licensed under the LGPLv2). @@ -33,10 +44,22 @@ class ContactTask(JointVelocityTask): 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), and :math:`\dot{q}` are the joint velocities being optimized. + the contact point expressed in the distal link frame (i.e. the link which is in contact)), and :math:`\dot{q}` are + the joint velocities being optimized. + Note that because the jacobian is expressed in the distal link frame, the entries of the contact matrix specify + the velocities that are fixed or can move in that particular frame. For instance, [0,0,1,0,0,0] means that the z + linear velocity (where z is the z axis of the distal link frame) is fixed. This is equivalent to the QP objective function :math:`||Ax - b||^2`, by setting :math:`A = C J_c(q)`, :math:`x = \dot{q}`, and :math:`b = 0`. + + 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, distal_link, contact_matrix=1., weight=1., constraints=[]): @@ -46,9 +69,10 @@ class ContactTask(JointVelocityTask): Args: model (ModelInterface): model interface. distal_link (int, str): distal link id or name. - contact_matrix (np.array[6,6], None): contact selector matrix (=a diagonal square matrix). If None, by - default it will be set to the identity matrix. - weight (float, np.array[6,6]): weight scalar or matrix associated to the task. + 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(ContactTask, self).__init__(model=model, weight=weight, constraints=constraints) @@ -85,12 +109,14 @@ class ContactTask(JointVelocityTask): # if numpy array, check its shape and make sure it is a diagonal matrix if isinstance(matrix, np.ndarray): - if matrix.shape != (6, 6): + 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))) - - # make sure the contact matrix is a diagonal matrix - matrix = np.diag(np.diag(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 @@ -103,4 +129,6 @@ class ContactTask(JointVelocityTask): """ Update the task by computing the A matrix and b vector that will be used by the task solver. """ - self._A = np.dot(self.contact_matrix, self.model.get_jacobian(self.distal_link)) + # get jacobian expressed in the distal link frame + jacobian = self.model.get_jacobian(link=self.distal_link, frame=self.distal_link) + self._A = np.dot(self.contact_matrix, jacobian) diff --git a/pyrobolearn/priorities/tasks/velocity/interaction.py b/pyrobolearn/priorities/tasks/velocity/interaction.py index f6d6277..0666a61 100644 --- a/pyrobolearn/priorities/tasks/velocity/interaction.py +++ b/pyrobolearn/priorities/tasks/velocity/interaction.py @@ -29,7 +29,8 @@ __status__ = "Development" class InteractionTask(JointVelocityTask): r"""Interaction Task - From [1], "The Interaction class implements an Admittance based force control using the admittance law: + From the documentation of the framework of [1], "The Interaction class implements an Admittance based force + control using the admittance law: .. math:: @@ -44,8 +45,11 @@ class InteractionTask(JointVelocityTask): Warnings: the :math:`w_d` is the desired wrench that the robot has to exert on the environment, so the measured wrench :math:`w` is the wrench produced by the robot on the environment (and not the opposite)!" + + The implementation of this class is inspired by [1] (which is licensed under the LGPLv2). + References: - - [1] OpenSoT framework + - [1] "OpenSoT: A whole-body control library for the compliant humanoid robot COMAN", Rocchi et al., 2015 """ def __init__(self, model, distal_link, base_link=-1, desired_wrench=0., weight=1., constraints=[]): @@ -56,8 +60,8 @@ class InteractionTask(JointVelocityTask): model (ModelInterface): model interface. distal_link (int, str): distal link id or name. base_link (int, str, None): base link id or name. If None, it will be the base root link. - desired_wrench (float, np.array[6]): desired wrench. - weight (float, np.array[6,6]): weight scalar or matrix associated to the task. + desired_wrench (float, np.array[float[6]]): desired wrench. + 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(InteractionTask, self).__init__(model=model, weight=weight, constraints=constraints) @@ -67,7 +71,7 @@ class InteractionTask(JointVelocityTask): raise NotImplementedError("This class has not been implemented yet.") - def update(self): + def _update(self): """ Update the task by computing the A matrix and b vector that will be used by the task solver. """ diff --git a/pyrobolearn/priorities/tasks/velocity/linear_momentum.py b/pyrobolearn/priorities/tasks/velocity/linear_momentum.py index d927fd6..42be645 100644 --- a/pyrobolearn/priorities/tasks/velocity/linear_momentum.py +++ b/pyrobolearn/priorities/tasks/velocity/linear_momentum.py @@ -1,6 +1,53 @@ #!/usr/bin/env python r"""Provide the linear momentum task. +The is the linear part of the `CentroidalMomentumTask`. The centroidal momentum task tries to minimize the +difference between the desired and current centroidal moment given by: + +.. math:: ||A_G \dot{q} - h_{G,d}||^2 + +where :math:`A_G \in \mathbb{R}^{6 \times N}` is the centroidal momentum matrix (CMM, see below for description), +:math:`\dot{q}` are the joint velocities being optimized, and :math:`h_{G,d}` is the desired centroidal momentum. + +This is equivalent to the QP objective function :math:`||Ax - b||^2`, by setting :math:`A=A_G`, :math:`x=\dot{q}`, +and :math:`b = h_{G,d}`. + +The centroidal momentum (which is the sum of all body spatial momenta computed wrt the CoM) is given by: + +.. math:: h_G = A_G \dot{q} + +where :math:`h_G = [k_G^\top, l_G^\top]^\top \in \mathbb{R}^6` is the centroidal momentum (the subscript :math:`G` +denotes the CoM) with :math:`k_G \in \mathbb{R}^3` being the angular momentum and :math:`l_G \in \mathbb{R}^3` the +linear part, :math:`\dot{q}` are the joint velocities, and :math:`A_G \in \mathbb{R}^{6 \times N}` (with :math:`N` +is the number of DoFs) is the centroidal momentum matrix (CMM). + +"The CMM is computed from the joint space inertia matrix :math:`H(q)`, given by: + +.. math:: A_G = ^1X_G^\top S_1 H(q) = ^1X_G^\top H_1(q) + +where :math:`^1X_G^\top \in \mathbb{R}^{6 \times 6}` is the spatial transformation matrix that transfers spatial +momentum from the floating base (Body 1) to the CoM (G), :math:`H(q)` is the full joint space inertia matrix, +:math:`H_1 = S_1 H` is the floating base (Body 1) inertia matrix selected using the selector matrix +:math:`S_1 = [1_{6 \times 6}, 0_{6 \times (N-6)}}`. + +The spatial transformation matrix is given by: + +.. math:: + + ^1X_G^\top = \left[ \begin{array}{cc} + ^GR_1 & ^GR_1 S(^1p_G)^\top \\ + 0 & ^GR_1 + \\end{array} \right] + +where :math:`^GR_1` is the rotation matrix of :math:`G` wrt the floating base (Body 1), +:math:`^1p_G = ^1R_0 (^0p_G - ^0p_1)` is the position vector from the floating base (Body 1) origin to the CoM +expressed in the floating base (Body 1) frame, :math:`S(\cdot)` provides the skew symmetric cross product matrix +such that :math:`S(p)v = p \cross v`. Note that the orientation of Frame :math:`G` (CoM) could be selected to be +parallel to the ground inertial (i.e. world) frame then the rotation matrix :math:`^GR_1 = ^0R_1`." [2] + + +The centroidal linear momentum task focuses on the linear part :math:`l_G \in \mathbb{R}^3` in the centroidal +momentum :math:`h_G = [k_G^\top, l_G^\top]^\top \in \mathbb{R}^6`. The implementation of this class is inspired by [1, 2] (where [1] is licensed under the LGPLv2) @@ -28,7 +75,7 @@ class LinearMomentumTask(JointVelocityTask): r"""(Centroidal) Linear Momentum Task The is the linear part of the `CentroidalMomentumTask`. The centroidal momentum task tries to minimize the - difference between the desired and current centroidal linear moment given by: + difference between the desired and current centroidal moment given by: .. math:: ||A_G \dot{q} - h_{G,d}||^2 @@ -71,31 +118,34 @@ class LinearMomentumTask(JointVelocityTask): such that :math:`S(p)v = p \cross v`. Note that the orientation of Frame :math:`G` (CoM) could be selected to be parallel to the ground inertial (i.e. world) frame then the rotation matrix :math:`^GR_1 = ^0R_1`." [3] - The centroidal linear momentum task focuses on the linear part :math:`l_G \in \mathbb{R}^3` in the centroidal momentum :math:`h_G = [k_G^\top, l_G^\top]^\top \in \mathbb{R}^6`. + The implementation of this class is inspired by [3, 4] (where [4] is licensed under the LGPLv2) + References: - [1] "Centroidal momentum matrix of a humanoid robot: structure and properties", Orin et al., 2008 - [2] "Centroidal dynamics of a humanoid robot", Orin et al., 2013 - [3] "Motion Planning and Control of Dynamic Humanoid Locomotion" (PhD thesis), Xin, 2018 + - [4] "OpenSoT: A whole-body control library for the compliant humanoid robot COMAN", Rocchi et al., 2015 """ - def __init__(self, model, l_desired=None, weight=1., constraints=[]): + def __init__(self, model, desired_linear_momentum=None, weight=1., constraints=[]): """ Initialize the task. Args: model (ModelInterface): model interface. - l_desired (np.array[3], None): desired centroidal linear momentum. - weight (float, np.array[3,3]): weight scalar or matrix associated to the task. - constraints (list of Constraint): list of constraints associated with the task. + desired_linear_momentum (np.array[float[3]], None): desired centroidal linear momentum. If None, it + will be set to zero. + 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(LinearMomentumTask, self).__init__(model=model, weight=weight, constraints=constraints) # define desired reference - self.x_desired = l_desired + self.desired_linear_momentum = desired_linear_momentum # first update self.update() @@ -104,6 +154,25 @@ class LinearMomentumTask(JointVelocityTask): # Properties # ############## + @property + def desired_linear_momentum(self): + """Get the desired centroidal linear momentum.""" + return self._l_desired + + @desired_linear_momentum.setter + def desired_linear_momentum(self, l_d): + """Set the desired centroidal linear momentum.""" + if l_d is None: + l_d = np.zeros(3) + if not isinstance(l_d, (np.ndarray, list, tuple)): + raise TypeError("Expecting the given desired centroidal linear momentum to be an instance of np.array, " + "instead got: {}".format(type(l_d))) + l_d = np.asarray(l_d) + if len(l_d) != 3: + raise ValueError("Expecting the length of the given desired linear centroidal momentum to be of length 3, " + "instead got: {}".format(len(l_d))) + self._l_desired = l_d + @property def x_desired(self): """Get the desired centroidal linear momentum.""" @@ -112,15 +181,7 @@ class LinearMomentumTask(JointVelocityTask): @x_desired.setter def x_desired(self, l_d): """Set the desired centroidal linear momentum.""" - if l_d is None: - l_d = np.zeros(3) - if not isinstance(l_d, np.ndarray): - raise TypeError("Expecting the given desired centroidal linear momentum to be an instance of np.array, " - "instead got: {}".format(type(l_d))) - if len(l_d) != 3: - raise ValueError("Expecting the length of the given desired linear centroidal momentum to be of length 3, " - "instead got: {}".format(len(l_d))) - self._l_desired = l_d + self.desired_linear_momentum = l_d ########### # Methods # @@ -130,7 +191,7 @@ class LinearMomentumTask(JointVelocityTask): """Set the desired references. Args: - x_des (np.array[3], None): desired centroidal linear momentum. + x_des (np.array[float[3]], None): desired centroidal linear momentum. """ self.x_desired = x_des @@ -138,7 +199,7 @@ class LinearMomentumTask(JointVelocityTask): """Return the desired references. Returns: - np.array[3]: desired centroidal linear momentum. + np.array[float[3]]: desired centroidal linear momentum. """ return self.x_desired diff --git a/pyrobolearn/priorities/tasks/velocity/minimum_acceleration.py b/pyrobolearn/priorities/tasks/velocity/minimum_acceleration.py index 013c9fb..1bc878c 100644 --- a/pyrobolearn/priorities/tasks/velocity/minimum_acceleration.py +++ b/pyrobolearn/priorities/tasks/velocity/minimum_acceleration.py @@ -1,6 +1,16 @@ #!/usr/bin/env python 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 + +where :math:`\dot{q}_t` are the current joint velocities being optimized, and :math:`\dot{q}_{t-1}` are the +previous 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}`. + The implementation of this class is inspired by [1] (which is licensed under the LGPLv2). @@ -35,6 +45,12 @@ class MinAccelerationTask(JointVelocityTask): 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}`. + + + 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, weight=1., constraints=[]): @@ -43,16 +59,17 @@ class MinAccelerationTask(JointVelocityTask): Args: model (ModelInterface): model interface. - weight (float, np.array[N,N]): weight scalar or matrix associated to the task. - constraints (list of Constraint): list of constraints associated with the task. + weight (float, np.array[float[N,N]]): weight scalar or matrix associated to the task. + constraints (list[Constraint]): list of constraints associated with the task. """ + # the variables A and b are initialized by default to be A=I and b=0 super(MinAccelerationTask, self).__init__(model=model, weight=weight, constraints=constraints) ########### # Methods # ########### - def update(self): + def _update(self): """ Update the task by computing the A matrix and b vector that will be used by the task solver. """ diff --git a/pyrobolearn/priorities/tasks/velocity/minimum_effort.py b/pyrobolearn/priorities/tasks/velocity/minimum_effort.py index 83d290a..b62abb3 100644 --- a/pyrobolearn/priorities/tasks/velocity/minimum_effort.py +++ b/pyrobolearn/priorities/tasks/velocity/minimum_effort.py @@ -40,8 +40,8 @@ class MinEffortTask(JointVelocityTask): Args: model (ModelInterface): model interface. - weight (float, np.array[N,N]): weight scalar or matrix associated to the task. - constraints (list of Constraint): list of constraints associated with the task. + weight (float, np.array[float[N,N]]): weight scalar or matrix associated to the task. + constraints (list[Constraint]): list of constraints associated with the task. """ super(MinEffortTask, self).__init__(model=model, weight=weight, constraints=constraints) diff --git a/pyrobolearn/priorities/tasks/velocity/minimum_velocity.py b/pyrobolearn/priorities/tasks/velocity/minimum_velocity.py index 8d569b9..6d414ec 100644 --- a/pyrobolearn/priorities/tasks/velocity/minimum_velocity.py +++ b/pyrobolearn/priorities/tasks/velocity/minimum_velocity.py @@ -1,6 +1,18 @@ #!/usr/bin/env python r"""Provide the minimum velocity task. +The minimum velocity task minimizes the joint velocities, that is it minimizes: + +.. math:: ||\dot{q}||^2, + +which is equivalent to the QP objective function :math:`||Ax - b||^2`, by setting :math:`A=I`, :math:`x=\dot{q}`, +and :math:`b=0`. + +This minimum velocity task is often used in conjunction with other tasks such as `PosturalTask` or `CartesianTask`. + +Note that this can also be achieved with the `tasks.velocity.PosturalTask` by setting the desired joint velocities to +zeros and not providing the desired joint positions. + The implementation of this class is inspired by [1] (which is licensed under the LGPLv2). @@ -32,6 +44,10 @@ class MinVelocityTask(JointVelocityTask): and :math:`b=0`. This minimum velocity task is often used in conjunction with other tasks such as `PosturalTask` or `CartesianTask`. + Note that this task can also be achieved with the `tasks.velocity.PosturalTask` by setting the desired joint + velocities to zeros and not providing the desired joint positions. + + 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 @@ -43,8 +59,8 @@ class MinVelocityTask(JointVelocityTask): Args: model (ModelInterface): model interface. - weight (float, np.array[N,N]): weight scalar or matrix associated to the task. - constraints (list of Constraint): list of constraints associated with the task. + weight (float, np.array[float[N,N]]): weight scalar or matrix associated to the task. + constraints (list[Constraint]): list of constraints associated with the task. """ # the variables A and b are initialized by default to be A=I and b=0 super(MinVelocityTask, self).__init__(model=model, weight=weight, constraints=constraints) diff --git a/pyrobolearn/priorities/tasks/velocity/momentum.py b/pyrobolearn/priorities/tasks/velocity/momentum.py index 592800e..ee6b284 100644 --- a/pyrobolearn/priorities/tasks/velocity/momentum.py +++ b/pyrobolearn/priorities/tasks/velocity/momentum.py @@ -1,6 +1,50 @@ #!/usr/bin/env python r"""Provide the CoM linear and angular momentum task. +The centroidal momentum task tries to minimize the difference between the desired and current centroidal moment +given by: + +.. math:: ||A_G \dot{q} - h_{G,d}||^2 + +where :math:`A_G \in \mathbb{R}^{6 \times N}` is the centroidal momentum matrix (CMM, see below for description), +:math:`\dot{q}` are the joint velocities being optimized, and :math:`h_{G,d}` is the desired centroidal momentum. + +This is equivalent to the QP objective function :math:`||Ax - b||^2`, by setting :math:`A=A_G`, :math:`x=\dot{q}`, +and :math:`b = h_{G,d}`. + +The centroidal momentum (which is the sum of all body spatial momenta computed wrt the CoM) is given by: + +.. math:: h_G = A_G \dot{q} + +where :math:`h_G = [k_G^\top, l_G^\top]^\top \in \mathbb{R}^6` is the centroidal momentum (the subscript :math:`G` +denotes the CoM) with :math:`k_G \in \mathbb{R}^3` being the angular momentum and :math:`l_G \in \mathbb{R}^3` the +linear part, :math:`\dot{q}` are the joint velocities, and :math:`A_G \in \mathbb{R}^{6 \times N}` (with :math:`N` +is the number of DoFs) is the centroidal momentum matrix (CMM). + +"The CMM is computed from the joint space inertia matrix :math:`H(q)`, given by: + +.. math:: A_G = ^1X_G^\top S_1 H(q) = ^1X_G^\top H_1(q) + +where :math:`^1X_G^\top \in \mathbb{R}^{6 \times 6}` is the spatial transformation matrix that transfers spatial +momentum from the floating base (Body 1) to the CoM (G), :math:`H(q)` is the full joint space inertia matrix, +:math:`H_1 = S_1 H` is the floating base (Body 1) inertia matrix selected using the selector matrix +:math:`S_1 = [1_{6 \times 6}, 0_{6 \times (N-6)}}`. + +The spatial transformation matrix is given by: + +.. math:: + + ^1X_G^\top = \left[ \begin{array}{cc} + ^GR_1 & ^GR_1 S(^1p_G)^\top \\ + 0 & ^GR_1 + \\end{array} \right] + +where :math:`^GR_1` is the rotation matrix of :math:`G` wrt the floating base (Body 1), +:math:`^1p_G = ^1R_0 (^0p_G - ^0p_1)` is the position vector from the floating base (Body 1) origin to the CoM +expressed in the floating base (Body 1) frame, :math:`S(\cdot)` provides the skew symmetric cross product matrix +such that :math:`S(p)v = p \cross v`. Note that the orientation of Frame :math:`G` (CoM) could be selected to be +parallel to the ground inertial (i.e. world) frame then the rotation matrix :math:`^GR_1 = ^0R_1`." [2] + The implementation of this class is inspired by [1, 2] (where [1] is licensed under the LGPLv2). @@ -71,27 +115,36 @@ class CentroidalMomentumTask(JointVelocityTask): such that :math:`S(p)v = p \cross v`. Note that the orientation of Frame :math:`G` (CoM) could be selected to be parallel to the ground inertial (i.e. world) frame then the rotation matrix :math:`^GR_1 = ^0R_1`." [3] + + The implementation of this class is inspired by [3, 4] (where [4] is licensed under the LGPLv2). + References: - [1] "Centroidal momentum matrix of a humanoid robot: structure and properties", Orin et al., 2008 - [2] "Centroidal dynamics of a humanoid robot", Orin et al., 2013 - [3] "Motion Planning and Control of Dynamic Humanoid Locomotion" (PhD thesis), Xin, 2018 + - [4] "OpenSoT: A whole-body control library for the compliant humanoid robot COMAN", Rocchi et al., 2015 """ - def __init__(self, model, h_desired=None, weight=1., constraints=[]): + def __init__(self, model, desired_angular_momentum=None, desired_linear_momentum=None, weight=1., constraints=[]): """ Initialize the task. Args: model (ModelInterface): model interface. - h_desired (np.array[6], None): desired centroidal momentum which is the concatenation of the desired - angular and linear momentum. If None, it will be set to zero. - 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. + desired_angular_momentum (np.array[float[3]], None): desired centroidal angular momentum. If None, it + will not be considered. However, if the next parameter :attr:`desired_linear_momentum` is also None, + then this argument will be set to zero. + desired_linear_momentum (np.array[float[3]], None): desired centroidal linear momentum. If None, it + will not be considered. However, if the previous parameter :attr:`desired_angular_momentum` was also set + to None, then this argument will be set to zero. + 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(CentroidalMomentumTask, self).__init__(model=model, weight=weight, constraints=constraints) # define desired reference - self.x_desired = h_desired + self.desired_angular_momentum = desired_angular_momentum + self.desired_linear_momentum = desired_linear_momentum # first update self.update() @@ -100,23 +153,77 @@ class CentroidalMomentumTask(JointVelocityTask): # Properties # ############## + @property + def desired_angular_momentum(self): + """Get the desired centroidal angular momentum.""" + return self._des_k + + @desired_angular_momentum.setter + def desired_angular_momentum(self, k_d): + """Set the desired centroidal angular momentum.""" + if k_d is not None: + if not isinstance(k_d, (np.ndarray, list, tuple)): + raise TypeError("Expecting the given desired centroidal angular momentum to be an instance of " + "np.array, instead got: {}".format(type(k_d))) + k_d = np.asarray(k_d) + if len(k_d) != 3: + raise ValueError("Expecting the length of the given desired angular centroidal momentum to be of " + "length 3, instead got: {}".format(len(k_d))) + self._des_k = k_d + + @property + def desired_linear_momentum(self): + """Get the desired centroidal linear momentum.""" + return self._des_l + + @desired_linear_momentum.setter + def desired_linear_momentum(self, l_d): + """Set the desired centroidal linear momentum.""" + if l_d is not None: + if not isinstance(l_d, (np.ndarray, list, tuple)): + raise TypeError("Expecting the given desired centroidal linear momentum to be an instance of " + "np.array, instead got: {}".format(type(l_d))) + l_d = np.asarray(l_d) + if len(l_d) != 3: + raise ValueError("Expecting the length of the given desired linear centroidal momentum to be of " + "length 3, instead got: {}".format(len(l_d))) + self._des_l = l_d + + @property + def desired_momentum(self): + """Get the desired centroidal momentum (angular and linear momentum).""" + if self._des_k is None: + if self._des_l is None: + return np.zeros(6) + return self._des_l + if self._des_l is None: + return self._des_k + return np.concatenate((self._des_k, self._des_l)) + + @desired_momentum.setter + def desired_momentum(self, h_d): + """Set the desired centroidal momentum (angular and linear momentum).""" + if h_d is None: + h_d = np.zeros(6) + if not isinstance(h_d, (np.ndarray, list, tuple)): + raise TypeError("Expecting the given desired centroidal momentum to be an instance of np.array, instead " + "got: {}".format(type(h_d))) + h_d = np.asarray(h_d) + if len(h_d) != 6: + raise ValueError("Expecting the length of the given desired centroidal momentum to be of length 6, " + "instead got: {}".format(len(h_d))) + self._des_k = h_d[:3] + self._des_l = h_d[3:] + @property def x_desired(self): """Get the desired centroidal momentum (angular and linear momentum).""" - return self._h_desired + return self.desired_momentum @x_desired.setter def x_desired(self, h_d): """Set the desired centroidal momentum (angular and linear momentum).""" - if h_d is None: - h_d = np.zeros(6) - if not isinstance(h_d, np.ndarray): - raise TypeError("Expecting the given desired centroidal momentum to be an instance of np.array, instead " - "got: {}".format(type(h_d))) - if len(h_d) != 6: - raise ValueError("Expecting the length of the given desired centroidal momentum to be of length 6, " - "instead got: {}".format(len(h_d))) - self._h_desired = h_d + self.desired_momentum = h_d ########### # Methods # @@ -126,7 +233,7 @@ class CentroidalMomentumTask(JointVelocityTask): """Set the desired references. Args: - x_des (np.array[6], None): desired centroidal momentum (angular and linear momentum). + x_des (np.array[float[6]], None): desired centroidal momentum (angular and linear momentum). """ self.x_desired = x_des @@ -134,7 +241,7 @@ class CentroidalMomentumTask(JointVelocityTask): """Return the desired references. Returns: - np.array[6]: desired centroidal momentum (angular and linear momentum). + np.array[float[6]]: desired centroidal momentum (angular and linear momentum). """ return self.x_desired @@ -143,4 +250,15 @@ class CentroidalMomentumTask(JointVelocityTask): Update the task by computing the A matrix and b vector that will be used by the task solver. """ self._A = self.model.get_centroidal_momentum_matrix() # shape: (6, N) - self._b = self._h_desired # shape: (6,) + if self._des_l is None: + if self._des_k is None: + self._b = np.zeros(6) # shape: (6,) + else: + self._A = self._A[:3] + self._b = self._des_k # shape: (3,) + else: + if self._des_k is None: + self._A = self._A[3:] + self._b = self._des_l # shape: (3,) + else: + self._b = np.concatenate((self._des_k, self._des_l)) # shape: (6,) diff --git a/pyrobolearn/priorities/tasks/velocity/postural.py b/pyrobolearn/priorities/tasks/velocity/postural.py index 313cf5b..a864260 100644 --- a/pyrobolearn/priorities/tasks/velocity/postural.py +++ b/pyrobolearn/priorities/tasks/velocity/postural.py @@ -1,6 +1,17 @@ #!/usr/bin/env python r"""Provide the postural (velocity) task. +The postural task tries to bring the robot to a reference posture; that is, it minimizes the joint velocities such +that it gets close to the specified posture (given by the desired joint positions and velocities): + +.. math:: || \dot{q} - (K_p (q_d - q) + \dot{q}_d) ||^2, + +which is equivalent to the QP objective function :math:`||Ax - b||^2`, by setting :math:`A=I`, :math:`x=\dot{q}`, +and :math:`b = K_p (q_d - q) + \dot{q}_d`, where :math:`K_p` is the stiffness gain and the subscript :math:`d` +means "desired". + +Note that specifying the joint positions is optional, you can only specify the joint velocities if you wish. + The implementation of this class is inspired by [1] (which is licensed under the LGPLv2). @@ -35,6 +46,8 @@ class PosturalTask(JointVelocityTask): and :math:`b = K_p (q_d - q) + \dot{q}_d`, where :math:`K_p` is the stiffness gain and the subscript :math:`d` means "desired". + 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 """ @@ -45,13 +58,13 @@ class PosturalTask(JointVelocityTask): Args: model (ModelInterface): model interface. - q_desired (np.array[N], None): desired joint positions, where :math:`N` is the number of DoFs. If None, - it will be set to 0. - dq_desired (np.array[N], None): desired joint velocities, where :math:`N` is the number of DoFs. If None, - it will be set to 0. - kp (float, np.array[N,N]): stiffness gain. - weight (float, np.array[N,N]): weight scalar or matrix associated to the task. - constraints (list of Constraint): list of constraints associated with the task. + q_desired (np.array[float[N]], None): desired joint positions, where :math:`N` is the number of DoFs. If + None, it will 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. + kp (float, np.array[float[N,N]]): stiffness gain. + weight (float, np.array[float[N,N]]): weight scalar or matrix associated to the task. + constraints (list[Constraint]): list of constraints associated with the task. """ super(PosturalTask, self).__init__(model=model, weight=weight, constraints=constraints) @@ -59,8 +72,8 @@ class PosturalTask(JointVelocityTask): self.kp = kp # define desired references - self.x_desired = q_desired - self.dx_desired = dq_desired + self.q_desired = q_desired + self.dq_desired = dq_desired # first update self.update() @@ -69,6 +82,43 @@ class PosturalTask(JointVelocityTask): # 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 x_desired(self): """Get the desired joint positions.""" @@ -77,15 +127,7 @@ class PosturalTask(JointVelocityTask): @x_desired.setter def x_desired(self, q_d): """Set the desired joint positions.""" - if q_d is None: - q_d = np.zeros(self.x_size) - if not isinstance(q_d, np.ndarray): - raise TypeError("Expecting the given desired joint positions to be an instance of np.array, instead got: " - "{}".format(type(q_d))) - if len(q_d) != self.x_size: - raise ValueError("Expecting the length of the given desired joint positions (={}) to be the same as the " - "number of DoFs (={})".format(len(q_d), self.x_size)) - self._q_d = q_d + self.q_desired = q_d @property def dx_desired(self): @@ -95,15 +137,7 @@ class PosturalTask(JointVelocityTask): @dx_desired.setter def dx_desired(self, dq_d): """Set the desired joint velocities.""" - if dq_d is None: - dq_d = np.zeros(self.x_size) - if not isinstance(dq_d, np.ndarray): - raise TypeError("Expecting the given desired joint velocities to be an instance of np.array, instead got: " - "{}".format(type(dq_d))) - if len(dq_d) != self.x_size: - raise ValueError("Expecting the length of the given desired joint velocities (={}) to be the same as the " - "number of DoFs (={})".format(len(dq_d), self.x_size)) - self._dq_d = dq_d + self.dq_desired = dq_d @property def kp(self): @@ -129,10 +163,10 @@ class PosturalTask(JointVelocityTask): """Set the desired references. Args: - x_des (np.array[N], None): desired joint positions, where :math:`N` is the number of DoFs. If None, - it will be set to 0. - dx_des (np.array[N], None): desired joint velocities, where :math:`N` is the number of DoFs. If None, - it will be set to 0. + x_des (np.array[float[N]], None): desired joint positions, where :math:`N` is the number of DoFs. If None, + it will be set to 0. + dx_des (np.array[float[N]], None): desired joint velocities, where :math:`N` is the number of DoFs. If + None, it will be set to 0. """ self.x_desired = x_des self.dx_desired = dx_des @@ -141,8 +175,8 @@ class PosturalTask(JointVelocityTask): """Return the desired references. Returns: - np.array[N]: desired joint positions. - np.array[N]: desired joint velocities. + np.array[float[N]]: desired joint positions. + np.array[float[N]]: desired joint velocities. """ return self.x_desired, self.dx_desired @@ -153,4 +187,7 @@ class PosturalTask(JointVelocityTask): q = self.model.get_joint_positions() # update b vector - self._b = np.dot(self.kp, (self._q_d - q)) + self._dq_d # shape: (N,) + if self._q_d is None: + self._b = self._dq_d + else: + self._b = np.dot(self.kp, (self._q_d - q)) + self._dq_d # shape: (N,)