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/misc.py
def middot(M, T):
    """
    Dot product of a matrix with the mid-coordinate of a 3D tensor.

    Parameters
    ----------
    M : array, shape=(n, m)
        Matrix to multiply.
    T : array, shape=(a, m, b)
        Tensor to multiply.

    Returns
    -------
    U : array, shape=(a, n, b)
        Dot product between `M` and `T`.
    """
    return tensordot(M, T, axes=(1, 1)).transpose([1, 0, 2])
pymanoid/transformations.py
def rotation_matrix_from_rpy(rpy):
    """
    Rotation matrix from roll-pitch-yaw angles.

    Parameters
    ----------
    rpy : (3,) array
        Vector of roll-pitch-yaw angles in [rad].

    Returns
    -------
    R : (3, 3) array
        Rotation matrix.
    """
    return rotation_matrix_from_quat(quat_from_rpy(rpy))
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/transformations.py
def quat_from_rpy(rpy):
    """
    Quaternion frmo roll-pitch-yaw angles.

    Parameters
    ----------
    rpy : (3,) array
        Vector of roll-pitch-yaw angles in [rad].

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

    Notes
    -----
    Roll-pitch-yaw are Euler angles corresponding to the sequence (1, 2, 3).
    """
    roll, pitch, yaw = rpy
    cr, cp, cy = cos(roll / 2.), cos(pitch / 2.), cos(yaw / 2.)
    sr, sp, sy = sin(roll / 2.), sin(pitch / 2.), sin(yaw / 2.)
    return array([
        cr * cp * cy + sr * sp * sy,
        -cr * sp * sy + cp * cy * sr,
        cr * cy * sp + sr * cp * sy,
        cr * cp * sy - sr * cy * sp])
pymanoid/transformations.py
def crossmat(x):
    """
    Cross-product matrix of a 3D vector.

    Parameters
    ----------
    x : (3,) array
        Vector on the left-hand side of the cross product.

    Returns
    -------
    C : (3, 3) array
        Cross-product matrix of `x`.
    """
    return array([
        [0., -x[2], x[1]],
        [x[2], 0., -x[0]],
        [-x[1], x[0], 0.]])
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/transformations.py
def rpy_from_quat(quat):
    """
    Roll-pitch-yaw angles of a quaternion.

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

    Returns
    -------
    rpy : (3,) array
        Array of roll-pitch-yaw angles, in [rad].

    Notes
    -----
    Roll-pitch-yaw are Euler angles corresponding to the sequence (1, 2, 3).
    """
    roll = atan2(
        2 * quat[2] * quat[3] + 2 * quat[0] * quat[1],
        quat[3] ** 2 - quat[2] ** 2 - quat[1] ** 2 + quat[0] ** 2)
    pitch = -asin(
        2 * quat[1] * quat[3] - 2 * quat[0] * quat[2])
    yaw = atan2(
        2 * quat[1] * quat[2] + 2 * quat[0] * quat[3],
        quat[1] ** 2 + quat[0] ** 2 - quat[3] ** 2 - quat[2] ** 2)
    return array([roll, pitch, yaw])
pymanoid/body.py
class Body(object):

    """
    Base class for rigid bodies. Wraps OpenRAVE's KinBody type.

    Parameters
    ----------
    rave_body : openravepy.KinBody
        OpenRAVE body to wrap.
    pos : array, shape=(3,), optional
        Initial position in inertial frame.
    rpy : array, shape=(3,), optional
        Initial orientation in inertial frame.
    pose : array, shape=(7,), optional
        Initial pose. Supersedes ``pos`` and ``rpy`` if they are provided at
        the same time.
    color : char, optional
        Color code in matplotlib convention ('b' for blue, 'g' for green, ...).
    visible : bool, optional
        Visibility in the GUI.
    """

    count = 0

    def __init__(self, rave_body, pos=None, rpy=None, pose=None, color=None,
                 visible=True):
        self.color = color
        self.rave = rave_body
        if not rave_body.GetName():
            self.set_name("%s%s" % (type(self).__name__, Body.count))
            Body.count += 1
        if pos is not None:
            self.set_pos(pos)
        if rpy is not None:
            self.set_rpy(rpy)
        if pose is not None:
            self.set_pose(pose)
        if color is not None:
            self.set_color(color)
        if not visible:
            self.hide()

    def __str__(self):
        return "pymanoid.Body('%s')" % self.name

    def set_color(self, color):
        """
        Set the color of the rigid body.

        Parameters
        ----------
        color : tuple or string
            RGB tuple, or color code in matplotlib convention.
        """
        if isinstance(color, str):
            color = matplotlib_to_rgb(color)
        for link in self.rave.GetLinks():
            for geom in link.GetGeometries():
                geom.SetAmbientColor(color)
                geom.SetDiffuseColor(color)
        self.color = color

    def set_name(self, name):
        """
        Set body name in OpenRAVE scope.

        name : string
            Body name.
        """
        self.rave.SetName(name)

    def set_transparency(self, transparency):
        """
        Set the transparency of the rigid body.

        Parameters
        ----------
        transparency : double, optional
            Transparency value from 0 (opaque) to 1 (invisible).
        """
        for link in self.rave.GetLinks():
            for geom in link.GetGeometries():
                geom.SetTransparency(transparency)

    def show(self):
        """Make the body visible."""
        self.rave.SetVisible(True)

    def hide(self):
        """Make the body invisible."""
        self.rave.SetVisible(False)

    @property
    def index(self):
        """
        OpenRAVE index of the body.

        Notes
        -----
        This index is notably used to compute jacobians and hessians.
        """
        return self.rave.GetIndex()

    @property
    def name(self):
        """Body name."""
        return str(self.rave.GetName())

    @property
    def T(self):
        """
        Homogeneous coordinates of the rigid body.

        These coordinates describe the orientation and position of the rigid
        body by the 4 x 4 transformation matrix

        .. math::

            T = \\left[
                \\begin{array}{cc}
                    R & p \\\\
                    0_{1 \\times 3} & 1
                \\end{array}
                \\right]

        where `R` is a `3 x 3` rotation matrix and `p` is the vector of
        position coordinates.

        Notes
        -----
        More precisely, `T` is the transformation matrix *from* the body frame
        *to* the world frame: if
        :math:`\\tilde{p}_\\mathrm{body} = [x\\ y\\ z\\ 1]` denotes the
        homogeneous coordinates of a point in the body frame, then the
        homogeneous coordinates of this point in the world frame are
        :math:`\\tilde{p}_\\mathrm{world} = T \\tilde{p}_\\mathrm{body}`.
        """
        return self.rave.GetTransform()

    @property
    def transform(self):
        """
        Homogeneous coordinates of the rigid body.

        These coordinates describe the orientation and position of the rigid
        body by the 4 x 4 transformation matrix

        .. math::

            T = \\left[
                \\begin{array}{cc}
                    R & p \\\\
                    0_{1 \\times 3} & 1
                \\end{array}
                \\right]

        where `R` is a `3 x 3` rotation matrix and `p` is the vector of
        position coordinates.

        Notes
        -----
        More precisely, `T` is the transformation matrix *from* the body frame
        *to* the world frame: if
        :math:`\\tilde{p}_\\mathrm{body} = [x\\ y\\ z\\ 1]` denotes the
        homogeneous coordinates of a point in the body frame, then the
        homogeneous coordinates of this point in the world frame are
        :math:`\\tilde{p}_\\mathrm{world} = T \\tilde{p}_\\mathrm{body}`.
        """
        return self.T

    @property
    def pose(self):
        """
        Body pose as a 7D quaternion + position vector.

        The pose vector :math:`[q_w\\,q_x\\,q_y\\,q_z\\,x\\,y\\,z]` consists of
        a quaternion :math:`q = [q_w\\,q_x\\,q_y\\,q_z]` (with the real term
        :math:`q_w` coming first) for the body orientation, followed by the
        coordinates :math:`p = [x\\,y\\,z]` in the world frame.
        """
        pose = self.rave.GetTransformPose()
        if pose[0] < 0:  # convention: cos(alpha) > 0
            # this convention enforces Slerp shortest path
            pose[:4] *= -1
        return pose

    @property
    def R(self):
        """Rotation matrix `R` from local to world coordinates."""
        return self.T[0:3, 0:3]

    @property
    def rotation_matrix(self):
        """Rotation matrix `R` from local to world coordinates."""
        return self.R

    @property
    def p(self):
        """Position coordinates `[x y z]` in the world frame."""
        return self.T[0:3, 3]

    @property
    def pos(self):
        """Position coordinates `[x y z]` in the world frame."""
        return self.p

    @property
    def x(self):
        """`x`-coordinate in the world frame."""
        return self.p[0]

    @property
    def y(self):
        """`y`-coordinate in the world frame."""
        return self.p[1]

    @property
    def z(self):
        """`z`-coordinate in the world frame."""
        return self.p[2]

    @property
    def t(self):
        """Tangent vector directing the `x`-axis of the body frame."""
        return self.T[0:3, 0]

    @property
    def b(self):
        """Binormal vector directing the `y`-axis of the body frame."""
        return self.T[0:3, 1]

    @property
    def n(self):
        """Normal vector directing the `z`-axis of the body frame."""
        return self.T[0:3, 2]

    @property
    def normal(self):
        """Normal vector directing the `z`-axis of the body frame."""
        return self.T[0:3, 2]

    @property
    def quat(self):
        """Quaternion of the rigid body orientation."""
        return self.pose[0:4]

    @property
    def rpy(self):
        """
        Roll-pitch-yaw angles.

        They correspond to Euleur angles for the sequence (1, 2, 3). See
        [Diebel06]_ for details.
        """
        return rpy_from_quat(self.quat)

    @property
    def roll(self):
        """Roll angle of the body orientation."""
        return self.rpy[0]

    @property
    def pitch(self):
        """Pitch angle of the body orientation."""
        return self.rpy[1]

    @property
    def yaw(self):
        """Yaw angle of the body orientation."""
        return self.rpy[2]

    def set_transform(self, T):
        """
        Set homogeneous coordinates of the rigid body.

        Parameters
        ----------
        T : array, shape=(4, 4)
            Transform matrix.
        """
        self.rave.SetTransform(T)

    def set_pos(self, pos):
        """
        Set the position of the body in the world frame.

        Parameters
        ----------
        pos : array, shape=(3,)
            3D vector of position coordinates.
        """
        T = self.T.copy()
        T[:3, 3] = pos
        self.set_transform(T)

    def set_rotation_matrix(self, R):
        """
        Set the orientation of the rigid body.

        Recall that this orientation is described by the rotation matrix `R`
        *from* the body frame *to* the world frame.

        Parameters
        ----------
        R : (3, 3) array
            Rotation matrix.
        """
        T = self.T.copy()
        T[:3, :3] = R
        self.set_transform(T)

    def set_x(self, x):
        """
        Set the `x`-coordinate of the body in the world frame.

        Parameters
        ----------
        x : scalar
            New `x`-coordinate.
        """
        T = self.T.copy()
        T[0, 3] = x
        self.set_transform(T)

    def set_y(self, y):
        """
        Set the `y`-coordinate of the body in the world frame.

        Parameters
        ----------
        y : scalar
            New `y`-coordinate.
        """
        T = self.T.copy()
        T[1, 3] = y
        self.set_transform(T)

    def set_z(self, z):
        """
        Set the `z`-coordinate of the body in the world frame.

        Parameters
        ----------
        z : scalar
            New `z`-coordinate.
        """
        T = self.T.copy()
        T[2, 3] = z
        self.set_transform(T)

    def set_rpy(self, rpy):
        """
        Set the roll-pitch-yaw angles of the body orientation.

        Parameters
        ----------
        rpy : scalar triplet
            Triplet `(r, p, y)` of roll-pitch-yaw angles.
        """
        T = self.T.copy()
        T[0:3, 0:3] = rotation_matrix_from_rpy(rpy)
        self.set_transform(T)

    def set_roll(self, roll):
        """
        Set the roll angle of the body orientation.

        Parameters
        ----------
        roll : scalar
            Roll angle in [rad].
        """
        return self.set_rpy([roll, self.pitch, self.yaw])

    def set_pitch(self, pitch):
        """
        Set the pitch angle of the body orientation.

        Parameters
        ----------
        pitch : scalar
            Pitch angle in [rad].
        """
        return self.set_rpy([self.roll, pitch, self.yaw])

    def set_yaw(self, yaw):
        """
        Set the yaw angle of the body orientation.

        Parameters
        ----------
        yaw : scalar
            Yaw angle in [rad].
        """
        return self.set_rpy([self.roll, self.pitch, yaw])

    def set_pose(self, pose):
        """
        Set the 7D pose of the body orientation.

        Parameters
        ----------
        pose : (7,) array
            Pose of the body, i.e. quaternion + position in world frame.
        """
        T = openravepy.matrixFromPose(pose)
        self.set_transform(T)

    def set_quat(self, quat):
        """
        Set the quaternion of the body orientation.

        Parameters
        ----------
        quat : (4,) array
            Quaternion in (w, x, y, z) format.
        """
        pose = self.pose.copy()
        pose[0:4] = quat
        self.set_pose(pose)

    def translate(self, translation):
        """
        Apply a translation to the body.

        Parameters
        ----------
        translation : (3,) array
            Offset to apply to the position (world coordinates) of the body.
        """
        self.set_pos(self.p + translation)

    def remove(self):
        """
        Remove body from OpenRAVE environment.
        """
        env = get_openrave_env()
        with env:
            env.Remove(self.rave)

    def __del__(self):
        """
        Add body removal to garbage collection step (effective).
        """
        try:
            self.remove()
        except Exception:  # __del__ exceptions are ignored
            pass

    def apply_twist(self, v, omega, dt):
        """
        Apply a twist :math:`[v\\ \\omega]` defined in the local coordinate
        frame.

        Parameters
        ----------
        v : (3,) array
            Linear velocity in local frame.
        omega : (3,) array
            Angular velocity in local frame.
        dt : scalar
            Duration of twist application in [s].
        """
        self.set_pos(self.p + v * dt)
        self.set_rotation_matrix(self.R + dot(crossmat(omega), self.R) * dt)

    def dist(self, point):
        """
        Distance from the body frame origin to another point.

        Parameters
        ----------
        point : array or Point
            Point to compute the distance to.
        """
        if isinstance(point, list):
            point = array(point)
        if isinstance(point, ndarray):
            return norm(point - self.p)
        return norm(point.p - self.p)

    @property
    def adjoint_matrix(self):
        """
        Adjoint matrix converting wrenches in the local frame :math:`\\cal L`
        to the inertial frame :math:`\\cal W`, that is the matrix
        :math:`{}^{\\cal W}A_{\\cal L}` such that:

        .. math::

            {}^{\\cal W}w = {}^{\\cal W}A_{\\cal L} {}^{\\cal L} w

        Returns
        -------
        A : array
            Adjoint matrix.
        """
        return vstack([
            hstack([self.R, eye(3)]),
            hstack([dot(crossmat(self.p), self.R), self.R])])
pymanoid/misc.py
def matplotlib_to_rgb(color):
    """
    Convert matplotlib color string to RGB tuple.

    Parameters
    ----------
    color : string
        Color code in matplotlib convention ('r' for red, 'b' for blue, etc.).

    Returns
    -------
    rgb : tuple
        Red-green-blue tuple with values between 0 and 1.
    """
    acolor = [0., 0., 0.]
    if color == 'k':
        return acolor
    if color == 'w':
        return [1., 1., 1.]
    if color in ['r', 'm', 'y', 'w']:
        acolor[0] += 0.5
    if color in ['g', 'y', 'c', 'w']:
        acolor[1] += 0.5
    if color in ['b', 'c', 'm', 'w']:
        acolor[2] += 0.5
    return acolor
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
pymanoid/ik.py
class IKSolver(Process):

    """
    Compute velocities bringing the system closer to fulfilling a set of tasks.

    Parameters
    ----------
    robot : Robot
        Robot to be updated.
    active_dofs : list of integers, optional
        List of DOFs updated by the IK solver.
    doflim_gain : scalar, optional
        DOF-limit gain as described in [Kanoun12]_. In `this implementation
        <https://scaron.info/teaching/inverse-kinematics.html>`_, it should be
        between zero and one.

    Attributes
    ----------
    doflim_gain : scalar, optional
        DOF-limit gain as described in [Kanoun12]_. In `this implementation
        <https://scaron.info/teaching/inverse-kinematics.html>`_, it should be
        between zero and one.
    lm_damping : scalar
        Add Levenberg-Marquardt damping as described in [Sugihara11]_. This
        damping significantly improves numerical stability, but convergence
        gets slower when its value is too high.
    slack_dof_limits : bool
        Add slack variables to maximize DOF range? This method is used in
        [Nozawa16]_ to keep joint angles as far away from their limits as
        possible. It slows down computations as there are twice as many
        optimization variables, but is more numerically stable and won't
        produce inconsistent constraints. Defaults to False.
    slack_maximize : scalar
        Linear cost weight applied when ``slack_dof_limits`` is True.
    slack_regularize : scalar
        Regularization weight applied when ``slack_dof_limits`` is True.
    qd : array
        Velocity returned by last solver call.
    robot : pymanoid.Robot
        Robot model.
    tasks : dict
        Dictionary of active IK tasks, indexed by task name.

    Notes
    -----
    One unsatisfactory aspect of the DOF-limit gain is that it slows down the
    robot when approaching DOF limits. For instance, it may slow down a foot
    motion when approaching the knee singularity, despite the robot being able
    to move faster with a fully extended knee.
    """

    DEFAULT_GAINS = {
        'COM': 0.85,
        'CONTACT': 0.85,
        'DOF': 0.85,
        'MIN_ACCEL': 0.85,
        'MIN_CAM': 0.85,
        'MIN_VEL': 0.85,
        'PENDULUM': 0.85,
        'POSE': 0.85,
        'POSTURE': 0.85,
    }

    DEFAULT_WEIGHTS = {
        'CONTACT': 1.,
        'COM': 1e-2,
        'POSE': 1e-3,
        'MIN_ACCEL': 1e-4,
        'MIN_CAM': 1e-4,
        'DOF': 1e-5,
        'POSTURE': 1e-6,
        'MIN_VEL': 1e-6,
    }

    def __init__(self, robot, active_dofs=None, doflim_gain=0.5):
        super(IKSolver, self).__init__()
        if active_dofs is None:
            active_dofs = range(robot.nb_dofs)
        assert 0. <= doflim_gain <= 1.
        self.__lock = Lock()
        self.doflim_gain = doflim_gain
        self.interaction_dist = 0.1  # [rad]
        self.lm_damping = 1e-3
        self.qd = zeros(robot.nb_dofs)
        self.robot = robot
        self.safety_dist = 0.01  # [rad]
        self.slack_dof_limits = False
        self.slack_maximize = 1e-3
        self.slack_regularize = 1e-5
        self.tasks = {}
        self.verbosity = 0
        #
        self.set_active_dofs(active_dofs)

    def clear(self):
        """
        Clear all tasks in the IK solver.
        """
        self.tasks = {}

    def set_active_dofs(self, active_dofs):
        """
        Set DOF indices modified by the IK.

        Parameters
        ----------
        active_dofs : list of integers
            List of DOF indices.
        """
        self.active_dofs = active_dofs
        self.nb_active_dofs = len(active_dofs)
        self.__reset_dof_limits()

    def __reset_dof_limits(self):
        """
        Read DOF position, velocity and acceleration limits from robot model.
        """
        self.q_max = self.robot.q_max[self.active_dofs]
        self.q_min = self.robot.q_min[self.active_dofs]
        self.qd_lim = self.robot.qd_lim[self.active_dofs]
        if self.robot.qdd_lim is not None:
            self.qdd_lim = self.robot.qdd_lim[self.active_dofs]
        else:  # robot model has no joint acceleration limit
            self.qdd_lim = None

    def set_gains(self, gains):
        """
        Set task gains from a dictionary.

        Parameters
        ----------
        gains : string -> double dictionary
            Dictionary mapping task labels to default gain values.
        """
        for (name, gain) in gains.iteritems():
            self.tasks[name].gain = gain

    def set_weights(self, weights):
        """
        Set task weights from a dictionary.

        Parameters
        ----------
        weights : string -> double dictionary
            Dictionary mapping task labels to default weight values.
        """
        for (name, weight) in weights.iteritems():
            self.tasks[name].weight = weight

    def __fill_gain(self, task):
        if task.name in self.DEFAULT_GAINS:
            task.gain = self.DEFAULT_GAINS[task.name]
        elif type(task) in [AxisAngleContactTask, ContactTask]:
            task.gain = self.DEFAULT_GAINS['CONTACT']
        elif type(task) is DOFTask:
            task.gain = self.DEFAULT_GAINS['DOF']
        elif type(task) is PoseTask:
            task.gain = self.DEFAULT_GAINS['POSE']
        else:  # task type is not accounted for
            raise Exception("no gain provided for task '%s'" % task.name)

    def __fill_weight(self, task):
        if task.name in self.DEFAULT_WEIGHTS:
            task.weight = self.DEFAULT_WEIGHTS[task.name]
        elif type(task) in [AxisAngleContactTask, ContactTask]:
            task.weight = self.DEFAULT_WEIGHTS['CONTACT']
        elif type(task) is DOFTask:
            task.weight = self.DEFAULT_WEIGHTS['DOF']
        elif type(task) is PoseTask:
            task.weight = self.DEFAULT_WEIGHTS['POSE']
        else:  # task type is not accounted for
            raise Exception("no weight provided for task '%s'" % task.name)

    def add(self, task):
        """
        Add a new task to the IK solver.

        Parameters
        ----------
        task : Task
            New task to add to the list.
        """
        if task.name in self.tasks:
            raise Exception("Task '%s' already present in IK" % task.name)
        if task.gain is None:
            self.__fill_gain(task)
        if task.weight is None:
            self.__fill_weight(task)
        with self.__lock:
            self.tasks[task.name] = task

    def print_costs(self, qd, dt):
        """
        Print task costs for the current IK step.

        Parameters
        ----------
        qd : array
            Robot DOF velocities.
        dt : scalar
            Timestep for the IK.
        """
        print("\n                TASK      COST")
        print("------------------------------")
        for task in self.tasks.itervalues():
            J = task.jacobian()
            r = task.residual(dt)
            print("%20s  %.2e" % (task.name, norm(dot(J, qd) - r)))
        print("")

    def remove(self, ident):
        """
        Remove a task.

        Parameters
        ----------
        ident : string or object
            Name or object with a ``name`` field identifying the task.
        """
        name = ident if type(ident) is str else ident.name
        with self.__lock:
            if name not in self.tasks:
                return
            del self.tasks[name]

    def compute_cost(self, dt):
        """
        Compute the IK cost of the present system state for a time step of dt.

        Parameters
        ----------
        dt : scalar
            Time step in [s].
        """
        return sum(task.cost(dt) for task in self.tasks.itervalues())

    def build_qp_matrices(self, dt):
        """
        Build matrices of the quatratic program.

        Parameters
        ----------
        dt : scalar
            Time step in [s].

        Returns
        -------
        P : (n, n) array
            Positive semi-definite cost matrix.
        q : array
            Cost vector.
        qd_max : array
            Maximum joint velocity vector.
        qd_min : array
            Minimum joint velocity vector.

        Notes
        -----
        When the robot model has joint acceleration limits, special care should
        be taken when computing the corresponding velocity bounds for the IK.
        In short, the robot now needs to avoid the velocity range where it (1)
        is not going to collide with a DOF limit in one iteration but (2)
        cannot brake fast enough to avoid a collision in the future due to
        acceleration limits. This function implements the solution to this
        problem described in Equation (14) of [Flacco15]_.
        """
        n = self.nb_active_dofs
        P = zeros((n, n))
        v = zeros(n)
        with self.__lock:
            for task in self.tasks.itervalues():
                J = task.jacobian()[:, self.active_dofs]
                r = task.residual(dt)
                mu = self.lm_damping * max(1e-3, dot(r, r))
                P += task.weight * (dot(J.T, J) + mu * eye(n))
                v += task.weight * dot(-r.T, J)
        q = self.robot.q[self.active_dofs]
        qd_max_doflim = (self.q_max - q) / dt
        qd_min_doflim = (self.q_min - q) / dt
        qd_max = minimum(+self.qd_lim, self.doflim_gain * qd_max_doflim)
        qd_min = maximum(-self.qd_lim, self.doflim_gain * qd_min_doflim)
        if self.qdd_lim is not None:  # straightforward acceleration bounds
            qd = self.robot.qd[self.active_dofs]
            qd_max_acc = qd + self.qdd_lim * dt
            qd_min_acc = qd - self.qdd_lim * dt
            qd_max = minimum(qd_max, qd_max_acc)
            qd_min = maximum(qd_min, qd_min_acc)
        if self.qdd_lim is not None:  # DOF-limit acceleration bounds
            Delta_q_max = maximum(self.q_max - q, 1e-32)
            Delta_q_min = maximum(q - self.q_min, 1e-32)
            qd_max_doflim_acc = +sqrt(2 * self.qdd_lim * Delta_q_max)
            qd_min_doflim_acc = -sqrt(2 * self.qdd_lim * Delta_q_min)
            qd_max = minimum(qd_max, self.doflim_gain * qd_max_doflim_acc)
            qd_min = maximum(qd_min, self.doflim_gain * qd_min_doflim_acc)
        return (P, v, qd_max, qd_min)

    def compute_velocity(self, dt):
        """
        Compute a new velocity satisfying all tasks at best.

        Parameters
        ----------
        dt : scalar
            Time step in [s].

        Returns
        -------
        qd : array
            Vector of active joint velocities.

        Note
        ----
        This QP formulation is the default for
        :func:`pymanoid.ik.IKSolver.solve` (posture generation) as it converges
        faster.

        Notes
        -----
        The method implemented in this function is reasonably fast but may
        become unstable when some tasks are widely infeasible. In such
        situations, you can either increase the Levenberg-Marquardt bias
        ``self.lm_damping`` or set ``slack_dof_limits=True`` which will call
        :func:`pymanoid.ik.IKSolver.compute_velocity_with_slack`.

        The returned velocity minimizes squared residuals as in the weighted
        cost function, which corresponds to the Gauss-Newton algorithm. Indeed,
        expanding the square expression in ``cost(task, qd)`` yields

        .. math::

            \\mathrm{minimize} \\ \\dot{q} J^T J \\dot{q} - 2 r^T J \\dot{q}

        Differentiating with respect to :math:`\\dot{q}` shows that the minimum
        is attained for :math:`J^T J \\dot{q} = r`, where we recognize the
        Gauss-Newton update rule.
        """
        n = self.nb_active_dofs
        P, v, qd_max, qd_min = self.build_qp_matrices(dt)
        G = vstack([+eye(n), -eye(n)])
        h = hstack([qd_max, -qd_min])
        try:
            x = solve_qp(P, v, G, h)
            self.qd[self.active_dofs] = x
        except ValueError as e:
            if "matrix G is not positive definite" in e:
                raise Exception(RANK_DEFICIENCY_MSG)
            raise
        return self.qd

    def compute_velocity_with_slack(self, dt):
        """
        Compute a new velocity satisfying all tasks at best, while trying to
        stay away from kinematic constraints.

        Parameters
        ----------
        dt : scalar
            Time step in [s].

        Returns
        -------
        qd : array
            Vector of active joint velocities.

        Note
        ----
        This QP formulation is the default for
        :func:`pymanoid.ik.IKSolver.step` as it has a more numerically-stable
        behavior.

        Notes
        -----
        Check out the discussion of this method around Equation (10) of
        [Nozawa16]_. DOF limits are better taken care of by slack variables,
        but the variable count doubles and the QP takes roughly 50% more time
        to solve.
        """
        n = self.nb_active_dofs
        E, Z = eye(n), zeros((n, n))
        P0, v0, qd_max, qd_min = self.build_qp_matrices(dt)
        P = vstack([hstack([P0, Z]), hstack([Z, self.slack_regularize * E])])
        v = hstack([v0, -self.slack_maximize * ones(n)])
        G = vstack([
            hstack([+E, +E / dt]), hstack([-E, +E / dt]), hstack([Z, -E])])
        h = hstack([qd_max, -qd_min, zeros(n)])
        try:
            x = solve_qp(P, v, G, h)
            self.qd[self.active_dofs] = x[:n]
        except ValueError as e:
            if "matrix G is not positive definite" in e:
                raise Exception(RANK_DEFICIENCY_MSG)
            raise
        return self.qd

    def step(self, dt):
        """
        Apply velocities computed by inverse kinematics.

        Parameters
        ----------
        dt : scalar
            Time step in [s].
        """
        q = self.robot.q
        if self.slack_dof_limits:
            qd = self.compute_velocity_with_slack(dt)
        else:  # default QP formulation
            qd = self.compute_velocity(dt)
        if self.verbosity >= 2:
            self.print_costs(qd, dt)
        self.robot.set_dof_values(q + qd * dt, clamp=True)
        self.robot.set_dof_velocities(qd)

    def solve(self, max_it=1000, cost_stop=1e-10, impr_stop=1e-5, dt=1e-2,
              warm_start=False, debug=False):
        """
        Compute joint-angles that satisfy all kinematic constraints at best.

        Parameters
        ----------
        max_it : integer
            Maximum number of solver iterations.
        cost_stop : scalar
            Stop when cost value is below this threshold.
        impr_stop : scalar, optional
            Stop when cost improvement (relative variation from one iteration
            to the next) is less than this threshold.
        dt : scalar, optional
            Time step in [s].
        warm_start : bool, optional
            Set to True if the current robot posture is a good guess for IK.
            Otherwise, the solver will start by an exploration phase with DOF
            velocity limits relaxed and no Levenberg-Marquardt damping.
        debug : bool, optional
            Set to True for additional debug messages.

        Returns
        -------
        nb_it : int
            Number of solver iterations.
        cost : scalar
            Final value of the cost function.

        Notes
        -----
        Good values of `dt` depend on the weights of the IK tasks. Small values
        make convergence slower, while big values make the optimization
        unstable (in which case there may be no convergence at all).
        """
        cost = 100000.
        init_lm_damping = self.lm_damping
        init_slack_dof_limits = self.slack_dof_limits
        exploration_phase = not warm_start
        if exploration_phase:
            self.lm_damping = 0
            self.slack_dof_limits = False
            self.qd_lim = 10. * self.robot.qd_lim[self.active_dofs]
            self.qdd_lim = None
        for itnum in range(max_it):
            prev_cost = cost
            cost = self.compute_cost(dt)
            impr = abs(cost - prev_cost) / prev_cost
            if debug or self.verbosity >= 1:
                print("%2d: %.3e (impr: %+.2e)" % (itnum, cost, impr))
            if abs(cost) < cost_stop or impr < impr_stop:
                break
            if exploration_phase and (itnum >= max_it / 2 or impr < 1e-2):
                exploration_phase = False
                self.lm_damping = init_lm_damping
                self.slack_dof_limits = init_slack_dof_limits
                self.qd_lim = self.robot.qd_lim[self.active_dofs]
            self.step(dt)
        self.lm_damping = init_lm_damping
        self.slack_dof_limits = init_slack_dof_limits
        self.__reset_dof_limits()
        self.robot.set_dof_velocities(zeros(self.robot.qd.shape))
        return 1 + itnum, cost

    def on_tick(self, sim):
        """
        Step the IK at each simulation tick.

        Parameters
        ----------
        sim : Simulation
            Simulation instance.
        """
        self.step(sim.dt)


Question: from numpy import array, cross, dot, eye, hstack, vstack, zeros
from numpy import concatenate, maximum, minimum
from os.path import basename, isfile, splitext
from warnings import warn
from .body import Body, PointMass
from .gui import draw_line, draw_point
from .ik import IKSolver
from .misc import matplotlib_to_rgb, middot
from .sim import get_openrave_env, gravity
from .transformations import crossmat, quat_from_rpy, rpy_from_quat
from .transformations import rotation_matrix_from_rpy
        com : PointMass
            Center of mass of the robot.
        """
        return PointMass(pos=self.com, mass=self.mass)

    def show_com(self):
        """Show a red ball at the location of the center of mass."""
        self.__show_com = True
        self.__com_handle = [
            draw_point(self.com, pointsize=0.0005 * self.mass, color='r'),
            draw_line(
                self.com, self.com + [0., 0., -1.], linewidth=2., color='y')]

    def hide_com(self):
        """Hide center of mass."""
        self.__show_com = False
        self.__com_handle = None

    """
    Angular Momentum
    ================
    """

    def compute_angular_momentum(self, p):
        """
        Compute the angular momentum with respect to point `p`.

        Parameters
        ----------
        p : array, shape=(3,)
            Application point `p` in world coordinates.

        Returns
        -------
        am : array, shape=(3,)
            Angular momentum of the robot at `p`.
        """
        am = zeros(3)
        for link in self.rave.GetLinks():
            T = link.GetTransform()
            m = link.GetMass()
            v = link.GetVelocity()
            c = link.GetGlobalCOM()
            R, r = T[0:3, 0:3], T[0:3, 3]
            I = dot(R, dot(link.GetLocalInertia(), R.T))
            rd, omega = v[:3], v[3:]
            cd = rd + cross(r - c, omega)
            am += cross(c - p, m * cd) + dot(I, omega)
        return am

    def compute_angular_momentum_jacobian(self, p):
        """
        Compute the Jacobian matrix J(q) such that the angular momentum of the
        robot at `P` is given by:

        .. math::

            L_P(q, \\dot{q}) = J(q) \\dot{q}

        Parameters
        ----------
        p : array, shape=(3,)
            Application point `P` in world coordinates.

        Returns
        -------
        J_am : array, shape=(3,)
            Jacobian giving the angular momentum of the robot at `P`.
        """
        J_am = zeros((3, self.nb_dofs))
        for link in self.rave.GetLinks():
            m = link.GetMass()
            i = link.GetIndex()
            c = link.GetGlobalCOM()
            R = link.GetTransform()[0:3, 0:3]
            I = dot(R, dot(link.GetLocalInertia(), R.T))
            J_trans = self.rave.ComputeJacobianTranslation(i, c)
            J_rot = self.rave.ComputeJacobianAxisAngle(i)
            J_am += dot(crossmat(c - p), m * J_trans) + dot(I, J_rot)
        return J_am

    def compute_angular_momentum_hessian(self, p):
        """
        Returns the Hessian tensor :math:`H(q)` such that the rate of change of
        the angular momentum with respect to point `P` is

        .. math::

            \\dot{L}_P(q, \\dot{q}) = J(q) \\ddot{q} + \\dot{q}^T H(q) \\dot{q}

        where :math:`J(q)` is the angular-momentum jacobian.

        Parameters
        ----------
        p : array, shape=(3,)
            Application point in world coordinates.

        Returns
        -------
        H : array, shape=(3, 3, 3)
            Hessian tensor of the angular momentum at the application point.
        """
        def crosstens(M):
            assert M.shape[0] == 3
            Z = zeros(M.shape[1])
            T = array([[Z, -M[2, :], M[1, :]],
                       [M[2, :], Z, -M[0, :]],
                       [-M[1, :], M[0, :], Z]])
            return T.transpose([2, 0, 1])  # T.shape == (M.shape[1], 3, 3)

        H = zeros((self.nb_dofs, 3, self.nb_dofs))
        for link in self.rave.GetLinks():
            m = link.GetMass()
            i = link.GetIndex()
            c = link.GetGlobalCOM()
            R = link.GetTransform()[0:3, 0:3]
            J_rot = self.rave.ComputeJacobianAxisAngle(i)
            H_trans = self.rave.ComputeHessianTranslation(i, c)
            H_rot = self.rave.ComputeHessianAxisAngle(i)
            I = dot(R, dot(link.GetLocalInertia(), R.T))

Answer:             H += middot(crossmat(c - p), m * H_trans) \