Exemple #1
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)
    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)
Exemple #4
0
    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)