from pynamics.variable_types import Differentiable, Constant from pynamics.system import System from pynamics.body import Body from pynamics.dyadic import Dyadic from pynamics.output import Output, PointsOutput from pynamics.particle import Particle import pynamics.integration import numpy import matplotlib.pyplot as plt plt.ion() from math import pi system = System() pynamics.set_system(__name__, system) e0, e0_d, e0_dd = Differentiable('e0') e1, e1_d, e1_dd = Differentiable('e1') e2, e2_d, e2_dd = Differentiable('e2') e3, e3_d, e3_dd = Differentiable('e3') qA, qA_d, qA_dd = Differentiable('qA') qB, qB_d, qB_dd = Differentiable('qB') qC, qC_d, qC_dd = Differentiable('qC') N = Frame('N', system) A = Frame('A', system) B = Frame('B', system) v = e1 * N.x + e2 * N.y + e3 * N.z eq = e0**2 + e1**2 + +e2**2 + e3**2 - 1
tstep = 1 / fps t = numpy.r_[tinitial:tfinal:tstep] force = t * 0 ii = (t == 3).nonzero()[0][0] jj = (t == 5).nonzero()[0][0] force[ii:jj] = 10 f_force = scipy.interpolate.interp1d(t, force, fill_value='extrapolate') # ### Differentiable State Variables # # Define your differentiable state variables that you will use to model the state of the system. In this case $qA$, $qB$, and $qC$ are the rotation angles of a three-link mechanism # In[8]: x, x_d, x_dd = Differentiable('x', system) y, y_d, y_dd = Differentiable('y', system) qA, qA_d, qA_dd = Differentiable('qA', system) qB, qB_d, qB_dd = Differentiable('qB', system) qC, qC_d, qC_dd = Differentiable('qC', system) x2, x2_d, x2_dd = Differentiable('x2', system) # ### Initial Values # Define a set of initial values for the position and velocity of each of your state variables. It is necessary to define a known. This code create a dictionary of initial values. # In[9]: initialvalues = {} initialvalues[x] = 0 initialvalues[x_d] = 0 initialvalues[y] = 2.01
import numpy import matplotlib.pyplot as plt plt.ion() import math system = System() pynamics.set_system(__name__,system) g = Constant(9.81,'g',system) tol = 1e-5 tinitial = 0 tfinal = 5 tstep = 1/30 t = numpy.r_[tinitial:tfinal:tstep] qA,qA_d,qA_dd = Differentiable('qA') qB,qB_d,qB_dd = Differentiable('qB') qC,qC_d,qC_dd = Differentiable('qC') mC = Constant(1,'mC') Ixx = Constant(2,'Ixx') Iyy = Constant(3,'Iyy') Izz = Constant(1,'Izz') initialvalues = {} initialvalues[qA]=0*math.pi/180 initialvalues[qA_d]=1 initialvalues[qB]=0*math.pi/180 initialvalues[qB_d]=1 initialvalues[qC]=0*math.pi/180
lA = Constant(1, 'lA', system) mA = Constant(1, 'mA', system) g = Constant(9.81, 'g', system) b = Constant(1e0, 'b', system) k = Constant(1e1, 'k', system) tinitial = 0 tfinal = 5 tstep = 1 / 30 t = numpy.r_[tinitial:tfinal:tstep] preload1 = Constant(0 * pi / 180, 'preload1', system) qA, qA_d, qA_dd = Differentiable('qA', system) x, x_d, x_dd = Differentiable('x', system) y, y_d, y_dd = Differentiable('y', system) initialvalues = {} initialvalues[qA] = 0 * pi / 180 initialvalues[qA_d] = 0 * pi / 180 initialvalues[x] = 1 initialvalues[y] = 0 initialvalues[x_d] = 0 initialvalues[y_d] = 0 statevariables = system.get_state_variables() ini = [initialvalues[item] for item in statevariables] N = Frame('N')
mB = Constant(.0145,'mB',system) zero = Constant(0,'zero',system) Ixx_A = Constant(8.6e-007,'Ixx_A',system) Iyy_A = Constant(2.2e-006,'Iyy_A',system) Izz_A = Constant(2.2e-006,'Izz_A',system) Ixx_B = Constant(8.6e-007,'Ixx_B',system) Iyy_B = Constant(2.2e-006,'Iyy_B',system) Izz_B = Constant(2.2e-006,'Izz_B',system) b = Constant(0.00001,'b',system) k = Constant(0.1,'k',system) #accelerationvariable('xA') #accelerationvariable('yA') qA,qA_d,qA_dd = Differentiable(system,'qA') xB,xB_d,xB_dd = Differentiable(system,'xB') yB,yB_d,yB_dd = Differentiable(system,'yB') qB,qB_d,qB_dd = Differentiable(system,'qB') fNAx = Variable('fNAx') fNAy = Variable('fNAy') fABx = Variable('fABx') fABy = Variable('fABy') initialvalues = {} #initialvalues[xA]=.02 #initialvalues[xA_d]=0 #initialvalues[yA]=0 #initialvalues[yA_d]=0 initialvalues[qA]=0*pi/180
lA = Constant('lA', 1, system) mA = Constant('mA', 1, system) g = Constant('g', 9.81, system) b = Constant('b', 1e0, system) k = Constant('k', 1e1, system) tinitial = 0 tfinal = 5 tstep = .001 t = numpy.r_[tinitial:tfinal:tstep] preload1 = Constant('preload1', 0 * pi / 180, system) qA, qA_d, qA_dd = Differentiable(system, 'qA') initialvalues = {} initialvalues[qA] = 0 * pi / 180 initialvalues[qA_d] = 0 * pi / 180 statevariables = system.get_q(0) + system.get_q(1) ini = [initialvalues[item] for item in statevariables] N = Frame('N') A = Frame('A') system.set_newtonian(N) A.rotate_fixed_axis_directed(N, [0, 0, 1], qA, system) pNA = 0 * N.x
#constants[I_main] = 1 constants[g] = 9.81 constants[b_joint] = .00025 constants[k_joint] = .0229183 constants[b_beam] = 0 constants[k_beam] = 14.0224 constants[stall_torque] = .00016 * 5.7 / 4.3586 constants[k_constraint] = 1e3 constants[b_constraint] = 1e1 constants[preload1] = 0 * pi / 180 constants[preload2] = 0 * pi / 180 constants[preload3] = -180 * pi / 180 constants[preload4] = 0 * pi / 180 constants[preload5] = 180 * pi / 180 x, x_d, x_dd = Differentiable(name='x', system=system) y, y_d, y_dd = Differentiable(name='y', system=system) qO, qO_d, qO_dd = Differentiable(name='qO', system=system) qA1, qA1_d, qA1_dd = Differentiable(name='qA1', system=system) qB1, qB1_d, qB1_dd = Differentiable(name='qB1', system=system) qC1, qC1_d, qC1_dd = Differentiable(name='qC1', system=system) qD1, qD1_d, qD1_dd = Differentiable(name='qD1', system=system) qA2, qA2_d, qA2_dd = Differentiable(name='qA2', system=system) qB2, qB2_d, qB2_dd = Differentiable(name='qB2', system=system) qC2, qC2_d, qC2_dd = Differentiable(name='qC2', system=system) qD2, qD2_d, qD2_dd = Differentiable(name='qD2', system=system) initialvalues = { x: 0, x_d: 0, y: .5,
V = Constant(name='V', system=system) R = Constant(name='R', system=system) Im = Constant(name='Im', system=system) Il = Constant(name='Il', system=system) Ib = Constant(name='Ib', system=system) G = Constant(name='G', system=system) b = Constant(name='b', system=system) kv = Constant(name='kv', system=system) kt = Constant(name='kt', system=system) m_pendulum = Constant(name='m_pendulum', system=system) m_motor = Constant(name='m_motor', system=system) m_body = Constant(name='m_body', system=system) g = Constant(name='g', system=system) l = Constant(name='l', system=system) x, x_d, x_dd = Differentiable('x', system) y, y_d, y_dd = Differentiable('y', system) qA, qA_d, qA_dd = Differentiable('qA', system) qB, qB_d, qB_dd = Differentiable('qB', system) qM, qM_d, qM_dd = Differentiable('qM', system) #wB,wB_d= Differentiable('wB',ii=1,limit=3,system=system) i, i_d = Differentiable('i', ii=1, system=system) constants = {} constants[L] = .541e-3 constants[V] = 12 constants[R] = 2.29 constants[G] = 10 constants[Im] = 52.3 * 1e-3 * (1e-2)**2 constants[Il] = .1 constants[Ib] = .1
def init_system(v, drag_direction, time_step): import pynamics from pynamics.frame import Frame from pynamics.variable_types import Differentiable, Constant from pynamics.system import System from pynamics.body import Body from pynamics.dyadic import Dyadic from pynamics.output import Output, PointsOutput from pynamics.particle import Particle import pynamics.integration import logging import sympy import numpy import matplotlib.pyplot as plt from math import pi from scipy import optimize from sympy import sin import pynamics.tanh as tanh from fit_qs import exp_fit import fit_qs # time_step = tstep x = numpy.zeros((7, 1)) friction_perp = x[0] friction_par = x[1] given_b = x[2] given_k = x[3] given_k1 = x[4] given_b1 = x[4] system = System() pynamics.set_system(__name__, system) global_q = True lO = Constant(7 / 1000, 'lO', system) lA = Constant(33 / 1000, 'lA', system) lB = Constant(33 / 1000, 'lB', system) lC = Constant(33 / 1000, 'lC', system) mO = Constant(10 / 1000, 'mA', system) mA = Constant(2.89 / 1000, 'mA', system) mB = Constant(2.89 / 1000, 'mB', system) mC = Constant(2.89 / 1000, 'mC', system) k = Constant(0.209, 'k', system) k1 = Constant(0.209, 'k1', system) friction_perp = Constant(1.2, 'f_perp', system) friction_par = Constant(-0.2, 'f_par', system) b_damping = Constant(given_b, 'b_damping', system) # time_step = 1/00 if v == 0: [ t, tinitial, tfinal, tstep, qAa1, qAb1, qAc1, qAa2, qAb2, qAc2, qAa3, qAb3, qAc3, qBa1, qBb1, qBc1, qBa2, qBb2, qBc2, qBa3, qBb3, qBc3, qCa1, qCb1, qCc1, qCa2, qCb2, qCc2, qCa3, qCb3, qCc3 ] = fit_qs.fit_0_amount(time_step) elif v == 10: [ t, tinitial, tfinal, tstep, qAa1, qAb1, qAc1, qAa2, qAb2, qAc2, qAa3, qAb3, qAc3, qBa1, qBb1, qBc1, qBa2, qBb2, qBc2, qBa3, qBb3, qBc3, qCa1, qCb1, qCc1, qCa2, qCb2, qCc2, qCa3, qCb3, qCc3 ] = fit_qs.fit_10_amount(time_step) elif v == 20: [ t, tinitial, tfinal, tstep, qAa1, qAb1, qAc1, qAa2, qAb2, qAc2, qAa3, qAb3, qAc3, qBa1, qBb1, qBc1, qBa2, qBb2, qBc2, qBa3, qBb3, qBc3, qCa1, qCb1, qCc1, qCa2, qCb2, qCc2, qCa3, qCb3, qCc3 ] = fit_qs.fit_20_amount(time_step) elif v == 30: [ t, tinitial, tfinal, tstep, qAa1, qAb1, qAc1, qAa2, qAb2, qAc2, qAa3, qAb3, qAc3, qBa1, qBb1, qBc1, qBa2, qBb2, qBc2, qBa3, qBb3, qBc3, qCa1, qCb1, qCc1, qCa2, qCb2, qCc2, qCa3, qCb3, qCc3 ] = fit_qs.fit_30_amount(time_step) elif v == 40: [ t, tinitial, tfinal, tstep, qAa1, qAb1, qAc1, qAa2, qAb2, qAc2, qAa3, qAb3, qAc3, qBa1, qBb1, qBc1, qBa2, qBb2, qBc2, qBa3, qBb3, qBc3, qCa1, qCb1, qCc1, qCa2, qCb2, qCc2, qCa3, qCb3, qCc3 ] = fit_qs.fit_40_amount(time_step) elif v == 50: [ t, tinitial, tfinal, tstep, qAa1, qAb1, qAc1, qAa2, qAb2, qAc2, qAa3, qAb3, qAc3, qBa1, qBb1, qBc1, qBa2, qBb2, qBc2, qBa3, qBb3, qBc3, qCa1, qCb1, qCc1, qCa2, qCb2, qCc2, qCa3, qCb3, qCc3 ] = fit_qs.fit_50_amount(time_step) distance = 200 / 1000 nums = int(tfinal / tstep) array_num = numpy.arange(0, nums) array_num1 = numpy.repeat(array_num, nums, axis=0) array_num1.shape = (nums, nums) error_k = array_num1 / 8000 + numpy.ones((nums, nums)) fit_t = t fit_qA = exp_fit(fit_t, qAa1, qAb1, qAc1, qAa2, qAb2, qAc2, qAa3, qAb3, qAc3) fit_qB = exp_fit(fit_t, qBa1, qBb1, qBc1, qBa2, qBb2, qBc2, qBa3, qBb3, qBc3) fit_qC = exp_fit(fit_t, qCa1, qCb1, qCc1, qCa2, qCb2, qCc2, qCa3, qCb3, qCc3) fit_qAd1 = numpy.diff(fit_qA) / numpy.diff(fit_t) fit_qAd = numpy.append(fit_qAd1[0], fit_qAd1) fit_qBd1 = numpy.diff(fit_qB) / numpy.diff(fit_t) fit_qBd = numpy.append(fit_qBd1[0], fit_qBd1) fit_qCd1 = numpy.diff(fit_qC) / numpy.diff(fit_t) fit_qCd = numpy.append(fit_qCd1[0], fit_qCd1) fit_states1 = numpy.stack( (fit_qA, fit_qB, fit_qC, fit_qAd, fit_qBd, fit_qCd), axis=1) fit_states1[:, 0:3] = fit_states1[:, 0:3] - fit_states1[0, 0:3] fit_states = -drag_direction * numpy.deg2rad(fit_states1) # plt.plot(t,fit_states) if drag_direction == -1: zero_shape = fit_states.shape fit_states = numpy.zeros(zero_shape) fit_vel = drag_direction * distance / (tfinal) if qAa1 == 0: fit_vel = 0 fit_v = numpy.ones(t.shape) * fit_vel if qAa1 == 0: fit_d = numpy.ones(t.shape) * fit_vel else: fit_d = drag_direction * numpy.r_[tinitial:distance:tstep * abs(fit_vel)] preload0 = Constant(0 * pi / 180, 'preload0', system) preload1 = Constant(0 * pi / 180, 'preload1', system) preload2 = Constant(0 * pi / 180, 'preload2', system) preload3 = Constant(0 * pi / 180, 'preload3', system) Ixx_O = Constant(1, 'Ixx_O', system) Iyy_O = Constant(1, 'Iyy_O', system) Izz_O = Constant(1, 'Izz_O', system) Ixx_A = Constant(1, 'Ixx_A', system) Iyy_A = Constant(1, 'Iyy_A', system) Izz_A = Constant(1, 'Izz_A', system) Ixx_B = Constant(1, 'Ixx_B', system) Iyy_B = Constant(1, 'Iyy_B', system) Izz_B = Constant(1, 'Izz_B', system) Ixx_C = Constant(1, 'Ixx_C', system) Iyy_C = Constant(1, 'Iyy_C', system) Izz_C = Constant(1, 'Izz_C', system) y, y_d, y_dd = Differentiable('y', system) qO, qO_d, qO_dd = Differentiable('qO', system) qA, qA_d, qA_dd = Differentiable('qA', system) qB, qB_d, qB_dd = Differentiable('qB', system) qC, qC_d, qC_dd = Differentiable('qC', system) initialvalues = {} initialvalues[y] = 0 + 1e-14 initialvalues[y_d] = fit_vel + 1e-14 initialvalues[qO] = 0 + 1e-14 initialvalues[qO_d] = 0 + 1e-14 initialvalues[qA] = fit_states[0, 0] + 1e-14 initialvalues[qA_d] = fit_states[0, 3] + 1e-14 initialvalues[qB] = fit_states[0, 1] + 1e-14 initialvalues[qB_d] = fit_states[0, 4] + 1e-14 initialvalues[qC] = fit_states[0, 2] + 1e-14 initialvalues[qC_d] = fit_states[0, 5] + 1e-14 statevariables = system.get_state_variables() ini = [initialvalues[item] for item in statevariables] N = Frame('N') O = Frame('O') A = Frame('A') B = Frame('B') C = Frame('C') drag_direction = drag_direction velocity = 200 / tfinal / 1000 vSoil = drag_direction * velocity * N.y nSoil = 1 / vSoil.length() * vSoil system.set_newtonian(N) if not global_q: O.rotate_fixed_axis_directed(N, [0, 0, 1], qO, system) A.rotate_fixed_axis_directed(O, [0, 0, 1], qA, system) B.rotate_fixed_axis_directed(A, [0, 0, 1], qB, system) C.rotate_fixed_axis_directed(B, [0, 0, 1], qC, system) else: O.rotate_fixed_axis_directed(N, [0, 0, 1], qO, system) A.rotate_fixed_axis_directed(N, [0, 0, 1], qA, system) B.rotate_fixed_axis_directed(N, [0, 0, 1], qB, system) C.rotate_fixed_axis_directed(N, [0, 0, 1], qC, system) pNO = 0 * N.x + y * N.y pOA = lO * N.x + y * N.y pAB = pOA + lA * A.x pBC = pAB + lB * B.x pCtip = pBC + lC * C.x pOcm = pNO + lO / 2 * N.x pAcm = pOA + lA / 2 * A.x pBcm = pAB + lB / 2 * B.x pCcm = pBC + lC / 2 * C.x wNO = N.getw_(O) wOA = N.getw_(A) wAB = A.getw_(B) wBC = B.getw_(C) IO = Dyadic.build(O, Ixx_O, Iyy_O, Izz_O) IA = Dyadic.build(A, Ixx_A, Iyy_A, Izz_A) IB = Dyadic.build(B, Ixx_B, Iyy_B, Izz_B) IC = Dyadic.build(C, Ixx_C, Iyy_C, Izz_C) BodyO = Body('BodyO', O, pOcm, mO, IO, system) BodyA = Body('BodyA', A, pAcm, mA, IA, system) BodyB = Body('BodyB', B, pBcm, mB, IB, system) BodyC = Body('BodyC', C, pCcm, mC, IC, system) # BodyC = Particle(pCcm,mC,'ParticleC',system) vOcm = pOcm.time_derivative() vAcm = pAcm.time_derivative() vBcm = pBcm.time_derivative() vCcm = pCcm.time_derivative() system.add_spring_force1(k1 + 10000 * (qA + abs(qA)), (qA - qO - preload1) * N.z, wOA) system.add_spring_force1(k + 10000 * (qB + abs(qB)), (qB - qA - preload2) * N.z, wAB) system.add_spring_force1(k + 10000 * (qC + abs(qC)), (qC - qB - preload3) * N.z, wBC) #new Method use nJoint nvAcm = 1 / vAcm.length() * vAcm nvBcm = 1 / vBcm.length() * vBcm nvCcm = 1 / vCcm.length() * vCcm faperp = friction_perp * nvAcm.dot(A.y) * A.y fapar = friction_par * nvAcm.dot(A.x) * A.x system.addforce(-(faperp + fapar), vAcm) fbperp = friction_perp * nvBcm.dot(B.y) * B.y fbpar = friction_par * nvBcm.dot(B.x) * B.x system.addforce(-(fbperp + fbpar), vBcm) fcperp = friction_perp * nvCcm.dot(C.y) * C.y fcpar = friction_par * nvCcm.dot(C.x) * C.x system.addforce(-(fcperp + fcpar), vCcm) system.addforce(-b_damping * wOA, wOA) system.addforce(-b_damping * wAB, wAB) system.addforce(-b_damping * wBC, wBC) eq = [] eq_d = [(system.derivative(item)) for item in eq] eq_d.append(y_d - fit_vel) eq_dd = [(system.derivative(item)) for item in eq_d] f, ma = system.getdynamics() func1 = system.state_space_post_invert(f, ma, eq_dd) points = [pNO, pOA, pAB, pBC, pCtip] constants = system.constant_values return system, f, ma, func1, points, t, ini, constants, b_damping, k, k1, tstep, fit_states
m2 = Constant('m1',1,system) m1 = Constant('m2',1,system) g = Constant('g',9.81,system) b = Constant('b',1e1,system) k = Constant('k',1e1,system) #preload1 = Constant('preload1',0*pi/180,system) k_controller = Constant('k_controller',1e2,system) q2_command = Constant('q2_command',90*pi/180,system) #q2_command= Variable('q2_command') tinitial = 0 tfinal = 1 tstep = .01 t = numpy.r_[tinitial:tfinal:tstep] x,x_d,x_dd = Differentiable(system,'x') y,y_d,y_dd = Differentiable(system,'y') q1,q1_d,q1_dd = Differentiable(system,'q1') q2,q2_d,q2_dd = Differentiable(system,'q2') initialvalues = {} initialvalues[x]=0 initialvalues[y]=1 initialvalues[x_d]=0 initialvalues[y_d]=0 initialvalues[q1]=0 initialvalues[q1_d]=0 initialvalues[q2]=0 initialvalues[q2_d]=0 statevariables = system.get_q(0)+system.get_q(1)
################################################ #other constants g = Constant(0, 'g', system) b = Constant(1e0, 'b', system) k = Constant(1e2, 'k', system) ################################################ #time parameters tinitial = 0 tfinal = 5 tstep = 1 / 30 t = numpy.r_[tinitial:tfinal:tstep] ################################################ #State variables qA1, qA1_d, qA1_dd = Differentiable('qA1', system) qA2, qA2_d, qA2_dd = Differentiable('qA2', system) qA3, qA3_d, qA3_dd = Differentiable('qA3', system) qB1, qB1_d, qB1_dd = Differentiable('qB1', system) qB2, qB2_d, qB2_dd = Differentiable('qB2', system) ################################################ #Define initial values for state variables initialvalues = {} initialvalues[qA1] = 1 * pi / 180 initialvalues[qA2] = 1 * pi / 180 initialvalues[qA3] = 1 * pi / 180 initialvalues[qB1] = -1 * pi / 180 initialvalues[qB2] = -1 * pi / 180 initialvalues[qA1_d] = 0 * pi / 180
Constant('Iyy_A', 5.31645644183654e-06, system) Constant('Izz_A', 5.31645644183654e-06, system) Constant('Ixx_B', 6.27600676796613e-07, system) Constant('Iyy_B', 1.98358014762822e-06, system) Constant('Izz_B', 1.98358014762822e-06, system) #Constant('Ixx_C',4.39320316677997e-07,system) #Constant('Iyy_C',7.9239401855911e-07,system) #Constant('Izz_C',7.9239401855911e-07,system) Constant('b', 1e-4, system) Constant('k', 1.0, system) Constant('preload1', -90 * pi / 180, system) Constant('preload2', 0 * pi / 180, system) #Constant('preload3',0*pi/180,system) Differentiable(system, 'qA') Differentiable(system, 'qB') #Differentiable('qC',system) initialvalues = {} initialvalues[qA] = 0 * pi / 180 initialvalues[qA_d] = 0 * pi / 180 initialvalues[qB] = 0 * pi / 180 initialvalues[qB_d] = 0 * pi / 180 #initialvalues[qC]=0*pi/180 #initialvalues[qC_d]=0*pi/180 statevariables = system.get_q(0) + system.get_q(1) ini = [initialvalues[item] for item in statevariables] Frame('N')
constants[preload5] = 180 * pi / 180 constants[L] = .5 #constants[V] = 1 constants[R] = 1 constants[G] = 10 constants[Im] = .01 #constants[Il] = .1 #constants[b] = .1 constants[kv] = .01 constants[kt] = .01 constants[Tl] = 0 constants[m_motor] = .1 #constants[g] = 9.81 x, x_d, x_dd = Differentiable(name='x', system=system) y, y_d, y_dd = Differentiable(name='y', system=system) qO, qO_d, qO_dd = Differentiable(name='qO', system=system) qMA, qMA_d, qMA_dd = Differentiable(name='qMA', system=system) qA1, qA1_d, qA1_dd = Differentiable(name='qA1', system=system) qA2, qA2_d, qA2_dd = Differentiable(name='qA2', system=system) qB, qB_d, qB_dd = Differentiable(name='qB', system=system) qMC, qMC_d, qMC_dd = Differentiable(name='qMC', system=system) qC1, qC1_d, qC1_dd = Differentiable(name='qC1', system=system) qC2, qC2_d, qC2_dd = Differentiable(name='qC2', system=system) qD, qD_d, qD_dd = Differentiable(name='qD', system=system) iA, iA_d = Differentiable('iA', ii=1, system=system) iC, iC_d = Differentiable('iC', ii=1, system=system) initialvalues = {
g = Constant('g', 9.81, system) tinitial = 0 tfinal = 10 tstep = .001 t = numpy.r_[tinitial:tfinal:tstep] Ixx_A = Constant('Ixx_A', 50 / 1000 / 100 / 100, system) Iyy_A = Variable('Iyy_A') Izz_A = Variable('Izz_A') Ixx_B = Constant('Ixx_B', 2500 / 1000 / 100 / 100, system) Iyy_B = Constant('Iyy_B', 500 / 1000 / 100 / 100, system) Izz_B = Constant('Izz_B', 2000 / 1000 / 100 / 100, system) qA, qA_d, qA_dd = Differentiable(system, 'qA') qB, qB_d, qB_dd = Differentiable(system, 'qB') initialvalues = {} initialvalues[qA] = 90 * pi / 180 initialvalues[qA_d] = 0 * pi / 180 initialvalues[qB] = .5 * pi / 180 initialvalues[qB_d] = 0 * pi / 180 statevariables = system.get_q(0) + system.get_q(1) ini = [initialvalues[item] for item in statevariables] N = Frame('N') A = Frame('A') B = Frame('B')
import sympy import numpy import matplotlib.pyplot as plt plt.ion() import math system = System() pynamics.set_system(__name__, system) g = Constant(9.81, 'g', system) tinitial = 0 tfinal = 5 tstep = 1 / 30 t = numpy.r_[tinitial:tfinal:tstep] qA, qA_d = Differentiable('qA', limit=2) qB, qB_d = Differentiable('qB', limit=2) qC, qC_d = Differentiable('qC', limit=2) wx, wx_d = Differentiable('wx', ii=1, limit=3) wy, wy_d = Differentiable('wy', ii=1, limit=3) wz, wz_d = Differentiable('wz', ii=1, limit=3) mC = Constant(1, 'mC') Ixx = Constant(2, 'Ixx') Iyy = Constant(3, 'Iyy') Izz = Constant(1, 'Izz') initialvalues = {} initialvalues[qA] = 0 * math.pi / 180 initialvalues[qB] = 0 * math.pi / 180
from mpl_toolkits.mplot3d import Axes3D import sympy import numpy plt.ion() from math import pi system = System() pynamics.set_system(__name__,system) mp1 = Constant(1,'mp1') g = Constant(9.81,'g') l1 = Constant(1,'l1') b = Constant(1,'b') k = Constant(1e1,'k',system) q1,q1_d,q1_dd = Differentiable('q1',ini = [0,1]) q2,q2_d,q2_dd = Differentiable('q2',ini = [0,0]) q3,q3_d,q3_dd = Differentiable('q3',ini = [0,0]) f1 = Frame(,system) f2 = Frame(,system) f3 = Frame(,system) f4 = Frame(,system) #f5 = Frame(,system) system.set_newtonian(f1) f2.rotate_fixed_axis(f1,[0,0,1],q1,system) f3.rotate_fixed_axis(f2,[1,0,0],q2,system) f4.rotate_fixed_axis(f3,[0,1,0],q3,system) p0 = 0*f1.x
#l1 = Constant(1,'l1',system) m1 = Constant(1e-1, 'm1', system) m2 = Constant(1e-3, 'm2', system) k = Constant(1.5e2, 'k', system) l0 = Constant(1, 'l0', system) b = Constant(1e0, 'b', system) g = Constant(9.81, 'g', system) k_constraint = Constant(1e4, 'k_constraint', system) b_constraint = Constant(1e5, 'b_constraint', system) tinitial = 0 tfinal = 2 tstep = 1 / 30 t = numpy.r_[tinitial:tfinal:tstep] x1, x1_d, x1_dd = Differentiable('x1', system) y1, y1_d, y1_dd = Differentiable('y1', system) #x2,x2_d,x2_dd = Differentiable('x2',system) #y2,y2_d,y2_dd = Differentiable('y2',system) q1, q1_d, q1_dd = Differentiable('q1', system) l1, l1_d, l1_dd = Differentiable('l1', system) vini = 5 aini = -60 * pi / 180 initialvalues = {} initialvalues[x1] = 0 initialvalues[x1_d] = vini * cos(aini) initialvalues[y1] = 1.2 initialvalues[y1_d] = vini * sin(aini)
constants[mD] = .1 constants[I_main] = 1 constants[g] = 9.81 constants[b] = 1e0 constants[k] = 1e2 constants[stall_torque] = 1e2 constants[k_constraint] = 1e5 constants[b_constraint] = 1e3 constants[preload1] = 0*pi/180 constants[preload2] = 0*pi/180 constants[preload3] = -180*pi/180 constants[preload4] = 0*pi/180 constants[preload5] = 180*pi/180 x,x_d,x_dd = Differentiable(name='x',system=system) y,y_d,y_dd = Differentiable(name='y',system=system) qO,qO_d,qO_dd = Differentiable(name='qO',system=system) qA,qA_d,qA_dd = Differentiable(name='qA',system=system) qB,qB_d,qB_dd = Differentiable(name='qB',system=system) qC,qC_d,qC_dd = Differentiable(name='qC',system=system) qD,qD_d,qD_dd = Differentiable(name='qD',system=system) initialvalues={ x: 0, x_d: 0, y: 1.25, y_d: 0, qO: 0, qO_d: 0, qA: -0.89,
Il = Constant(name='Il', system=system) G = Constant(name='G', system=system) b = Constant(name='b', system=system) kv = Constant(name='kv', system=system) kt = Constant(name='kt', system=system) Tl = Constant(name='Tl', system=system) m = Constant(name='m', system=system) g = Constant(name='g', system=system) tinitial = 0 tfinal = 3 tstep = .01 t = numpy.r_[tinitial:tfinal:tstep] #qA,qA_d,qA_dd = Differentiable('qA',system) qB, qB_d, qB_dd = Differentiable('qB', system) #wB,wB_d= Differentiable('wB',ii=1,limit=3,system=system) i, i_d = Differentiable('i', ii=1, system=system) constants = {} constants[L] = .5 constants[V] = 1 constants[R] = 1 constants[G] = 10 constants[Im] = .01 constants[Il] = .1 constants[b] = .1 constants[kv] = .01 constants[kt] = .01 constants[Tl] = 0 constants[m] = 1
freq = Constant(.1, 'freq', system) torque = Constant(10, 'torque', system) Area = Constant(.1, 'Area', system) # b = Constant(1e0,'b',system) k = Constant(1e1, 'k', system) rho = Constant(1000, 'rho') Ixx_motor = Constant(.1, 'Ixx_motor') Iyy_motor = Constant(.1, 'Iyy_motor') Izz_motor = Constant(1, 'Izz_motor') Ixx_plate = Constant(.1, 'Ixx_plate') Iyy_plate = Constant(.1, 'Iyy_plate') Izz_plate = Constant(1, 'Izz_plate') qA, qA_d, qA_dd = Differentiable('qA', system) initialvalues = {} initialvalues[qA] = 0 * pi / 180 initialvalues[qA_d] = 0 * pi / 180 N = Frame('N', system) A = Frame('A', system) system.set_newtonian(N) A.rotate_fixed_axis(N, [0, 0, 1], qA, system) pNA = 0 * N.x pAcm = pNA + lA / 2 * A.x pAtip = pNA + lA * A.x vAcm = pAcm.time_derivative(N, system)
k_constraint = Constant(1e4, 'k_constraint', system) b_constraint = Constant(1e2, 'b_constraint', system) tinitial = 0 tfinal = 10 tstep = 1 / 30 t = numpy.r_[tinitial:tfinal:tstep] preload1 = Constant(0 * pi / 180, 'preload1', system) preload2 = Constant(0 * pi / 180, 'preload2', system) preload3 = Constant(-180 * pi / 180, 'preload3', system) preload4 = Constant(0 * pi / 180, 'preload4', system) preload5 = Constant(180 * pi / 180, 'preload5', system) preload6 = Constant(0 * pi / 180, 'preload6', system) x, x_d, x_dd = Differentiable('x', system) y, y_d, y_dd = Differentiable('y', system) qO, qO_d, qO_dd = Differentiable('qO', system) qA, qA_d, qA_dd = Differentiable('qA', system) qB, qB_d, qB_dd = Differentiable('qB', system) qC, qC_d, qC_dd = Differentiable('qC', system) qD, qD_d, qD_dd = Differentiable('qD', system) qE, qE_d, qE_dd = Differentiable('qE', system) initialvalues = { x: 0, x_d: .5, y: 2, y_d: 0, qO: 5 * pi / 180, qO_d: 0,
import matplotlib.pyplot as plt plt.ion() from math import pi, sin, cos system = System() pynamics.set_system(__name__, system) tinitial = 0 tfinal = 5 tstep = 1 / 30 t = numpy.r_[tinitial:tfinal:tstep] ang_ini = 0 v = 1 x, x_d, x_dd = Differentiable('x', ini=[0, v * cos(ang_ini * pi / 180)]) y, y_d, y_dd = Differentiable('y', ini=[1, v * sin(ang_ini * pi / 180)]) z, z_d, z_dd = Differentiable('z', ini=[0, 0]) qA, qA_d, qA_dd = Differentiable('qA', ini=[0, 0]) qB, qB_d, qB_dd = Differentiable('qB', ini=[0, 0]) qC, qC_d, qC_dd = Differentiable('qC', ini=[ang_ini * pi / 180, 0]) mC = Constant(.05, 'mC') g = Constant(9.81, 'g') I_11 = Constant(6e-3, 'I_11') rho = Constant(1.292, 'rho') Sw = Constant(.1, 'Sw') Se = Constant(.025, 'Se') l = Constant(.35, 'l') lw = Constant(-.03, 'lw')
# preload1 = Constant(0*pi/180,'preload1',system) # preload2 = Constant(0*pi/180,'preload2',system) # preload3 = Constant(0*pi/180,'preload3',system) # Ixx_A = Constant(1,'Ixx_A',system) # Iyy_A = Constant(1,'Iyy_A',system) # Izz_A = Constant(1,'Izz_A',system) # Ixx_B = Constant(1,'Ixx_B',system) # Iyy_B = Constant(1,'Iyy_B',system) # Izz_B = Constant(1,'Izz_B',system) # Ixx_C = Constant(1,'Ixx_C',system) # Iyy_C = Constant(1,'Iyy_C',system) # Izz_C = Constant(1,'Izz_C',system) qA, qA_d, qA_dd = Differentiable('qA', system) qB, qB_d, qB_dd = Differentiable('qB', system) qC, qC_d, qC_dd = Differentiable('qC', system) initialvalues = {} initialvalues[qA] = -90 * pi / 180 initialvalues[qA_d] = 0 * pi / 180 initialvalues[qB] = 90 * pi / 180 initialvalues[qB_d] = 0 * pi / 180 initialvalues[qC] = 90 * pi / 180 initialvalues[qC_d] = 0 * pi / 180 statevariables = system.get_state_variables() N = Frame('N') A = Frame('A')
#preload1 = Constant('preload1',0*pi/180,system) l1 = Constant(1,'l1',system) m1 = Constant(1e1,'m1',system) m2 = Constant(1e0,'m2',system) k = Constant(1e4,'k',system) l0 = Constant(1,'l0',system) b = Constant(5e0,'b',system) g = Constant(9.81,'g',system) tinitial = 0 tfinal = 10 tstep = 1/30 t = numpy.r_[tinitial:tfinal:tstep] x1,x1_d,x1_dd = Differentiable('x1',system) x2,x2_d,x2_dd = Differentiable('x2',system) initialvalues = {} initialvalues[x1]=2 initialvalues[x1_d]=0 initialvalues[x2]=0 initialvalues[x2_d]=1 statevariables = system.get_state_variables() ini = [initialvalues[item] for item in statevariables] N = Frame('N',system) system.set_newtonian(N) pNA=0*N.x
system = System() lA = Constant('lA', 2, system) mA = Constant('mA', 1, system) g = Constant('g', 9.81, system) b = Constant('b', 1e0, system) k = Constant('k', 1e1, system) tinitial = 0 tfinal = 5 tstep = .001 t = numpy.r_[tinitial:tfinal:tstep] preload1 = Constant('preload1', 0 * pi / 180, system) x, x_d, x_dd = Differentiable(system, 'x') y, y_d, y_dd = Differentiable(system, 'y') initialvalues = {} initialvalues[x] = 1 initialvalues[y] = 0 initialvalues[x_d] = 0 initialvalues[y_d] = 0 statevariables = system.get_q(0) + system.get_q(1) ini = [initialvalues[item] for item in statevariables] N = Frame('N') system.set_newtonian(N) pNA = 0 * N.x
error = 1e-12 tinitial = 0 tfinal = 10 tstep = .001 t = numpy.r_[tinitial:tfinal:tstep] m = Constant(1, 'm', system) g = Constant(9.81, 'g', system) I = Constant(1, 'I', system) J = Constant(1, 'J', system) r = Constant(1, 'L', system) H, H_d = Differentiable( 'H', system, limit=2, ) L, L_d = Differentiable( 'L', system, limit=2, ) Q, Q_d = Differentiable( 'Q', system, limit=2, ) x, x_d, x_dd = Differentiable('x', system) y, y_d, y_dd = Differentiable('y', system)
from math import pi system = System() pynamics.set_system(__name__,system) mA = Constant(1,'mA',system) g = Constant(9.81,'g',system) b = Constant(1e0,'b',system) k = Constant(1e6,'k',system) tinitial = 0 tfinal = 5 tstep = 1/100 t = numpy.r_[tinitial:tfinal:tstep] x,x_d,x_dd = Differentiable('x',system) y,y_d,y_dd = Differentiable('y',system) initialvalues = {} initialvalues[x]=0 initialvalues[x_d]=1 initialvalues[y]=.1 initialvalues[y_d]=0 statevariables = system.get_state_variables() ini = [initialvalues[item] for item in statevariables] N = Frame('N') system.set_newtonian(N)
constants[preload5] = 180 * pi / 180 constants[L] = .5 #constants[V] = 1 constants[R] = 1 constants[G] = 10 constants[Im] = .01 #constants[Il] = .1 #constants[b] = .1 constants[kv] = .01 constants[kt] = .01 constants[Tl] = 0 constants[m_motor] = 1 #constants[g] = 9.81 x, x_d, x_dd = Differentiable(name='x', system=system) y, y_d, y_dd = Differentiable(name='y', system=system) qO, qO_d, qO_dd = Differentiable(name='qO', system=system) qMA, qMA_d, qMA_dd = Differentiable(name='qMA', system=system) qA, qA_d, qA_dd = Differentiable(name='qA', system=system) qB, qB_d, qB_dd = Differentiable(name='qB', system=system) qMC, qMC_d, qMC_dd = Differentiable(name='qMC', system=system) qC, qC_d, qC_dd = Differentiable(name='qC', system=system) qD, qD_d, qD_dd = Differentiable(name='qD', system=system) iA, iA_d = Differentiable('iA', ii=1, system=system) iC, iC_d = Differentiable('iC', ii=1, system=system) initialvalues = { x: 0, x_d: 0,
m = Constant(10, 'm', system) I_xx = Constant(9, 'I_xx', system) I_yy = Constant(9, 'I_yy', system) I_zz = Constant(9, 'I_zz', system) g = Constant(9.81, 'g', system) b = Constant(1e3, 'b', system) k = Constant(1e2, 'k', system) tinitial = 0 tfinal = 10 tstep = 1 / 30 t = numpy.r_[tinitial:tfinal:tstep] x, x_d, x_dd = Differentiable('x', system) q, q_d, q_dd = Differentiable('q', system) initialvalues = {} initialvalues[x] = 1 initialvalues[x_d] = 0 initialvalues[q] = 30 * pi / 180 initialvalues[q_d] = 0 * pi / 180 statevariables = system.get_state_variables() ini = [initialvalues[item] for item in statevariables] N = Frame('N', system) A = Frame('A', system)
from mpl_toolkits.mplot3d import Axes3D import sympy import numpy plt.ion() from math import pi system = System() pynamics.set_system(__name__, system) mp1 = Constant(1, 'mp1') g = Constant(9.81, 'g') l1 = Constant(1, 'l1') b = Constant(1, 'b') k = Constant(1e1, 'k', system) x, x_d, x_dd = Differentiable('x', ini=[1, 0]) y, y_d, y_dd = Differentiable('y', ini=[0, 0]) z, z_d, z_dd = Differentiable('z', ini=[0, 0]) q1, q1_d, q1_dd = Differentiable('q1', ini=[0.01, 1]) q2, q2_d, q2_dd = Differentiable('q2', ini=[0.01, 0]) q3, q3_d, q3_dd = Differentiable('q3', ini=[0.01, 0]) f1 = Frame() f2 = Frame() f3 = Frame() f4 = Frame() #f5 = Frame() system.set_newtonian(f1) f2.rotate_fixed_axis_directed(f1, [0, 0, 1], q1) f3.rotate_fixed_axis_directed(f2, [1, 0, 0], q2)