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_zmp_support_area(self, com, plane, method='bretl'):
"""
Compute the (pendular) ZMP support area for a given COM position.
INPUT:
- ``com`` -- COM position
- ``plane`` -- origin (in world frame) of the virtual plane
- ``method`` -- (optional) choice between 'bretl' or 'cdd'
OUTPUT:
List of vertices of the ZMP support area.
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
"""
z_com, z_zmp = com[2], plane[2]
crossmat_n = array([[0, -1, 0], [1, 0, 0], [0, 0, 0]]) # n = [0, 0, 1]
G = self.compute_grasp_matrix([0, 0, 0])
F = self.compute_stacked_wrench_faces()
mass = 42. # [kg]
# mass has no effect on the output polygon, c.f. Section IV.C in
# <https://hal.archives-ouvertes.fr/hal-01349880>
pp = PolytopeProjector()
pp.set_inequality(
F,
zeros(F.shape[0]))
B = vstack([
hstack([z_com * eye(3), crossmat_n]),
hstack([zeros(3), com])]) # \sim hstack([-(cross(n, p_in)), n])])
pp.set_equality(
1. / (mass * 9.81) * dot(B, G),
hstack([com, [0]]))
pp.set_output(
(z_zmp - z_com) / (mass * 9.81) * G[:2, :],
array([com[0], com[1]]))
return pp.project(method)
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)
def compute_zmp_support_area(self, com, plane, method='bretl'):
"""
Compute the (pendular) ZMP support area for a given COM position.
Parameters
----------
com : array, shape=(3,)
COM position.
plane : array, shape=(3,)
Origin of the virtual plane.
method : string, default='bretl'
Choice between ``"bretl"`` or ``"cdd"``.
Returns
-------
vertices : list of arrays
List of vertices of the ZMP support area.
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.
"""
z_com, z_zmp = com[2], plane[2]
crossmat_n = array([[0, -1, 0], [1, 0, 0], [0, 0, 0]]) # n = [0, 0, 1]
G = self.compute_grasp_matrix([0, 0, 0])
F = block_diag(*[contact.wrench_face for contact in self.contacts])
mass = 42. # [kg]
# mass has no effect on the output polygon, c.f. Section IV.C in
# <https://hal.archives-ouvertes.fr/hal-01349880>
pp = PolytopeProjector()
pp.set_inequality(F, zeros(F.shape[0]))
B = vstack(
[hstack([z_com * eye(3), crossmat_n]),
hstack([zeros(3), com])]) # \sim hstack([-(cross(n, p_in)), n])])
pp.set_equality(1. / (mass * 9.81) * dot(B, G), hstack([com, [0]]))
pp.set_output((z_zmp - z_com) / (mass * 9.81) * G[:2, :],
array([com[0], com[1]]))
return pp.project(method)