Esempio n. 1
0
def rotation_matrix_from_rpy(roll, pitch, yaw):
    """
    Conversion of roll, pitch, yaw to 4x4 rotation matrix according to:
    https://github.com/orocos/orocos_kinematics_dynamics/blob/master/orocos_kdl/src/frames.cpp#L167
    :type roll: Union[float, Symbol]
    :type pitch: Union[float, Symbol]
    :type yaw: Union[float, Symbol]
    :return: 4x4 Matrix
    :rtype: Matrix
    """
    # TODO don't split this into 3 matrices

    rx = se.Matrix([[1, 0, 0, 0],
                    [0, se.cos(roll), -se.sin(roll), 0],
                    [0, se.sin(roll), se.cos(roll), 0],
                    [0, 0, 0, 1]])
    ry = se.Matrix([[se.cos(pitch), 0, se.sin(pitch), 0],
                    [0, 1, 0, 0],
                    [-se.sin(pitch), 0, se.cos(pitch), 0],
                    [0, 0, 0, 1]])
    rz = se.Matrix([[se.cos(yaw), -se.sin(yaw), 0, 0],
                    [se.sin(yaw), se.cos(yaw), 0, 0],
                    [0, 0, 1, 0],
                    [0, 0, 0, 1]])
    return (rz * ry * rx)
    def __init__(self, settings, args, times_to_use):
        par = Params()
        self.times_to_use = times_to_use
        self.starting_parameters = [2.26E-04, 0.0699, 3.45E-05, 0.05462, 0.0873, 8.92E-03, 5.150E-3, 0.03158, 0.1524]
        # Create symbols for symbolic functions
        p, y, v = CreateSymbols(settings)

        # Choose starting parameters (from J Physiol paper)
        para = [2.26E-04, 0.0699, 3.45E-05, 0.05462, 0.0873, 8.92E-03, 5.150E-3, 0.03158, 0.1524]

        # Create symbols for symbolic functions
        p, y, v = CreateSymbols(par)

        # Define system equations and initial conditions
        k1 = p[0] * se.exp(p[1] * v)
        k2 = p[2] * se.exp(-p[3] * v)
        k3 = p[4] * se.exp(p[5] * v)
        k4 = p[6] * se.exp(-p[7] * v)

        # Write in matrix form taking y = ([C], [O], [I])^T

        A = se.Matrix([[-k1 - k3 - k4, k2 -  k4, -k4], [k1, -k2 - k3, k4], [-k1, k3 - k1, -k2 - k4 - k1]])
        B = se.Matrix([k4, 0, k1])

        rhs = np.array(A * y + B)

        self.funcs = GetSensitivityEquations(par, p, y, v, A, B, para, times_to_use, sine_wave=args.sine_wave)
def test_sens_limits():
    par = Params()
    p, y, v = CreateSymbols(par)
    reversal_potential = par.Erev
    para = np.array([
        2.07, 7.17E1, 3.44E-2, 6.18E1, 4.18E2, 2.58E1, 4.75E1, 2.51E1, 3.33E1
    ])
    para = para * 1E-3

    # Define system equations and initial conditions
    k1 = p[0] * se.exp(p[1] * v)
    k2 = p[2] * se.exp(-p[3] * v)
    k3 = p[4] * se.exp(p[5] * v)
    k4 = p[6] * se.exp(-p[7] * v)

    current_limit = (p[-1] * (par.holding_potential - reversal_potential) *
                     k1 / (k1 + k2) * k4 / (k3 + k4)).subs(
                         v, par.holding_potential)
    print("{} Current limit computed as {}".format(
        __file__,
        current_limit.subs(p, para).evalf()))

    sens_inf = [
        float(se.diff(current_limit, p[j]).subs(p, para).evalf())
        for j in range(0, par.n_params)
    ]
    print("{} sens_inf calculated as {}".format(__file__, sens_inf))

    k = se.symbols('k1, k2, k3, k4')

    # Notation is consistent between the two papers
    A = se.Matrix([[-k1 - k3 - k4, k2 - k4, -k4], [k1, -k2 - k3, k4],
                   [-k1, k3 - k1, -k2 - k4 - k1]])
    B = se.Matrix([k4, 0, k1])

    # Use results from HH equations
    current_limit = (p[-1] * (par.holding_potential - reversal_potential) *
                     k1 / (k1 + k2) * k4 / (k3 + k4)).subs(
                         v, par.holding_potential)

    funcs = GetSensitivityEquations(par, p, y, v, A, B, para, [0])
    sens_inf = [
        float(se.diff(current_limit, p[j]).subs(p, para).evalf())
        for j in range(0, par.n_params)
    ]

    sens = funcs.SimulateForwardModelSensitivities(para)[1]

    # Check sens = sens_inf
    error = np.abs(sens_inf - sens)

    equal = np.all(error < 1e-10)
    assert (equal)
    return
Esempio n. 4
0
def rotation3_rpy(roll, pitch, yaw):
    """ Conversion of roll, pitch, yaw to 4x4 rotation matrix according to:
        https://github.com/orocos/orocos_kinematics_dynamics/blob/master/orocos_kdl/src/frames.cpp#L167
    """
    rx = sp.Matrix([[1, 0, 0, 0], [0, sp.cos(roll), -sp.sin(roll), 0],
                    [0, sp.sin(roll), sp.cos(roll), 0], [0, 0, 0, 1]])
    ry = sp.Matrix([[sp.cos(pitch), 0, sp.sin(pitch), 0], [0, 1, 0, 0],
                    [-sp.sin(pitch), 0, sp.cos(pitch), 0], [0, 0, 0, 1]])
    rz = sp.Matrix([[sp.cos(yaw), -sp.sin(yaw), 0, 0],
                    [sp.sin(yaw), sp.cos(yaw), 0, 0], [0, 0, 1, 0],
                    [0, 0, 0, 1]])
    return (rz * ry * rx)
Esempio n. 5
0
    def __init__(self, settings, args, times_to_use):
        par = Params()
        self.times_to_use = times_to_use
        self.starting_parameters = [
            2.26E-04, 0.0699, 3.45E-05, 0.05462, 0.0873, 8.92E-03, 5.150E-3,
            0.03158, 0.1524
        ]
        # Create symbols for symbolic functions
        p, y, v = CreateSymbols(settings)

        # Choose starting parameters (from J Physiol paper)
        para = [
            2.26E-04, 0.0699, 3.45E-05, 0.05462, 0.0873, 8.92E-03, 5.150E-3,
            0.03158, 0.1524
        ]

        # Define system equations and initial conditions
        k1 = p[0] * se.exp(p[1] * v)
        k2 = p[2] * se.exp(-p[3] * v)
        k3 = p[4] * se.exp(p[5] * v)
        k4 = p[6] * se.exp(-p[7] * v)

        # Write in matrix form taking y = ([C], [O], [I])^T

        A = se.Matrix([[-k1 - k3 - k4, k2 - k4, -k4], [k1, -k2 - k3, k4],
                       [-k1, k3 - k1, -k2 - k4 - k1]])
        B = se.Matrix([k4, 0, k1])

        rhs = np.array(A * y + B)
        protocol = pd.read_csv(
            os.path.join(
                os.path.dirname(os.path.dirname(os.path.realpath(__file__))),
                "protocols", "protocol-staircaseramp.csv"))
        times = 10000 * protocol["time"].values
        voltages = protocol["voltage"].values

        staircase_protocol = scipy.interpolate.interp1d(times,
                                                        voltages,
                                                        kind="linear")
        staircase_protocol_safe = lambda t: staircase_protocol(t) if t < times[
            -1] else par.holding_potential

        self.funcs = GetSensitivityEquations(par,
                                             p,
                                             y,
                                             v,
                                             A,
                                             B,
                                             para,
                                             times_to_use,
                                             voltage=staircase_protocol_safe)
def CreateSymbols(par):
    """
    Create SymEngine symbols to contain the parameters, state variables and the voltage.
    These are used to generate functions for the right hand side and Jacobian

    """

    # Create parameter symbols
    p = se.Matrix([se.symbols('p%d' % j) for j in range(par.n_params)])
    # Create state variable symbols
    y = se.Matrix([se.symbols('y%d' % i) for i in range(par.n_state_vars)])
    # Create voltage symbol
    v = se.symbols('v')
    return p, y, v
Esempio n. 7
0
    def subs(self, d):
        """Substitute expressions

        Parameters
        ----------
        d : dict
            Dictionary of from: to pairs for substitution

        Examples
        --------
        >>> import sympy
        >>> from pharmpy import RandomVariable, Parameter
        >>> omega = Parameter("OMEGA_CL", 0.1)
        >>> rv = RandomVariable.normal("IIV_CL", "IIV", 0, omega.symbol)
        >>> rv.subs({omega.symbol: sympy.Symbol("OMEGA_NEW")})
        >>> rv
        IIV_CL ~ 𝒩 (0, OMEGA_NEW)

        """
        if self._mean is not None:
            self._mean = self._mean.subs(d)
            self._variance = self._variance.subs(d)
            self._symengine_variance = symengine.Matrix(
                self._variance.rows, self._variance.cols, self._variance)
        if self._sympy_rv is not None:
            self._sympy_rv = self._sympy_rv.subs(d)
Esempio n. 8
0
def quaternion_from_rpy(roll, pitch, yaw):
    """
    :type roll: Union[float, Symbol]
    :type pitch: Union[float, Symbol]
    :type yaw: Union[float, Symbol]
    :return: 4x1 Matrix
    :type: Matrix
    """
    roll_half = roll / 2.0
    pitch_half = pitch / 2.0
    yaw_half = yaw / 2.0

    c_roll = se.cos(roll_half)
    s_roll = se.sin(roll_half)
    c_pitch = se.cos(pitch_half)
    s_pitch = se.sin(pitch_half)
    c_yaw = se.cos(yaw_half)
    s_yaw = se.sin(yaw_half)

    cc = c_roll * c_yaw
    cs = c_roll * s_yaw
    sc = s_roll * c_yaw
    ss = s_roll * s_yaw

    x = c_pitch * sc - s_pitch * cs
    y = c_pitch * ss + s_pitch * cc
    z = c_pitch * cs - s_pitch * sc
    w = c_pitch * cc + s_pitch * ss

    return se.Matrix([x, y, z, w])
Esempio n. 9
0
def rotation_matrix_from_axis_angle(axis, angle):
    """
    Conversion of unit axis and angle to 4x4 rotation matrix according to:
    http://www.euclideanspace.com/maths/geometry/rotations/conversions/angleToMatrix/index.htm
    :param axis: 3x1 Matrix
    :type axis: Matrix
    :type angle: Union[float, Symbol]
    :return: 4x4 Matrix
    :rtype: Matrix
    """
    ct = se.cos(angle)
    st = se.sin(angle)
    vt = 1 - ct
    m_vt_0 = vt * axis[0]
    m_vt_1 = vt * axis[1]
    m_vt_2 = vt * axis[2]
    m_st_0 = axis[0] * st
    m_st_1 = axis[1] * st
    m_st_2 = axis[2] * st
    m_vt_0_1 = m_vt_0 * axis[1]
    m_vt_0_2 = m_vt_0 * axis[2]
    m_vt_1_2 = m_vt_1 * axis[2]
    return se.Matrix(
        [[ct + m_vt_0 * axis[0], -m_st_2 + m_vt_0_1, m_st_1 + m_vt_0_2, 0],
         [m_st_2 + m_vt_0_1, ct + m_vt_1 * axis[1], -m_st_0 + m_vt_1_2, 0],
         [-m_st_1 + m_vt_0_2, m_st_0 + m_vt_1_2, ct + m_vt_2 * axis[2], 0],
         [0, 0, 0, 1]])
Esempio n. 10
0
def quatrotateabout(q1, q2):
    return sp.Matrix([
        (q1[0] * q2[0]) - (q1[1] * q2[1]) - (q1[2] * q2[2]) - (q1[3] * q2[3]),
        (q1[0] * q2[1]) + (q1[1] * q2[0]) + (q1[2] * q2[3]) - (q1[3] * q2[2]),
        (q1[0] * q2[2]) - (q1[1] * q2[3]) + (q1[2] * q2[0]) + (q1[3] * q2[1]),
        (q1[0] * q2[3]) + (q1[1] * q2[2]) - (q1[2] * q2[1]) + (q1[3] * q2[0])
    ])
Esempio n. 11
0
def json_decoder(dct):
    if '__type__' in dct:
        t = dct['__type__']
        if t == json_sym_matrix:
            return sp.Matrix(dct['data'])  # nested_list_to_sym(dct['data'])
        elif t == json_sym_expr:
            try:
                return sp.sympify(dct['data'].encode('utf-8')) if isinstance(
                    dct['data'], unicode) else sp.sympify(dct['data'])
            except NameError as e:
                raise Exception(
                    'NameError Occured while trying to sympify string {}. Error: {}\n{}'
                    .format(repr(dct['data']), str(e), traceback.format_exc()))
        elif t[:8] == '<class \'' and t[-2:] == '\'>':
            t = t[8:-2]
            if t not in class_registry:
                class_registry[t] = import_class(t)
            t = class_registry[t]
            if not issubclass(t, JSONSerializable):
                raise Exception(
                    'Can not decode JSON object tagged with type "{}". The type is not a subtype of JSONSerializable'
                    .format(t))

            try:
                if hasattr(t, 'json_factory'):
                    return t.json_factory(
                        **{k: v
                           for k, v in dct.items() if k != '__type__'})
                return t(**{k: v for k, v in dct.items() if k != '__type__'})
            except TypeError as e:
                raise TypeError(
                    'Error occurred while instantiating an object of type "{}":\n  {}'
                    .format(t.__name__, e))
    return dct
Esempio n. 12
0
def axisanglerotationmatrix(axis_angle):
    R = axisanglemagnitude(axis_angle)

    x = Piecewise((axis_angle[0] / R, R > 0), (1, True))
    y = Piecewise((axis_angle[1] / R, R > 0), (0, True))
    z = Piecewise((axis_angle[2] / R, R > 0), (0, True))

    csr = sp.cos(R)
    one_minus_csr = (1 - csr)
    snr = sp.sin(R)

    return sp.Matrix([[
        csr + x * x * (1 - csr), x * y * one_minus_csr - z * snr,
        x * z * one_minus_csr + y * snr
    ],
                      [
                          y * x * one_minus_csr + z * snr,
                          csr + y * y * one_minus_csr,
                          y * z * one_minus_csr - x * snr
                      ],
                      [
                          z * x * one_minus_csr - y * snr,
                          z * y * one_minus_csr + x * snr,
                          csr + z * z * one_minus_csr
                      ]])
Esempio n. 13
0
def axisanglerotationmatrix(axis_angle):
    R = axisanglemagnitude(axis_angle)

    x = axis_angle[0] / R
    y = axis_angle[1] / R
    z = axis_angle[2] / R

    csr = sp.cos(R)
    one_minus_csr = (1 - csr)
    snr = sp.sin(R)

    return sp.Matrix([[
        csr + x * x * (1 - csr), x * y * one_minus_csr - z * snr,
        x * z * one_minus_csr + y * snr
    ],
                      [
                          y * x * one_minus_csr + z * snr,
                          csr + y * y * one_minus_csr,
                          y * z * one_minus_csr - x * snr
                      ],
                      [
                          z * x * one_minus_csr - y * snr,
                          z * y * one_minus_csr + x * snr,
                          csr + z * z * one_minus_csr
                      ]])
Esempio n. 14
0
def diffable_slerp(q1, q2, t):
    """
    !takes a long time to compile!
    :param q1: 4x1 Matrix
    :type q1: Matrix
    :param q2: 4x1 Matrix
    :type q2: Matrix
    :param t: float, 0-1
    :type t:  Union[float, Symbol]
    :return: 4x1 Matrix; Return spherical linear interpolation between two quaternions.
    :rtype: Matrix
    """
    cos_half_theta = dot(q1, q2)

    if0 = -cos_half_theta
    q2 = diffable_if_greater_zero(if0, -q2, q2)
    cos_half_theta = diffable_if_greater_zero(if0, -cos_half_theta, cos_half_theta)

    if1 = diffable_abs(cos_half_theta) - 1.0

    # enforce acos(x) with -1 < x < 1
    cos_half_theta = diffable_min_fast(1, cos_half_theta)
    cos_half_theta = diffable_max_fast(-1, cos_half_theta)

    half_theta = acos(cos_half_theta)

    sin_half_theta = sqrt(1.0 - cos_half_theta * cos_half_theta)
    # prevent /0
    sin_half_theta = diffable_if_eq_zero(sin_half_theta, 1, sin_half_theta)
    if2 = 0.001 - diffable_abs(sin_half_theta)

    ratio_a = sin((1.0 - t) * half_theta) / sin_half_theta
    ratio_b = sin(t * half_theta) / sin_half_theta
    return diffable_if_greater_eq_zero(if1, se.Matrix(q1),
                                       diffable_if_greater_zero(if2, 0.5 * q1 + 0.5 * q2, ratio_a * q1 + ratio_b * q2))
Esempio n. 15
0
def axis_angle_from_matrix(rotation_matrix):
    """
    :param rotation_matrix: 4x4 or 3x3 Matrix
    :type rotation_matrix: Matrix
    :return: 3x1 Matrix, angle
    :rtype: (Matrix, Union[float, Symbol])
    """
    rm = rotation_matrix
    angle = (trace(rm[:3, :3]) - 1) / 2
    angle = se.Min(angle, 1)
    angle = se.Max(angle, -1)
    angle = se.acos(angle)
    x = (rm[2, 1] - rm[1, 2])
    y = (rm[0, 2] - rm[2, 0])
    z = (rm[1, 0] - rm[0, 1])
    n = se.sqrt(x * x + y * y + z * z)
    m = if_eq_zero(n, 1, n)
    axis = se.Matrix([
        if_eq_zero(n, 0, x / m),
        if_eq_zero(n, 0, y / m),
        if_eq_zero(n, 1, z / m)
    ])
    sign = diffable_sign(angle)
    axis *= sign
    angle = sign * angle
    return axis, angle
Esempio n. 16
0
def rotation_of(frame):
    """
    :param frame: 4x4 Matrix
    :type frame: Matrix
    :return: 4x4 Matrix; sets the translation part of a frame to 0
    :rtype: Matrix
    """
    return frame[:4, :3].row_join(se.Matrix([0, 0, 0, 1]))
Esempio n. 17
0
def translation_of(frame):
    """
    :param frame: 4x4 Matrix
    :type frame: Matrix
    :return: 4x4 Matrix; sets the rotation part of a frame to identity
    :rtype: Matrix
    """
    return se.eye(3).col_join(se.Matrix([[0] * 3])).row_join(frame[:4, 3:])
Esempio n. 18
0
def vector3(x, y, z):
    """
    :param x: Union[float, Symbol]
    :param y: Union[float, Symbol]
    :param z: Union[float, Symbol]
    :rtype: Matrix
    """
    return se.Matrix([x, y, z, 0])
Esempio n. 19
0
def quaternion_conjugate(quaternion):
    """
    :param quaternion: 4x1 Matrix
    :type quaternion: Matrix
    :return: 4x1 Matrix
    :rtype: Matrix
    """
    return se.Matrix([-quaternion[0], -quaternion[1], -quaternion[2], quaternion[3]])
Esempio n. 20
0
def quatrotationmatrix(q):
    qw, qi, qj, qk = q
    s = quatmagnitude(q)
    return sp.Matrix(
        [[1 - 2 * s * (qj * qj + qk * qk), 2 * s * (qi * qj - qk * qw), 2 * s * (qi * qk + qj * qw)],
         [2 * s * (qi * qj + qk * qw), 1 - 2 * s * (qi * qi + qk * qk), 2 * s * (qj * qk - qi * qw)],
         [2 * s * (qi * qk - qj * qw), 2 * s * (qj * qk + qi * qw), 1 - 2 * s * (qi * qi + qj * qj)]
         ])
Esempio n. 21
0
def point3(x, y, z):
    """
    :param x: Union[float, Symbol]
    :param y: Union[float, Symbol]
    :param z: Union[float, Symbol]
    :rtype: Matrix
    """
    return se.Matrix([x, y, z, 1])
Esempio n. 22
0
def axisanglecompose(axis_angle, axis_angle2):
    a = axisanglemagnitude(axis_angle)
    ah = axisanglenormalize(axis_angle)

    b = axisanglemagnitude(axis_angle2)
    bh = axisanglenormalize(axis_angle2)

    sina = sin(a / 2)
    asina = [ah[0] * sina, ah[1] * sina, ah[2] * sina]

    sinb = sin(b / 2)
    bsinb = [bh[0] * sinb, bh[1] * sinb, bh[2] * sinb]

    c = 2 * acos(cos(a / 2)*cos(b / 2) - dot3d(asina, bsinb))
    d = axisanglenormalize(cos(a / 2) * sp.Matrix(bsinb) + cos(b / 2) * sp.Matrix(asina) + cross3d(asina, bsinb))
    return sp.Matrix([
            c * d[0],
            c * d[1],
            c * d[2]
        ])
Esempio n. 23
0
def cross(u, v):
    """
    :param u: 1d matrix
    :type u: Matrix
    :param v: 1d matrix
    :type v: Matrix
    :return: 1d Matrix. If u and v have length 4, it ignores the last entry and adds a zero to the result.
    :rtype: Matrix
    """
    if len(u) != len(v):
        raise ValueError('lengths {} and {} don\'t align'.format(len(u), len(v)))
    if len(u) == 3:
        return se.Matrix([u[1] * v[2] - u[2] * v[1],
                          u[2] * v[0] - u[0] * v[2],
                          u[0] * v[1] - u[1] * v[0]])
    if len(u) == 4:
        return se.Matrix([u[1] * v[2] - u[2] * v[1],
                          u[2] * v[0] - u[0] * v[2],
                          u[0] * v[1] - u[1] * v[0],
                          0])
Esempio n. 24
0
def rotation3_quaternion(x, y, z, w):
    """ Unit quaternion to 4x4 rotation matrix according to:
        https://github.com/orocos/orocos_kinematics_dynamics/blob/master/orocos_kdl/src/frames.cpp#L167
    """
    x2 = x * x
    y2 = y * y
    z2 = z * z
    w2 = w * w
    return sp.Matrix(
        [[w2 + x2 - y2 - z2, 2 * x * y - 2 * w * z, 2 * x * z + 2 * w * y, 0],
         [2 * x * y + 2 * w * z, w2 - x2 + y2 - z2, 2 * y * z - 2 * w * x, 0],
         [2 * x * z - 2 * w * y, 2 * y * z + 2 * w * x, w2 - x2 - y2 + z2, 0],
         [0, 0, 0, 1]])
Esempio n. 25
0
def imu_correct_up(mu, imu_rot, up_in_obj):
    rot = imu_rot[0:4]
    rotv = imu_rot[4:7]

    up = quatrotatevector(rot, up_in_obj)

    G = [ 0, 0, 1]
    N = cross(up, G)
    angle = atan2(sqrt(up[0] * up[0] + up[1] * up[1]), up[2])
    for i in range(3):
        N[i] = N[i] * angle * mu / 2.
    qc = axisangle2quat(N)
    return sp.Matrix([*quatrotateabout(qc, rot), *rotv])
Esempio n. 26
0
def quaternion_from_axis_angle(axis, angle):
    """
    :param axis: 3x1 Matrix
    :type axis: Matrix
    :type angle: Union[float, Symbol]
    :return: 4x1 Matrix
    :rtype: Matrix
    """
    half_angle = angle / 2
    return se.Matrix([axis[0] * se.sin(half_angle),
                      axis[1] * se.sin(half_angle),
                      axis[2] * se.sin(half_angle),
                      se.cos(half_angle)])
Esempio n. 27
0
def quaternion_multiply(q1, q2):
    """
    :param q1: 4x1 Matrix
    :type q1: Matrix
    :param q2: 4x1 Matrix
    :type q2: Matrix
    :return: 4x1 Matrix
    :rtype: Matrix
    """
    x0, y0, z0, w0 = q2
    x1, y1, z1, w1 = q1
    return se.Matrix([x1 * w0 + y1 * z0 - z1 * y0 + w1 * x0,
                      -x1 * z0 + y1 * w0 + z1 * x0 + w1 * y0,
                      x1 * y0 - y1 * x0 + z1 * w0 + w1 * z0,
                      -x1 * x0 - y1 * y0 - z1 * z0 + w1 * w0])
Esempio n. 28
0
def axis_angle_from_quaternion(x, y, z, w):
    """
    :type x: Union[float, Symbol]
    :type y: Union[float, Symbol]
    :type z: Union[float, Symbol]
    :type w: Union[float, Symbol]
    :return: 4x1 Matrix
    :rtype: Matrix
    """
    # TODO buggy, angle goes from 0 - 2*pi instead of -pi - +pi
    w2 = sp.sqrt(1 - w**2)
    angle = (2 * sp.acos(w))
    x = x / w2
    y = y / w2
    z = z / w2
    return sp.Matrix([x, y, z]), angle
Esempio n. 29
0
def axis_angle_from_quaternion(x, y, z, w):
    """
    :type x: Union[float, Symbol]
    :type y: Union[float, Symbol]
    :type z: Union[float, Symbol]
    :type w: Union[float, Symbol]
    :return: 4x1 Matrix
    :rtype: Matrix
    """
    l = norm([x, y, z, w])
    x, y, z, w = x / l, y / l, z / l, w / l
    w2 = se.sqrt(1 - w**2)
    angle = (2 * se.acos(se.Min(se.Max(-1, w), 1)))
    m = if_eq_zero(w2, 1, w2)  # avoid /0
    x = if_eq_zero(w2, 0, x / m)
    y = if_eq_zero(w2, 0, y / m)
    z = if_eq_zero(w2, 1, z / m)
    return se.Matrix([x, y, z]), angle
Esempio n. 30
0
    def joint_normal(cls, names, level, mu, sigma):
        """Create joint normally distributed random variables

        Parameters
        ----------
        names : list
            Names of the random variables
        level : str
            Variability level
        mu : matrix or list
            Vector of the means of the random variables
        sigma : matrix or list of lists
            Covariance matrix of the random variables

        Example
        -------
        >>> from pharmpy import RandomVariable, Parameter
        >>> omega_cl = Parameter("OMEGA_CL", 0.1)
        >>> omega_v = Parameter("OMEGA_V", 0.1)
        >>> corr_cl_v = Parameter("OMEGA_CL_V", 0.01)
        >>> rv1, rv2 = RandomVariable.joint_normal(["IIV_CL", "IIV_V"], 'IIV', [0, 0],
        ...     [[omega_cl.symbol, corr_cl_v.symbol], [corr_cl_v.symbol, omega_v.symbol]])
        >>> rv1
        ⎡IIV_CL⎤     ⎧⎡0⎤  ⎡ OMEGA_CL   OMEGA_CL_V⎤⎫
        ⎢      ⎥ ~ 𝒩 ⎪⎢ ⎥, ⎢                      ⎥⎪
        ⎣IIV_V ⎦     ⎩⎣0⎦  ⎣OMEGA_CL_V   OMEGA_V  ⎦⎭

        """

        mean = sympy.Matrix(mu)
        variance = sympy.Matrix(sigma)
        if variance.is_positive_semidefinite is False:
            raise ValueError('Sigma matrix is not positive semidefinite')
        rvs = []
        for name in names:
            rv = cls(name, level)
            rv._mean = mean.copy()
            rv._variance = variance.copy()
            rv._symengine_variance = symengine.Matrix(variance.rows,
                                                      variance.cols, sigma)
            rv._joint_names = names.copy()
            rvs.append(rv)
        return rvs