Ejemplo n.º 1
0
    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")
Ejemplo n.º 2
0
    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)
Ejemplo n.º 3
0
    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")
Ejemplo n.º 4
0
    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)