def compute_pendular_accel_cone(self, com, zdd_max=None, reduced=False):
"""
Compute the (pendular) COM acceleration cone for a given COM position.
This pendular cone is the reduction of the Contact Wrench Cone when the
angular momentum at the COM is zero.
INPUT:
- ``com`` -- COM position, or list of COM vertices
- ``zdd_max`` -- (optional) maximum vertical acceleration in output cone
- ``reduced`` -- (optional) if True, will return the 2D reduced form
OUTPUT:
List of 3D vertices of the (truncated) COM acceleration cone, or of the
2D vertices of the reduced form if ``reduced`` is ``True``.
When ``com`` is a list of vertices, the returned cone corresponds to COM
accelerations that are feasible from *all* COM located inside the
polytope. See [CK16] for details on this conservative criterion.
ALGORITHM:
The method is based on a rewriting of the cone formula, followed by a 2D
convex hull on dual vertices. The algorithm is described in [CK16].
REFERENCES:
.. [CK16] https://hal.archives-ouvertes.fr/hal-01349880
"""
com_vertices = [com] if type(com) is not list else com
CWC_O = self.compute_wrench_face([0., 0., 0.])
B_list, c_list = [], []
for (i, v) in enumerate(com_vertices):
B = CWC_O[:, :3] + cross(CWC_O[:, 3:], v)
c = dot(B, gravity)
B_list.append(B)
c_list.append(c)
B = vstack(B_list)
c = hstack(c_list)
try:
g = -gravity[2] # gravity constant (positive)
check = c / B[:, 2]
assert max(check) - min(check) < 1e-10, \
"max - min failed (%.1e)" % ((max(check) - min(check)))
assert abs(check[0] - (-g)) < 1e-10, "check is not -g?"
B_2d = hstack([B[:, j].reshape((B.shape[0], 1)) for j in [0, 1]])
sigma = c / g # see Equation (30) in [CK16]
reduced_hull = compute_polygon_hull(B_2d, sigma)
if reduced:
return reduced_hull
return self._expand_reduced_pendular_cone(reduced_hull, zdd_max)
except QhullError:
raise Exception("Cannot compute 2D polar for acceleration cone")
def compute_static_equilibrium_polygon(self, method='hull'):
"""
Compute the static-equilibrium polygon of the center of mass.
Parameters
----------
method : string, optional
choice between 'bretl', 'cdd' or 'hull'
Returns
-------
vertices : list of arrays
2D vertices of the static-equilibrium polygon.
Notes
-----
The method 'bretl' is adapted from in [BL08]_ where the
static-equilibrium polygon was introduced. The method 'cdd' corresponds
to the double-description approach described in [CPN16]_. See the
Appendix from [CK16]_ for a performance comparison.
References
----------
.. [BL08] T. Bretl and S. Lall, "Testing Static Equilibrium for Legged
Robots," IEEE Transactions on Robotics, vol. 24, no. 4, pp. 794-807,
August 2008.
.. [CPN16] Stéphane Caron, Quang-Cuong Pham and Yoshihiko Nakamura, "ZMP
support areas for multi-contact mobility under frictional
constraints," IEEE Transactions on Robotics, Dec. 2016.
`[pdf] <https://scaron.info/papers/journal/caron-tro-2016.pdf>`__
.. [CK16] Stéphane Caron and Abderrahmane Kheddar, "Multi-contact
Walking Pattern Generation based on Model Preview Control of 3D COM
Accelerations," 2016 IEEE-RAS 16th International Conference on
Humanoid Robots (Humanoids), Cancun, Mexico, 2016, pp. 550-557.
`[pdf] <https://hal.archives-ouvertes.fr/hal-01349880>`__
"""
if method == 'hull':
A_O = self.compute_wrench_face([0, 0, 0])
k, a_Oz, a_x, a_y = A_O.shape[0], A_O[:, 2], A_O[:, 3], A_O[:, 4]
B, c = hstack([-a_y.reshape((k, 1)), +a_x.reshape((k, 1))]), -a_Oz
return compute_polygon_hull(B, c)
p = [0, 0, 0] # point where contact wrench is taken at
G = self.compute_grasp_matrix(p)
F = block_diag(*[contact.wrench_face for contact in self.contacts])
mass = 42. # [kg]
# mass has no effect on the output polygon, see Section IV.B in [CK16]_
pp = PolytopeProjector()
pp.set_inequality(F, zeros(F.shape[0]))
pp.set_equality(G[(0, 1, 2, 5), :], array([0, 0, mass * 9.81, 0]))
pp.set_output(1. / (mass * 9.81) * vstack([-G[4, :], +G[3, :]]),
array([p[0], p[1]]))
return pp.project(method)
def compute_pendular_accel_cone(self, com, zdd_max=None, reduced=False):
"""
Compute the pendular COM acceleration cone for a given COM position.
The pendular cone is the reduction of the Contact Wrench Cone when the
angular momentum at the COM is zero.
Parameters
----------
com : array, shape=(3,), or list of such arrays
COM position, or list of COM vertices.
zdd_max : scalar, optional
Maximum vertical acceleration in the output cone.
reduced : bool, optional
If ``True``, returns the reduced 2D form rather than a 3D cone.
Returns
-------
vertices : list of (3,) arrays
List of 3D vertices of the (truncated) COM acceleration cone, or of
the 2D vertices of the reduced form if ``reduced`` is ``True``.
Notes
-----
The method is based on a rewriting of the CWC formula, followed by a 2D
convex hull on dual vertices. The algorithm is described in [CK16]_.
When ``com`` is a list of vertices, the returned cone corresponds to COM
accelerations that are feasible from *all* COM located inside the
polytope. See [CK16]_ for details on this conservative criterion.
"""
com_vertices = [com] if type(com) is not list else com
CWC_O = self.compute_wrench_face([0., 0., 0.])
B_list, c_list = [], []
for (i, v) in enumerate(com_vertices):
B = CWC_O[:, :3] + cross(CWC_O[:, 3:], v)
c = dot(B, gravity)
B_list.append(B)
c_list.append(c)
B = vstack(B_list)
c = hstack(c_list)
try:
g = -gravity[2] # gravity constant (positive)
B_2d = hstack([B[:, j].reshape((B.shape[0], 1)) for j in [0, 1]])
sigma = c / g # see Equation (30) in [CK16]
reduced_hull = compute_polygon_hull(B_2d, sigma)
if reduced:
return reduced_hull
return self._expand_reduced_pendular_cone(reduced_hull, zdd_max)
except QhullError:
raise Exception("Cannot compute 2D polar for acceleration cone")
def compute_static_equilibrium_polygon(self, method='hull'):
"""
Compute the static-equilibrium polygon of the center of mass.
INPUT:
- ``method`` -- (optional) choice between 'bretl', 'cdd' or 'hull'
OUTPUT:
List of 2D vertices of the static-equilibrium polygon.
ALGORITHM:
The method 'bretl' is adapted from in [BL08] where the
static-equilibrium polygon was introduced. The method 'cdd' corresponds
to the double-description approach described in [CPN16]. See the
Appendix from [CK16] for a performance comparison.
REFERENCES:
.. [BL08] https://dx.doi.org/10.1109/TRO.2008.2001360
.. [CPN16] https://dx.doi.org/10.1109/TRO.2016.2623338
.. [CK16] https://hal.archives-ouvertes.fr/hal-01349880
"""
if method == 'hull':
A_O = self.compute_wrench_face([0, 0, 0])
k, a_Oz, a_x, a_y = A_O.shape[0], A_O[:, 2], A_O[:, 3], A_O[:, 4]
B, c = hstack([-a_y.reshape((k, 1)), +a_x.reshape((k, 1))]), -a_Oz
return compute_polygon_hull(B, c)
p = [0, 0, 0] # point where contact wrench is taken at
G = self.compute_grasp_matrix(p)
F = self.compute_stacked_wrench_faces()
mass = 42. # [kg]
# mass has no effect on the output polygon, see Section IV.B in
# <https://hal.archives-ouvertes.fr/hal-01349880> for details
pp = PolytopeProjector()
pp.set_inequality(
F,
zeros(F.shape[0]))
pp.set_equality(
G[(0, 1, 2, 5), :],
array([0, 0, mass * 9.81, 0]))
pp.set_output(
1. / (mass * 9.81) * vstack([-G[4, :], +G[3, :]]),
array([p[0], p[1]]))
return pp.project(method)