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

update priorities

Browse Source
This commit is contained in:
Brian Delhaisse
2019-09-02 06:01:22 +02:00
parent 8b4edf28fa
commit 2cc009fefe
21 changed files with 762 additions and 91 deletions
Download Patch File Download Diff File Expand all files Collapse all files
pyrobolearn/optimizers/nlopt_optimizer.py
+3 -3
View File
@@ -47,9 +47,9 @@ class NLopt(Optimizer):
- algorithms: https://nlopt.readthedocs.io/en/latest/NLopt_Algorithms/
References:
[1] NLopt: https://nlopt.readthedocs.io/en/latest/
[2] NLopt with Python: with Python: https://nlopt.readthedocs.io/en/latest/NLopt_Python_Reference/
[3] Github repo: https://github.com/stevengj/nlopt
- [1] NLopt: https://nlopt.readthedocs.io/en/latest/
- [2] NLopt with Python: with Python: https://nlopt.readthedocs.io/en/latest/NLopt_Python_Reference/
- [3] Github repo: https://github.com/stevengj/nlopt
"""
def __init__(self, method, submethod=None, seed=None, *args, **kwargs):
pyrobolearn/optimizers/qpsolvers_optimizer.py
+5 -5
View File
@@ -140,13 +140,13 @@ class QP(Optimizer):
# Methods #
###########
def optimize(self, P, q, x0=None, G=None, h=None, A=None, b=None):
def optimize(self, Q, p, x0=None, G=None, h=None, A=None, b=None):
r"""
Optimize the given quadratic problem.
.. math::
\min_{x \in \mathbb{R}^N} \frac{1}{2} x^T P x + q^T x
\min_{x \in \mathbb{R}^N} \frac{1}{2} x^T Q x + p^T x
subject to
@@ -156,9 +156,9 @@ class QP(Optimizer):
Ax = b
Args:
P (np.array[N,N]): matrix used in the QP objective function where `N` is the size of the vector `x` being
Q (np.array[N,N]): matrix used in the QP objective function where `N` is the size of the vector `x` being
optimized.
q (np.array[N]): vector used in the QP objective function where `N` is the size of the vector `x` being
p (np.array[N]): vector used in the QP objective function where `N` is the size of the vector `x` being
optimized.
G (np.array[M,N]): matrix used in the inequality constraint, where `M` is the number of inequalities, and
`N` is the size of the vector `x` being optimized. Note that if you have lower and upper bounds for
@@ -174,4 +174,4 @@ class QP(Optimizer):
Returns:
np.array: QP solution
"""
return qpsolvers.solve_qp(P, q, G, h, A, b, solver=self.method, initvals=x0, sym_proj=self.sym_proj)
return qpsolvers.solve_qp(Q, p, G, h, A, b, solver=self.method, initvals=x0, sym_proj=self.sym_proj)
pyrobolearn/priorities/constraints/README.rst
+7 -1
View File
@@ -4,7 +4,13 @@ Constraints
In this folder, we define the most common inequality and equality optimization constraints used in robotics for
priority tasks. Several of them were provided in [1].
Constraints include joint limits, joint velocity limits, collision avoidance, and others.
Constraints include joint limits, joint velocity limits, collision avoidance, and others. They are separated into
4 folders (velocity, acceleration, torque, and cartesian force); one for each optimization variable vector that is
being optimized. Note that different type of tasks (and thus constraints) can be combined together; for instance,
we can combine acceleration tasks with force tasks. This will create an optimization variable vector
:math:`x = [\ddot{q}^\top, F^\top]^\top` which can then be used with the joint space dynamic equation
:math:`\tau = H \ddot{q} + C(q,\dot{q})\dot{q} + g(q) - J^\top F` to get the equivalent joint torques to be applied
on the robot.
References:
pyrobolearn/priorities/constraints/__init__.py
+1
View File
@@ -1,6 +1,7 @@
# import constraint
from .constraint import *
from .constraint_from_task import ConstraintFromTask
# import velocity constraints
from . import velocity
pyrobolearn/priorities/constraints/constraint.py
+70 -31
View File
@@ -111,6 +111,7 @@ References:
import numpy as np
import pyrobolearn as prl
from pyrobolearn.priorities.models import ModelInterface
@@ -168,6 +169,10 @@ class Constraint(object):
self.constraints = constraints
self.model = model
# if the constraint is enabled or not, by default it is. This allows to dynamically enable and disable
# constraints.
self._enabled = True
# variables to be set in the corresponding child classes
self._lower_bound = None
self._upper_bound = None
@@ -238,6 +243,9 @@ class Constraint(object):
raise TypeError("Expecting the {}th inequality constraint to be an instance of `BoundConstraint`,"
" `UnilateralConstraint`, `BilateralConstraint`, but got: "
"{}".format(i, type(constraint)))
elif isinstance(constraint, prl.priorities.tasks.Task):
constraint = prl.priorities.constraints.ConstraintFromTask(constraint)
constraint_dict.setdefault(EqualityConstraint, []).append(constraint)
else:
raise TypeError("Expecting the {}th constraint to be an instance of `EqualityConstraint` or "
"`InequalityConstraint`, but got: {}".format(i, type(constraint)))
@@ -253,7 +261,7 @@ class Constraint(object):
"""
if self.constraints:
constraints = self.constraints.get(BoundConstraint, [])
return [constraint.lower_bound for constraint in constraints]
return [constraint.lower_bound for constraint in constraints if constraint.enabled]
return self._lower_bound
@property
@@ -265,7 +273,7 @@ class Constraint(object):
"""
if self.constraints:
constraints = self.constraints.get(BoundConstraint, [])
return [constraint.lower_bound for constraint in constraints]
return [constraint.lower_bound for constraint in constraints if constraint.enabled]
return self._upper_bound
@property
@@ -277,7 +285,7 @@ class Constraint(object):
"""
if self.constraints:
constraints = self.constraints.get(EqualityConstraint, [])
return [constraint.A_eq for constraint in constraints]
return [constraint.A_eq for constraint in constraints if constraint.enabled]
return self._A_eq
@property
@@ -289,7 +297,7 @@ class Constraint(object):
"""
if self.constraints:
constraints = self.constraints.get(EqualityConstraint, [])
return [constraint.b_eq for constraint in constraints]
return [constraint.b_eq for constraint in constraints if constraint.enabled]
return self._b_eq
@property
@@ -302,7 +310,8 @@ class Constraint(object):
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]
constraints = unilateral_constraints + bilateral_constraints
return [constraint.A_ineq for constraint in constraints if constraint.enabled]
return self._A_ineq
@property
@@ -315,7 +324,8 @@ class Constraint(object):
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]
constraints = unilateral_constraints + bilateral_constraints
return [constraint.b_lower_bound for constraint in constraints if constraint.enabled]
return self._b_lower_bound
@property
@@ -328,7 +338,8 @@ class Constraint(object):
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]
constraints = unilateral_constraints + bilateral_constraints
return [constraint.b_upper_bound for constraint in constraints if constraint.enabled]
return self._b_upper_bound
@property
@@ -342,31 +353,35 @@ class Constraint(object):
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))
if constraint.enabled:
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)
if constraint.enabled:
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)
if constraint.enabled:
results.append(-constraint.A_ineq)
upper_unilateral_constraints = self.constraints.get(UpperUnilateralConstraint, [])
for constraint in upper_unilateral_constraints:
results.append(constraint.A_ineq)
if constraint.enabled:
results.append(constraint.A_ineq)
if results:
return np.concatenate(results)
else:
if isinstance(self, BoundConstraint):
if isinstance(self, BoundConstraint) and self.enabled:
x_size = len(self._lower_bound)
return np.concatenate((-np.identity(x_size), np.identity(x_size)))
elif isinstance(self, BilateralConstraint):
elif isinstance(self, BilateralConstraint) and self.enabled:
return np.concatenate((-self._A_ineq, self._A_ineq))
elif isinstance(self, LowerUnilateralConstraint):
elif isinstance(self, LowerUnilateralConstraint) and self.enabled:
return -self._A_ineq
elif isinstance(self, UpperUnilateralConstraint):
elif isinstance(self, UpperUnilateralConstraint) and self.enabled:
return self._A_ineq
@property
@@ -380,29 +395,33 @@ class Constraint(object):
results = []
bound_constraints = self.constraints.get(BoundConstraint, [])
for constraint in bound_constraints:
results.append(-constraint.lower_bound)
results.append(constraint.upper_bound)
if constraint.enabled:
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)
if constraint.enabled:
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)
if constraint.enabled:
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 constraint.enabled:
results.append(constraint.b_upper_bound)
if results:
return np.concatenate(results)
else:
if isinstance(self, BoundConstraint):
if isinstance(self, BoundConstraint) and self.enabled:
return np.concatenate((-self._lower_bound, self._upper_bound))
elif isinstance(self, BilateralConstraint):
elif isinstance(self, BilateralConstraint) and self.enabled:
return np.concatenate((-self._b_lower_bound, self._upper_bound))
elif isinstance(self, LowerUnilateralConstraint):
elif isinstance(self, LowerUnilateralConstraint) and self.enabled:
return -self._b_lower_bound
elif isinstance(self, UpperUnilateralConstraint):
elif isinstance(self, UpperUnilateralConstraint) and self.enabled:
return self._b_upper_bound
@property
@@ -423,6 +442,11 @@ class Constraint(object):
"""
return self.b_eq
@property
def enabled(self):
"""Return if the task is enabled or not."""
return self._enabled
##################
# Static methods #
##################
@@ -457,6 +481,18 @@ class Constraint(object):
# Methods #
###########
def enable(self, enable=True):
"""Enable the single constraint, or each inner constraint."""
if not self.is_single_constraint():
for constraint in self.constraints:
constraint.enable(enable=enable)
else:
self._enabled = enable
def disable(self, disable=True):
"""Disable the single constraint, or each inner constraint."""
self.enable(not disable)
def has_constraints(self):
"""Return True if it has inner constraints."""
return len(self.constraints) > 0
@@ -477,7 +513,7 @@ class Constraint(object):
raise TypeError("Expecting the given 'constraint' to be an instance of `Constraint` but got instead: "
"{}".format(type(constraint)))
if not constraint.is_single_constraint():
raise ValueError("Expecting to append a single constraint, but the given constraint as a list of "
raise ValueError("Expecting to append a single constraint, but the given constraint has a list of "
"inner constraints")
if isinstance(constraint, EqualityConstraint):
@@ -497,6 +533,9 @@ class Constraint(object):
else:
raise TypeError("Expecting the inequality constraint to be an instance of `BoundConstraint`, "
"`UnilateralConstraint`, `BilateralConstraint`, but got: {}".format(type(constraint)))
elif isinstance(constraint, prl.priorities.tasks.Task):
constraint = prl.priorities.constraints.ConstraintFromTask(constraint)
self.constraints.setdefault(EqualityConstraint, []).append(constraint)
else:
raise TypeError("The given type of constraint is not currently supported.")
@@ -643,6 +682,6 @@ class JointTorqueConstraint(DynamicConstraint):
pass
class JointForceConstraint(DynamicConstraint):
r"""Joint Force constraint."""
class ForceConstraint(DynamicConstraint):
r"""Force constraint."""
pass
pyrobolearn/priorities/constraints/force/__init__.py
+10
View File
@@ -1,2 +1,12 @@
# cartesian force constraints
from .contact import ContactConstraint
# from .cop import CoPConstraint
from .friction import FrictionPyramidConstraint # , FrictionConeConstraint
from .wrench_limits import WrenchLimitsConstraint
from .zmp import ZMPConstraint
pyrobolearn/priorities/constraints/force/contact.py
+76 -7
View File
@@ -1,17 +1,29 @@
#!/usr/bin/env python
r"""Provide the contact (force normal) constraint.
The lower unilateral contact force constraint is given by :math:`0 \leq f^i_n` where :math:`f^i_n` is the normal
force with respect to the contact surface applied on the link in contact :math:`i` defined in the local frame.
The implementation of this class is inspired by [1] (which is licensed under the LGPLv2).
The optimization variables are these contact forces :math:`f^i_n` expressed in the world frame, thus they are
rotated to their local frame. This formulation can be rewritten as a unilateral inequality constraint
:math:`b_l \leq A_{ineq} x` in QP, with :math:`x = f^w \in \mathbb{R}^{6N_c}` which is the concatenation of all
the contact force variables (one for each contact point) expressed in the world frame,
:math:`A_{ineq} = R^l_w \in \mathbb{R}^{6N_c \times 6N_c}` is the block diagonal matrix that rotates the force
variables expressed in the world frame :math:`w` to their respective local frame :math:`l`, and
:math:`b_l = [-\infty, -\infty, 0, -\infty, -\infty, -\infty] * N_c`, where :math:`N_c` is the total number of
contact points.
The implementation of this class is inspired by [1].
References:
- [1] "Motion Planning and Control of Dynamics Humanoid Locomotion" (PhD thesis), Xin, 2018
"""
import numpy as np
from scipy.linalg import block_diag
from pyrobolearn.priorities.constraints.constraint import Constraint
from pyrobolearn.priorities.constraints.constraint import LowerUnilateralConstraint, ForceConstraint
from pyrobolearn.utils.transformation import get_matrix_from_quaternion
__author__ = "Brian Delhaisse"
__copyright__ = "Copyright 2019, PyRoboLearn"
@@ -23,15 +35,72 @@ __email__ = "briandelhaisse@gmail.com"
__status__ = "Development"
class ContactConstraint(Constraint):
class ContactConstraint(LowerUnilateralConstraint, ForceConstraint):
r"""Contact force constraint
The contact force constraint is given by :math:`0 \leq f^i_n` where :math:`f^i_n` is the normal force with respect
to the contact surface applied on the link in contact :math:`i` defined in the local frame.
The lower unilateral contact force constraint is given by :math:`0 \leq f^i_n` where :math:`f^i_n` is the normal
force with respect to the contact surface applied on the link in contact :math:`i` defined in the local frame.
The optimization variables are these contact forces :math:`f^i_n` expressed in the world frame, thus they are
rotated to their local frame. This formulation can be rewritten as a unilateral inequality constraint
:math:`b_l \leq A_{ineq} x` in QP, with :math:`x = f^w \in \mathbb{R}^{6N_c}` which is the concatenation of all
the contact force variables (one for each contact point) expressed in the world frame,
:math:`A_{ineq} = R^l_w \in \mathbb{R}^{6N_c \times 6N_c}` is the block diagonal matrix that rotates the force
variables expressed in the world frame :math:`w` to their respective local frame :math:`l`, and
:math:`b_l = [-\infty, -\infty, 0, -\infty, -\infty, -\infty] * N_c`, where :math:`N_c` is the total number of
contact points.
The implementation of this class is inspired by [1].
References:
- [1] "Motion Planning and Control of Dynamics Humanoid Locomotion" (PhD thesis), Xin, 2018
"""
def __init__(self, model):
def __init__(self, model, contacts=[]):
r"""
Initialize the constraint.
Args:
model (ModelInterface): model interface.
contacts (list[int], list[str]): list of contact links (ids or names).
"""
super(ContactConstraint, self).__init__(model)
# set variables
self.contacts = contacts
self._vector = -np.infty * np.ones(6)
self._vector[2] = 0
# first update
self.update()
##############
# Properties #
##############
@property
def contacts(self):
"""Get the list of contact links."""
return self._contacts
@contacts.setter
def contacts(self, contacts):
"""Set the contact links."""
if not isinstance(contacts, (list, tuple, np.ndarray)):
raise TypeError("Expecting the given 'contacts' to be a list of int/str, but instead got: "
"{}".format(type(contacts)))
self._contacts = contacts
###########
# Methods #
###########
def _update(self):
"""Update the lower unilateral inequality matrix and vector."""
rotations = []
for contact in self.contacts:
link = self.model.get_link_id(contact)
rot = get_matrix_from_quaternion(self.model.get_orientation(link)).T # (3,3)
rotations.append(block_diag((rot, rot))) # (6,6)
self._A_ineq = block_diag(rotations) # (M*6,M*6)
self._b_lower_bound = np.concatenate([self._vector for _ in self.contacts]) # (M*6,)
pyrobolearn/priorities/constraints/force/friction.py
+108 -8
View File
@@ -10,9 +10,10 @@ References:
"""
import numpy as np
from scipy.linalg import block_diag
from pyrobolearn.priorities.constraints.constraint import Constraint
from pyrobolearn.priorities.constraints.constraint import UpperUnilateralConstraint, ForceConstraint
from pyrobolearn.utils.transformation import get_matrix_from_quaternion
__author__ = "Brian Delhaisse"
__copyright__ = "Copyright 2019, PyRoboLearn"
@@ -25,7 +26,7 @@ __email__ = "briandelhaisse@gmail.com"
__status__ = "Development"
class FrictionConeConstraint(Constraint):
class FrictionConeConstraint(UpperUnilateralConstraint, ForceConstraint):
r"""Friction Cone constraint
The friction cone is defined as:
@@ -47,18 +48,28 @@ class FrictionConeConstraint(Constraint):
- [4] "Motion Planning and Control of Dynamics Humanoid Locomotion" (PhD thesis), Xin, 2018
"""
def __init__(self, model):
def __init__(self, model, mu=0.7):
r"""
Initialize the constraint.
Args:
model (ModelInterface): model interface.
mu (float): friction coefficient.
"""
super(FrictionConeConstraint, self).__init__(model)
# set variable
self.mu = mu
class FrictionPyramidConstraint(Constraint):
class FrictionPyramidConstraint(UpperUnilateralConstraint, ForceConstraint):
r"""Friction Pyramid constraint
The friction pyramid constraint is a linear approximation of the friction cone.
The friction pyramid is defined as:
.. math:: P^i_s = {(f^i_x, f^i_y, f^i_z) \in \mathbb{R}^3 | f^i_x \leq \mu_i f^i_z, f^i_y \leq \mu_i f^i_z}
.. math:: P^i_s = {(f^i_x, f^i_y, f^i_z) \in \mathbb{R}^3 | |f^i_x| \leq \mu_i f^i_z, |f^i_y| \leq \mu_i f^i_z}
where where :math:`i` denotes the ith support/contact, :math:`f^i_s` is the contact spatial force exerted at
the contact point :math:`C_i`, and :math:`\mu_i` is the static friction coefficient at that contact point.
@@ -69,11 +80,100 @@ class FrictionPyramidConstraint(Constraint):
This linear approximation is often used as a linear constraint in a quadratic optimization problem along with
the unilateral constraint :math:`f^i_z \geq 0`.
QP formulation
--------------
The friction pyramid constraints given by:
.. math::
-\mu f^i_z \leq f^i_x \leq \mu f^i_z \\
-\mu f^i_z \leq f^i_y \leq \mu f^i_z
can be rewritten as:
.. math::
f^i_x - \mu f^i_z \leq 0 \\
-f^i_x - \mu f^i_z \leq 0 \\
f^i_y - \mu f^i_z \leq 0 \\
-f^i_y - \mu f^i_z \leq 0
Thus, it can be rewritten as the inequality constraint :math:`G x \leq h` in QP, with:
.. math::
G = \left[\begin{array}{cccccc}
1 & 0 & -\mu & 0 & 0 & 0 \\
-1 & 0 & -\mu & 0 & 0 & 0 \\
0 & 1 & -\mu & 0 & 0 & 0 \\
0 & -1 & -\mu & 0 & 0 & 0 \\
\end{array}\right],
:math:`x = [f^i_x, f^i_y, f^i_z, n^i_x, n^i_y, n^i_z]` being the optimized variables, and :math:`h = [0,0,0,0]`.
Note that the optimized variables are expressed in the world frame, and thus a rotation to express them in their
contact local frame is performed beforehand.
References:
- [1] https://scaron.info/teaching/friction-cones.html
- [2] "Stability of Surface Contacts for Humanoid Robots: Closed-Form Formulae of the Contact Wrench Cone
for Rectangular Support Areas", Caron et al., 2015
- [3] "Motion Planning and Control of Dynamics 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):
super(FrictionPyramidConstraint, self).__init__(model)
def __init__(self, model, mu=0.7, contacts=[]):
r"""
Initialize the constraint.
Args:
model (ModelInterface): model interface.
mu (float): friction coefficient.
contacts (list[int], list[str]): list of contact link unique id or name.
"""
super(FrictionPyramidConstraint, self).__init__(model)
# set variable
self.mu = mu
self._friction_matrix = np.zeros((4, 6))
self._friction_matrix[0, 0] = 1
self._friction_matrix[1, 0] = -1
self._friction_matrix[2, 1] = 1
self._friction_matrix[3, 1] = -1
self._friction_matrix[:, 2] = -self.mu
self.contacts = contacts
# first update
self.update()
##############
# Properties #
##############
@property
def contacts(self):
"""Get the list of contact links."""
return self._contacts
@contacts.setter
def contacts(self, contacts):
"""Set the contact links."""
if not isinstance(contacts, (list, tuple, np.ndarray)):
raise TypeError("Expecting the given 'contacts' to be a list of int/str, but instead got: "
"{}".format(type(contacts)))
self._contacts = contacts
###########
# Methods #
###########
def _update(self):
"""Update the lower unilateral inequality matrix and vector."""
self._A_ineq = np.zeros(4 * len(self.contacts), 6 * len(self.contacts))
for i, contact in enumerate(self.contacts):
rot = get_matrix_from_quaternion(self.model.get_orientation(self._link)).T
rot = block_diag((rot, rot))
self._A_ineq[i*4:(i+1)*4, i*6:(i+1)*6] = self._friction_matrix.dot()
self._b_upper_bound = np.zeros(4 * len(self.contacts))
pyrobolearn/priorities/constraints/force/wrench_limits.py
+98 -3
View File
@@ -1,6 +1,17 @@
#!/usr/bin/env python
r"""Provide the wrench limits constraint.
This provides bounds/limits on the wrenches:
.. math:: F_{lb} \leq F \leq F_{ub}
where :math:`(F_{lb}, F_{ub})` are the lower and upper bound on the wrenches, and
:math:`F` is the wrench vector being optimized.
This formulation can be rewritten as the inequality constraint math:`lb \leq x \leq ub` in QP, with
:math:`lb = F_{lb}`, :math:`ub = F_{ub}`, and :math:`x = F`. This can also be rewritten as :math:`Gx \leq h`,
with :math:`G = [-I, I]^\top` and :math:`h = [-F_{lb}^\top, F_{ub}^\top]^\top` where :math:`I` is the square
identity matrix.
The implementation of this class is inspired by [1] (which is licensed under the LGPLv2).
@@ -10,7 +21,7 @@ References:
import numpy as np
from pyrobolearn.priorities.constraints.constraint import Constraint
from pyrobolearn.priorities.constraints.constraint import BoundConstraint, ForceConstraint
__author__ = "Brian Delhaisse"
@@ -23,10 +34,94 @@ __email__ = "briandelhaisse@gmail.com"
__status__ = "Development"
class WrenchLimitsConstraint(Constraint):
class WrenchLimitsConstraint(BoundConstraint, ForceConstraint):
r"""Wrench Limits constraint.
This provides bounds/limits on the wrenches:
.. math:: F_{lb} \leq F \leq F_{ub}
where :math:`(F_{lb}, F_{ub})` are the lower and upper bound on the wrenches, and
:math:`F` is the wrench vector being optimized.
This formulation can be rewritten as the inequality constraint math:`lb \leq x \leq ub` in QP, with
:math:`lb = F_{lb}`, :math:`ub = F_{ub}`, and :math:`x = F`. This can also be rewritten as :math:`Gx \leq h`,
with :math:`G = [-I, I]^\top` and :math:`h = [-F_{lb}^\top, F_{ub}^\top]^\top` where :math:`I` is the square
identity matrix.
The implementation of this class is inspired by [1] (which is licensed under the LGPLv2).
References:
- [1] "OpenSoT: A whole-body control library for the compliant humanoid robot COMAN", Rocchi et al., 2015
"""
def __init__(self, model):
def __init__(self, model, bounds):
r"""
Initialize the constraint.
Args:
model (ModelInterface): model interface.
bounds (tuple[2 * np.array[float[M]]], np.array[float[M]]): wrench limits, where `M` is 3 (vector of
forces) or 6 (vector of forces and torques). If tuple, it is the lower and upper bounds on the wrenches.
If np.array, then the lower and upper bound will be set to (-bounds, bounds).
"""
super(WrenchLimitsConstraint, self).__init__(model)
# set variables
self.bounds = bounds
# first update
self.update()
##############
# Properties #
##############
@property
def bounds(self):
"""Get the wrench bounds."""
return self._bounds
@bounds.setter
def bounds(self, bounds):
"""Set the wrench bounds."""
if bounds is not None:
if isinstance(bounds, tuple):
if len(bounds) != 2:
raise ValueError("Expecting the bounds to be a tuple of length 2, but got a length of "
"{}".format(len(bounds)))
for i, bound in enumerate(bounds):
if isinstance(bound, (int, float)):
bound = np.ones(3) * bound
bounds[i] = bound
elif isinstance(bound, np.ndarray) and len(bound) != 3:
raise ValueError("Expecting the given bound to be of length 3, but instead got a length of "
"{}".format(len(bound)))
else:
raise TypeError("Expecting the given bound to be a np.array, but got instead: "
"{}".format(type(bound)))
elif isinstance(bounds, np.ndarray):
bounds = bounds.reshape(-1)
if len(bounds) == 3:
bounds = (-bounds, bounds)
elif len(bounds) == 6:
bounds = (bounds[:3], bounds[3:])
elif len(bounds) == 12:
bounds = (bounds[:6], bounds[6:])
else:
raise ValueError("Expecting the given bounds to be of length 3 or 6 but got instead a length of: "
"{}".format(len(bounds)))
elif isinstance(bounds, (int, float)):
bounds = (-np.ones(3) * bounds, np.ones(3) * bounds)
else:
raise TypeError("Expecting the given bounds to be a tuple of np.array, a np.array, or None, but "
"instead got: {}".format(type(bounds)))
self._bounds = bounds
###########
# Methods #
###########
def _update(self):
"""Update the lower and upper bounds."""
self._b_lower_bound, self._b_upper_bound = self._bounds
pyrobolearn/priorities/constraints/force/zmp.py
+187 -6
View File
@@ -1,15 +1,68 @@
#!/usr/bin/env python
r"""Provide the Zero-Moment Point constraint.
The ZMP constraints can be expressed as:
.. math::
d_x^{-} \leq -\frac{n^i_y}{f^i_z} \leq d_x^{+} \\
d_y^{-} \leq \frac{n^i_x}{f^i_z} \leq d_y^{+}
which ensures the stability of the foot/ground contact. The :math:`(d_x^{-}, d_x^{+})` and :math:`(d_y^{-}, d_y^{+})`
defines the size of the sole in the x and y directions respectively. Basically, this means that the ZMP point must be
inside the convex hull in order to have a static stability. The :math:`n^i` are the contact torques around the contact
point :math:`i`, and :math:`f` is the contact force at the contact point :math:`i`.
Notes:
- the ZMP and CoP are equivalent for horizontal ground surfaces. For irregular ground surfaces they are
distinct. [2]
QP formulation
--------------
The ZMP constraints given by:
.. math::
d_x^{-} \leq -\frac{n^i_y}{f^i_z} \leq d_x^{+} \\
d_y^{-} \leq \frac{n^i_x}{f^i_z} \leq d_y^{+}
can be rewritten as:
.. math::
d_x^{-} f^i_z + n^i_y \leq 0 \\
-d_x^{+} f^i_z - n^i_y \leq 0 \\
d_y^{-} f^i_z - n^i_x \leq 0 \\
-d_y^{+} f^i_z + n^i_x \leq 0
Thus, it can be rewritten as the inequality constraint :math:`G x \leq h` in QP, with:
.. math::
G = \left[\begin{array}{cccccc}
0 & 0 & d_x^{-} & 0 & 1 & 0 \\
0 & 0 & -d_x^{+} & 0 & -1 & 0 \\
0 & 0 & d_y^{-} & -1 & 0 & 0 \\
0 & 0 & -d_y^{+} & 1 & 0 & 0 \\
\end{array}\right],
:math:`x = [f^i_x, f^i_y, f^i_z, n^i_x, n^i_y, n^i_z]` being the optimized variables, and :math:`h = [0,0,0,0]`.
References:
- [1] "Motion Planning and Control of Dynamics Humanoid Locomotion" (PhD thesis), Xin, 2018
- [2] "Ground Reference Points in Legged Locomotion: Definitions, Biological Trajectories and Control
Implications", Popovic et al., 2005
"""
# TODO: correct this file when there are multiple links
import numpy as np
from scipy.linalg import block_diag
from pyrobolearn.priorities.constraints.constraint import Constraint
from pyrobolearn.priorities.constraints.constraint import UpperUnilateralConstraint, ForceConstraint
from pyrobolearn.utils.transformation import get_matrix_from_quaternion
__author__ = "Brian Delhaisse"
__copyright__ = "Copyright 2019, PyRoboLearn"
@@ -21,7 +74,7 @@ __email__ = "briandelhaisse@gmail.com"
__status__ = "Development"
class ZMPConstraint(Constraint):
class ZMPConstraint(UpperUnilateralConstraint, ForceConstraint):
r"""Zero-Moment Point constraint.
"The ZMP is the point on the ground surface about which the horizontal component of the moment of ground
@@ -54,8 +107,8 @@ class ZMPConstraint(Constraint):
.. math::
d_x^{-} \leq \frac{n^i_y}{f^i_z} \leq d_x^{+} \\
d_y^{-} \leq -\frac{n^i_x}{f^i_z} \leq d_y^{+}
d_x^{-} \leq -\frac{n^i_y}{f^i_z} \leq d_x^{+} \\
d_y^{-} \leq \frac{n^i_x}{f^i_z} \leq d_y^{+}
which ensures the stability of the foot/ground contact. The :math:`(d_x^{-}, d_x^{+})` and
:math:`(d_y^{-}, d_y^{+})` defines the size of the sole in the x and y directions respectively. Basically,
@@ -69,12 +122,140 @@ class ZMPConstraint(Constraint):
- the FRI coincides with the ZMP when the foot is stationary. [1]
- the CMP coincides with the ZMP, when the moment about the CoM is zero. [1]
QP formulation
--------------
The ZMP constraints given by:
.. math::
d_x^{-} \leq -\frac{n^i_y}{f^i_z} \leq d_x^{+} \\
d_y^{-} \leq \frac{n^i_x}{f^i_z} \leq d_y^{+}
can be rewritten as:
.. math::
d_x^{-} f^i_z + n^i_y \leq 0 \\
-d_x^{+} f^i_z - n^i_y \leq 0 \\
d_y^{-} f^i_z - n^i_x \leq 0 \\
-d_y^{+} f^i_z + n^i_x \leq 0
Thus, it can be rewritten as the inequality constraint :math:`G x \leq h` in QP, with:
.. math::
G = \left[\begin{array}{cccccc}
0 & 0 & d_x^{-} & 0 & 1 & 0 \\
0 & 0 & -d_x^{+} & 0 & -1 & 0 \\
0 & 0 & d_y^{-} & -1 & 0 & 0 \\
0 & 0 & -d_y^{+} & 1 & 0 & 0
\end{array}\right],
:math:`x = [f^i_x, f^i_y, f^i_z, n^i_x, n^i_y, n^i_z]` being the optimized variables, and :math:`h = [0,0,0,0]`.
Note that the optimized variables are expressed in the world frame, and thus a rotation to express them in their
contact local frame is performed beforehand.
The implementation is based from insights provided in [4].
References:
- [1] "Ground Reference Points in Legged Locomotion: Definitions, Biological Trajectories and Control
Implications", Popovic et al., 2005
- [2] "Biped Walking Pattern Generation by using Preview Control of ZMP", Kajita et al., 2003
- [3] "Exploiting Angular Momentum to Enhance Bipedal Center-of-Mass Control", Hofmann et al., 2009
- [4] "Motion Planning and Control of Dynamics Humanoid Locomotion" (PhD thesis), Xin, 2018
"""
def __init__(self, model):
def __init__(self, model, x_bounds, y_bounds, link): # TODO: correct when multiple links
r"""
Initialize the constraint.
Args:
model (ModelInterface): model interface.
x_bounds (tuple[2*float]): lower and upper bound of the size of the sole in the x direction.
y_bounds (tuple[2*float]): lower and upper bound of the size of the sole in the y direction.
link (int, str): unique link id or name.
"""
super(ZMPConstraint, self).__init__(model)
# set optimization variables
self._b_upper_bound = np.zeros(4)
self._zmp_matrix = np.zeros((4, 6))
self._zmp_matrix[0, 4] = 1
self._zmp_matrix[1, 4] = -1
self._zmp_matrix[2, 3] = -1
self._zmp_matrix[3, 3] = 1
# the rest of A_ineq is set when setting the bounds
# set variables
self.x_bounds = x_bounds
self.y_bounds = y_bounds
self._link = self.model.get_link_id(link)
# first update
self.update()
##############
# Properties #
##############
@property
def x_bounds(self):
"""Get the sole size bounds in the x-direction."""
return self._x_bounds
@x_bounds.setter
def x_bounds(self, bounds):
"""Set the sole size bounds in the x-direction."""
if not isinstance(bounds, (tuple, list, np.ndarray)):
raise TypeError("Expecting the given 'x_bounds' to be a tuple, list or np.array of 2 float, but got "
"instead: {}".format(bounds))
if len(bounds) != 2:
raise ValueError("Expecting the given 'x_bounds' to be of size 2, but got a size of "
"{}".format(len(bounds)))
self._x_bounds = bounds
self._zmp_matrix[0, 2] = bounds[0]
self._zmp_matrix[1, 2] = -bounds[1]
@property
def y_bounds(self):
"""Get the sole size bounds in the y-direction."""
return self._y_bounds
@y_bounds.setter
def y_bounds(self, bounds):
"""Set the sole size bounds in the y-direction."""
if not isinstance(bounds, (tuple, list, np.ndarray)):
raise TypeError("Expecting the given 'y_bounds' to be a tuple, list or np.array of 2 float, but got "
"instead: {}".format(bounds))
if len(bounds) != 2:
raise ValueError("Expecting the given 'y_bounds' to be of size 2, but got a size of "
"{}".format(len(bounds)))
self._y_bounds = bounds
self._zmp_matrix[2, 2] = bounds[0]
self._zmp_matrix[3, 2] = -bounds[1]
# @property
# def contacts(self):
# """Get the list of contact links."""
# return self._contacts
#
# @contacts.setter
# def contacts(self, contacts):
# """Set the contact links."""
# if not isinstance(contacts, (list, tuple, np.ndarray)):
# raise TypeError("Expecting the given 'contacts' to be a list of int/str, but instead got: "
# "{}".format(type(contacts)))
# self._contacts = contacts
###########
# Methods #
###########
def _update(self):
"""Update the upper unilateral inequality matrix and vector."""
rot = get_matrix_from_quaternion(self.model.get_orientation(self._link)).T
rot = block_diag((rot, rot))
self._A_ineq = self._zmp_matrix.dot(rot) # (4,6)
pyrobolearn/priorities/constraints/torque/joint_limits.py
+1 -1
View File
@@ -26,7 +26,7 @@ __status__ = "Development"
class JointLimitsConstraint(BoundConstraint, JointTorqueConstraint):
r"""Joint Limits constraint.
This provides bounds/limits on the joint torques:
This provides bounds/limits on the joint torques (based on a PD control feedback law):
.. math:: k_p (q_{lb} - q) - k_d \dot{q} \leq \tau \leq k_p (q_{ub} - q) - k_d \dot{q}
pyrobolearn/priorities/constraints/velocity/__init__.py
+16 -2
View File
@@ -1,8 +1,22 @@
# joint velocity constraints
# from .capture_point import CapturePointConstraint
from .cartesian_position import CartesianPositionConstraint
from .cartesian_velocity import CartesianVelocityConstraint
from .com_velocity import CoMVelocityConstraint
from .convex_hull import ConvexHullConstraint
# from .dynamics import DynamicsConstraint
from .joint_limits import JointPositionLimitsConstraint
from .velocity_limits import JointVelocityLimitsConstraint
from .joint_velocity import DifferentialKinematicsConstraint
# from .self_collision_avoidance import SelfCollisionAvoidanceConstraint
from .velocity_limits import JointVelocityLimitsConstraint
pyrobolearn/priorities/constraints/velocity/capture_point.py
+15 -2
View File
@@ -8,9 +8,11 @@ References:
- [1] "OpenSoT: A whole-body control library for the compliant humanoid robot COMAN", Rocchi et al., 2015
"""
# TODO: finish to implement this
import numpy as np
from pyrobolearn.priorities.constraints.constraint import Constraint
from pyrobolearn.priorities.constraints.constraint import UnilateralConstraint, JointVelocityConstraint
__author__ = "Brian Delhaisse"
@@ -23,9 +25,20 @@ __email__ = "briandelhaisse@gmail.com"
__status__ = "Development"
class CapturePointConstraint(Constraint):
class CapturePointConstraint(UnilateralConstraint, JointVelocityConstraint):
r"""Capture Point constraint.
Definition: "For a biped in state :math:`x`, a Capture Point (CP) :math:`P`, is a point on the ground such that
if the biped covers :math:`P` (makes its base of support include :math:`P`), either with its stance foot or by
stepping to :math:`P` in a single step, and then maintains its Center of Pressure (CoP) to lie on :math:`P`, then
there exists a safe feasible trajectory leading to a capture state (i.e. a state in which the kinetic energy of
the biped is zero and can remain zero with suitable joint torque (note that the CoM must lie above the CoP in a
capture state))." [1] "Intuitively, the CP is the point on the floor onto which the robot has to step to come
to a complete rest" [2].
References:
- [1] "Capture Point: A Step toward Humanoid PushRecovery", Pratt et al., 2006
- [2] "Bipedal walking control based on Capture Point dynamics", Englsberger et al., 2011
"""
def __init__(self, model):
pyrobolearn/priorities/constraints/velocity/com_velocity.py
+94 -4
View File
@@ -1,6 +1,15 @@
#!/usr/bin/env python
r"""Provide the Center of Mass velocity constraint.
The bilateral inequality CoM velocity constraint is given by:
.. math:: v_{lb} \leq J_{CoM}(q) \dot{q} \leq v_{ub}
where :math:`v_{lb}, v_{ub}` are the lower and upper bound of the CoM velocities, :math:`\dot{q}` are the joint
velocities being optimized, and :math:`J_{CoM}(q)` is the CoM Jacobian.
This formulation can be rewritten as a bilateral inequality constraint :math:`b_l \leq A_{ineq} x \leq b_u` in QP,
with :math:`x = \dot{q}`, :math:`A_{ineq} = J_{CoM}(q)`, :math:`b_l = v_{lb}` and :math:`b_u = v_{ub}`.
The implementation of this class is inspired by [1] (which is licensed under the LGPLv2).
@@ -10,7 +19,7 @@ References:
import numpy as np
from pyrobolearn.priorities.constraints.constraint import Constraint
from pyrobolearn.priorities.constraints.constraint import BilateralConstraint, JointVelocityConstraint
__author__ = "Brian Delhaisse"
@@ -23,10 +32,91 @@ __email__ = "briandelhaisse@gmail.com"
__status__ = "Development"
class CoMVelocityConstraint(Constraint):
r"""Center of Mass Velocity constraint.
class CoMVelocityConstraint(BilateralConstraint, JointVelocityConstraint):
r"""Center of Mass (CoM) Velocity constraint.
The bilateral inequality CoM velocity constraint is given by:
.. math:: v_{lb} \leq J_{CoM}(q) \dot{q} \leq v_{ub}
where :math:`v_{lb}, v_{ub}` are the lower and upper bound of the CoM linear velocities, :math:`\dot{q}` are the
joint velocities being optimized, and :math:`J_{CoM}(q)` is the CoM Jacobian.
This formulation can be rewritten as a bilateral inequality constraint :math:`b_l \leq A_{ineq} x \leq b_u` in QP,
with :math:`x = \dot{q}`, :math:`A_{ineq} = J_{CoM}(q)`, :math:`b_l = v_{lb}` and :math:`b_u = v_{ub}`.
The implementation of this class is inspired by [1] (which is licensed under the LGPLv2).
References:
- [1] "OpenSoT: A whole-body control library for the compliant humanoid robot COMAN", Rocchi et al., 2015
"""
def __init__(self, model):
def __init__(self, model, velocity_bounds):
r"""
Initialize the constraint.
Args:
model (ModelInterface): model interface.
velocity_bounds (tuple[2 * np.array[float[3]]], np.array[float[3]], float, None): If tuple, it is the
lower and upper bounds on the linear velocity. If np.array or float, then the lower and upper bound
would be set to (-velocity_bounds, velocity_bounds). If None, it will not be considered.
"""
super(CoMVelocityConstraint, self).__init__(model)
# define variables
self.velocity_bounds = velocity_bounds
# first update
self.update()
##############
# Properties #
##############
@property
def velocity_bounds(self):
"""Get the CoM linear velocity bounds."""
return self._vel_bounds
@velocity_bounds.setter
def velocity_bounds(self, bounds):
"""Set the CoM linear velocity bounds."""
if bounds is not None:
if isinstance(bounds, tuple):
if len(bounds) != 2:
raise ValueError("Expecting the bounds to be a tuple of length 2, but got a length of "
"{}".format(len(bounds)))
for i, bound in enumerate(bounds):
if isinstance(bound, (int, float)):
bound = np.ones(3) * bound
bounds[i] = bound
elif isinstance(bound, np.ndarray) and len(bound) != 3:
raise ValueError("Expecting the given bound to be of length 3, but instead got a length of "
"{}".format(len(bound)))
else:
raise TypeError("Expecting the given bound to be a np.array, but got instead: "
"{}".format(type(bound)))
elif isinstance(bounds, np.ndarray):
bounds = bounds.reshape(-1)
if len(bounds) == 3:
bounds = (-bounds, bounds)
elif len(bounds) == 6:
bounds = (bounds[:3], bounds[3:])
else:
raise ValueError("Expecting the given bounds to be of length 3 or 6 but got instead a length of: "
"{}".format(len(bounds)))
elif isinstance(bounds, (int, float)):
bounds = (-np.ones(3) * bounds, np.ones(3) * bounds)
else:
raise TypeError("Expecting the given bounds to be a tuple of np.array, a np.array, or None, but "
"instead got: {}".format(type(bounds)))
self._vel_bounds = bounds
###########
# Methods #
###########
def _update(self):
"""Update the inequality matrix and vectors."""
self._A_ineq = self.model.get_com_jacobian(full=False) # (3,N)
self._b_lower_bound, self._b_upper_bound = self._vel_bounds
pyrobolearn/priorities/constraints/velocity/convex_hull.py
+2 -2
View File
@@ -10,7 +10,7 @@ References:
import numpy as np
from pyrobolearn.priorities.constraints.constraint import Constraint
from pyrobolearn.priorities.constraints.constraint import UnilateralConstraint, JointVelocityConstraint
__author__ = "Brian Delhaisse"
@@ -23,7 +23,7 @@ __email__ = "briandelhaisse@gmail.com"
__status__ = "Development"
class ConvexHullConstraint(Constraint):
class ConvexHullConstraint(UnilateralConstraint, JointVelocityConstraint):
r"""Convex Hull constraint.
"""
pyrobolearn/priorities/tasks/README.rst
+9
View File
@@ -104,6 +104,15 @@ the identity matrix and :math:`b=0` is the zero/null vector).
Tasks include cartesian CoM tracking, cartesian end-effector position tracking, postural positioning, and others.
Tasks are separated into 4 folders (velocity, acceleration, torque, and cartesian force); one for each optimization
variable vector that is being optimized. Note that different type of tasks can be combined together; for instance,
we can combine acceleration tasks with force tasks. This will create an optimization variable vector
:math:`x = [\ddot{q}^\top, F^\top]^\top` which can then be used with the joint space dynamic equation
:math:`\tau = H \ddot{q} + C(q,\dot{q})\dot{q} + g(q) - J^\top F` to get the equivalent joint torques to be applied
on the robot.
References:
.. [1] `Quadratic Programming in Python <https://scaron.info/blog/quadratic-programming-in-python.html>`_, Caron, 2017
.. [2] "OpenSoT: A whole-body control library for the compliant humanoid robot COMAN", Rocchi et al., 2015
pyrobolearn/priorities/tasks/force/wrench.py
+2
View File
@@ -26,6 +26,8 @@ __status__ = "Development"
class WrenchTask(Task):
r"""Wrench Task
The wrench task
"""
def __init__(self, model, constraints=[]):
pyrobolearn/priorities/tasks/task.py
+37 -12
View File
@@ -111,6 +111,7 @@ References:
import numpy as np
import copy
import pyrobolearn as prl
from pyrobolearn.priorities.models import ModelInterface
from pyrobolearn.priorities.constraints.constraint import Constraint, NullConstraint
@@ -125,9 +126,6 @@ __email__ = "briandelhaisse@gmail.com"
__status__ = "Development"
# TODO: take into account constraints
# TODO: take into account hard priority tasks
class Task(object):
r"""Task (abstract) class.
@@ -163,6 +161,9 @@ class Task(object):
self.weight = weight
self.constraints = constraints
# if the task is enabled or not. A disabled task is the same as setting the weight to 0.
self._enabled = True
# check that the task is a valid task or a stack o tasks
if self.model is None and not self.is_stack_of_tasks():
raise RuntimeError("Expecting the task to be a valid task or a stack of tasks. You can not instantiate "
@@ -205,8 +206,12 @@ class Task(object):
# go through each soft task in the hard task
for j, soft_task in enumerate(hard_task):
if not isinstance(soft_task, Task):
raise TypeError("The given task positioned at ({}, {}) is not an instance of `Task`, but: "
"{}".format(i, j, type(soft_task)))
if isinstance(soft_task, Constraint):
# if constraint (must be an equality constraint), try to convert it into a task
tasks[i][j] = prl.priorities.tasks.TaskFromConstraint(soft_task)
else:
raise TypeError("The given task positioned at ({}, {}) is not an instance of `Task`, "
"but: {}".format(i, j, type(soft_task)))
else: # if not, check that the hard task is an instance of Task
if not isinstance(hard_task, Task):
raise TypeError("Expecting the {}th hard task to be an instance of `Task` or a list of `Task`, "
@@ -458,9 +463,6 @@ class Task(object):
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)
@@ -655,6 +657,11 @@ class Task(object):
# return results[0]
return results
@property
def enabled(self):
"""Return if the task is enabled or not."""
return self._enabled
##################
# Static Methods #
##################
@@ -699,6 +706,19 @@ class Task(object):
"""Return True if the current task is a single task."""
return self.model is not None and not self.is_stack_of_tasks()
def enable(self, enable=True):
"""Enable the single task, or each task if stack of tasks."""
if self.is_stack_of_tasks():
for hard_task in self.tasks:
for soft_task in hard_task:
soft_task.enable(enable=enable)
else:
self._enabled = enable
def disable(self, disable=True):
"""Disable the single task, or each task if stack of tasks."""
self.enable(not disable)
def add_hard_task(self, task):
"""Add the given hard task.
@@ -714,11 +734,14 @@ class Task(object):
constraints = task.constraints
if task.is_stack_of_tasks():
task = task.tasks
tasks = task.tasks
else:
task = [[task]]
if isinstance(task, Constraint):
# if given task is a constraint (must be an equality constraint), try to convert it into a task
task = prl.priorities.tasks.TaskFromConstraint(task)
tasks = [[task]]
for hard_task, constraint in zip(task, constraints):
for hard_task, constraint in zip(tasks, constraints):
self.tasks.append(hard_task)
self.constraints.append(constraint)
@@ -747,6 +770,9 @@ class Task(object):
for soft_task in hard_task:
self.tasks[-1].append(soft_task)
else: # task is single task
if isinstance(task, Constraint):
# if given task is a constraint (must be an equality constraint), try to convert it into a task
task = prl.priorities.tasks.TaskFromConstraint(task)
self.tasks[-1].append(task)
self._constraints[-1] = self._constraints[-1] + task.constraint
@@ -1104,7 +1130,6 @@ class JointTorqueTask(Task):
# Tests
if __name__ == '__main__':
import pyrobolearn as prl
sim = prl.simulators.Bullet(render=False)
robot = prl.robots.KukaIIWA(sim)
pyrobolearn/robots/legged_robot.py
+2 -2
View File
@@ -253,8 +253,8 @@ class LeggedRobot(Robot):
.. math::
d_x^{-} \leq \frac{n^i_y}{f^i_z} \leq d_x^{+} \\
d_y^{-} \leq -\frac{n^i_x}{f^i_z} \leq d_y^{+}
d_x^{-} \leq -\frac{n^i_y}{f^i_z} \leq d_x^{+} \\
d_y^{-} \leq \frac{n^i_x}{f^i_z} \leq d_y^{+}
which ensures the stability of the foot/ground contact. The :math:`(d_x^{-}, d_x^{+})` and
:math:`(d_y^{-}, d_y^{+})` defines the size of the sole in the x and y directions respectively. Basically,
pyrobolearn/robots/robot.py
+5 -2
View File
@@ -4281,9 +4281,12 @@ class Robot(ControllableBody):
# compute centroidal momentum matrix and the dot product between the derivative of this centroidal momentum
# matrix with the generalized velocities vector
A_G = X_iG_T.dot(Psi_1.T).dot(U_1).dot(H) # shape = (6,n+6)
A_Gd_dq = X_iG_T.dot(Psi_1.T).dot(U_1).dot(C_dq) # shape = (6,)
dA_G_dq = X_iG_T.dot(Psi_1.T).dot(U_1).dot(C_dq) # shape = (6,)
return A_G, A_Gd_dq
return A_G, dA_G_dq
def get_centroidal_momentum_matrix(self, q=None, dq=None, inertia=None):
return self.get_centroidal_dynamics(q=q, dq=dq, inertia=inertia)[0]
def get_centroidal_momentum(self, q=None, dq=None, inertia=None):
r"""
pyrobolearn/utils/transformation.py
+14
View File
@@ -10,6 +10,7 @@ References:
"""
import numpy as np
from scipy.linalg import block_diag
import quaternion
# from pyquaternion import Quaternion # TODO: check API at http://kieranwynn.github.io/pyquaternion
import sympy
@@ -45,6 +46,19 @@ def min_angle_difference(q1, q2):
return diff
def get_adjoint_from_rotation(rotation_matrix):
r"""
Get the adjoint matrix from a rotation matrix.
Args:
rotation_matrix (np.array[float[3,3]]): rotation matrix.
Returns:
np.array[float[6,6]]: adjoint matrix.
"""
return block_diag(rotation_matrix, rotation_matrix)
def get_homogeneous_transform(position, orientation):
r"""
Return the Homogeneous transform matrix given the position vector and the orientation.