def rhs(xx, xxd, uu): """ Right hand side of the equation of motion in nonlinear state space form :param xx: system states :param xxd: first derivation of system states :param uu: system input :return mod: system class """ m1 = 0.05 # mass of the iron ball in kg m2 = 0.1 # mass of the brass ball in kg kf = 30 # Spring constant in N/m g = 9.8 # acceleration of gravity in m/s^2 # kinetic energy of the bodies T0 = 0.5 * m1 * xxd[0]**2 T1 = 0.5 * m2 * xxd[1]**2 T = T0 + T1 # total kinetic energy # total potential energy V = -g * (m1 * xx[0] + m2 * xx[1]) \ + kf * 0.5 * (xx[0] - xx[1]) ** 2 external_forces = [uu, 0] # magnetic force as input # symbolic model of the system mod = mt.generate_symbolic_model(T, V, xx, external_forces) mod.calc_state_eq() return mod
def test_cart_pole_with_dissi(self): p1, q1 = ttheta = sp.Matrix(sp.symbols('p1, q1')) F1, tau_dissi = sp.Matrix(sp.symbols('F1, tau_dissi')) params = sp.symbols('m0, m1, l1, g, d1') m0, m1, l1, g, d1 = params pdot1 = st.time_deriv(p1, ttheta) # q1dd = st.time_deriv(q1, ttheta, order=2) 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 # 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 = T_rot + T_trans[0] 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]) self.assertEqual(len(mod.tau), 1) self.assertEqual(len(mod.extforce_list), 1) self.assertEqual(mod.tau[0], F1) mod.calc_coll_part_lin_state_eq()
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) ) ) ttheta = st.row_stack(pp, qq) Q1, Q2 = sp.symbols('Q1, Q2') p1_d, q1_d, q2_d = mu = st.time_deriv(ttheta, ttheta) p1_dd, q1_dd, q2_dd = mu_d = st.time_deriv(ttheta, ttheta, order=2) p1, q1, q2 = ttheta # reordering according to chain kk = sp.Matrix([q1, q2, p1]) kd1, kd2, kd3 = q1_d, q2_d, p1_d params = sp.symbols('l1, l2, l3, l4, s1, s2, s3, s4, J1, J2, J3, J4, m1, m2, m3, m4, g') l1, l2, l3, l4, s1, s2, s3, s4, J1, J2, J3, J4, m1, m2, m3, m4, g = params # geometry 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 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 G3 = G2 + mt.Rz(sum(kk[:3]))*mey*l3 # velocities of joints and center of inertia Sd1 = st.time_deriv(S1, ttheta) Sd2 = st.time_deriv(S2, ttheta) 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 = T_rot + T_trans[0] 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]]) self.assertEqual(eqns_ref, mod.eqns)
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_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)) p1, q1, q2 = ttheta = st.row_stack(pp, qq) pdot1, qdot1, qdot2 = tthetad = st.time_deriv(ttheta, ttheta) mud = st.time_deriv(ttheta, ttheta, order=2) params = sp.symbols('l3, l4, s3, s4, J3, J4, m1, m2, m3, m4, g') l3, l4, s3, s4, J3, J4, m1, m2, m3, m4, g = params tau1, tau2 = ttau = st.symb_vector("tau1, tau2") ## Geometry ex = sp.Matrix([1, 0]) ey = sp.Matrix([0, 1]) # Koordinaten der Schwerpunkte und Gelenke S1 = ex * q1 S2 = ex * q1 + ey * q2 G3 = S2 # Gelenk # Schwerpunkt des Pendels #zeigt nach oben S3 = G3 + mt.Rz(p1) * ey * s3 # Zeitableitungen der Schwerpunktskoordinaten 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 = T_rot + T_trans[0] V = m1 * g * S1[1] + m2 * g * S2[1] + m3 * g * S3[1] external_forces = [0, tau1, tau2] assert not any(external_forces[:np]) mod = mt.generate_symbolic_model(T, V, ttheta, external_forces) mod.calc_coll_part_lin_state_eq(simplify=True) #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 self.assertEqual(mod.ff, ff_ref) self.assertEqual(mod.gg[-3, :], gg_ref_part)
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)) p1, q1, q2 = ttheta = st.row_stack(pp, qq) pdot1, qdot1, qdot2 = st.time_deriv(ttheta, ttheta) params = sp.symbols('l3, l4, s3, s4, J3, J4, m1, m2, m3, m4, g') l3, l4, s3, s4, J3, J4, m1, m2, m3, m4, g = params tau1, tau2 = st.symb_vector("tau1, tau2") # Geometry 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 G3 = S2 # Joints # com of pendulum (points upwards) S3 = G3 + mt.Rz(p1) * ey * s3 # timederivatives 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 = T_rot + T_trans[0] V = m1 * g * S1[1] + m2 * g * S2[1] + m3 * g * S3[1] external_forces = [0, tau1, tau2] assert not any(external_forces[:np]) mod = mt.generate_symbolic_model(T, V, ttheta, external_forces) mod.calc_coll_part_lin_state_eq(simplify=True) # 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 self.assertEqual(mod.ff, ff_ref) self.assertEqual(mod.gg[-3, :], gg_ref_part)
def test_two_link_manipulator(self): p1, q1 = ttheta = sp.Matrix(sp.symbols('p1, q1')) pdot1, qdot1 = st.time_deriv(ttheta, ttheta) tau1, = ttau = sp.Matrix(sp.symbols('F1,')) params = sp.symbols('m1, m2, l1, l2, s1, s2, J1, J2') m1, m2, l1, l2, s1, s2, J1, J2 = params ex = Matrix([1,0]) 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 #velocity S1d = st.time_deriv(S1, ttheta) S2d = st.time_deriv(S2, ttheta) T_rot = (J1*qdot1**2 + J2*(qdot1 + pdot1)**2) / 2 T_trans = ( m1*S1d.T*S1d + m2*S2d.T*S2d )/2 T = T_rot + T_trans[0] V = 0 mod = mt.generate_symbolic_model(T, V, ttheta, [tau1, 0]) #mod.eqns.simplify() #mod.calc_coll_part_lin_state_eq(simplify=True) mod.calc_lbi_nf_state_eq(simplify=True) 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]]) 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 self.assertEqual(diff.simplify(), 0)
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)) p1, q1, q2 = ttheta = st.row_stack(pp, qq) pdot1, qdot1, qdot2 = tthetad = st.time_deriv(ttheta, ttheta) mud = st.time_deriv(ttheta, ttheta, order=2) params = sp.symbols('l3, l4, s3, s4, J3, J4, m1, m2, m3, m4, g') l3, l4, s3, s4, J3, J4, m1, m2, m3, m4, g = params tau1, tau2 = ttau= st.symb_vector("tau1, tau2") ## Geometry ex = sp.Matrix([1,0]) ey = sp.Matrix([0,1]) # Koordinaten der Schwerpunkte und Gelenke S1 = ex*q1 S2 = ex*q1 + ey*q2 G3 = S2 # Gelenk # Schwerpunkt des Pendels #zeigt nach oben S3 = G3 + mt.Rz(p1)*ey*s3 # Zeitableitungen der Schwerpunktskoordinaten 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 = T_rot + T_trans[0] V = m1*g*S1[1] + m2*g*S2[1] + m3*g*S3[1] external_forces = [0, tau1, tau2] assert not any(external_forces[:np]) mod = mt.generate_symbolic_model(T, V, ttheta, external_forces) mod.calc_coll_part_lin_state_eq(simplify=True) #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 self.assertEqual(mod.ff, ff_ref) self.assertEqual(mod.gg[-3, :], gg_ref_part)
def __init__(self, inverted=True, calc_coll_part_lin=True): self.inverted = inverted self.calc_coll_part_lin = calc_coll_part_lin # ----------------------------------------- # Pendel-Wagen System mit hängendem Pendel # ----------------------------------------- pp = st.symb_vector(("varphi", )) qq = st.symb_vector(("q", )) ttheta = st.row_stack(pp, qq) st.make_global(ttheta) params = sp.symbols('m1, m2, l, g, q_r, t, T') st.make_global(params) ex = sp.Matrix([1, 0]) ey = sp.Matrix([0, 1]) # Koordinaten der Schwerpunkte und Gelenke S1 = ex * q # Schwerpunkt Wagen G2 = S1 # Pendel-Gelenk # Schwerpunkt des Pendels (Pendel zeigt für kleine Winkel nach oben) if inverted: S2 = G2 + mt.Rz(varphi) * ey * l else: S2 = G2 + mt.Rz(varphi) * -ey * l # Zeitableitungen der Schwerpunktskoordinaten S1d, S2d = st.col_split(st.time_deriv(st.col_stack(S1, S2), ttheta)) # Energie E_rot = 0 # (Punktmassenmodell) E_trans = (m1 * S1d.T * S1d + m2 * S2d.T * S2d) / 2 E = E_rot + E_trans[0] V = m2 * g * S2[1] # Partiell linearisiertes Model mod = mt.generate_symbolic_model(E, V, ttheta, [0, sp.Symbol("u")]) mod.calc_state_eq() mod.calc_coll_part_lin_state_eq() self.mod = mod
def test_two_link_manipulator(self): p1, q1 = ttheta = sp.Matrix(sp.symbols('p1, q1')) pdot1, qdot1 = st.time_deriv(ttheta, ttheta) tau1, = ttau = sp.Matrix(sp.symbols('F1,')) params = sp.symbols('m1, m2, l1, l2, s1, s2, J1, J2') m1, m2, l1, l2, s1, s2, J1, J2 = params ex = Matrix([1, 0]) 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 #velocity S1d = st.time_deriv(S1, ttheta) S2d = st.time_deriv(S2, ttheta) T_rot = (J1 * qdot1**2 + J2 * (qdot1 + pdot1)**2) / 2 T_trans = (m1 * S1d.T * S1d + m2 * S2d.T * S2d) / 2 T = T_rot + T_trans[0] V = 0 mod = mt.generate_symbolic_model(T, V, ttheta, [tau1, 0]) #mod.eqns.simplify() #mod.calc_coll_part_lin_state_eq(simplify=True) mod.calc_lbi_nf_state_eq(simplify=True) 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]]) 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 self.assertEqual(diff.simplify(), 0)
def test_unicycle(self): # test the generation of Lagrange-Byrnes-Isidori-Normal form theta = st.symb_vector("p1, q1, q2") p1, q1, q2 = theta params = sp.symbols( 'l1, l2, s1, s2, delta0, delta1, delta2, J0, J1, J2, m0, m1, m2, r, g' ) l1, l2, s1, s2, delta0, delta1, delta2, J0, J1, J2, m0, m1, m2, r, g = params QQ = sp.symbols("Q0, Q1, Q2") Q0, Q1, Q2 = QQ mu = st.time_deriv(theta, theta) p1d, q1d, q2d = mu # Geometry # noinspection PyUnusedLocal ex = Matrix([1, 0]) ey = Matrix([0, 1]) 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 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 = T_rot + T_trans[0] 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) self.assertEqual(mod.gz, None) mod.calc_lbi_nf_state_eq() self.assertEqual(mod.fz.shape, (6, 1)) self.assertEqual(mod.gz, sp.eye(6)[:, 2:4])
def test_simple1(self): 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 V = 0 mod = mt.generate_symbolic_model(T, V, qq, FF) eq = m*q1dd - F1 self.assertEqual(mod.eqns[0], eq) # test the application of the @property M = mod.MM self.assertEqual(M[0], m)
def test_simple1(self): 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 V = 0 mod = mt.generate_symbolic_model(T, V, qq, FF) eq = m * q1dd - F1 self.assertEqual(mod.eqns[0], eq) # test the application of the @property M = mod.MM self.assertEqual(M[0], m)
def test_unicycle(self): # test the generation of Lagrange-Byrnes-Isidori-Normal form theta = st.symb_vector("p1, q1, q2") p1, q1, q2 = theta theta params = sp.symbols('l1, l2, s1, s2, delta0, delta1, delta2, J0, J1, J2, m0, m1, m2, r, g') l1, l2, s1, s2, delta0, delta1, delta2, J0, J1, J2, m0, m1, m2, r, g = params QQ = sp.symbols("Q0, Q1, Q2") Q0, Q1, Q2 = QQ mu = st.time_deriv(theta, theta) p1d, q1d, q2d = mu # Geometry ex = Matrix([1,0]) ey = Matrix([0,1]) 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 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 = T_rot + T_trans[0] V = m1*g*S1[1] + m2*g*S2[1] mod = mt.generate_symbolic_model(T, V, theta, [0, Q1, Q2]) mod.calc_lbi_nf_state_eq()
S1 = G0 + mt.Rz(p1)*ey*s1 S2 = G1 + mt.Rz(p1+p2)*ey*s2 S3 = G2 + mt.Rz(p1+p2+p3)*ey*s3 # Zeitableitungen der Schwerpunktskoordinaten Sd0, Sd1, Sd2, Sd3 = st.col_split(st.time_deriv(st.col_stack(S0, S1, S2, S3), ttheta)) # Energie T_rot = (J1*pdot1**2)/2 + (J2*(pdot1 + pdot2)**2)/2 + (J3*(pdot1 + pdot2 + pdot3)**2)/2 T_trans = (m0*Sd0.T*Sd0 + 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] mod = mt.generate_symbolic_model(T, V, ttheta, [0, 0, 0, tau1]) # Zustandsraummodell, partiell linearisiert mod.calc_coll_part_lin_state_eq(simplify=True) x_dot = mod.ff + mod.gg*qddot1 # Zustandsdefinition anpassen und ZRM speichern replacements = {'Matrix': 'sp.Matrix', 'sin': 'sp.sin', 'cos': 'sp.cos', 'q1': 'x1', 'qdot1': 'x2', 'qddot1': 'u1', 'p1': 'x3', 'pdot1': 'x4', 'p2': 'x5',
def test_simple_constraints(self): q1, q2 = qq = sp.Matrix(sp.symbols('q1, q2')) F1, = FF = sp.Matrix(sp.symbols('F1,')) m = sp.Symbol('m') 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 V = 0 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) # noinspection PyUnusedLocal with self.assertRaises(ValueError) as cm: # no parameters passed dae = mod.calc_dae_eq() dae.generate_eqns_funcs() dae = mod.calc_dae_eq(parameter_values=[(m, 1)]) ttheta_1, ttheta_d_1 = dae.calc_consistent_conf_vel(q1=0.123, _disp=False) self.assertTrue(npy.allclose(ttheta_1, npy.array([0.123, 0.123]))) self.assertTrue(npy.allclose(ttheta_d_1, npy.array([0.0, 0.0]))) ttheta_2, ttheta_d_2 = dae.calc_consistent_conf_vel(q2=567, qdot2=-100, _disp=False) self.assertTrue(npy.allclose(ttheta_2, npy.array([567., 567.]))) self.assertTrue(npy.allclose(ttheta_d_2, npy.array([-100.0, -100.0]))) # noinspection PyUnusedLocal with self.assertRaises(ValueError) as cm: # wrong argument dae.calc_consistent_conf_vel(q2=567, q2d=-100, _disp=False) # noinspection PyUnusedLocal with self.assertRaises(ValueError) as cm: # unexpected argument dae.calc_consistent_conf_vel(q2=567, qdot2=-100, foobar="fnord", _disp=False) 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.assertTrue(npy.allclose(b, [1, 0, 0])) # in that situation there is no force acc_1, llmd_1 = dae.calc_consistent_accel_lmd((ttheta_1, ttheta_d_1)) self.assertTrue(npy.allclose(acc_1, [0, 0])) self.assertTrue(npy.allclose(llmd_1, [0, 0])) # in that situation there is also no force acc_2, llmd_2 = dae.calc_consistent_accel_lmd((ttheta_2, ttheta_d_2)) self.assertTrue(npy.allclose(acc_2, [0, 0])) self.assertTrue(npy.allclose(llmd_2, [0, 0])) yy_1, yyd_1 = dae.calc_consistent_init_vals(q2=12.7, qdot2=100) dae.generate_eqns_funcs() res = dae.model_func(0, yy_1, yyd_1) self.assertEqual(res.shape, (dae.ntt * 2 + dae.nll, )) self.assertTrue(npy.allclose(res, 0))
def test_cart_pole(self): p1, q1 = ttheta = sp.Matrix(sp.symbols('p1, q1')) F1, = FF = sp.Matrix(sp.symbols('F1,')) params = sp.symbols('m0, m1, l1, g') m0, m1, l1, g = params pdot1 = st.time_deriv(p1, ttheta) # q1dd = st.time_deriv(q1, ttheta, order=2) 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 #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 = T_rot + T_trans[0] V = m1*g*S1[1] with self.assertRaises(ValueError) as cm: # wrong length of external forces mt.generate_symbolic_model(T, V, ttheta, [F1]) with self.assertRaises(ValueError) as cm: # wrong length of external forces mt.generate_symbolic_model(T, V, ttheta, [F1, 0, 0]) QQ = sp.Matrix([0, F1]) 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]]) self.assertEqual(M, M_ref) rest = mod.eqns.subs(st.zip0((mod.ttdd))) rest_ref = Matrix([[g*l1*m1*sin(p1)], [-F1 - l1*m1*pdot1**2*sin(p1)]]) self.assertEqual(M, M_ref) mod.calc_state_eq(simplify=True) mod.calc_coll_part_lin_state_eq(simplify=True) 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]]) self.assertEqual(mod.ff, ff_ref) self.assertEqual(mod.gg, gg_ref)
def solve(problem_spec): # problem_spec is dummy t = sp.Symbol('t') # time variable np = 2 nq = 2 ns = 2 n = np + nq + ns p1, p2 = pp = st.symb_vector("p1:{0}".format(np + 1)) q1, q2 = qq = st.symb_vector("q1:{0}".format(nq + 1)) s1, s2 = ss = st.symb_vector("s1:{0}".format(ns + 1)) ttheta = st.row_stack(qq[0], pp[0], ss[0], qq[1], pp[1], ss[1]) qdot1, pdot1, sdot1, qdot2, pdot2, sdot2 = tthetad = st.time_deriv(ttheta, ttheta) tthetadd = st.time_deriv(ttheta, ttheta, order=2) ttheta_all = st.concat_rows(ttheta, tthetad, tthetadd) c1, c2, c3, c4, c5, c6, m1, m2, m3, m4, m5, m6, J1, J2, J3, J4, J5, J6, l1, l2, l3, l4, l5, l6, d, g = params = sp.symbols( 'c1, c2, c3, c4, c5, c6, m1, m2, m3, m4, m5, m6, J1, J2, J3, J4, J5, J6, l1, l2, l3, l4, l5, l6, d, g') tau1, tau2, tau3, tau4, tau5, tau6 = ttau = st.symb_vector("tau1, tau2, tau3, tau4, tau5, tau6 ") # unit vectors ex = sp.Matrix([1, 0]) ey = sp.Matrix([0, 1]) # coordinates of centers of mass and joints # left G0 = 0 * ex ##: C1 = G0 + Rz(q1) * ex * c1 ##: G1 = G0 + Rz(q1) * ex * l1 ##: C2 = G1 + Rz(q1 + p1) * ex * c2 ##: G2 = G1 + Rz(q1 + p1) * ex * l2 ##: C3 = G2 + Rz(q1 + p1 + s1) * ex * c3 ##: G3 = G2 + Rz(q1 + p1 + s1) * ex * l3 ##: # right G6 = d * ex ##: C6 = G6 + Rz(q2) * ex * c6 ##: G5 = G6 + Rz(q2) * ex * l6 ##: C5 = G5 + Rz(q2 + p2) * ex * c5 ##: G4 = G5 + Rz(q2 + p2) * ex * l5 ##: C4 = G4 + Rz(q2 + p2 + s2) * ex * c4 ##: G3b = G4 + Rz(q2 + p2 + s2) * ex * l4 ##: # time derivatives of centers of mass Sd1, Sd2, Sd3, Sd4, Sd5, Sd6 = st.col_split(st.time_deriv(st.col_stack(C1, C2, C3, C4, C5, C6), ttheta)) # Kinetic Energy (note that angles are relative) T_rot = (J1 * qdot1 ** 2) / 2 + (J2 * (qdot1 + pdot1) ** 2) / 2 + (J3 * (qdot1 + pdot1 + sdot1) ** 2) / 2 + \ (J4 * (qdot2 + pdot2 + sdot2) ** 2) / 2 + (J5 * (qdot2 + pdot2) ** 2) / 2 + (J6 * qdot2 ** 2) / 2 T_trans = ( m1 * Sd1.T * Sd1 + m2 * Sd2.T * Sd2 + m3 * Sd3.T * Sd3 + m4 * Sd4.T * Sd4 + m5 * Sd5.T * Sd5 + m6 * Sd6.T * Sd6) / 2 T = T_rot + T_trans[0] # Potential Energy V = m1 * g * C1[1] + m2 * g * C2[1] + m3 * g * C3[1] + m4 * g * C4[1] + m5 * g * C5[1] + m6 * g * C6[1] parameter_values = list(dict(c1=0.4 / 2, c2=0.42 / 2, c3=0.55 / 2, c4=0.55 / 2, c5=0.42 / 2, c6=0.4 / 2, m1=6.0, m2=12.0, m3=39.6, m4=39.6, m5=12.0, m6=6.0, J1=(6 * 0.4 ** 2) / 12, J2=(12 * 0.42 ** 2) / 12, J3=(39.6 * 0.55 ** 2) / 12, J4=(39.6 * 0.55 ** 2) / 12, J5=(12 * 0.42 ** 2) / 12, J6=(6 * 0.4 ** 2) / 12, l1=0.4, l2=0.42, l3=0.55, l4=0.55, l5=0.42, l6=0.4, d=0.26, g=9.81).items()) external_forces = [tau1, tau2, tau3, tau4, tau5, tau6] dir_of_this_file = os.path.dirname(os.path.abspath(sys.modules.get(__name__).__file__)) fpath = os.path.join(dir_of_this_file, "7L-dae-2020-07-15.pcl") if not os.path.isfile(fpath): # if model is not present it could be regenerated # however this might take long (approx. 20min) mod = mt.generate_symbolic_model(T, V, ttheta, external_forces, constraints=[G3 - G3b], simplify=False) mod.calc_state_eq(simplify=False) mod.f_sympy = mod.f.subs(parameter_values) mod.G_sympy = mod.g.subs(parameter_values) with open(fpath, "wb") as pfile: pickle.dump(mod, pfile) else: with open(fpath, "rb") as pfile: mod = pickle.load(pfile) # calculate DAE equations from symbolic model dae = mod.calc_dae_eq(parameter_values) dae.generate_eqns_funcs() torso1_unit = Rz(q1 + p1 + s1) * ex torso2_unit = Rz(q2 + p2 + s2) * ex neck_length = 0.12 head_radius = 0.1 body_width = 15 neck_width = 15 H1 = G3 + neck_length * torso1_unit H1r = G3 + (neck_length - head_radius) * torso1_unit H2 = G3b + neck_length * torso2_unit H2r = G3b + (neck_length - head_radius) * torso2_unit vis = vt.Visualiser(ttheta, xlim=(-1.5, 1.5), ylim=(-.2, 2)) # get default colors and set them explicitly # this prevents color changes in onion skin plot default_colors = plt.get_cmap("tab10") guy1_color = default_colors(0) guy1_joint_color = "darkblue" guy2_color = default_colors(1) guy2_joint_color = "red" guy1_head_fc = guy1_color # facecolor guy1_head_ec = guy1_head_fc # edgecolor guy2_head_fc = guy2_color # facecolor guy2_head_ec = guy2_head_fc # edgecolor # guy 1 body vis.add_linkage(st.col_stack(G0, G1, G2, G3).subs(parameter_values), color=guy1_color, solid_capstyle='round', lw=body_width, ms=body_width, mfc=guy1_joint_color) # guy 1 neck # vis.add_linkage(st.col_stack(G3, H1r).subs(parameter_values), color=head_color, solid_capstyle='round', lw=neck_width) # guy 1 head vis.add_disk(st.col_stack(H1, H1r).subs(parameter_values), fc=guy1_head_fc, ec=guy1_head_ec, plot_radius=False, fill=True) # guy 2 body vis.add_linkage(st.col_stack(G6, G5, G4, G3b).subs(parameter_values), color=guy2_color, solid_capstyle='round', lw=body_width, ms=body_width, mfc=guy2_joint_color) # guy 2 neck # vis.add_linkage(st.col_stack(G3b, H2r).subs(parameter_values), color=head_color, solid_capstyle='round', lw=neck_width) # guy 2 head vis.add_disk(st.col_stack(H2, H2r).subs(parameter_values), fc=guy2_head_fc, ec=guy2_head_ec, plot_radius=False, fill=True) eq_stat = mod.eqns.subz0(tthetadd).subz0(tthetad).subs(parameter_values) # vector for tau and lambda together ttau_symbols = sp.Matrix(mod.uu) ##:T mmu = st.row_stack(ttau_symbols, mod.llmd) ##:T # linear system of equations (and convert to function w.r.t. ttheta) K0_expr = eq_stat.subz0(mmu) ##:i K1_expr = eq_stat.jacobian(mmu) ##:i K0_func = st.expr_to_func(ttheta, K0_expr) K1_func = st.expr_to_func(ttheta, K1_expr, keep_shape=True) def get_mu_stat_for_theta(ttheta_arg, rho=10): # weighting matrix for mu K0 = K0_func(*ttheta_arg) K1 = K1_func(*ttheta_arg) return solve_qlp(K0, K1, rho) def solve_qlp(K0, K1, rho): R_mu = npy.diag([1, 1, 1, rho, rho, rho, .1, .1]) n1, n2 = K1.shape # construct the equation system for least squares with linear constraints M1 = npy.column_stack((R_mu, K1.T)) M2 = npy.column_stack((K1, npy.zeros((n1, n1)))) M_coeff = npy.row_stack((M1, M2)) M_rhs = npy.concatenate((npy.zeros(n2), -K0)) mmu_stat = npy.linalg.solve(M_coeff, M_rhs)[:n2] return mmu_stat ttheta_start = npy.r_[0.9, 1.5, -1.9, 2.1, -2.175799453493845, 1.7471971159642905] mmu_start = get_mu_stat_for_theta(ttheta_start) connection_point_func = st.expr_to_func(ttheta, G3.subs(parameter_values)) cs_ttau = mpc.casidify(mod.uu, mod.uu)[0] cs_llmd = mpc.casidify(mod.llmd, mod.llmd)[0] controls_sp = mmu controls_cs = cs.vertcat(cs_ttau, cs_llmd) coords_cs, _ = mpc.casidify(ttheta, ttheta) # parameters: 0: value of y_connection, 1: x_connection_last, # 2: y_connection_last, 3: delta_r_max, 4: rho (penalty factor for 2nd persons torques), # 5:11: ttheta_old[6], 11:17: ttheta:old2 # P = SX.sym('P', 5 + 12) rho = P[10] # weightning of inputs R = mpc.SX_diag_matrix((1, 1, 1, rho, rho, rho, 0.1, 0.1)) # Construction of Constraints g1 = [] # constraints vector (system dynamics) g2 = [] # inequality-constraints closed_chain_constraint, _ = mpc.casidify(mod.dae.constraints, ttheta, cs_vars=coords_cs) connection_position, _ = mpc.casidify(list(G3.subs(parameter_values)), ttheta, cs_vars=coords_cs) ##:i connection_y_value, _ = mpc.casidify([G3[1].subs(parameter_values)], ttheta, cs_vars=coords_cs) ##:i stationary_eqns, _, _ = mpc.casidify(eq_stat, ttheta, controls_sp, cs_vars=(coords_cs, controls_cs)) ##:i g1.extend(mpc.unpack(stationary_eqns)) g1.extend(mpc.unpack(closed_chain_constraint)) # force the connecting joint to a given hight (which will be provided later) g1.append(connection_y_value - P[0]) ng1 = len(g1) # squared distance from the last reference should be smaller than P[3] (delta_r_max): # this will be a restriction between -inf and 0 r = connection_position - P[1:3] g2.append(r.T @ r - P[3]) # change of angles should be smaller than a given bound (P[5:11] are the old coords) coords_old = P[5:11] coords_old2 = P[11:17] pseudo_vel = (coords_cs - coords_old) / 1 pseudo_acc = (coords_cs - 2 * coords_old + coords_old2) / 1 g2.extend(mpc.unpack(pseudo_vel)) g2.extend(mpc.unpack(pseudo_acc)) g_all = mpc.seq_to_SX_matrix(g1 + g2) ### Construction of objective Function obj = controls_cs.T @ R @ controls_cs + 1e5 * pseudo_acc.T @ pseudo_acc + 0.3e6 * pseudo_vel.T @ pseudo_vel OPT_variables = cs.vertcat(coords_cs, controls_cs) # for debugging g_all_cs_func = cs.Function("g_all_cs_func", (OPT_variables, P), (g_all,)) nlp_prob = dict(f=obj, x=OPT_variables, g=g_all, p=P) ipopt_settings = dict(max_iter=5000, print_level=0, acceptable_tol=1e-8, acceptable_obj_change_tol=1e-6) opts = dict(print_time=False, ipopt=ipopt_settings) xx_guess = npy.r_[ttheta_start, mmu_start] # note: g1 contains the equality constraints (system dynamics) (lower bound = upper bound) delta_phi = .05 d_delta_phi = .02 eps = 1e-9 lbg = npy.r_[[-eps] * ng1 + [-inf] + [-delta_phi] * n, [-d_delta_phi] * n] ubg = npy.r_[[eps] * ng1 + [0] + [delta_phi] * n, [d_delta_phi] * n] # ubx = [inf]*OPT_variables.shape[0]##: # lower and upper bounds for decision variables: # lbx = [-inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf]*N1 + [tau1_min, tau4_min, -inf, -inf]*N # ubx = [inf, inf, inf, inf, inf, inf, inf, inf]*N1 + [tau1_max, tau4_max, inf, inf]*N rho = 3 P_init = npy.r_[connection_point_func(*ttheta_start)[1], connection_point_func(*ttheta_start), 0.01, rho, ttheta_start, ttheta_start] args = dict(lbx=-inf, ubx=inf, lbg=lbg, ubg=ubg, # unconstrained optimization p=P_init, # initial and final state x0=xx_guess # initial guess ) solver = cs.nlpsol("solver", "ipopt", nlp_prob, opts) sol = solver(**args) global_vars = ipydex.Container(old_sol=xx_guess, old_sol2=xx_guess) def get_optimal_equilibrium(y_value, rho=3): ttheta_old = global_vars.old_sol[:n] ttheta_old2 = global_vars.old_sol2[:n] opt_prob_params = npy.r_[y_value, connection_point_func(*ttheta_old), 0.01, rho, ttheta_old, ttheta_old2] args.update(dict(p=opt_prob_params, x0=global_vars.old_sol)) sol = solver(**args) stats = solver.stats() if not stats['success']: raise ValueError(stats["return_status"]) XX = sol["x"].full().squeeze() # save the last two results global_vars.old_sol2 = global_vars.old_sol global_vars.old_sol = XX return XX y_start = connection_point_func(*ttheta_start)[1] N = 100 y_end = 1.36 y_func = st.expr_to_func(t, st.condition_poly(t, (0, y_start, 0, 0), (1, y_end, 0, 0))) def get_qs_trajectory(rho): pseudo_time = npy.linspace(0, 1, N) yy_connection = y_func(pseudo_time) # reset the initial guess global_vars.old_sol = xx_guess global_vars.old_sol2 = xx_guess XX_list = [] for i, y_value in enumerate(yy_connection): # print(i, y_value) XX_list.append(get_optimal_equilibrium(y_value, rho=rho)) XX = npy.array(XX_list) return XX rho = 30 XX = get_qs_trajectory(rho=rho) def smooth_time_scaling(Tend, N, phase_fraction=.5): """ :param Tend: :param N: :param phase_fraction: fraction of Tend for smooth initial and end phase """ T0 = 0 T1 = Tend * phase_fraction y0 = 0 y1 = 1 # for initial phase poly1 = st.condition_poly(t, (T0, y0, 0, 0), (T1, y1, 0, 0)) # for end phase poly2 = poly1.subs(t, Tend - t) # there should be a phase in the middle with constant slope deriv_transition = st.piece_wise((y0, t < T0), (poly1, t < T1), (y1, t < Tend - T1), (poly2, t < Tend), (y0, True)) scaling = sp.integrate(deriv_transition, (t, T0, Tend)) time_transition = sp.integrate(deriv_transition * N / scaling, t) # deriv_transition_func = st.expr_to_func(t, full_transition) time_transition_func = st.expr_to_func(t, time_transition) deriv_func = st.expr_to_func(t, deriv_transition * N / scaling) deriv_func2 = st.expr_to_func(t, deriv_transition.diff(t) * N / scaling) C = ipydex.Container(fetch_locals=True) return C N = XX.shape[0] Tend = 4 res = smooth_time_scaling(Tend, N) def get_derivatives(XX, time_scaling, res=100): """ :param XX: Nxm array :param time_scaling: container for time scaling :param res: time resolution of the returned arrays """ N = XX.shape[0] Tend = time_scaling.Tend assert npy.isclose(time_scaling.time_transition_func([0, Tend])[-1], N) tt = npy.linspace(time_scaling.T0, time_scaling.Tend, res) NN = npy.arange(N) # high_resolution version of index arry NN2 = npy.linspace(0, N, res, endpoint=False) # time-scaled verion of index-array NN3 = time_scaling.time_transition_func(tt) NN3d = time_scaling.deriv_func(tt) NN3dd = time_scaling.deriv_func2(tt) XX_num, XXd_num, XXdd_num = [], [], [] # iterate over every column for col in XX.T: spl = splrep(NN, col) # function value and derivatives XX_num.append(splev(NN3, spl)) XXd_num.append(splev(NN3, spl, der=1)) XXdd_num.append(splev(NN3, spl, der=2)) XX_num = npy.array(XX_num).T XXd_num = npy.array(XXd_num).T XXdd_num = npy.array(XXdd_num).T NN3d_bc = npy.broadcast_to(NN3d, XX_num.T.shape).T NN3dd_bc = npy.broadcast_to(NN3dd, XX_num.T.shape).T XXd_n = XXd_num * NN3d_bc # apply chain rule XXdd_n = XXdd_num * NN3d_bc ** 2 + XXd_num * NN3dd_bc C = ipydex.Container(fetch_locals=True) return C C = XX_derivs = get_derivatives(XX[:, :], time_scaling=res) expr = mod.eqns.subz0(mod.uu, mod.llmd).subs(parameter_values) dynterm_func = st.expr_to_func(ttheta_all, expr) def get_torques(dyn_term_func, XX_derivs, static1=False, static2=False): ttheta_num_all = npy.c_[XX_derivs.XX_num[:, :n], XX_derivs.XXd_n[:, :n], XX_derivs.XXdd_n[:, :n]] ##:S if static1: # set velocities to 0 ttheta_num_all[:, n:2 * n] = 0 if static2: # set accelerations to 0 ttheta_num_all[:, 2 * n:] = 0 res = dynterm_func(*ttheta_num_all.T) return res lhs_static = get_torques(dynterm_func, XX_derivs, static1=True, static2=True) ##:i lhs_dynamic = get_torques(dynterm_func, XX_derivs, static2=False) ##:i mmu_stat_list = [] for L_k_stat, L_k_dyn, ttheta_k in zip(lhs_static, lhs_dynamic, XX_derivs.XX_num[:, :n]): K1_k = K1_func(*ttheta_k) mmu_stat_k = solve_qlp(L_k_stat, K1_k, rho) mmu_stat_list.append(mmu_stat_k) mmu_stat_all = npy.array(mmu_stat_list) solution_data = SolutionData() solution_data.tt = XX_derivs.tt solution_data.xx = XX_derivs.XX_num solution_data.mmu = mmu_stat_all solution_data.vis = vis save_plot(problem_spec, solution_data) return solution_data
def test_cart_pole(self): p1, q1 = ttheta = sp.Matrix(sp.symbols('p1, q1')) F1, = FF = sp.Matrix(sp.symbols('F1,')) params = sp.symbols('m0, m1, l1, g') m0, m1, l1, g = params pdot1 = st.time_deriv(p1, ttheta) # q1dd = st.time_deriv(q1, ttheta, order=2) 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 #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 = T_rot + T_trans[0] V = m1 * g * S1[1] with self.assertRaises(ValueError) as cm: # wrong length of external forces mt.generate_symbolic_model(T, V, ttheta, [F1]) with self.assertRaises(ValueError) as cm: # wrong length of external forces mt.generate_symbolic_model(T, V, ttheta, [F1, 0, 0]) QQ = sp.Matrix([0, F1]) 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]]) self.assertEqual(M, M_ref) rest = mod.eqns.subs(st.zip0((mod.ttdd))) rest_ref = Matrix([[g * l1 * m1 * sin(p1)], [-F1 - l1 * m1 * pdot1**2 * sin(p1)]]) self.assertEqual(M, M_ref) mod.calc_state_eq(simplify=True) mod.calc_coll_part_lin_state_eq(simplify=True) 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]]) self.assertEqual(mod.ff, ff_ref) self.assertEqual(mod.gg, gg_ref)
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))) ttheta = st.row_stack(pp, qq) Q1, Q2 = sp.symbols('Q1, Q2') p1_d, q1_d, q2_d = mu = st.time_deriv(ttheta, ttheta) p1_dd, q1_dd, q2_dd = mu_d = st.time_deriv(ttheta, ttheta, order=2) p1, q1, q2 = ttheta # reordering according to chain kk = sp.Matrix([q1, q2, p1]) kd1, kd2, kd3 = q1_d, q2_d, p1_d params = sp.symbols( 'l1, l2, l3, l4, s1, s2, s3, s4, J1, J2, J3, J4, m1, m2, m3, m4, g' ) l1, l2, l3, l4, s1, s2, s3, s4, J1, J2, J3, J4, m1, m2, m3, m4, g = params # geometry 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 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 G3 = G2 + mt.Rz(sum(kk[:3])) * mey * l3 # velocities of joints and center of inertia Sd1 = st.time_deriv(S1, ttheta) Sd2 = st.time_deriv(S2, ttheta) 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 = T_rot + T_trans[0] 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]]) self.assertEqual(eqns_ref, mod.eqns)
def test_four_bar_constraints(self): # 2 passive Gelenke np = 2 nq = 1 p1, p2 = pp = st.symb_vector("p1:{0}".format(np + 1)) q1, = qq = st.symb_vector("q1:{0}".format(nq + 1)) ttheta = st.row_stack(pp, qq) pdot1, pdot2, qdot1 = tthetad = st.time_deriv(ttheta, ttheta) tthetadd = st.time_deriv(ttheta, ttheta, order=2) st.make_global(ttheta, tthetad, tthetadd) params = sp.symbols( 's1, s2, s3, m1, m2, m3, J1, J2, J3, l1, l2, l3, kappa1, kappa2, g' ) 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() # ttau = sp.symbols('tau') tau1, tau2 = st.symb_vector("tau1, tau2") # unit vectors ex = sp.Matrix([1, 0]) # Basis 1 and 2 # B1 = sp.Matrix([0, 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 # one link manioulator 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 = T_rot + T_trans[0] # potential energy V = m1 * g * S1[1] + m2 * g * S2[1] + m3 * g * S3[1] # # Kinetische Energie mit Platzhaltersymbolen einführen (jetzt nicht): # # M1, M2, M3 = MM = st.symb_vector('M1:4') # MM_subs = [(J1 + m1*s1 ** 2 + m2*l1 ** 2, M1), (J2 + m2*s2 ** 2, M2), (m2*l1*s2, M3)] # # MM_rplm = st.rev_tuple(MM_subs) # Umkehrung der inneren Tupel -> [(M1, J1+... ), ...] external_forces = [0, 0, tau1] mod = mt.generate_symbolic_model(T, V, ttheta, external_forces, constraints=[G2 - G2b]) self.assertEqual(mod.llmd.shape, (2, 1)) self.assertEqual(mod.constraints.shape, (2, 1)) dae = mod.calc_dae_eq(parameter_values=parameter_values) nc = len(mod.llmd) 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) 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)) self.assertTrue(npy.allclose(ttheta_d_1, eres_v)) # in that situation there is no force 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])) # qdot1 ≠ 0 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.]) self.assertTrue(npy.allclose(ttheta_2, eres_c)) self.assertTrue(npy.allclose(ttheta_d_2, eres_v)) yy_1, yyd_1 = dae.calc_consistent_init_vals(q1=12.7, qdot1=100) dae.generate_eqns_funcs() res = dae.model_func(0, yy_1, yyd_1) self.assertEqual(res.shape, (dae.ntt * 2 + dae.nll, )) self.assertTrue(npy.allclose(res, 0))
def test_cart_pole(self): p1, q1 = ttheta = sp.Matrix(sp.symbols('p1, q1')) F1, = FF = sp.Matrix(sp.symbols('F1,')) params = sp.symbols('m0, m1, l1, g') parameter_values = list(dict(m0=2.0, m1=1.0, l1=2.0, g=9.81).items()) m0, m1, l1, g = params pdot1 = st.time_deriv(p1, ttheta) # q1dd = st.time_deriv(q1, ttheta, order=2) 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 # 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 = T_rot + T_trans[0] V = m1 * g * S1[1] # noinspection PyUnusedLocal with self.assertRaises(ValueError) as cm: # wrong length of external forces mt.generate_symbolic_model(T, V, ttheta, [F1]) # noinspection PyUnusedLocal with self.assertRaises(ValueError) as cm: # wrong length of external forces mt.generate_symbolic_model(T, V, ttheta, [F1, 0, 0]) QQ = sp.Matrix([0, F1]) 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]]) 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)]]) self.assertEqual(M, M_ref) mod.calc_state_eq(simplify=True) mod.calc_coll_part_lin_state_eq(simplify=True) # ensure that recalculation is carried out only when we want it tmp_f, tmp_g = mod.f, mod.g mod.f, mod.g = "foo", "bar" mod.calc_state_eq(simplify=True) self.assertEqual(mod.f, "foo") mod.calc_state_eq(simplify=True, force_recalculation=True) self.assertEqual((mod.f, mod.g), (tmp_f, tmp_g)) 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]]) self.assertEqual(mod.ff, ff_ref) self.assertEqual(mod.gg, gg_ref) 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]]) be = sp.Matrix([0, 0, -0.5, 1]) self.assertEqual(A, Ae) self.assertEqual(b, be) 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]]) be = sp.Matrix([0, 0, -.25, 0.5]) self.assertEqual(A, Ae) self.assertEqual(b, be)
def __init__(self, n_p=1): self.n_p = n_p # passive Koordinate n_q = 1 q = sp.symbols("q") if n_p > 0: pp = st.symb_vector("p1:{0}".format(n_p + 1)) else: pp = [] theta = st.row_stack(q, pp) thetadot = st.time_deriv(theta, theta) mass_sym = sp.symbols(", ".join([f'm{n}' for n in range(n_p+n_q)])) damping_sym = sp.symbols(", ".join([f'd{n}' for n in range(n_p+n_q)])) stiffness_sym = sp.symbols(", ".join([f'c{n}' for n in range(n_p+n_q)])) ttau = sp.symbols(", ".join([f'tau{n}' for n in range(n_p+n_q)])) if not isinstance(mass_sym, tuple): mass_sym = [mass_sym] if not isinstance(damping_sym, tuple): damping_sym = [damping_sym] if not isinstance(stiffness_sym, tuple): stiffness_sym = [stiffness_sym] if not isinstance(ttau, tuple): ttau = [ttau] # Koordinaten der Massenschwerpunkte S = [q] + [p for p in pp] # Zeitableitungen der Schwerpunktskoordinaten Sdot = [st.time_deriv(s, theta) for s in S] # Energie # Kinetische Energie T = 0.5 * sum([m * sdot**2 for m, sdot in zip(mass_sym, Sdot)]) # Potentielle Energie if n_p > 0: #V = 0.5 * sum([c * p**2 for p, c in zip(pp, stiffness_sym)]) S = [0] + S V = 0.5 * sum([c * (S[i+1] - S[i])**2 for i, c in enumerate(stiffness_sym)]) else: V = 0 # Partiell linearisiertes Model mod = mt.generate_symbolic_model(T, V, theta, ttau) Sdot = [0] + Sdot F_diss = [d * (Sdot[i+1] - Sdot[i]) for i, d in enumerate(damping_sym)] + [0] tau_subs = [[tau, - F_diss[i] + F_diss[i+1]] for i, tau in enumerate(ttau)] tau_subs[0][1] += ttau[0] mod.eqns = sp.simplify(mod.eqns.subs(tau_subs)) mod.calc_state_eq() self.mod = mod self.mod.g = self.mod.g[:, 0]