pymanoid/misc.py
def normalize(v):
    """
    Normalize a vector.

    Parameters
    ----------
    v : array
        Any vector.

    Returns
    -------
    nv : array
        Unit vector directing `v`.

    Notes
    -----
    This method doesn't catch ``ZeroDivisionError`` exceptions on purpose.
    """
    return v / norm(v)
pymanoid/contact.py
class Contact(Box):

    """
    Rectangular contact surface.

    Parameters
    ----------
    shape : (scalar, scalar)
        Surface dimensions (half-length, half-width) in [m].
    pos : ndarray
        Contact position in world frame.
    rpy : ndarray
        Contact orientation in world frame.
    pose : ndarray
        Initial pose. Supersedes ``pos`` and ``rpy`` if they are provided at
        the same time.
    friction : scalar
        Static friction coefficient.
    link : body.Manipulator, optional
        Robot link frame in contact.
    slab_thickness : scalar, optional
        Thickness of the contact slab displayed in the GUI, in [m].
    max_pressure : scalar, optional
        Maximum pressure on contact.

    Attributes
    ----------
    wrench : array, shape=(6,)
        Contact wrench coordinates at the contact point in the inertial frame.
    """

    def __init__(self, shape, pos=None, rpy=None, pose=None, friction=None,
                 link=None, slab_thickness=0.01):
        X, Y = shape
        super(Contact, self).__init__(
            X, Y, Z=slab_thickness, pos=pos, rpy=rpy, pose=pose, color='r',
            dZ=-slab_thickness)
        self.friction = friction  # isotropic Coulomb friction
        self.is_managed = False
        self.link = link
        self.max_pressure = None
        self.shape = shape
        self.wrench = None

    def copy(self, link=None, hide=False):
        """
        Return a copy of the contact.

        Parameters
        ----------
        link : body.Manipulator, optional
            Robot link frame in contact in the copy.
        hide : bool, optional
            Hide copy?
        """
        if link is None:
            link = self.link
        contact_copy = Contact(
            self.shape, pose=self.pose, friction=self.friction, link=link)
        contact_copy.max_pressure = self.max_pressure
        contact_copy.wrench = self.wrench
        if hide:
            contact_copy.hide()
        return contact_copy

    @property
    def dict_repr(self):
        return {
            'friction': self.friction,
            'pos': list(self.p),
            'rpy': list(self.rpy),
            'shape': list(self.shape),
        }

    @property
    def force(self):
        """
        Resultant of contact forces in the world frame (if defined).
        """
        if self.wrench is None:
            return None
        return self.wrench[0:3]

    @property
    def moment(self):
        """
        Moment of contact forces in the world frame (if defined).
        """
        if self.wrench is None:
            return None
        return self.wrench[3:6]

    @property
    def force_inequalities(self):
        """
        Matrix of force friction cone inequalities in the world frame.

        Notes
        -----
        All linearized friction cones in pymanoid use the inner (conservative)
        approximation. See for instance this `introduction to friction cones
        <https://scaron.info/teaching/friction-cones.html>`_ for details.
        """
        mu = self.friction / sqrt(2)
        hrep_local = array([
            [-1, 0, -mu],
            [+1, 0, -mu],
            [0, -1, -mu],
            [0, +1, -mu]])
        return dot(hrep_local, self.R.T)

    @property
    def force_rays(self):
        """
        Rays of the force friction cone in the world frame.

        Notes
        -----
        All linearized friction cones in pymanoid use the inner (conservative)
        approximation. See for instance this `introduction to friction cones
        <https://scaron.info/teaching/friction-cones.html>`_ for details.
        """
        mu = self.friction / sqrt(2)
        f1 = dot(self.R, [+mu, +mu, +1])
        f2 = dot(self.R, [+mu, -mu, +1])
        f3 = dot(self.R, [-mu, +mu, +1])
        f4 = dot(self.R, [-mu, -mu, +1])
        return [f1, f2, f3, f4]

    @property
    def force_span(self):
        """
        Span matrix of the force friction cone in world frame.

        This matrix `S` is such that all valid contact forces can be written:

        .. math::

            f = S \\lambda, \\quad \\lambda \\geq 0

        Notes
        -----
        All linearized friction cones in pymanoid use the inner (conservative)
        approximation. See for instance this `introduction to friction cones
        <https://scaron.info/teaching/friction-cones.html>`_ for details.
        """
        return array(self.force_rays).T

    def compute_grasp_matrix(self, p):
        """
        Compute the grasp matrix for a given destination point.

        The grasp matrix :math:`G_P` converts the local contact wrench `w` to
        the contact wrench :math:`w_P` at another point `P`:

        .. math::

            w_P = G_P w

        All wrench coordinates being taken in the world frame.

        Parameters
        ----------
        p : array, shape=(3,)
            Point, in world frame coordinates, where the wrench is taken.

        Returns
        -------
        G : ndarray
            Grasp matrix :math:`G_P`.
        """
        x, y, z = self.p - p
        return array([
            # fx fy  fz taux tauy tauz
            [1,   0,  0,   0,   0,   0],
            [0,   1,  0,   0,   0,   0],
            [0,   0,  1,   0,   0,   0],
            [0,  -z,  y,   1,   0,   0],
            [z,   0, -x,   0,   1,   0],
            [-y,  x,  0,   0,   0,   1]])

    @property
    def vertices(self):
        """
        Vertices of the contact area.
        """
        return self.get_scaled_contact_area(1.)

    def get_scaled_contact_area(self, scale):
        """
        Get the vertices of the scaled contact area.

        Parameters
        ----------
        scale : scalar
            Contact area is scaled by this ratio.

        Returns
        -------
        vertices : list of arrays
            List of vertex coordinates in the world frame.
        """
        X = scale * self.shape[0]
        Y = scale * self.shape[1]
        v1 = dot(self.T, array([+X, +Y, 0., 1.]))[:3]
        v2 = dot(self.T, array([+X, -Y, 0., 1.]))[:3]
        v3 = dot(self.T, array([-X, -Y, 0., 1.]))[:3]
        v4 = dot(self.T, array([-X, +Y, 0., 1.]))[:3]
        return [v1, v2, v3, v4]

    def set_wrench(self, wrench):
        """
        Set contact wrench directly.

        Parameters
        ----------
        wrench : array, shape=(6,)
            Wrench coordinates given in the contact frame.

        Notes
        -----
        This function switches the contact to "managed" mode, as opposed to the
        default "supporting" mode where the wrench distributor finds contact
        wrenches by numerical optimization.
        """
        if not type(wrench) is ndarray:
            wrench = array(wrench)
        if not self.is_managed:
            self.set_color('b')
        self.is_managed = True
        self.wrench = dot(block_diag(self.R, self.R), wrench)

    def unset_wrench(self):
        """
        Return contact to supporting mode.
        """
        if self.is_managed:
            self.set_color('r')
        self.is_managed = False
        self.wrench = None

    def wrench_at(self, point):
        """
        Get contact wrench at a given point in the world frame.

        Parameters
        ----------
        point : array, shape=(3,)
            Point `P` where the wrench is expressed.

        Returns
        -------
        wrench : array, shape=(6,)
            Contact wrench :math:`w_P` at `P` in the world frame.
        """
        if self.wrench is None:
            return None
        X_world_point = vstack([
            hstack([eye(3), eye(3)]),
            hstack([crossmat(point), eye(3)])])
        return dot(X_world_point, self.wrench)

    @property
    def wrench_inequalities(self):
        """
        Matrix `F` of friction inequalities in world frame.

        This matrix describes the linearized Coulomb friction model (in the
        fixed contact mode) by:

        .. math::

            F w \\leq 0

        where `w` is the contact wrench at the contact point (``self.p``) in
        the world frame. See [Caron15]_ for the derivation of the formula for
        `F`.
        """
        X, Y = self.shape
        mu = self.friction / sqrt(2)  # inner approximation
        local_cone = array([
            # fx fy             fz taux tauy tauz
            [-1,  0,           -mu,   0,   0,   0],
            [+1,  0,           -mu,   0,   0,   0],
            [0,  -1,           -mu,   0,   0,   0],
            [0,  +1,           -mu,   0,   0,   0],
            [0,   0,            -Y,  -1,   0,   0],
            [0,   0,            -Y,  +1,   0,   0],
            [0,   0,            -X,   0,  -1,   0],
            [0,   0,            -X,   0,  +1,   0],
            [-Y, -X, -(X + Y) * mu, +mu, +mu,  -1],
            [-Y, +X, -(X + Y) * mu, +mu, -mu,  -1],
            [+Y, -X, -(X + Y) * mu, -mu, +mu,  -1],
            [+Y, +X, -(X + Y) * mu, -mu, -mu,  -1],
            [+Y, +X, -(X + Y) * mu, +mu, +mu,  +1],
            [+Y, -X, -(X + Y) * mu, +mu, -mu,  +1],
            [-Y, +X, -(X + Y) * mu, -mu, +mu,  +1],
            [-Y, -X, -(X + Y) * mu, -mu, -mu,  +1]])
        return dot(local_cone, block_diag(self.R.T, self.R.T))

    @property
    def wrench_hrep(self):
        """
        H-representation of friction inequalities (and optional pressure
        limits) in world frame.

        This matrix-vector pair describes the linearized Coulomb friction model
        (in the fixed contact mode) and pressure limits by:

        .. math::

            F w \\leq g

        where `w` is the contact wrench at the contact point (``self.p``) in
        the world frame. See [Caron15]_ for the derivation of the formula for
        `F`.
        """
        if self.max_pressure is None:
            F = self.wrench_inequalities
            b = zeros(F.shape[0])
        else:  # self.max_pressure is not None
            pressure_select = hstack([self.n, zeros(3)])
            F = vstack([self.wrench_inequalities, pressure_select])
            b = zeros(F.shape[0])
            b[-1] = self.max_pressure
        return (F, b)

    @property
    def wrench_rays(self):
        """
        Rays (V-rep) of the contact wrench cone in world frame.
        """
        return [
            hstack([f, cross(v - self.p, f)])
            for v in self.vertices
            for f in self.force_rays]

    @property
    def wrench_span(self):
        """
        Span matrix of the contact wrench cone in world frame.

        This matrix is such that all valid contact wrenches can be written as:

        .. math::

            w_P = S \\lambda, \\quad \\lambda \\geq 0

        where `S` is the friction span and :math:`\\lambda` is a vector with
        positive coordinates.

        Returns
        -------
        S : array, shape=(6, 16)
            Span matrix of the contact wrench cone.

        Notes
        -----
        Note that the contact wrench coordinates :math:`w_P` ("output" of `S`)
        are taken at the contact point `P` (``self.p``) and in the world frame.
        Meanwhile, the number of columns of `S` results from our choice of 4
        contact points (one for each vertex of the rectangular area) with
        4-sided friction pyramids at each.
        """
        return hstack([
            dot(vstack([eye(3), crossmat(v - self.p)]), self.force_span)
            for v in self.vertices])
pymanoid/body.py
class Box(Body):

    """
    Rectangular box.

    Parameters
    ----------
    X : scalar
        Box half-length in [m].
    Y : scalar
        Box half-width in [m].
    Z : scalar
        Box half-height in [m].
    pos : array, shape=(3,)
        Initial position in the world frame.
    rpy : array, shape=(3,)
        Initial orientation in the world frame.
    pose : array, shape=(7,)
        Initial pose in the world frame.
    color : char
        Color letter in ['r', 'g', 'b'].
    visible : bool, optional
        Visibility in the GUI.
    dZ : scalar, optional
        Shift in box normal coordinates used to make Contact slabs.
    """

    def __init__(self, X, Y, Z, pos=None, rpy=None, pose=None, color='r',
                 visible=True, dZ=0.):
        aabb = [0., 0., dZ, X, Y, Z]
        env = get_openrave_env()
        with env:
            box = openravepy.RaveCreateKinBody(env, '')
            box.InitFromBoxes(array([array(aabb)]), True)
            super(Box, self).__init__(
                box, pos=pos, rpy=rpy, pose=pose, color=color, visible=visible)
            env.Add(box, True)
pymanoid/gui.py
def draw_line(start_point, end_point, color='g', linewidth=1.):
    """
    Draw a line between two points.

    Parameters
    ----------
    start_point : array, shape=(3,)
        One end of the line, in world frame coordinates.
    end_point : array, shape=(3,)
        Other end of the line, in world frame coordinates.
    color : char or triplet, optional
        Color letter or RGB values, default is 'g' for green.
    linewidth : scalar
        Thickness of drawn line.

    Returns
    -------
    handle : openravepy.GraphHandle
        OpenRAVE graphical handle. Must be stored in some variable, otherwise
        the drawn object will vanish instantly.
    """
    if type(color) is str:
        color = matplotlib_to_rgb(color)
    return get_openrave_env().drawlinelist(
        array([start_point, end_point]), linewidth=linewidth, colors=color)
pymanoid/tasks.py
class DOFTask(Task):

    """
    Track a reference DOF value.

    Parameters
    ----------
    robot : Robot
        Target robot.
    index : string or integer
        DOF index or string of DOF identifier.
    target : scalar
        Target DOF value.
    weight : scalar, optional
        Task weight used in IK cost function. If None, needs to be set later.
    gain : scalar, optional
        Proportional gain of the task.
    exclude_dofs : list of integers, optional
        DOF indices not used by task.
    """

    def __init__(self, robot, index, target, weight=None, gain=None,
                 exclude_dofs=None):
        super(DOFTask, self).__init__(weight, gain, exclude_dofs)
        if type(index) is str:
            index = robot.__getattribute__(index)
        J = zeros((1, robot.nb_dofs))
        J[0, index] = 1.
        self.__J = J
        self.index = index
        self.name = robot.get_dof_name_from_index(index)
        self.robot = robot
        self.target = target

    def _jacobian(self):
        return self.__J

    def _residual(self, dt):
        return array([self.target - self.robot.q[self.index]]) / dt

    def update_target(self, target):
        self.target = target
pymanoid/gui.py
def draw_point(point, color='g', pointsize=0.01):
    """
    Draw a point.

    Parameters
    ----------
    point : array, shape=(3,)
        Point coordinates in the world frame.
    pointsize : scalar, optional
        Radius of the drawn sphere in [m].

    Returns
    -------
    handle : openravepy.GraphHandle
        OpenRAVE graphical handle. Must be stored in some variable, otherwise
        the drawn object will vanish instantly.
    """
    return draw_points([point], color, pointsize)
pymanoid/mpc.py
class LinearPredictiveControl(object):

    """
    Predictive control for a system with linear dynamics and linear
    constraints.

    The discretized dynamics of a linear system are described by:

    .. math::

        x_{k+1} = A x_k + B u_k

    where :math:`x` is assumed to be the first-order state of a configuration
    variable :math:`p`, i.e., it stacks both the position :math:`p` and its
    time-derivative :math:`\\dot{p}`. Meanwhile, the system is linearly
    constrained by:

    .. math::

        x_0 & = x_\\mathrm{init} \\\\
        \\forall k, \\ C_k x_k + D_k u_k & \\leq e_k \\\\

    The output control law minimizes a weighted combination of two types of
    costs:

    - Terminal state error
        :math:`\\|x_\\mathrm{nb\\_steps} - x_\\mathrm{goal}\\|^2`
        with weight :math:`w_{xt}`.
    - Cumulated state error:
        :math:`\\sum_k \\|x_k - x_\\mathrm{goal}\\|^2`
        with weight :math:`w_{xc}`.
    - Cumulated control costs:
        :math:`\\sum_k \\|u_k\\|^2`
        with weight :math:`w_{u}`.

    Parameters
    ----------
    A : array, shape=(n, n)
        State linear dynamics matrix.
    B : array, shape=(n, dim(u))
        Control linear dynamics matrix.
    x_init : array, shape=(n,)
        Initial state as stacked position and velocity.
    x_goal : array, shape=(n,)
        Goal state as stacked position and velocity.
    nb_steps : int
        Number of discretization steps in the preview window.
    C : array, shape=(m, dim(u)), list of arrays, or None
        Constraint matrix on state variables. When this argument is an array,
        the same matrix `C` is applied at each step `k`. When it is ``None``,
        the null matrix is applied.
    D : array, shape=(l, n), or list of arrays, or None
        Constraint matrix on control variables. When this argument is an array,
        the same matrix `D` is applied at each step `k`. When it is ``None``,
        the null matrix is applied.
    e : array, shape=(m,), list of arrays
        Constraint vector. When this argument is an array, the same vector `e`
        is applied at each step `k`.
    wxt : scalar, optional
        Weight on terminal state cost, or ``None`` to disable.
    wxc : scalar, optional
        Weight on cumulated state costs, or ``None`` to disable (default).
    wu : scalar, optional
        Weight on cumulated control costs.

    Notes
    -----
    In numerical analysis, there are three classes of methods to solve boundary
    value problems: single shooting, multiple shooting and collocation. The
    solver implemented in this class follows the `single shooting method
    <https://en.wikipedia.org/wiki/Shooting_method>`_.
    """

    def __init__(self, A, B, C, D, e, x_init, x_goal, nb_steps, wxt=None,
                 wxc=None, wu=1e-3):
        assert C is not None or D is not None, "use LQR for unconstrained case"
        assert wu > 0., "non-negative control weight needed for regularization"
        assert wxt is not None or wxc is not None, "set either wxt or wxc"
        u_dim = B.shape[1]
        x_dim = A.shape[1]
        self.A = A
        self.B = B
        self.C = C
        self.D = D
        self.G = None
        self.P = None
        self.U = None
        self.U_dim = u_dim * nb_steps
        self.__X = None
        self.build_time = None
        self.e = e
        self.h = None
        self.nb_steps = nb_steps
        self.q = None
        self.solve_time = None
        self.u_dim = u_dim
        self.wu = wu
        self.wxc = wxc
        self.wxt = wxt
        self.x_dim = x_dim
        self.x_goal = x_goal
        self.x_init = x_init
        #
        self.__build()

    def __build(self):
        """
        Compute internal matrices defining the preview QP.

        Notes
        -----
        See [Audren14]_ for details on the matrices :math:`\\Phi` and
        :math:`\\Psi`, as we use similar notations below.
        """
        t_build_start = time()
        phi = eye(self.x_dim)
        psi = zeros((self.x_dim, self.U_dim))
        G_list, h_list = [], []
        phi_list, psi_list = [], []
        for k in range(self.nb_steps):
            # Loop invariant: x == psi * U + phi * x_init
            if self.wxc is not None:
                phi_list.append(phi)
                psi_list.append(psi)
            C = self.C[k] if type(self.C) is list else self.C
            D = self.D[k] if type(self.D) is list else self.D
            e = self.e[k] if type(self.e) is list else self.e
            G = zeros((e.shape[0], self.U_dim))
            h = e if C is None else e - dot(dot(C, phi), self.x_init)
            if D is not None:
                # we rely on G == 0 to avoid a slower +=
                G[:, k * self.u_dim:(k + 1) * self.u_dim] = D
            if C is not None:
                G += dot(C, psi)
            G_list.append(G)
            h_list.append(h)
            phi = dot(self.A, phi)
            psi = dot(self.A, psi)
            psi[:, self.u_dim * k:self.u_dim * (k + 1)] = self.B
        P = self.wu * eye(self.U_dim)
        q = zeros(self.U_dim)
        if self.wxt is not None and self.wxt > 1e-10:
            c = dot(phi, self.x_init) - self.x_goal
            P += self.wxt * dot(psi.T, psi)
            q += self.wxt * dot(c.T, psi)
        if self.wxc is not None and self.wxc > 1e-10:
            Phi = vstack(phi_list)
            Psi = vstack(psi_list)
            X_goal = hstack([self.x_goal] * self.nb_steps)
            c = dot(Phi, self.x_init) - X_goal
            P += self.wxc * dot(Psi.T, Psi)
            q += self.wxc * dot(c.T, Psi)
        self.P = P
        self.q = q
        self.G = vstack(G_list)
        self.h = hstack(h_list)
        self.build_time = time() - t_build_start

    def solve(self, **kwargs):
        """
        Compute the series of controls that minimizes the preview QP.

        Parameters
        ----------
        solver : string, optional
            Name of the QP solver in ``qpsolvers.available_solvers``.
        initvals : array, optional
            Vector of initial `U` values used to warm-start the QP solver.
        """
        t_solve_start = time()
        kwargs['sym_proj'] = False  # self.P is symmetric
        U = solve_qp(self.P, self.q, self.G, self.h, **kwargs)
        self.U = U.reshape((self.nb_steps, self.u_dim))
        self.solve_time = time() - t_solve_start

    @property
    def X(self):
        """
        Series of system states over the preview window.

        Note
        ----
        This property is only available after ``solve()`` has been called.
        """
        if self.__X is not None:
            return self.__X
        assert self.U is not None, "you need to solve() the MPC problem first"
        X = zeros((self.nb_steps + 1, self.x_dim))
        X[0] = self.x_init
        for k in range(self.nb_steps):
            X[k + 1] = dot(self.A, X[k]) + dot(self.B, self.U[k])
        self.__X = X
        return X

    @property
    def solve_and_build_time(self):
        """
        Total computation time taken by MPC computations.
        """
        return self.build_time + self.solve_time
pymanoid/tasks.py
class MinCAMTask(Task):

    """
    Minimize the centroidal angular momentum (not its derivative).

    Parameters
    ----------
    robot : Humanoid
        Target robot.
    weight : scalar, optional
        Task weight used in IK cost function. If None, needs to be set later.
    gain : scalar, optional
        Proportional gain of the task.
    exclude_dofs : list of integers, optional
        DOF indices not used by task.
    """

    def __init__(self, robot, weight=None, gain=None, exclude_dofs=None):
        super(MinCAMTask, self).__init__(weight, gain, exclude_dofs)
        self.__zero_cam = zeros((3,))
        self.name = 'MIN_CAM'
        self.robot = robot

    def _jacobian(self):
        return self.robot.compute_cam_jacobian()

    def _residual(self, dt):
        return self.__zero_cam
pymanoid/gui.py
def draw_points(points, color='g', pointsize=0.01):
    """
    Draw a list of points.

    Parameters
    ----------
    point : list of arrays
        List of point coordinates in the world frame.
    pointsize : scalar, optional
        Radius of the drawn sphere in [m].

    Returns
    -------
    handle : openravepy.GraphHandle
        OpenRAVE graphical handle. Must be stored in some variable, otherwise
        the drawn object will vanish instantly.
    """
    if type(color) is str:
        color = matplotlib_to_rgba(color, alpha=1.)
    return get_openrave_env().plot3(
        array(points), pointsize=pointsize, drawstyle=1,
        colors=color)
pymanoid/robots/jvrc1.py
class JVRC1(Humanoid):

    """
    JVRC-1 robot model.

    See <https://github.com/stephane-caron/jvrc_models/tree/openrave/JVRC-1>.
    """

    MODEL_URL = 'https://raw.githubusercontent.com/stephane-caron/' \
        'openrave_models/master/JVRC-1/JVRC-1.dae'

    sole_shape = (0.11, 0.05)  # (half-length [m], half-width [m]) of foot sole
    leg_length = 0.85         # [m] (for a stretched leg)
    mass = 62.                # [kg]

    # DOF indices
    R_HIP_P = 0
    R_HIP_R = 1
    R_HIP_Y = 2
    R_KNEE_P = 3
    R_ANKLE_R = 4
    R_ANKLE_P = 5
    L_HIP_P = 6
    L_HIP_R = 7
    L_HIP_Y = 8
    L_KNEE_P = 9
    L_ANKLE_R = 10
    L_ANKLE_P = 11
    CHEST_Y = 12  # compatibility with HRP-4
    WAIST_Y = 12
    CHEST_P = 13  # compatibility with HRP-4
    WAIST_P = 13
    WAIST_R = 14
    NECK_Y = 15
    NECK_R = 16
    NECK_P = 17
    R_SHOULDER_P = 18
    R_SHOULDER_R = 19
    R_SHOULDER_Y = 20
    R_ELBOW_P = 21
    R_ELBOW_Y = 22
    R_WRIST_R = 23
    R_WRIST_Y = 24
    R_UTHUMB = 25
    R_LTHUMB = 26
    R_UINDEX = 27
    R_LINDEX = 28
    R_ULITTLE = 29
    R_LLITTLE = 30
    L_SHOULDER_P = 31
    L_SHOULDER_R = 32
    L_SHOULDER_Y = 33
    L_ELBOW_P = 34
    L_ELBOW_Y = 35
    L_WRIST_R = 36
    L_WRIST_Y = 37
    L_UTHUMB = 38
    L_LTHUMB = 39
    L_UINDEX = 40
    L_LINDEX = 41
    L_ULITTLE = 42
    L_LLITTLE = 43
    TRANS_X = 44
    TRANS_Y = 45
    TRANS_Z = 46
    ROT_Y = 47
    ROT_P = 48
    ROT_R = 49

    # Joints
    chest = [WAIST_Y, WAIST_P, WAIST_R]
    free_pos = [TRANS_X, TRANS_Y, TRANS_Z]
    free_rpy = [ROT_R, ROT_P, ROT_Y]
    left_ankle = [L_ANKLE_R, L_ANKLE_P]
    left_elbow = [L_ELBOW_P, L_ELBOW_Y]
    left_hip = [L_HIP_P, L_HIP_R, L_HIP_Y]
    left_index = [L_UINDEX, L_LINDEX]
    left_knee = [L_KNEE_P]
    left_little = [L_ULITTLE, L_LLITTLE]
    left_shoulder = [L_SHOULDER_P, L_SHOULDER_R, L_SHOULDER_Y]
    left_thumb = [L_UTHUMB, L_LTHUMB]
    left_wrist = [L_WRIST_R, L_WRIST_Y]
    neck = [NECK_Y, NECK_R, NECK_P]
    right_ankle = [R_ANKLE_R, R_ANKLE_P]
    right_elbow = [R_ELBOW_P, R_ELBOW_Y]
    right_hip = [R_HIP_P, R_HIP_R, R_HIP_Y]
    right_index = [R_UINDEX, R_LINDEX]
    right_knee = [R_KNEE_P]
    right_little = [R_ULITTLE, R_LLITTLE]
    right_shoulder = [R_SHOULDER_P, R_SHOULDER_R, R_SHOULDER_Y]
    right_thumb = [R_UTHUMB, R_LTHUMB]
    right_wrist = [R_WRIST_R, R_WRIST_Y]

    # Limbs
    left_arm = left_shoulder + left_elbow + left_wrist
    left_hand = left_thumb + left_index + left_little
    left_leg = left_hip + left_knee + left_ankle
    right_arm = right_shoulder + right_elbow + right_wrist
    right_hand = right_thumb + right_index + right_little
    right_leg = right_hip + right_knee + right_ankle

    # Body
    arms = left_arm + right_arm
    free = free_pos + free_rpy
    legs = left_leg + right_leg
    whole_body = arms + legs + chest + free

    # Half-sitting posture
    q_halfsit = array([
        -0.38, -0.01, 0., 0.72, -0.01, -0.33, -0.38, 0.02, 0., 0.72, -0.02,
        -0.33, 0., 0.13, 0., 0., 0., 0., -0.052, -0.17, 0., -0.52, 0., 0., 0.,
        0., 0., 0., 0., 0., 0., -0.052, 0.17, 0., -0.52, 0., 0., 0., 0., 0.,
        0., 0., 0., 0., 0., 0., 0., 0., 0., 0.])

    # Compatibility
    chest_p_name = 'WAIST_P'
    chest_y_name = 'WAIST_Y'

    def __init__(self, path='JVRC-1.dae', root_body='PELVIS_S',
                 download_if_needed=True):
        """
        Add the JVRC-1 model to the environment.

        Parameters
        ----------
        path : string, optional
            Path to the COLLADA model of the robot.
        root_body : string, optional
            Name of the root body in the kinematic chain.
        download_if_needed : bool, optional
            If True and there is no model file in ``path``, will attempt to
            download it from ``JVRC1.MODEL_URL``.
        """
        if download_if_needed and not isfile(path):
            rc = system('wget %s -O %s' % (JVRC1.MODEL_URL, path))
            assert rc == 0, "Download of model file failed"
        super(JVRC1, self).__init__(path, root_body)
        self.ik.set_active_dofs(self.whole_body)
        self.left_foot = Manipulator(
            self.rave.GetManipulator("left_foot_base"), shape=self.sole_shape,
            friction=0.8)
        self.left_hand = Manipulator(
            self.rave.GetManipulator("left_hand_palm"), friction=0.8)
        self.mass = sum([link.GetMass() for link in self.rave.GetLinks()])
        self.right_foot = Manipulator(
            self.rave.GetManipulator("right_foot_base"), shape=self.sole_shape,
            friction=0.8)
        self.right_hand = Manipulator(
            self.rave.GetManipulator("right_hand_palm"), friction=0.8)
pymanoid/gui.py
class TrajectoryDrawer(Process):

    """
    Draw the trajectory of a rigid body.

    Parameters
    ----------
    body : Body
        Rigid body whose trajectory to draw.
    combined : string, optional
        Drawing spec of the trajectory in matplotlib fashion.
    color : char or RGBA tuple, optional
        Drawing color.
    linewidth : scalar, optional
        Thickness of drawn lines.
    linestyle : char, optional
        Choix between '-' for continuous and '.' for dotted.
    buffer_size : int, optional
        Number of trajectory segments to display. Old segments will be replaced
        by new ones.
    """

    def __init__(self, body, combined='b-', color=None, linewidth=3,
                 linestyle=None, buffer_size=1000):
        super(TrajectoryDrawer, self).__init__()
        color = color if color is not None else combined[0]
        linestyle = linestyle if linestyle is not None else combined[1]
        assert linestyle in ['-', '.']
        self.body = body
        self.buffer_size = buffer_size
        self.color = color
        self.handles = [None] * buffer_size
        self.next_index = 0
        self.last_pos = body.p
        self.linestyle = linestyle
        self.linewidth = linewidth

    def on_tick(self, sim):
        if self.linestyle == '-':
            h = self.handles[self.next_index]
            if h is not None:
                h.Close()
            self.handles[self.next_index] = draw_line(
                self.last_pos, self.body.p, color=self.color,
                linewidth=self.linewidth)
            self.next_index = (self.next_index + 1) % self.buffer_size
        self.last_pos = self.body.p

    def dash_graph_handles(self):
        for i in range(len(self.handles)):
            if i % 2 == 0:
                self.handles[i] = None
pymanoid/gui.py
def draw_cone(apex, axis, section, combined='r-#', color=None, linewidth=2.,
              pointsize=0.05):
    """
    Draw a 3D cone defined from its apex, axis vector and a cross-section
    polygon (defined in the plane orthogonal to the axis vector).

    Parameters
    ----------
    apex : array
        Position of the origin of the cone in world coordinates.
    axis : array
        Unit vector directing the cone axis and lying inside.
    combined : string, optional
        Drawing spec in matplotlib fashion. Default is 'g-#'.
    linewidth : scalar, optional
        Thickness of the edges of the cone.
    pointsize : scalar, optional
        Point size in [m].

    Returns
    -------
    handles : list of GUI handles
        Must be stored in some variable, otherwise the drawn object will
        vanish instantly.
    """
    if len(section) < 1:
        warn("Trying to draw an empty cone")
        return []
    color = color if color is not None else matplotlib_to_rgba(combined[0])
    handles = draw_polygon(
        points=section, normal=axis, combined=combined, color=color)
    edges = vstack([[apex, vertex] for vertex in section])
    edges = array(edges)
    edge_color = array(color) * 0.7
    edge_color[3] = 1.
    handles.append(get_openrave_env().drawlinelist(
        edges, linewidth=linewidth, colors=edge_color))
    return handles
pymanoid/gui.py
class PointMassWrenchDrawer(WrenchDrawer):

    """
    Draw contact wrenches applied to a point-mass system in multi-contact.

    Parameters
    ----------
    point_mass : PointMass
        Point-mass to which forces are applied.
    contact_set : ContactSet
        Set of contacts providing interaction forces.
    """

    def __init__(self, point_mass, contact_set):
        super(PointMassWrenchDrawer, self).__init__()
        self.contact_set = contact_set
        self.point_mass = point_mass

    def find_supporting_wrenches(self, sim):
        mass = self.point_mass.mass
        p = self.point_mass.p
        pdd = self.point_mass.pdd
        wrench = hstack([mass * (pdd - sim.gravity), zeros(3)])
        support = self.contact_set.find_supporting_wrenches(wrench, p)
        return support

    def on_tick(self, sim):
        if not hasattr(self.point_mass, 'pdd') or self.point_mass.pdd is None:
            # needs to be stored by the user
            return
        super(PointMassWrenchDrawer, self).on_tick(sim)
pymanoid/stance.py
class Stance(ContactSet):

    """
    Set of contact and center of mass (COM) targets for the humanoid.

    Parameters
    ----------
    com : PointMass
        Center of mass target.
    left_foot : Contact, optional
        Left-foot contact target.
    right_foot : Contact, optional
        Right-foot contact target.
    left_hand : Contact, optional
        Left-hand contact target.
    right_hand : Contact, optional
        Right-hand contact target.
    """

    def __init__(self, com, left_foot=None, right_foot=None,
                 left_hand=None, right_hand=None):
        # NB: do not call the parent (ContactSet) constructor
        assert issubclass(type(com), PointMass), \
            "stance COM should be a PointMass object"
        self.com = com
        self.dof_tasks = {}
        self.left_foot = left_foot
        self.left_hand = left_hand
        self.right_foot = right_foot
        self.right_hand = right_hand
        self.robot = None
        self.sep_hrep = None

    def load(self, path):
        """
        Load stance from JSON file.

        Parameters
        ----------
        path : string
            Path to the JSON file.
        """
        def cfd(contact_dict):
            keys = ['shape', 'friction']
            pose_keys = ['rpy', 'quat', 'pos', 'pose']
            keys.extend([k for k in pose_keys if k in contact_dict])
            return Contact(**{k: contact_dict[k] for k in keys})
        with open(path, 'r') as fp:
            d = simplejson.load(fp)
        if 'com' in d:
            self.com = PointMass(**d['com'])
        self.left_foot = cfd(d['left_foot']) if 'left_foot' in d else None
        self.right_foot = cfd(d['right_foot']) if 'right_foot' in d else None
        self.left_hand = cfd(d['left_hand']) if 'left_hand' in d else None
        self.right_hand = cfd(d['right_hand']) if 'right_hand' in d else None

    def save(self, path):
        """
        Save stance into JSON file.

        Parameters
        ----------
        path : string
            Path to JSON file.
        """
        d = {}
        if self.com is not None:
            d['com'] = {"pos": list(self.com.p)}
        if self.left_foot is not None:
            d['left_foot'] = self.left_foot.dict_repr
        if self.right_foot is not None:
            d['right_foot'] = self.right_foot.dict_repr
        if self.left_hand is not None:
            d['left_hand'] = self.left_hand.dict_repr
        if self.right_hand is not None:
            d['right_hand'] = self.right_hand.dict_repr
        with open(path, 'w') as fp:
            simplejson.dump(d, fp, indent=4, sort_keys=True)

    @staticmethod
    def from_json(path):
        """
        Create a new stance from a JSON file.

        Parameters
        ----------
        path : string
            Path to the JSON file.
        """
        com = PointMass([0., 0., 0.], 0.)
        stance = Stance(com)
        stance.load(path)
        return stance

    def bind(self, robot, reg='posture'):
        """
        Bind stance as robot IK targets.

        Parameters
        ----------
        robot : pymanoid.Robot
            Target robot.
        reg : string, optional
            Regularization task, either "posture" or "min_vel".
        """
        tasks = []
        if self.left_foot is not None:
            self.left_foot.link = robot.left_foot
            tasks.append(ContactTask(robot, robot.left_foot, self.left_foot))
        if self.left_hand is not None:
            self.left_hand.link = robot.left_hand
            tasks.append(ContactTask(robot, robot.left_hand, self.left_hand))
        if self.right_foot is not None:
            self.right_foot.link = robot.right_foot
            tasks.append(ContactTask(robot, robot.right_foot, self.right_foot))
        if self.right_hand is not None:
            self.right_hand.link = robot.right_hand
            tasks.append(ContactTask(robot, robot.right_hand, self.right_hand))
        for dof_id, dof_target in self.dof_tasks.iteritems():
            tasks.append(DOFTask(robot, dof_id, dof_target))
        tasks.append(COMTask(robot, self.com))
        if reg == 'posture':
            tasks.append(PostureTask(robot, robot.q_halfsit))
        else:  # reg == 'min_vel'
            tasks.append(MinVelTask(robot))
        robot.ik.clear()
        for task in tasks:
            robot.ik.add(task)
        robot.stance = self
        robot.wrench_distributor = StanceWrenchDistributor(robot.stance)
        self.robot = robot

    @property
    def bodies(self):
        """
        List of end-effector and COM bodies.
        """
        return filter(None, [
            self.com, self.left_foot, self.left_hand, self.right_foot,
            self.right_hand])

    @property
    def contacts(self):
        """
        List of active contacts.
        """
        return filter(None, [
            self.left_foot, self.left_hand, self.right_foot, self.right_hand])

    @property
    def nb_contacts(self):
        """
        Number of active contacts.
        """
        nb_contacts = 0
        if self.left_foot is not None:
            nb_contacts += 1
        if self.left_hand is not None:
            nb_contacts += 1
        if self.right_foot is not None:
            nb_contacts += 1
        if self.right_hand is not None:
            nb_contacts += 1
        return nb_contacts

    def hide(self):
        """
        Hide end-effector and COM body handles.
        """
        for body in self.bodies:
            body.hide()

    def show(self):
        """
        Show end-effector and COM body handles.
        """
        for body in self.bodies:
            body.show()

    def compute_static_equilibrium_polygon(self, method='hull'):
        """
        Compute the halfspace and vertex representations of the
        static-equilibrium polygon (SEP) of the stance.

        Parameters
        ----------
        method : string, optional
            Which method to use to perform the projection. Choices are 'bretl',
            'cdd' and 'hull' (default).
        """
        sep_vertices = super(Stance, self).compute_static_equilibrium_polygon(
            method=method)
        self.sep_hrep = compute_polytope_halfspaces(sep_vertices)
        self.sep_norm = array([norm(a) for a in self.sep_hrep[0]])
        self.sep_vertices = sep_vertices
        return sep_vertices

    def compute_pendular_accel_cone(self, com_vertices=None, zdd_max=None,
                                    reduced=False):
        """
        Compute the pendular COM acceleration cone of the stance.

        The pendular cone is the reduction of the Contact Wrench Cone when the
        angular momentum at the COM is zero.

        Parameters
        ----------
        com_vertices : list of (3,) arrays, optional
            Vertices of a COM bounding polytope.
        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 [Caron16]_.

        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 [Caron16]_ for details on this conservative criterion.
        """
        def expand_reduced_pendular_cone(reduced_hull, zdd_max=None):
            g = -gravity[2]  # gravity constant (positive)
            zdd = +g if zdd_max is None else zdd_max
            vertices_at_zdd = [
                array([a * (g + zdd), b * (g + zdd), zdd])
                for (a, b) in reduced_hull]
            return [gravity] + vertices_at_zdd

        if com_vertices is None:
            com_vertices = [self.com.p]
        CWC_O = self.compute_wrench_inequalities([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 expand_reduced_pendular_cone(reduced_hull, zdd_max)
        except QhullError:
            raise Exception("Cannot compute 2D polar for acceleration cone")

    def compute_zmp_support_area(self, height, method='bretl'):
        """
        Compute an extension of the (pendular) multi-contact ZMP support area
        with optional pressure limits on each contact.

        Parameters
        ----------
        height : array, shape=(3,)
            Height at which the ZMP support area is computed.
        method : string, default='bretl'
            Polytope projection algorithm, can be ``"bretl"`` or ``"cdd"``.

        Returns
        -------
        vertices : list of arrays
            Vertices of the ZMP support area.

        Notes
        -----
        There are two polytope projection algorithms: 'bretl' is adapted from
        in [Bretl08]_ while 'cdd' corresponds to the double-description
        formulation from [Caron17z]_. See the Appendix from [Caron16]_ for a
        performance comparison.
        """
        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_list, b_list = zip(*[ct.wrench_hrep for ct in self.contacts])
        F = block_diag(*F_list)
        b = hstack(b_list)
        mass = 42.  # [kg], does not affect the output
        # mass has no effect on the output polygon, c.f. Section IV.C in
        # <https://hal.archives-ouvertes.fr/hal-01349880>
        B1 = hstack([self.com.z * eye(3), crossmat_n])
        B2 = hstack([zeros(3), self.com.p])
        # B2 = hstack([-(cross(n, p_in)), n])]) yields same result
        B = vstack([B1, B2])
        C = 1. / (mass * 9.81) * dot(B, G)
        d = hstack([self.com.p, [0]])
        E = (height - self.com.z) / (mass * 9.81) * G[:2, :]
        f = array([self.com.x, self.com.y])
        return project_polytope(
            proj=(E, f),
            ineq=(F, b),
            eq=(C, d),
            method=method)

    def dist_to_sep_edge(self, com):
        """
        Algebraic distance of a COM position to the edge of the
        static-equilibrium polygon.

        Parameters
        ----------
        com : array, shape=(3,)
            COM position to evaluate the distance from.

        Returns
        -------
        dist : scalar
            Algebraic distance to the edge of the polygon. Inner points get a
            positive value, outer points a negative one.
        """
        A, b = self.sep_hrep
        alg_dists = (b - dot(A, com[:2])) / self.sep_norm
        return min(alg_dists)

    def find_static_supporting_wrenches(self):
        """
        Find supporting contact wrenches in static-equilibrium.

        Returns
        -------
        support : list of (Contact, array) couples
            Mapping between each contact `i` in the contact set and a
            supporting contact wrench :math:`w^i_{C_i}`.
        """
        wrench = hstack([array([0., 0., self.com.mass * 9.81]), zeros(3)])
        return self.find_supporting_wrenches(wrench, self.com.p)

    def free_contact(self, effector_name):
        """
        Free a contact from the stance and get the corresponding end-effector
        target. Its task stays in the IK, but the effector is not considered in
        contact any more (e.g. for contact wrench computations).

        Parameters
        ----------
        effector_name : string
            Name of end-effector target, for example "left_foot".

        Returns
        -------
        effector : pymanoid.Contact
            IK target contact.
        """
        if self.__dict__[effector_name] is None:
            raise Exception("Stance's %s is not in contact" % effector_name)
        effector = self.__dict__[effector_name]
        self.__dict__[effector_name] = None
        return effector

    def set_contact(self, effector):
        """
        Set contact on a effector.

        Parameters
        ----------
        effector : pymanoid.Contact
            IK target contact.
        """
        if effector.link.name == self.robot.left_foot.name:
            effector_name = 'left_foot'
        elif effector.link.name == self.robot.left_hand.name:
            effector_name = 'left_hand'
        elif effector.link.name == self.robot.right_foot.name:
            effector_name = 'right_foot'
        elif effector.link.name == self.robot.right_hand.name:
            effector_name = 'right_hand'
        else:
            raise Exception("Unknown robot effector: " + effector.link.name)
        if self.__dict__[effector_name] is not None:
            raise Exception("Stance's %s already in contact" % effector_name)
        self.__dict__[effector_name] = effector
pymanoid/interp.py
def interpolate_cubic_bezier(p0, p1, p2, p3):
def interpolate_cubic_hermite(p0, v0, p1, v1):
def interpolate_pose_linear(pose0, pose1, x):
def interpolate_pose_quadratic(pose0, pose1, x):
    def __init__(self, start_pose, end_pose, duration, body=None):
    def eval_quat(self, s):
    def eval_pos(self, s):
    def __call__(self, t):
    def update(self, t):
    def draw(self, color='b'):
    def __init__(self, start_pose, end_pose, duration, body=None):
    def eval_pos(self, s):
    def __init__(self, start_pose, end_pose, duration, body=None):
    def eval_pos(self, s):
    def __init__(self, start_pose, end_pose, duration, body=None):
    def eval_pos(self, s):
    def __init__(self, start_pos, end_pos, duration, body=None):
    def __call__(self, t):
    def __init__(self, start_pos, end_pos, duration, body=None):
    def __call__(self, t):
    def __init__(self, start_pos, end_pos, duration, body=None):
    def __call__(self, t):
class PoseInterpolator(object):
class LinearPoseInterpolator(PoseInterpolator):
class CubicPoseInterpolator(PoseInterpolator):
class QuinticPoseInterpolator(PoseInterpolator):
class LinearPosInterpolator(LinearPoseInterpolator):
class CubicPosInterpolator(CubicPoseInterpolator):
class QuinticPosInterpolator(QuinticPoseInterpolator):
pymanoid/tasks.py
class PoseTask(Task):

    """
    Pose task for a given link.

    Parameters
    ----------
    robot : Robot
        Target robot.
    link : Link or string
        Robot Link, or name of the link field in the ``robot`` argument.
    target : list or array (shape=(7,)) or pymanoid.Body
        Pose coordinates of the link's target.
    weight : scalar, optional
        Task weight used in IK cost function. If None, needs to be set later.
    gain : scalar, optional
        Proportional gain of the task.
    exclude_dofs : list of integers, optional
        DOF indices not used by task.
    """

    def __init__(self, robot, link, target, weight=None, gain=None,
                 exclude_dofs=None):
        super(PoseTask, self).__init__(weight, gain, exclude_dofs)
        if type(link) is str:
            link = robot.__getattribute__(link)
        self.link = link
        self.name = self.link.name
        self.robot = robot
        self.update_target(target)

    def _jacobian(self):
        return self.robot.compute_link_pose_jacobian(self.link)

    def _residual(self, dt):
        pose_diff = self.target.pose - self.link.pose
        if dot(pose_diff[0:4], pose_diff[0:4]) > 1.:
            pose_diff = _OPPOSE_QUAT * self.target.pose - self.link.pose
        return pose_diff / dt

    def update_target(self, target):
        """
        Update the task residual with a new target.

        Parameters
        ----------
        target : Point or array or list
            New link position target.
        """
        self.target = target if hasattr(target, 'pose') else PoseWrap(target)
pymanoid/gui.py
def draw_polytope(points, combined='g-#', color=None, faces=None,
                  linewidth=1., pointsize=0.01, hull=None):
    """
    Draw a polyhedron defined as the convex hull of a set of points.

    Parameters
    ----------
    points : list of arrays
        List of 3D points in the world frame.
    combined : string, optional
        Drawing spec in matplotlib fashion. Default: 'g-#'.
    color : char or triplet, optional
        Color letter or RGB values, default is 'g' for green.
    faces : string, optional
        Faces of the polytope to draw. Use '.' for vertices, '-' for edges
        and '#' for facets.
    hull : scipy.spatial.ConvexHull
        2D convex hull provided when drawing polygons, in which case the 3D
        hull has zero volume.
    linewidth : scalar
        Thickness of drawn line.
    pointsize : scalar
        Vertex size.

    Returns
    -------
    handles : list of openravepy.GraphHandle
        OpenRAVE graphical handles. Must be stored in some variable, otherwise
        the drawn object will vanish instantly.

    Notes
    -----
    In the ``faces`` or ``combined`` strings, use '.' for vertices, '-' for
    edges and '#' for facets.
    """
    is_2d = hull is not None
    if color is None:
        color = combined[0]
    if faces is None:
        faces = combined[1:]
    if type(color) is str:
        color = matplotlib_to_rgba(color)
    if hull is None:
        try:
            hull = ConvexHull(points)
        except QhullError:
            warn("QhullError: maybe polyhedron is empty?")
            return []
    vertices = array([points[i] for i in hull.vertices])
    points = array(points)
    color = array(color if color is not None else (0.0, 0.5, 0.0, 0.5))
    handles = []
    env = get_openrave_env()
    if '-' in faces:  # edges
        edge_color = color * 0.7
        edge_color[3] = 1.
        edges = vstack([[points[i], points[j]]
                        for s in hull.simplices
                        for (i, j) in itertools.combinations(s, 2)])
        edges = array(edges)
        handles.append(env.drawlinelist(
            edges, linewidth=linewidth, colors=edge_color))
    if '#' in faces:  # facets
        if is_2d:
            nv = len(vertices)
            indices = array([(0, i, i + 1) for i in range(nv - 1)], int64)
            handles.append(env.drawtrimesh(vertices, indices, colors=color))
        else:
            indices = array(hull.simplices, int64)
            handles.append(env.drawtrimesh(points, indices, colors=color))
    if '.' in faces:  # vertices
        color[:3] *= 0.75
        color[3] = 1.
        handles.append(env.plot3(
            vertices, pointsize=pointsize, drawstyle=1, colors=color))
    return handles
pymanoid/transformations.py
def rotation_matrix_from_quat(quat):
    """
    Rotation matrix from quaternion.

    Parameters
    ----------
    quat : (4,) array
        Quaternion in `[w x y z]` format.

    Returns
    -------
    R : (3, 3) array
        Rotation matrix.
    """
    return __rotationMatrixFromQuat(quat)
pymanoid/sim.py
def get_openrave_env():
    def __init__(self, dt, env_path=None, env_xml=None):
    def camera_transform(self):
    def time(self):
    def __del__(self):
    def schedule(self, process, paused=False, log_comp_times=False):
    def schedule_extra(self, process):
    def step(self, n=1):
    def unschedule(self, process):
    def _tick_processes(self):
    def _tick_extras(self):
    def run_thread(self):
    def start(self):
    def stop(self):
    def set_viewer(self, plugin='qtcoin'):
    def set_background(self, color=None):
    def set_camera_back(self, x=None, y=None, z=None):
    def set_camera_bottom(self, x=None, y=None, z=None):
    def set_camera_front(self, x=None, y=None, z=None):
    def set_camera_left(self, x=None, y=None, z=None):
    def set_camera_right(self, x=None, y=None, z=None):
    def set_camera_top(self, x=None, y=None, z=None):
    def set_camera_transform(self, T):
    def move_camera_to(self, T, duration=0., dt=3e-2):
    def get_viewer_window_id(self):
    def take_screenshot(self, fname):
    def load_mesh(self, path):
    def log_comp_time(self, label, ctime):
    def print_comp_times(self, unit='ms'):
    def record(self, fname=None):
class Simulation(object):
pymanoid/tasks.py
class ContactTask(PoseTask):

    """
    In essence, a contact task is a :class:`pymanoid.tasks.PoseTask` with a
    much (much) larger weight. For plausible motions, no task should have a
    weight higher than (or even comparable to) that of contact tasks.
    """

    def __init__(self, robot, link, target, weight=None, gain=None,
                 exclude_dofs=None):
        super(ContactTask, self).__init__(
            robot, link, target, weight, gain, exclude_dofs)
pymanoid/body.py
class PointMass(Point):

    """
    Point with a mass property and a size proportional to it.

    Parameters
    ----------
    pos : (3,) array
        Initial position in the world frame.
    mass : scalar
        Total mass in [kg].
    vel : (3,) array, optional
        Initial velocity in the world frame.
    color : char, optional
        Color letter in ['r', 'g', 'b'].
    visible : bool, optional
        Visibility in the GUI.
    size : scalar, optional
        Half-length of a side of the CoM cube handle, in [m].
    """

    def __init__(self, pos, mass, vel=None, color='r', visible=True,
                 size=None):
        if size is None:
            size = max(5e-3, 6e-4 * mass)
        super(PointMass, self).__init__(
            pos, vel=vel, size=size, color=color, visible=visible)
        self.mass = mass

    def copy(self, color='r', visible=True):
        """
        Copy constructor.

        Parameters
        ----------
        color : char, optional
            Color of the copy, in ['r', 'g', 'b'].
        visible : bool, optional
            Should the copy be visible?
        """
        return PointMass(
            self.p, self.mass, self.pd, color=color, visible=visible)

    @property
    def momentum(self):
        """Linear momentum in the world frame."""
        return self.mass * self.pd


Question: import IPython
import pyclipper
import time
import pymanoid
from numpy import arange, array, bmat, cross, dot, eye, hstack, zeros
from numpy import cos, pi, sin
from numpy.random import random, seed
from threading import Lock
from pymanoid import Contact, PointMass, Stance
from pymanoid.body import Box
from pymanoid.gui import PointMassWrenchDrawer, TrajectoryDrawer
from pymanoid.gui import draw_cone, draw_polytope
from pymanoid.gui import draw_line, draw_point, draw_points
from pymanoid.interp import interpolate_pose_linear, quat_slerp
from pymanoid.misc import normalize
from pymanoid.mpc import LinearPredictiveControl
from pymanoid.pypoman import compute_polytope_halfspaces
from pymanoid.pypoman import intersect_polygons
from pymanoid.robots import JVRC1
from pymanoid.sim import gravity
from pymanoid.tasks import ContactTask, DOFTask, PoseTask, MinCAMTask
from pymanoid.transformations import rotation_matrix_from_quat
    Parameters
    ----------
    stances : list of Stances
        Consecutives stances traversed by the FSM.
    robot : Robot
        Controller robot.
    swing_height : scalar
        Relative height in [m] for the apex of swing foot trajectories.
    cycle : bool, optional
        If ``True``, the first stance will succeed the last one.
    """

    def __init__(self, stances, robot, swing_height, cycle=False):
        super(MultiContactWalkingFSM, self).__init__()
        self.cur_phase = stances[0].label
        self.cur_stance = stances[0]
        self.cur_stance_id = 0
        self.cycle = cycle
        self.is_over = False
        self.nb_stances = len(stances)
        self.rem_time = stances[0].duration
        self.robot = robot
        self.stances = stances
        self.swing_foot = SwingFoot(swing_height)

    @property
    def next_stance(self):
        next_stance_id = self.cur_stance_id + 1
        if next_stance_id >= self.nb_stances:
            next_stance_id = 0 if self.cycle else self.cur_stance_id
        return self.stances[next_stance_id]

    @property
    def next_next_stance(self):
        next_next_stance_id = self.cur_stance_id + 2
        if next_next_stance_id >= self.nb_stances:
            if self.cycle:
                next_next_stance_id %= self.nb_stances
            else:  # not self.cycle:
                next_next_stance_id = self.nb_stances - 1
        return self.stances[next_next_stance_id]

    def get_preview_targets(self):
        stance_foot = self.cur_stance.left_foot \
            if self.cur_stance.label.endswith('L') else \
            self.cur_stance.right_foot
        if self.cur_stance.label.startswith('SS') \
                and self.rem_time < 0.5 * self.cur_stance.duration:
            horizon = self.rem_time \
                + self.next_stance.duration \
                + 0.5 * self.next_next_stance.duration
            target_com = self.next_stance.com.p
            target_comd = (target_com - self.cur_stance.com.p) / horizon
        elif self.cur_stance.label.startswith('DS'):
            horizon = self.rem_time + 0.5 * self.next_stance.duration
            target_com = self.cur_stance.com.p
            target_comd = 0.4 * stance_foot.t
        else:  # single support with plenty of time ahead
            horizon = self.rem_time
            target_com = self.cur_stance.com.p
            target_comd = 0.4 * stance_foot.t
        return (self.rem_time, horizon, target_com, target_comd)

    def on_tick(self, sim):
        """
        Update the FSM after a tick of the control loop.

        Parameters
        ----------
        sim : Simulation
            Instance of the current simulation.
        """
        if self.is_over:
            return

        def can_switch_to_ss():
            com = self.robot.com
            dist_inside_sep = self.next_stance.dist_to_sep_edge(com)
            return dist_inside_sep > -0.15

        if self.rem_time > 0.:
            if self.cur_stance.label.startswith('SS'):
                progress = 1. - self.rem_time / self.cur_stance.duration
                self.swing_foot.update_pose(progress)
            self.rem_time -= sim.dt
        elif (self.cur_stance.label.startswith('DS') and not
              can_switch_to_ss()):
            print("FSM: not ready for single-support yet...")
        elif self.cur_stance_id == self.nb_stances - 1 and not self.cycle:
            self.is_over = True
        else:
            # NB: in the following block, cur_stance has not been updated yet
            if self.cur_stance.label == 'DS-R' \
                    and self.next_next_stance.label == 'DS-L':
                self.swing_foot.reset(
                    self.cur_stance.left_foot.pose,
                    self.next_next_stance.left_foot.pose)
            elif self.cur_stance.label == 'DS-L' \
                    and self.next_next_stance.label == 'DS-R':
                self.swing_foot.reset(
                    self.cur_stance.right_foot.pose,
                    self.next_next_stance.right_foot.pose)
            # now that we have read swing foot poses, we update cur_stance
            self.cur_stance_id = (self.cur_stance_id + 1) % self.nb_stances
            self.cur_stance = self.stances[self.cur_stance_id]
            self.rem_time = self.cur_stance.duration
            self.update_robot_ik()

    def update_robot_ik(self):
        prev_lf_task = self.robot.ik.tasks[self.robot.left_foot.name]
        prev_rf_task = self.robot.ik.tasks[self.robot.right_foot.name]
        contact_weight = max(prev_lf_task.weight, prev_rf_task.weight)
        swing_weight = 1e-1
        self.robot.ik.remove(self.robot.left_foot.name)
        self.robot.ik.remove(self.robot.right_foot.name)
        if self.cur_stance.left_foot is not None:
            left_foot_task = ContactTask(
                self.robot, self.robot.left_foot, self.cur_stance.left_foot,
                weight=contact_weight)
        else:  # left_foot is swinging

Answer:             left_foot_task = PoseTask(