From 9534326ffb7654cc8098cee820e5cb35ff14e154 Mon Sep 17 00:00:00 2001 From: Wxyalons014 Date: Tue, 13 Oct 2020 14:55:25 +0200 Subject: [PATCH] add function to calculate equilibrium points --- symbtools/modeltools.py | 136 +++++++++---- symbtools/test/test_modeltools.py | 305 ++++++++++++++++++++---------- 2 files changed, 298 insertions(+), 143 deletions(-) diff --git a/symbtools/modeltools.py b/symbtools/modeltools.py index 57c7b5d..4654362 100644 --- a/symbtools/modeltools.py +++ b/symbtools/modeltools.py @@ -11,6 +11,7 @@ from scipy.optimize import fmin import symbtools as st from symbtools import lzip +from enum import Enum, auto # noinspection PyPep8Naming @@ -45,7 +46,7 @@ def point_velocity(point, coord_symbs, velo_symbs, t): v1_f = p1.diff(t) backsubs = lzip(coord_funcs, coord_symbs) + lzip(coord_funcs.diff(t), - velo_symbs) + velo_symbs) return v1_f.subs(backsubs) @@ -117,6 +118,7 @@ def __init__(self, src=None): self.f = None self.g = None self.tau = None + self.eqlbr = None self.x = None # deprecated @@ -213,8 +215,8 @@ def solve_for_acc(self, simplify=False): if simplify: d = d.simplify() adj.simplify() - Minv = adj/d - res = Minv*rhs + Minv = adj / d + res = Minv * rhs return res @@ -242,6 +244,66 @@ def calc_state_eq(self, simplify=True, force_recalculation=False): self.f.simplify() self.g.simplify() + def calc_eqlbr(self, parameter_values=None, ttheta_guess=None, etype='one_ep', display=False, **kwargs): + """In the simplest case(RT1 and 2) only one of the equilibrium points of + a system is used for linearization and other researches. Such a equilibrium + point is calculated by minimizing the target function for a certain + input variable. + In other case(NT) all of the equilibrium points of a system are needed, which can be calculated + by using Slover in Sympy to solve the differential equations for certain input values. + + :param: uu: certain input value defined by user + :param: parameter_values: unknown system parameters in list.(Default: all parameters are known) + :param: ttheta_guess: initial values for minimizing the target function.(Default: 0) + :param: etype: 'one_ep': one equilibrium point, 'all_ep': all of the equilibrium points. + :param: display: display all information of the result of the 'fmin' fucntion + :return: equilibrium point(s) in list + """ + + if parameter_values is None: + parameter_values = [] + + if kwargs.get('uu') is None: + # if the system doesn't have input + assert self.tau is None + uu = [] + all_vars = st.row_stack(self.tt) + uu_para = [] + + else: + uu = kwargs.get('uu') + all_vars = st.row_stack(self.tt, self.tau) + uu_para = list(zip(self.tau, uu)) + + class Type_equilibrium(Enum): + one_ep = auto() + all_ep = auto() + + if etype == Type_equilibrium.one_ep.name: + # set all of the derivatives of the system states to zero + state_eqns = self.eqns.subz0(self.ttd, self.ttdd) + + # target function for minimizing + state_eqns_func = st.expr_to_func(all_vars, state_eqns.subs(parameter_values)) + + if ttheta_guess is None: + ttheta_guess = st.to_np(self.tt * 0) + + def target(ttheta): + """target function for the certain global input values uu + """ + all_values = np.r_[ttheta, uu] # combine arrays + rhs = state_eqns_func(*all_values) + + return np.sum(rhs ** 2) + + self.eqlbr = fmin(target, ttheta_guess, disp=display) + + elif etype == Type_equilibrium.all_ep.name: + state_eqns = self.eqns.subz0(self.ttd, self.ttdd) + all_vars_value = uu_para + parameter_values + self.eqlbr = sp.solve(state_eqns.subs(all_vars_value), self.tt) + def calc_dae_eq(self, parameter_values=None): """ In case of present constraints ode representation is not possible. @@ -287,14 +349,14 @@ def calc_coll_part_lin_state_eq(self, simplify=True): if simplify: d = d.simplify() adj.simplify() - M11inv = adj/d + M11inv = adj / d # setting input and acceleration to 0 C1K1 = self.eqns[:np, :].subs(st.zip0(self.ttdd, self.tau)) # eq_passive = -M11inv*C1K1 - M11inv*M12*self.aa - self.ff = st.row_stack(self.ttd, -M11inv*C1K1, self.aa*0) - self.gg = st.row_stack(sp.zeros(np + nq, nq), -M11inv*M12, sp.eye(nq)) + self.ff = st.row_stack(self.ttd, -M11inv * C1K1, self.aa * 0) + self.gg = st.row_stack(sp.zeros(np + nq, nq), -M11inv * M12, sp.eye(nq)) if simplify: self.ff.simplify() @@ -310,7 +372,7 @@ def calc_lbi_nf_state_eq(self, simplify=False): n = len(self.tt) nq = len(self.tau) np = n - nq - nx = 2*n + nx = 2 * n # make sure that the system has the desired structure B = self.eqns.jacobian(self.tau) @@ -324,7 +386,7 @@ def calc_lbi_nf_state_eq(self, simplify=False): qq = self.tt[np:, :] uu = self.ttd[:np, :] vv = self.ttd[np:, :] - ww = st.symb_vector('w1:{0}'.format(np+1)) + ww = st.symb_vector('w1:{0}'.format(np + 1)) assert len(vv) == nq # state w.r.t normal form @@ -336,7 +398,7 @@ def calc_lbi_nf_state_eq(self, simplify=False): # input vectorfield self.gz = sp.zeros(nx, nq) - self.gz[nq:2*nq, :] = sp.eye(nq) # identity matrix for the active coordinates + self.gz[nq:2 * nq, :] = sp.eye(nq) # identity matrix for the active coordinates # drift vectorfield (will be completed below) self.fz = sp.zeros(nx, 1) @@ -353,22 +415,22 @@ def calc_lbi_nf_state_eq(self, simplify=False): if simplify: d = d.simplify() adj.simplify() - M11inv = adj/d + M11inv = adj / d # defining equation for ww: ww := uu + M11inv*M12*vv - uu_expr = ww - M11inv*M12*vv + uu_expr = ww - M11inv * M12 * vv # setting input tau and acceleration to 0 in the equations of motion C1K1 = self.eqns[:np, :].subs(st.zip0(self.ttdd, self.tau)) - N = st.time_deriv(M11inv*M12, self.tt) - ww_dot = -M11inv*C1K1.subs(lzip(uu, uu_expr)) + N.subs(lzip(uu, uu_expr))*vv + N = st.time_deriv(M11inv * M12, self.tt) + ww_dot = -M11inv * C1K1.subs(lzip(uu, uu_expr)) + N.subs(lzip(uu, uu_expr)) * vv - self.fz[2*nq:2*nq+np, :] = uu_expr - self.fz[2*nq+np:, :] = ww_dot + self.fz[2 * nq:2 * nq + np, :] = uu_expr + self.fz[2 * nq + np:, :] = ww_dot # how the new coordinates are defined: - self.ww_def = uu + M11inv*M12*vv + self.ww_def = uu + M11inv * M12 * vv if simplify: self.fz.simplify() @@ -409,7 +471,7 @@ def calc_jac_lin(self, base="force_input", eqlbr=None, parameter_values=None): if parameter_values is None: parameter_values = [] if eqlbr is None: - eqlbr = list(zip(self.tt, self.tt*0)) + list(zip(self.ttd, self.tt*0)) + eqlbr = list(zip(self.tt, self.tt * 0)) + list(zip(self.ttd, self.tt * 0)) parameter_values = list(eqlbr) + list(parameter_values) @@ -432,7 +494,7 @@ class DAE_System(object): """ This class encapsulates an differential algebraic equation (DAE). """ - + info = "encapsulate all dae-relevant information" def __init__(self, mod, parameter_values=None): @@ -472,13 +534,13 @@ def __init__(self, mod, parameter_values=None): self.yyd = yyd = st.symb_vector("ydot1:{}".format(1 + nx + self.nll)) # list of flags whether a variable occurs differentially (1) or only algebraically (0) - self.diff_alg_vars = [1]*len(mod.xx) + [0]*len(mod.llmd) + self.diff_alg_vars = [1] * len(mod.xx) + [0] * len(mod.llmd) # definiotric equations eqns1 = yyd[:self.ntt, :] - mod.ttd # dynamic equations (second order; M(tt)*ttdd + ... = 0) - eqns2 = mod.eqns.subz(mod.ttdd, yyd[self.ntt:2*self.ntt]) + eqns2 = mod.eqns.subz(mod.ttdd, yyd[self.ntt:2 * self.ntt]) self.constraints = mod.constraints = sp.Matrix(mod.constraints).subs(parameter_values) self.constraints_d = None @@ -552,7 +614,7 @@ def generate_eqns_funcs(self, parameter_values=None): self.eq_func = st.expr_to_func(fvars, eqns) # only the ode part - self.deq_func = st.expr_to_func(fvars, eqns[:2*self.ntt, :]) + self.deq_func = st.expr_to_func(fvars, eqns[:2 * self.ntt, :]) def model_func(t, yy, yyd): """ @@ -576,7 +638,7 @@ def model_func(t, yy, yyd): args = np.concatenate((yy, yyd, external_forces)) ttheta = yy[:ntt] - ttheta_d = yy[ntt:2*ntt] + ttheta_d = yy[ntt:2 * ntt] # not needed, just for comprehension # llmd = yy[2*ntt:2*ntt + nll] @@ -705,9 +767,9 @@ def acc_of_lmd_func(*args): """ ttheta = args[:ntt] - ttheta_d = args[ntt:2*ntt] - llmd = args[2*ntt:2*ntt+nll] - ttau = args[2*ntt+nll:] + ttheta_d = args[ntt:2 * ntt] + llmd = args[2 * ntt:2 * ntt + nll] + ttau = args[2 * ntt + nll:] args1 = np.concatenate((ttheta, ttheta_d, ttau)) @@ -758,7 +820,7 @@ def calc_consistent_conf_vel(self, **kwargs): # or if symbol names clash with additional options keys = set(kwargs.keys()) valid_symbol_names = set([s.name for s in list(self.mod.tt) + list(self.mod.ttd)]) - valid_estimate_names = set([n+"_estimate" for n in valid_symbol_names]) + valid_estimate_names = set([n + "_estimate" for n in valid_symbol_names]) assert not valid_symbol_names.intersection(option_names) assert not valid_estimate_names.intersection(option_names) @@ -874,11 +936,10 @@ def calc_consistent_accel_lmd(self, xx, t=0): nll = self.nll if isinstance(xx, (tuple, list)) and len(xx) == 2 \ - and isinstance(xx[0], np.ndarray) and isinstance(xx[1], np.ndarray): - + and isinstance(xx[0], np.ndarray) and isinstance(xx[1], np.ndarray): xx = np.concatenate(xx) - assert xx.shape == (2*ntt,) + assert xx.shape == (2 * ntt,) external_forces = self.input_func(t) @@ -909,7 +970,7 @@ def calc_consistent_init_vals(self, t=0, **kwargs): acc, llmd = self.calc_consistent_accel_lmd((ttheta, ttheta_d), t=t) yy = np.concatenate((ttheta, ttheta_d, llmd)) - yyd = np.concatenate((ttheta_d, acc, llmd*0)) + yyd = np.concatenate((ttheta_d, acc, llmd * 0)) # a b c # write unit test for this function, then try IDA algorithm @@ -1045,7 +1106,7 @@ def generate_symbolic_model(T, U, tt, F, simplify=True, constraints=None, dissip dissipation_function = sp.sympify(dissipation_function) assert isinstance(dissipation_function, sp.Expr) - llmd = st.symb_vector("lambda_1:{}".format(nC+1)) + llmd = st.symb_vector("lambda_1:{}".format(nC + 1)) F = sp.Matrix(F) @@ -1054,7 +1115,7 @@ def generate_symbolic_model(T, U, tt, F, simplify=True, constraints=None, dissip F = F.T if not F.shape == (n, 1): msg = "Vector of external forces has the wrong length. Should be " + \ - str(n) + " but is %i!" % F.shape[0] + str(n) + " but is %i!" % F.shape[0] raise ValueError(msg) # introducing symbols for the derivatives @@ -1080,7 +1141,7 @@ def generate_symbolic_model(T, U, tt, F, simplify=True, constraints=None, dissip # constraints - constraint_terms = list(llmd.T*jac_constraints) + constraint_terms = list(llmd.T * jac_constraints) # lagrange equation 1st kind (which include 2nd kind as special case if constraints are empty) @@ -1139,7 +1200,7 @@ def solve_eq(model): def is_zero_equilibrium(model): """ checks model for equilibrium point zero """ # substitution of state variables and their derivatives to zero - #substitution of input to zero --> stand-alone system + # substitution of input to zero --> stand-alone system subslist = st.zip0(model.qs) + st.zip0(model.qds) + st.zip0( model.qdds) + st.zip0(model.extforce_list) eq_1 = model.eq_list.subs(subslist) @@ -1171,15 +1232,15 @@ def transform_2nd_to_1st_order_matrices(P0, P1, P2, xx): """ assert P0.shape == P1.shape == P2.shape - assert xx.shape == (P0.shape[1]*2, 1) + assert xx.shape == (P0.shape[1] * 2, 1) xxd = st.time_deriv(xx, xx) - N = int(xx.shape[0]/2) + N = int(xx.shape[0] / 2) # definitional equations like xdot1 - x3 = 0 (for N=2) eqns_def = xx[N:, :] - xxd[:N, :] - eqns_mech = P2*xxd[N:, :] + P1*xxd[:N, :] + P0*xx[:N, :] + eqns_mech = P2 * xxd[N:, :] + P1 * xxd[:N, :] + P0 * xx[:N, :] F = st.row_stack(eqns_def, eqns_mech) @@ -1187,4 +1248,3 @@ def transform_2nd_to_1st_order_matrices(P0, P1, P2, xx): P1_bar = F.jacobian(xxd) return P0_bar, P1_bar - diff --git a/symbtools/test/test_modeltools.py b/symbtools/test/test_modeltools.py index 329ead9..9f2a1c1 100644 --- a/symbtools/test/test_modeltools.py +++ b/symbtools/test/test_modeltools.py @@ -14,7 +14,6 @@ from symbtools.modeltools import Rz import sys - from IPython import embed as IPS @@ -25,20 +24,20 @@ def setUp(self): pass def test_simple1(self): - q1, = qq = sp.Matrix(sp.symbols('q1,')) - F1, = FF = sp.Matrix(sp.symbols('F1,')) + q1, = qq = sp.Matrix(sp.symbols('q1,')) + F1, = FF = sp.Matrix(sp.symbols('F1,')) m = sp.Symbol('m') q1d = st.time_deriv(q1, qq) q1dd = st.time_deriv(q1, qq, order=2) - T = q1d**2*m/2 + T = q1d ** 2 * m / 2 V = 0 mod = mt.generate_symbolic_model(T, V, qq, FF) - eq = m*q1dd - F1 + eq = m * q1dd - F1 self.assertEqual(mod.eqns[0], eq) @@ -46,6 +45,56 @@ def test_simple1(self): M = mod.MM self.assertEqual(M[0], m) + def test_equilibrium_one(self): + k1, k2, k3, m1, m2 = sp.symbols('k1, k2, k3, m1, m2') + F = sp.Symbol('F') + params_values = [(k1, 10), + (k2, 40), + (k3, 8), + (m1, 0.5), + (m2, 1)] + Np = 2 # number of active coordinates + pp = st.symb_vector("p1:{0}".format(Np + 1)) # active coordinates + ttheta = pp # system states + tthetad = st.time_deriv(ttheta, ttheta) + + # kinetic energy of the bodies + T = 1 / 2 * (m1 * tthetad[0] ** 2 + m2 * tthetad[1] ** 2) # total kinetic energy + # total potential energy + V = 1 / 2 * (k1 * ttheta[0] ** 2 + k2 * ttheta[1] ** 2 + k3 * (ttheta[1] - ttheta[0]) ** 2) + external_forces = [0, F] + + # symbolic model of the system + mod = mt.generate_symbolic_model(T, V, ttheta, external_forces) + + # calculate equilibrium point for external force = 2N + mod.calc_eqlbr(params_values, etype='one_ep', uu=[2]) + eqrt_point = list(zip(mod.tt, mod.eqlbr)) # equilibrium points of states + all_vars = params_values + eqrt_point + list(zip(mod.tau, [2])) + + state_eqns = mod.eqns.subz0(mod.ttd, mod.ttdd).subs(all_vars) + + for i in range(len(ttheta)): + self.assertAlmostEqual(0, state_eqns[i], delta=6) + + def test_equilibrium_all(self): + mod = mt.SymbolicModel() + Np = 2 # number of active coordinates + pp = st.symb_vector("p1:{0}".format(Np + 1)) # active coordinates + mod.tt = ttheta = pp # position of the bodies in m + mod.ttd = st.time_deriv(ttheta, ttheta) # velocities of the bodies in m/s + mod.ttdd = st.time_deriv(ttheta, ttheta, order=2) # accelerations in m/s^2 + mod.eqns = sp.Matrix([[ttheta[0] - ttheta[1]], [3 * ttheta[1] * (1 - ttheta[0] ** 2)]]) + + # calculate equilibrium point + mod.calc_eqlbr(etype='all_ep') + + for i in mod.eqlbr: + eqrt_point = list(zip(mod.tt, list(i))) # equilibrium points of states + state_eqns = mod.eqns.subz0(mod.ttd, mod.ttdd).subs(eqrt_point) + for j in range(len(mod.tt)): + self.assertAlmostEqual(0, state_eqns[j], delta=6) + def test_simple_constraints(self): q1, q2 = qq = sp.Matrix(sp.symbols('q1, q2')) F1, = FF = sp.Matrix(sp.symbols('F1,')) @@ -55,10 +104,10 @@ def test_simple_constraints(self): qdot1, qdot2 = st.time_deriv(qq, qq) # q1dd, q2dd = st.time_deriv(qq, qq, order=2) - T = qdot1**2*m/4 + qdot2**2*m/4 + T = qdot1 ** 2 * m / 4 + qdot2 ** 2 * m / 4 V = 0 - mod = mt.generate_symbolic_model(T, V, qq, [F1, 0], constraints=[q1-q2]) + mod = mt.generate_symbolic_model(T, V, qq, [F1, 0], constraints=[q1 - q2]) self.assertEqual(mod.llmd.shape, (1, 1)) self.assertEqual(mod.constraints[0], q1 - q2) @@ -93,8 +142,8 @@ def test_simple_constraints(self): dae.gen_leqs_for_acc_llmd() A, b = dae.leqs_acc_lmd_func(*npy.r_[ttheta_1, ttheta_1, 1]) - self.assertEqual(A.shape, (dae.ntt+dae.nll,)*2) - self.assertEqual(b.shape, (dae.ntt+dae.nll,)) + self.assertEqual(A.shape, (dae.ntt + dae.nll,) * 2) + self.assertEqual(b.shape, (dae.ntt + dae.nll,)) self.assertTrue(npy.allclose(b, [1, 0, 0])) # in that situation there is no force @@ -112,7 +161,7 @@ def test_simple_constraints(self): dae.generate_eqns_funcs() res = dae.model_func(0, yy_1, yyd_1) - self.assertEqual(res.shape, (dae.ntt*2 + dae.nll,)) + self.assertEqual(res.shape, (dae.ntt * 2 + dae.nll,)) self.assertTrue(npy.allclose(res, 0)) # End of test_simple_constraints @@ -134,8 +183,9 @@ def test_four_bar_constraints(self): s1, s2, s3, m1, m2, m3, J1, J2, J3, l1, l2, l3, kappa1, kappa2, g = params st.make_global(params) - parameter_values = dict(s1=1/2, s2=1/2, s3=1/2, m1=1, m2=1, m3=3, J1=1/12, J2=1/12, J3=1/12, l1=1, l2=1.5, - l3=1.5, kappa1=3/2, kappa2=14.715, g=9.81).items() + parameter_values = dict(s1=1 / 2, s2=1 / 2, s3=1 / 2, m1=1, m2=1, m3=3, J1=1 / 12, J2=1 / 12, J3=1 / 12, l1=1, + l2=1.5, + l3=1.5, kappa1=3 / 2, kappa2=14.715, g=9.81).items() # ttau = sp.symbols('tau') tau1, tau2 = st.symb_vector("tau1, tau2") @@ -145,30 +195,30 @@ def test_four_bar_constraints(self): # Basis 1 and 2 # B1 = sp.Matrix([0, 0]) - B2 = sp.Matrix([2*l1, 0]) + B2 = sp.Matrix([2 * l1, 0]) # Koordinaten der Schwerpunkte und Gelenke - S1 = Rz(q1)*ex*s1 - G1 = Rz(q1)*ex*l1 - S2 = G1 + Rz(q1 + p1)*ex*s2 - G2 = G1 + Rz(q1 + p1)*ex*l2 + S1 = Rz(q1) * ex * s1 + G1 = Rz(q1) * ex * l1 + S2 = G1 + Rz(q1 + p1) * ex * s2 + G2 = G1 + Rz(q1 + p1) * ex * l2 # one link manioulator - G2b = B2 + Rz(p2)*ex*l3 - S3 = B2 + Rz(p2)*ex*s3 + G2b = B2 + Rz(p2) * ex * l3 + S3 = B2 + Rz(p2) * ex * s3 # timederivatives of centers of masses Sd1, Sd2, Sd3 = st.col_split(st.time_deriv(st.col_stack(S1, S2, S3), ttheta)) # kinetic energy - T_rot = (J1*qdot1 ** 2 + J2*(qdot1 + pdot1) ** 2 + J3*(pdot2) ** 2)/2 - T_trans = (m1*Sd1.T*Sd1 + m2*Sd2.T*Sd2 + m3*Sd3.T*Sd3)/2 + T_rot = (J1 * qdot1 ** 2 + J2 * (qdot1 + pdot1) ** 2 + J3 * (pdot2) ** 2) / 2 + T_trans = (m1 * Sd1.T * Sd1 + m2 * Sd2.T * Sd2 + m3 * Sd3.T * Sd3) / 2 T = T_rot + T_trans[0] # potential energy - V = m1*g*S1[1] + m2*g*S2[1] + m3*g*S3[1] + V = m1 * g * S1[1] + m2 * g * S2[1] + m3 * g * S3[1] # # Kinetische Energie mit Platzhaltersymbolen einführen (jetzt nicht): # @@ -189,9 +239,9 @@ def test_four_bar_constraints(self): self.assertEqual(dae.eqns[-nc:, :], mod.constraints) # qdot1 = 0 - ttheta_1, ttheta_d_1 = dae.calc_consistent_conf_vel(q1=npy.pi/4, _disp=False) + ttheta_1, ttheta_d_1 = dae.calc_consistent_conf_vel(q1=npy.pi / 4, _disp=False) - eres_c = npy.array([-0.2285526, 1.58379902, 0.78539816]) + eres_c = npy.array([-0.2285526, 1.58379902, 0.78539816]) eres_v = npy.array([0, 0, 0]) self.assertTrue(npy.allclose(ttheta_1, eres_c)) @@ -201,13 +251,13 @@ def test_four_bar_constraints(self): acc_1, llmd_1 = dae.calc_consistent_accel_lmd((ttheta_1, ttheta_d_1)) # these values seem reasonable but have yet not been checked analytically self.assertTrue(npy.allclose(acc_1, [13.63475466, -1.54473017, -8.75145644])) - self.assertTrue(npy.allclose(llmd_1, [-0.99339947, 0.58291489])) + self.assertTrue(npy.allclose(llmd_1, [-0.99339947, 0.58291489])) # qdot1 ≠ 0 - ttheta_2, ttheta_d_2 = dae.calc_consistent_conf_vel(q1=npy.pi/8*7, qdot1=3, _disp=False) + ttheta_2, ttheta_d_2 = dae.calc_consistent_conf_vel(q1=npy.pi / 8 * 7, qdot1=3, _disp=False) - eres_c = npy.array([-0.85754267, 0.89969149, 0.875])*npy.pi - eres_v = npy.array([-3.42862311, 2.39360715, 3.]) + eres_c = npy.array([-0.85754267, 0.89969149, 0.875]) * npy.pi + eres_v = npy.array([-3.42862311, 2.39360715, 3.]) self.assertTrue(npy.allclose(ttheta_2, eres_c)) self.assertTrue(npy.allclose(ttheta_d_2, eres_v)) @@ -217,7 +267,7 @@ def test_four_bar_constraints(self): dae.generate_eqns_funcs() res = dae.model_func(0, yy_1, yyd_1) - self.assertEqual(res.shape, (dae.ntt*2 + dae.nll,)) + self.assertEqual(res.shape, (dae.ntt * 2 + dae.nll,)) self.assertTrue(npy.allclose(res, 0)) def test_cart_pole(self): @@ -234,18 +284,18 @@ def test_cart_pole(self): ex = Matrix([1, 0]) ey = Matrix([0, 1]) - S0 = ex*q1 # joint of the pendulum - S1 = S0 - mt.Rz(p1)*ey*l1 # center of mass + S0 = ex * q1 # joint of the pendulum + S1 = S0 - mt.Rz(p1) * ey * l1 # center of mass # velocity S0d = st.time_deriv(S0, ttheta) S1d = st.time_deriv(S1, ttheta) T_rot = 0 # no moment of inertia (mathematical pendulum) - T_trans = ( m0*S0d.T*S0d + m1*S1d.T*S1d )/2 + T_trans = (m0 * S0d.T * S0d + m1 * S1d.T * S1d) / 2 T = T_rot + T_trans[0] - V = m1*g*S1[1] + V = m1 * g * S1[1] # noinspection PyUnusedLocal with self.assertRaises(ValueError) as cm: @@ -261,22 +311,22 @@ def test_cart_pole(self): mod = mt.generate_symbolic_model(T, V, ttheta, [0, F1]) mod_1 = mt.generate_symbolic_model(T, V, ttheta, QQ) mod_2 = mt.generate_symbolic_model(T, V, ttheta, QQ.T) - + self.assertEqual(mod.eqns, mod_1.eqns) self.assertEqual(mod.eqns, mod_2.eqns) - + mod.eqns.simplify() M = mod.MM M.simplify() - M_ref = Matrix([[l1**2*m1, l1*m1*cos(p1)], [l1*m1*cos(p1), m1 + m0]]) + M_ref = Matrix([[l1 ** 2 * m1, l1 * m1 * cos(p1)], [l1 * m1 * cos(p1), m1 + m0]]) self.assertEqual(M, M_ref) # noinspection PyUnusedLocal rest = mod.eqns.subs(st.zip0(mod.ttdd)) # noinspection PyUnusedLocal - rest_ref = Matrix([[g*l1*m1*sin(p1)], [-F1 - l1*m1*pdot1**2*sin(p1)]]) + rest_ref = Matrix([[g * l1 * m1 * sin(p1)], [-F1 - l1 * m1 * pdot1 ** 2 * sin(p1)]]) self.assertEqual(M, M_ref) @@ -294,8 +344,8 @@ def test_cart_pole(self): pdot1, qdot1 = mod.ttd - ff_ref = sp.Matrix([[pdot1],[qdot1], [-g*sin(p1)/l1], [0]]) - gg_ref = sp.Matrix([[0], [0], [-cos(p1)/l1], [1]]) + ff_ref = sp.Matrix([[pdot1], [qdot1], [-g * sin(p1) / l1], [0]]) + gg_ref = sp.Matrix([[0], [0], [-cos(p1) / l1], [1]]) self.assertEqual(mod.ff, ff_ref) self.assertEqual(mod.gg, gg_ref) @@ -303,10 +353,10 @@ def test_cart_pole(self): A, b = mod.calc_jac_lin(base="coll_part_lin", parameter_values=parameter_values) Ae = sp.Matrix([ - [0, 0, 1, 0], - [0, 0, 0, 1], - [-4.905, 0, 0, 0], - [0, 0, 0, 0]]) + [0, 0, 1, 0], + [0, 0, 0, 1], + [-4.905, 0, 0, 0], + [0, 0, 0, 0]]) be = sp.Matrix([0, 0, -0.5, 1]) @@ -316,10 +366,10 @@ def test_cart_pole(self): A, b = mod.calc_jac_lin(base="force_input", parameter_values=parameter_values) Ae = sp.Matrix([ - [0, 0, 1, 0], - [0, 0, 0, 1], - [-7.3575, 0, 0, 0], - [4.905, 0, 0, 0]]) + [0, 0, 1, 0], + [0, 0, 0, 1], + [-7.3575, 0, 0, 0], + [4.905, 0, 0, 0]]) be = sp.Matrix([0, 0, -.25, 0.5]) @@ -339,23 +389,23 @@ def test_cart_pole_with_dissi(self): ex = Matrix([1, 0]) ey = Matrix([0, 1]) - S0 = ex*q1 # joint of the pendulum - S1 = S0 - mt.Rz(p1)*ey*l1 # center of mass + S0 = ex * q1 # joint of the pendulum + S1 = S0 - mt.Rz(p1) * ey * l1 # center of mass # velocity S0d = st.time_deriv(S0, ttheta) S1d = st.time_deriv(S1, ttheta) T_rot = 0 # no moment of inertia (mathematical pendulum) - T_trans = (m0*S0d.T*S0d + m1*S1d.T*S1d)/2 + T_trans = (m0 * S0d.T * S0d + m1 * S1d.T * S1d) / 2 T = T_rot + T_trans[0] - V = m1*g*S1[1] + V = m1 * g * S1[1] QQ = sp.Matrix([tau_dissi, F1]) mod = mt.generate_symbolic_model(T, V, ttheta, QQ) - mod.substitute_ext_forces(QQ, [d1*pdot1, F1], [F1]) + mod.substitute_ext_forces(QQ, [d1 * pdot1, F1], [F1]) self.assertEqual(len(mod.tau), 1) self.assertEqual(len(mod.extforce_list), 1) self.assertEqual(mod.tau[0], F1) @@ -367,8 +417,8 @@ def test_simple_pendulum_with_actuated_mountpoint(self): np = 1 nq = 2 n = np + nq - pp = st.symb_vector("p1:{0}".format(np+1)) - qq = st.symb_vector("q1:{0}".format(nq+1)) + pp = st.symb_vector("p1:{0}".format(np + 1)) + qq = st.symb_vector("q1:{0}".format(nq + 1)) p1, q1, q2 = ttheta = st.row_stack(pp, qq) pdot1, qdot1, qdot2 = st.time_deriv(ttheta, ttheta) @@ -379,25 +429,25 @@ def test_simple_pendulum_with_actuated_mountpoint(self): # Geometry - ex = sp.Matrix([1,0]) - ey = sp.Matrix([0,1]) + ex = sp.Matrix([1, 0]) + ey = sp.Matrix([0, 1]) # Coordinates of centers of masses (com) and joints - S1 = ex*q1 - S2 = ex*q1 + ey*q2 + S1 = ex * q1 + S2 = ex * q1 + ey * q2 G3 = S2 # Joints # com of pendulum (points upwards) - S3 = G3 + mt.Rz(p1)*ey*s3 + S3 = G3 + mt.Rz(p1) * ey * s3 # timederivatives - Sd1, Sd2, Sd3 = st.col_split(st.time_deriv(st.col_stack(S1, S2, S3), ttheta)) ## + Sd1, Sd2, Sd3 = st.col_split(st.time_deriv(st.col_stack(S1, S2, S3), ttheta)) ## # Energy - T_rot = ( J3*pdot1**2 )/2 - T_trans = ( m1*Sd1.T*Sd1 + m2*Sd2.T*Sd2 + m3*Sd3.T*Sd3 )/2 + T_rot = (J3 * pdot1 ** 2) / 2 + T_trans = (m1 * Sd1.T * Sd1 + m2 * Sd2.T * Sd2 + m3 * Sd3.T * Sd3) / 2 T = T_rot + T_trans[0] - V = m1*g*S1[1] + m2*g*S2[1] + m3*g*S3[1] + V = m1 * g * S1[1] + m2 * g * S2[1] + m3 * g * S3[1] external_forces = [0, tau1, tau2] assert not any(external_forces[:np]) @@ -406,8 +456,8 @@ def test_simple_pendulum_with_actuated_mountpoint(self): # Note: pdot1, qdot1, qdot2 = mod.ttd - ff_ref = sp.Matrix([[pdot1], [qdot1], [qdot2], [g*m3*s3*sin(p1)/(J3 + m3*s3**2)], [0], [0]]) - gg_ref_part = sp.Matrix([m3*s3*cos(p1)/(J3 + m3*s3**2), m3*s3*sin(p1)/(J3 + m3*s3**2)]).T + ff_ref = sp.Matrix([[pdot1], [qdot1], [qdot2], [g * m3 * s3 * sin(p1) / (J3 + m3 * s3 ** 2)], [0], [0]]) + gg_ref_part = sp.Matrix([m3 * s3 * cos(p1) / (J3 + m3 * s3 ** 2), m3 * s3 * sin(p1) / (J3 + m3 * s3 ** 2)]).T self.assertEqual(mod.ff, ff_ref) self.assertEqual(mod.gg[-3, :], gg_ref_part) @@ -424,17 +474,17 @@ def test_two_link_manipulator(self): # noinspection PyUnusedLocal ey = Matrix([0, 1]) - S1 = mt.Rz(q1)*ex*s1 # center of mass (first link) - G1 = mt.Rz(q1)*ex*l1 # first joint - S2 = G1 + mt.Rz(q1+p1)*ex*s2 + S1 = mt.Rz(q1) * ex * s1 # center of mass (first link) + G1 = mt.Rz(q1) * ex * l1 # first joint + S2 = G1 + mt.Rz(q1 + p1) * ex * s2 # velocity S1d = st.time_deriv(S1, ttheta) S2d = st.time_deriv(S2, ttheta) - T_rot = (J1*qdot1**2 + J2*(qdot1 + pdot1)**2) / 2 + T_rot = (J1 * qdot1 ** 2 + J2 * (qdot1 + pdot1) ** 2) / 2 - T_trans = ( m1*S1d.T*S1d + m2*S2d.T*S2d )/2 + T_trans = (m1 * S1d.T * S1d + m2 * S2d.T * S2d) / 2 T = T_rot + T_trans[0] V = 0 @@ -444,21 +494,21 @@ def test_two_link_manipulator(self): w1 = mod.ww[0] - kappa = l1*m2*s2 / (J2 + m2*s2**2) - fz4 = - kappa * qdot1*sin(p1)*(w1 - kappa*qdot1*cos(p1)) - fzref = sp.Matrix([[ qdot1], - [ 0], - [ (-qdot1*(1 + kappa*cos(p1) ) ) + w1], - [ fz4]]) + kappa = l1 * m2 * s2 / (J2 + m2 * s2 ** 2) + fz4 = - kappa * qdot1 * sin(p1) * (w1 - kappa * qdot1 * cos(p1)) + fzref = sp.Matrix([[qdot1], + [0], + [(-qdot1 * (1 + kappa * cos(p1))) + w1], + [fz4]]) - w_def_ref = pdot1 + qdot1*(1+kappa*cos(p1)) + w_def_ref = pdot1 + qdot1 * (1 + kappa * cos(p1)) self.assertEqual(mod.gz, sp.Matrix([0, 1, 0, 0])) fz_diff = mod.fz - fzref fz_diff.simplify() self.assertEqual(fz_diff, sp.Matrix([0, 0, 0, 0])) - diff = mod.ww_def[0,0] - w_def_ref + diff = mod.ww_def[0, 0] - w_def_ref self.assertEqual(diff.simplify(), 0) def test_unicycle(self): @@ -481,22 +531,22 @@ def test_unicycle(self): ex = Matrix([1, 0]) ey = Matrix([0, 1]) - M0 = Matrix([-r*p1, r]) + M0 = Matrix([-r * p1, r]) - S1 = M0 + mt.Rz(p1+q1)*ey*s1 - S2 = M0 + mt.Rz(p1+q1)*ey*l1+mt.Rz(p1+q1+q2)*ey*s2 + S1 = M0 + mt.Rz(p1 + q1) * ey * s1 + S2 = M0 + mt.Rz(p1 + q1) * ey * l1 + mt.Rz(p1 + q1 + q2) * ey * s2 M0d = st.time_deriv(M0, theta) S1d = st.time_deriv(S1, theta) S2d = st.time_deriv(S2, theta) # Energy - T_rot = ( J0*p1d**2 + J1*(p1d+q1d)**2 + J2*(p1d+ q1d+q2d)**2 )/2 - T_trans = ( m0*M0d.T*M0d + m1*S1d.T*S1d + m2*S2d.T*S2d )/2 + T_rot = (J0 * p1d ** 2 + J1 * (p1d + q1d) ** 2 + J2 * (p1d + q1d + q2d) ** 2) / 2 + T_trans = (m0 * M0d.T * M0d + m1 * S1d.T * S1d + m2 * S2d.T * S2d) / 2 T = T_rot + T_trans[0] - V = m1*g*S1[1] + m2*g*S2[1] + V = m1 * g * S1[1] + m2 * g * S2[1] mod = mt.generate_symbolic_model(T, V, theta, [0, Q1, Q2]) self.assertEqual(mod.fz, None) @@ -511,8 +561,8 @@ def test_triple_pendulum(self): np = 1 nq = 2 - pp = sp.Matrix( sp.symbols("p1:{0}".format(np+1) ) ) - qq = sp.Matrix( sp.symbols("q1:{0}".format(nq+1) ) ) + pp = sp.Matrix(sp.symbols("p1:{0}".format(np + 1))) + qq = sp.Matrix(sp.symbols("q1:{0}".format(nq + 1))) ttheta = st.row_stack(pp, qq) Q1, Q2 = sp.symbols('Q1, Q2') @@ -529,18 +579,18 @@ def test_triple_pendulum(self): l1, l2, l3, l4, s1, s2, s3, s4, J1, J2, J3, J4, m1, m2, m3, m4, g = params # geometry - mey = -Matrix([0,1]) + mey = -Matrix([0, 1]) # coordinates for centers of inertia and joints - S1 = mt.Rz(kk[0])*mey*s1 - G1 = mt.Rz(kk[0])*mey*l1 + S1 = mt.Rz(kk[0]) * mey * s1 + G1 = mt.Rz(kk[0]) * mey * l1 - S2 = G1 + mt.Rz(sum(kk[:2]))*mey*s2 - G2 = G1 + mt.Rz(sum(kk[:2]))*mey*l2 + S2 = G1 + mt.Rz(sum(kk[:2])) * mey * s2 + G2 = G1 + mt.Rz(sum(kk[:2])) * mey * l2 - S3 = G2 + mt.Rz(sum(kk[:3]))*mey*s3 + S3 = G2 + mt.Rz(sum(kk[:3])) * mey * s3 # noinspection PyUnusedLocal - G3 = G2 + mt.Rz(sum(kk[:3]))*mey*l3 + G3 = G2 + mt.Rz(sum(kk[:3])) * mey * l3 # velocities of joints and center of inertia Sd1 = st.time_deriv(S1, ttheta) @@ -548,19 +598,65 @@ def test_triple_pendulum(self): Sd3 = st.time_deriv(S3, ttheta) # energy - T_rot = ( J1*kd1**2 + J2*(kd1 + kd2)**2 + J3*(kd1 + kd2 + kd3)**2)/2 - T_trans = ( m1*Sd1.T*Sd1 + m2*Sd2.T*Sd2 + m3*Sd3.T*Sd3)/2 + T_rot = (J1 * kd1 ** 2 + J2 * (kd1 + kd2) ** 2 + J3 * (kd1 + kd2 + kd3) ** 2) / 2 + T_trans = (m1 * Sd1.T * Sd1 + m2 * Sd2.T * Sd2 + m3 * Sd3.T * Sd3) / 2 T = T_rot + T_trans[0] - V = m1*g*S1[1] + m2*g*S2[1] + m3*g*S3[1] + V = m1 * g * S1[1] + m2 * g * S2[1] + m3 * g * S3[1] external_forces = [0, Q1, Q2] mod = mt.generate_symbolic_model(T, V, ttheta, external_forces, simplify=False) - eqns_ref = Matrix([[J3*(2*p1_dd + 2*q1_dd + 2*q2_dd)/2 + g*m3*s3*sin(p1 + q1 + q2) + m3*s3*(-l1*q1_d**2*sin(q1) + l1*q1_dd*cos(q1) - l2*(q1_d + q2_d)**2*sin(q1 + q2) + l2*(q1_dd + q2_dd)*cos(q1 + q2) - s3*(p1_d + q1_d + q2_d)**2*sin(p1 + q1 + q2) + s3*(p1_dd + q1_dd + q2_dd)*cos(p1 + q1 + q2))*cos(p1 + q1 + q2) + m3*s3*(l1*q1_d**2*cos(q1) + l1*q1_dd*sin(q1) + l2*(q1_d + q2_d)**2*cos(q1 + q2) + l2*(q1_dd + q2_dd)*sin(q1 + q2) + s3*(p1_d + q1_d + q2_d)**2*cos(p1 + q1 + q2) + s3*(p1_dd + q1_dd + q2_dd)*sin(p1 + q1 + q2))*sin(p1 + q1 + q2)], [J1*q1_dd + J2*(2*q1_dd + 2*q2_dd)/2 + J3*(2*p1_dd + 2*q1_dd + 2*q2_dd)/2 - Q1 + g*m1*s1*sin(q1) + \ - g*m2*(l1*sin(q1) + s2*sin(q1 + q2)) + g*m3*(l1*sin(q1) + l2*sin(q1 + q2) + s3*sin(p1 + q1 + q2)) + m1*q1_dd*s1**2*sin(q1)**2 + m1*q1_dd*s1**2*cos(q1)**2 + m2*(2*l1*sin(q1) + 2*s2*sin(q1 + q2))*(l1*q1_d**2*cos(q1) + l1*q1_dd*sin(q1) + s2*(q1_d + q2_d)**2*cos(q1 + q2) + s2*(q1_dd + q2_dd)*sin(q1 + q2))/2 + m2*(2*l1*cos(q1) + 2*s2*cos(q1 + q2))*(-l1*q1_d**2*sin(q1) + l1*q1_dd*cos(q1) - s2*(q1_d + q2_d)**2*sin(q1 + q2) + s2*(q1_dd + q2_dd)*cos(q1 + q2))/2 + m3*(2*l1*sin(q1) + 2*l2*sin(q1 + q2) + 2*s3*sin(p1 + q1 + q2))*(l1*q1_d**2*cos(q1) + l1*q1_dd*sin(q1) + l2*(q1_d + q2_d)**2*cos(q1 + q2) + l2*(q1_dd + q2_dd)*sin(q1 + q2) + s3*(p1_d + q1_d + q2_d)**2*cos(p1 + q1 + q2) + s3*(p1_dd + q1_dd + q2_dd)*sin(p1 + q1 + q2))/2 + m3*(2*l1*cos(q1) + 2*l2*cos(q1 + q2) + 2*s3*cos(p1 + q1 + q2))*(-l1*q1_d**2*sin(q1) + l1*q1_dd*cos(q1) - l2*(q1_d + q2_d)**2*sin(q1 + q2) + l2*(q1_dd + q2_dd)*cos(q1 + q2) - \ - s3*(p1_d + q1_d + q2_d)**2*sin(p1 + q1 + q2) + s3*(p1_dd + q1_dd + q2_dd)*cos(p1 + q1 + q2))/2], [J2*(2*q1_dd + 2*q2_dd)/2 + J3*(2*p1_dd + 2*q1_dd + 2*q2_dd)/2 - Q2 + g*m2*s2*sin(q1 + q2) + g*m3*(l2*sin(q1 + q2) + s3*sin(p1 + q1 + q2)) + m2*s2*(-l1*q1_d**2*sin(q1) + l1*q1_dd*cos(q1) - s2*(q1_d + q2_d)**2*sin(q1 + q2) + s2*(q1_dd + q2_dd)*cos(q1 + q2))*cos(q1 + q2) + \ - m2*s2*(l1*q1_d**2*cos(q1) + l1*q1_dd*sin(q1) + s2*(q1_d + q2_d)**2*cos(q1 + q2) + s2*(q1_dd + q2_dd)*sin(q1 + q2))*sin(q1 + q2) + m3*(2*l2*sin(q1 + q2) + 2*s3*sin(p1 + q1 + q2))*(l1*q1_d**2*cos(q1) + l1*q1_dd*sin(q1) + l2*(q1_d + q2_d)**2*cos(q1 + q2) + l2*(q1_dd + q2_dd)*sin(q1 + q2) + s3*(p1_d + q1_d + q2_d)**2*cos(p1 + q1 + q2) + s3*(p1_dd + q1_dd + q2_dd)*sin(p1 + q1 + q2))/2 + m3*(2*l2*cos(q1 + q2) + 2*s3*cos(p1 + q1 + q2))*(-l1*q1_d**2*sin(q1) + l1*q1_dd*cos(q1) - l2*(q1_d + q2_d)**2*sin(q1 + q2) + l2*(q1_dd + q2_dd)*cos(q1 + q2) - s3*(p1_d + q1_d + q2_d)**2*sin(p1 + q1 + q2) + s3*(p1_dd + q1_dd + q2_dd)*cos(p1 + q1 + q2))/2]]) + eqns_ref = Matrix([[J3 * (2 * p1_dd + 2 * q1_dd + 2 * q2_dd) / 2 + g * m3 * s3 * sin(p1 + q1 + q2) + m3 * s3 * ( + -l1 * q1_d ** 2 * sin(q1) + l1 * q1_dd * cos(q1) - l2 * (q1_d + q2_d) ** 2 * sin(q1 + q2) + l2 * ( + q1_dd + q2_dd) * cos(q1 + q2) - s3 * (p1_d + q1_d + q2_d) ** 2 * sin(p1 + q1 + q2) + s3 * ( + p1_dd + q1_dd + q2_dd) * cos(p1 + q1 + q2)) * cos(p1 + q1 + q2) + m3 * s3 * ( + l1 * q1_d ** 2 * cos(q1) + l1 * q1_dd * sin(q1) + l2 * (q1_d + q2_d) ** 2 * cos( + q1 + q2) + l2 * (q1_dd + q2_dd) * sin(q1 + q2) + s3 * ( + p1_d + q1_d + q2_d) ** 2 * cos(p1 + q1 + q2) + s3 * ( + p1_dd + q1_dd + q2_dd) * sin(p1 + q1 + q2)) * sin(p1 + q1 + q2)], [ + J1 * q1_dd + J2 * (2 * q1_dd + 2 * q2_dd) / 2 + J3 * ( + 2 * p1_dd + 2 * q1_dd + 2 * q2_dd) / 2 - Q1 + g * m1 * s1 * sin(q1) + \ + g * m2 * (l1 * sin(q1) + s2 * sin(q1 + q2)) + g * m3 * ( + l1 * sin(q1) + l2 * sin(q1 + q2) + s3 * sin( + p1 + q1 + q2)) + m1 * q1_dd * s1 ** 2 * sin( + q1) ** 2 + m1 * q1_dd * s1 ** 2 * cos(q1) ** 2 + m2 * ( + 2 * l1 * sin(q1) + 2 * s2 * sin(q1 + q2)) * ( + l1 * q1_d ** 2 * cos(q1) + l1 * q1_dd * sin(q1) + s2 * ( + q1_d + q2_d) ** 2 * cos(q1 + q2) + s2 * (q1_dd + q2_dd) * sin( + q1 + q2)) / 2 + m2 * (2 * l1 * cos(q1) + 2 * s2 * cos(q1 + q2)) * ( + -l1 * q1_d ** 2 * sin(q1) + l1 * q1_dd * cos(q1) - s2 * ( + q1_d + q2_d) ** 2 * sin(q1 + q2) + s2 * (q1_dd + q2_dd) * cos( + q1 + q2)) / 2 + m3 * ( + 2 * l1 * sin(q1) + 2 * l2 * sin(q1 + q2) + 2 * s3 * sin(p1 + q1 + q2)) * ( + l1 * q1_d ** 2 * cos(q1) + l1 * q1_dd * sin(q1) + l2 * ( + q1_d + q2_d) ** 2 * cos(q1 + q2) + l2 * (q1_dd + q2_dd) * sin( + q1 + q2) + s3 * (p1_d + q1_d + q2_d) ** 2 * cos(p1 + q1 + q2) + s3 * ( + p1_dd + q1_dd + q2_dd) * sin(p1 + q1 + q2)) / 2 + m3 * ( + 2 * l1 * cos(q1) + 2 * l2 * cos(q1 + q2) + 2 * s3 * cos(p1 + q1 + q2)) * ( + -l1 * q1_d ** 2 * sin(q1) + l1 * q1_dd * cos(q1) - l2 * ( + q1_d + q2_d) ** 2 * sin(q1 + q2) + l2 * (q1_dd + q2_dd) * cos(q1 + q2) - \ + s3 * (p1_d + q1_d + q2_d) ** 2 * sin(p1 + q1 + q2) + s3 * ( + p1_dd + q1_dd + q2_dd) * cos(p1 + q1 + q2)) / 2], [ + J2 * (2 * q1_dd + 2 * q2_dd) / 2 + J3 * ( + 2 * p1_dd + 2 * q1_dd + 2 * q2_dd) / 2 - Q2 + g * m2 * s2 * sin( + q1 + q2) + g * m3 * (l2 * sin(q1 + q2) + s3 * sin(p1 + q1 + q2)) + m2 * s2 * ( + -l1 * q1_d ** 2 * sin(q1) + l1 * q1_dd * cos(q1) - s2 * ( + q1_d + q2_d) ** 2 * sin(q1 + q2) + s2 * (q1_dd + q2_dd) * cos( + q1 + q2)) * cos(q1 + q2) + \ + m2 * s2 * (l1 * q1_d ** 2 * cos(q1) + l1 * q1_dd * sin(q1) + s2 * ( + q1_d + q2_d) ** 2 * cos(q1 + q2) + s2 * (q1_dd + q2_dd) * sin( + q1 + q2)) * sin(q1 + q2) + m3 * ( + 2 * l2 * sin(q1 + q2) + 2 * s3 * sin(p1 + q1 + q2)) * ( + l1 * q1_d ** 2 * cos(q1) + l1 * q1_dd * sin(q1) + l2 * ( + q1_d + q2_d) ** 2 * cos(q1 + q2) + l2 * (q1_dd + q2_dd) * sin( + q1 + q2) + s3 * (p1_d + q1_d + q2_d) ** 2 * cos(p1 + q1 + q2) + s3 * ( + p1_dd + q1_dd + q2_dd) * sin(p1 + q1 + q2)) / 2 + m3 * ( + 2 * l2 * cos(q1 + q2) + 2 * s3 * cos(p1 + q1 + q2)) * ( + -l1 * q1_d ** 2 * sin(q1) + l1 * q1_dd * cos(q1) - l2 * ( + q1_d + q2_d) ** 2 * sin(q1 + q2) + l2 * (q1_dd + q2_dd) * cos( + q1 + q2) - s3 * (p1_d + q1_d + q2_d) ** 2 * sin(p1 + q1 + q2) + s3 * ( + p1_dd + q1_dd + q2_dd) * cos(p1 + q1 + q2)) / 2]]) self.assertEqual(eqns_ref, mod.eqns) @@ -572,16 +668,15 @@ def setUp(self): pass def testRz(self): - Rz1 = mt.Rz(sp.pi*0.4) - Rz2 = mt.Rz(npy.pi*0.4, to_numpy=True) + Rz1 = mt.Rz(sp.pi * 0.4) + Rz2 = mt.Rz(npy.pi * 0.4, to_numpy=True) self.assertTrue(npy.allclose(st.to_np(Rz1), Rz2)) def test_transform_2nd_to_1st(self): - def intern_test(np, nq): N = np + nq - xx = st.symb_vector('x1:{0}'.format(N*2+1)) + xx = st.symb_vector('x1:{0}'.format(N * 2 + 1)) P0 = st.symbMatrix(np, N, s='A') P1 = st.symbMatrix(np, N, s='B') @@ -604,7 +699,7 @@ def main(): # remove command line args which should not be passed to the testframework if 'all' in sys.argv: sys.argv.remove('all') - + unittest.main()