wassname/pyrobolearn
1
0
Fork 0
mirror of https://github.com/wassname/pyrobolearn.git synced 2026-09-20 13:11:01 +08:00
Code Issues Packages Projects Releases Wiki Activity

update priorities

Browse Source
This commit is contained in:
Brian Delhaisse
2019-08-31 06:03:34 +02:00
parent 656612b885
commit 9cac32be9d
18 changed files with 1372 additions and 346 deletions
Download Patch File Download Diff File Expand all files Collapse all files
pyrobolearn/priorities/constraints/constraint.py
+115 -51
View File
@@ -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
pyrobolearn/priorities/constraints/velocity/joint_limits.py
+2 -2
View File
@@ -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"""
pyrobolearn/priorities/ik.py
+71
View File
@@ -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)
pyrobolearn/priorities/models/model.py
+17 -1
View File
@@ -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`.
pyrobolearn/priorities/models/robot_model.py
+27 -8
View File
@@ -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()
pyrobolearn/priorities/solvers/qp_task_solver.py
+38 -14
View File
@@ -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:
pyrobolearn/priorities/tasks/task.py
+342 -66
View File
@@ -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)
pyrobolearn/priorities/tasks/velocity/angular_momentum.py
+84 -20
View File
@@ -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,)
pyrobolearn/priorities/tasks/velocity/cartesian.py
+225 -57
View File
@@ -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))
pyrobolearn/priorities/tasks/velocity/com.py
+78 -35
View File
@@ -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,)
pyrobolearn/priorities/tasks/velocity/contact.py
+37 -9
View File
@@ -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)
pyrobolearn/priorities/tasks/velocity/interaction.py
+9 -5
View File
@@ -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.
"""
pyrobolearn/priorities/tasks/velocity/linear_momentum.py
+79 -18
View File
@@ -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
pyrobolearn/priorities/tasks/velocity/minimum_acceleration.py
+20 -3
View File
@@ -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.
"""
pyrobolearn/priorities/tasks/velocity/minimum_effort.py
+2 -2
View File
@@ -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)
pyrobolearn/priorities/tasks/velocity/minimum_velocity.py
+18 -2
View File
@@ -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)
pyrobolearn/priorities/tasks/velocity/momentum.py
+137 -19
View File
@@ -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,)
pyrobolearn/priorities/tasks/velocity/postural.py
+71 -34
View File
@@ -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,)