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)