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
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)
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
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)
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])
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]])
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]) ])
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
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 ]])
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 ]])
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))
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
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]))
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:])
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])
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]])
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)] ])
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])
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] ])
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])
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]])
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])
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)])
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])
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
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
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