bindings/python/pinocchio/explog.py

Go to the documentation of this file.

#
# Copyright (c) 2015-2018 CNRS INRIA
# Copyright (c) 2015 Wandercraft, 86 rue de Paris 91400 Orsay, France.
#
 
import math
 
import numpy as np
 
from . import pinocchio_pywrap_default as pin
 
 
def exp(x):
    if isinstance(x, pin.Motion):
        return pin.exp6(x)
    if np.isscalar(x):
        return math.exp(x)
    if isinstance(x, np.ndarray):
        if x.shape == (6, 1) or x.shape == (6,):
            return pin.exp6(pin.Motion(x))
        if x.shape == (3, 1) or x.shape == (3,):
            return pin.exp3(x)
        raise ValueError("Error only 3 and 6 vectors are allowed.")
    raise ValueError(
        "Error exp is only defined for real, vector3, vector6 and pin.Motion objects."
    )
 
 
def log(x):
    if isinstance(x, pin.SE3):
        return pin.log6(x)
    if np.isscalar(x):
        return math.log(x)
    if isinstance(x, np.ndarray):
        if x.shape == (4, 4):
            return pin.log6(x)
        if x.shape == (3, 3):
            return pin.log3(x)
        raise ValueError("Error only 3 and 4 matrices are allowed.")
    raise ValueError(
        "Error log is only defined for real, matrix3, matrix4 and pin.SE3 objects."
    )
 
 
__all__ = ["exp", "log"]