pCcm=0*N.x w1 = N.get_w_to(C) IC = Dyadic.build(C,Ixx,Iyy,Izz) BodyC = Body('BodyC',C,pCcm,mC,IC) system.addforcegravity(-g*N.y) # system.addforce(1*C.x+2*C.y+3*C.z,w1) points = [1*C.x,0*C.x,1*C.y,0*C.y,1*C.z] f,ma = system.getdynamics() # func1 = system.state_space_pre_invert(f,ma) func1 = system.state_space_post_invert(f,ma) ini = [initialvalues[item] for item in system.get_state_variables()] states=pynamics.integration.integrate_odeint(func1,ini,t,rtol = tol, atol=tol,args=({'constants':system.constant_values},)) po = PointsOutput(points,system) po.calc(states,t) po.animate(fps = 30,lw=2) so = Output([qA,qB,qC]) so.calc(states,t) so.plot_time()
stretch = q - q0 system.add_spring_force1(k, (stretch) * N.z, wNA) system.addforce(-b * v2, v2) system.addforcegravity(-g * N.y) pos = sympy.cos(system.t * 2 * pi / 2) eq = pos * N.x - p1 eq_d = eq.time_derivative() eq_dd = eq_d.time_derivative() eq_dd_scalar = [] eq_dd_scalar.append(eq_dd.dot(N.x)) system.add_constraint(AccelerationConstraint(eq_dd_scalar)) f, ma = system.getdynamics() func1, lambda1 = system.state_space_post_invert( f, ma, constants=system.constant_values, return_lambda=True) states = pynamics.integration.integrate_odeint(func1, ini, t, rtol=tol, atol=tol, args=({ 'constants': {}, 'alpha': 1e2, 'beta': 1e1 }, )) lambda1_n = numpy.array([lambda1(tt, ss) for tt, ss in zip(t, states)]) plt.figure() plt.plot(t, lambda1_n)
ParticleA = Particle(pAB, mA, 'ParticleA', system) ParticleB = Particle(pAB2, mA, 'ParticleB', system) system.addforce(-b * vAB, vAB) system.addforce(-b * vAB2, vAB2) system.addforcegravity(-g * N.y) v = pAB2 - pNA2 u = (v.dot(v))**.5 eq1 = [(v.dot(v)) - lA**2] eq1_dd = system.derivative(system.derivative(eq1[0])) eq = [eq1_dd] f, ma = system.getdynamics() func = system.state_space_post_invert(f, ma, eq, constants=system.constant_values) states = pynamics.integration.integrate(func, ini, t, rtol=1e-12, atol=1e-12, hmin=1e-14) points = [pNA, pAB, pNA, pNA2, pAB2] points_output = PointsOutput(points, system) points_output.calc(states) points_output.animate(fps=30, lw=2)
f # In[24]: ma # ## Solve for Acceleration # # The next line of code solves the system of equations F=ma plus any constraint equations that have been added above. It returns one or two variables. func1 is the function that computes the velocity and acceleration given a certain state, and lambda1(optional) supplies the function that computes the constraint forces as a function of the resulting states # # There are a few ways of solveing for a. The below function inverts the mass matrix numerically every time step. This can be slower because the matrix solution has to be solved for, but is sometimes more tractable than solving the highly nonlinear symbolic expressions that can be generated from the previous step. The other options would be to use ```state_space_pre_invert```, which pre-inverts the equations symbolically before generating a numerical function, or ```state_space_post_invert2```, which adds Baumgarte's method for intermittent constraints. # In[25]: func1, lambda1 = system.state_space_post_invert( f, ma, eq_dd, return_lambda=True, variable_functions={force_var: f_force}) # ## Integrate # # The next line of code integrates the function calculated # In[26]: states = pynamics.integration.integrate_odeint(func1, ini, t, rtol=tol, atol=tol, hmin=tol, args=({ 'constants':
f_aero_C2 = rho * vAcm.length() * (vAcm.dot(A.y)) * Area * A.y system.addforce(-f_aero_C2, vAcm) system.add_spring_force1(k, qA * N.z, wNA) tin = torque * sympy.sin(2 * sympy.pi * freq * system.t) system.addforce(tin * N.z, wNA) f, ma = system.getdynamics() changing_constants = [freq] static_constants = system.constant_values.copy() for key in changing_constants: del static_constants[key] func = system.state_space_post_invert(f, ma, constants=static_constants) statevariables = system.get_state_variables() ini = [initialvalues[item] for item in statevariables] points = [pNA, pAcm, pAtip] points_output = PointsOutput(points, system) out1 = Output([tin, qA]) my_constants = {} freq_sweep = numpy.r_[-1.5:1:20j] freq_sweep = 1 * 10**freq_sweep amps = [] for ff in freq_sweep: tol = 1e-4
torque = my_signal * stall_torque system.addforce(torque * O.z, wOA) system.addforce(-torque * O.z, wOC) # eq = [] eq.append((pBtip - pDtip).dot(N.x)) eq.append((pBtip - pDtip).dot(N.y)) #eq.append((O.y.dot(N.y))) eq_d = [system.derivative(item) for item in eq] eq_dd = [system.derivative(item) for item in eq_d] # f, ma = system.getdynamics() func1 = system.state_space_post_invert(f, ma, eq_dd, constants=system.constant_values, variable_functions={my_signal: ft2}) states = pynamics.integration.integrate_odeint(func1, ini1, t, rtol=tol, atol=tol) KE = system.get_KE() PE = system.getPEGravity(0 * N.x) - system.getPESprings() energy = Output([KE - PE, toeforce, heelforce]) energy.calc(states) energy.plot_time() #torque_plot = Output([torque])
# ParticleB = Particle(pAB2,mA,'ParticleB',system) # system.addforce(t*vAB,vAB) system.addforce(torque * N.z, wNA) # system.addforce(-b*vAB2,vAB2) system.addforcegravity(-g * N.y) # v = pAB2-pNA2 # u = (v.dot(v))**.5 # eq1 = [(v.dot(v)) - lA**2] eq = [] f, ma = system.getdynamics() func = system.state_space_post_invert(f, ma, eq) states = pynamics.integration.integrate_odeint(func, ini, t, rtol=1e-12, atol=1e-12, hmin=1e-14, args=({ 'constants': system.constant_values }, )) points = [pNA, pAB] points_output = PointsOutput(points, system) points_output.calc(states) points_output.animate(fps=30, lw=2)
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
def Cal_system(initial_states, drag_direction, tinitial, tstep, tfinal, fit_vel, f1, f2): g_k, g_b_damping, g_b_damping1 = [0.30867935, 1.42946955, 1.08464536] 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(g_k, 'k', system) k1 = Constant(0.4, 'k1', system) friction_perp = Constant(f1, 'f_perp', system) friction_par = Constant(f2, 'f_par', system) b_damping = Constant(g_b_damping, 'b_damping', system) b_damping1 = Constant(g_b_damping1, 'b_damping1', system) 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) fit_states = initial_states initialvalues = {} initialvalues[y] = fit_states[0] initialvalues[y_d] = fit_states[5] initialvalues[qO] = 0 initialvalues[qO_d] = 0 initialvalues[qA] = fit_states[2] initialvalues[qA_d] = fit_states[7] initialvalues[qB] = fit_states[3] initialvalues[qB_d] = fit_states[8] initialvalues[qC] = fit_states[4] initialvalues[qC_d] = fit_states[9] 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') 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) # 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) system.add_spring_force1(k1, (qA - qO - preload1) * N.z, wOA) system.add_spring_force1(k, (qB - qA - preload2) * N.z, wAB) system.add_spring_force1(k, (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 vSoil = drag_direction * 1 * N.y nSoil = 1 / vSoil.length() * vSoil if fit_vel == 0: vSoil = 1 * 1 * N.y nSoil = 1 / vSoil.length() * vSoil faperp = friction_perp * nSoil.dot(A.y) * A.y fapar = friction_par * nSoil.dot(A.x) * A.x system.addforce(-(faperp + fapar), vAcm) fbperp = friction_perp * nSoil.dot(B.y) * B.y fbpar = friction_par * nSoil.dot(B.x) * B.x system.addforce(-(fbperp + fbpar), vBcm) fcperp = friction_perp * nSoil.dot(C.y) * C.y fcpar = friction_par * nSoil.dot(C.x) * C.x system.addforce(-(fcperp + fcpar), vCcm) else: 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_damping1 * 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 states = pynamics.integration.integrate_odeint(func1, ini, t, args=({ 'constants': constants }, )) points_output = PointsOutput(points, system, constant_values=constants) y = points_output.calc(states) final = numpy.asarray(states[-1, :]) time1 = time.time() points_output.animate(fps=30, movie_name=str(time1) + 'video_1.mp4', lw=2, marker='o', color=(1, 0, 0, 1), linestyle='-') return final, states, y, system
eq1.append(p2.dot(f1.z)) eq1_d = [system.derivative(item) for item in eq1] eq1_dd = [system.derivative(item) for item in eq1_d] points = [particle1.pCM] points_x = [item.dot(f1.x) for item in points] points_y = [item.dot(f1.y) for item in points] points_z = [item.dot(f1.z) for item in points] output_x = Output(points_x) output_y = Output(points_y) output_z = Output(points_z) f, ma = system.getdynamics() func = system.state_space_post_invert(f, ma, eq_dd=eq1_dd) t = numpy.r_[0:5:.001] states = pynamics.integration.integrate_odeint(func, system.get_ini(), t, atol=1e-5, rtol=1e-5, args=({ 'constants': system.constant_values }, )) KE = system.get_KE() PE = system.getPEGravity(0 * f1.x) - system.getPESprings() output = Output([KE - PE])
The last tip of the four bar mechanism (pCD) is fixed to the ground. """ # Define constraints eq = [] eq.append(pCD.dot(N.x)) eq.append(pCD.dot(N.y)) eq_d=[(system.derivative(item)) for item in eq] eq_dd=[(system.derivative(item)) for item in eq_d] """# 5. Solution: Add the code from the bottom of the pendulum example for solving for f=ma, integrating, plotting, and animating. Run the code to see your results. It should look similar to the pendulum example with constraints added, as in like a rag-doll or floppy""" f,ma = system.getdynamics() func1,lambda1 = system.state_space_post_invert(f,ma,eq_dd,return_lambda = True) states=pynamics.integration.integrate(func1,ini,t,rtol=tol,atol=tol, args=({'constants':system.constant_values},)) # Plot --- output plt.figure() artists = plt.plot(t,states[:,:3]) plt.legend(artists,['qA','qB','qC']) # Plot --- energy KE = system.get_KE() PE = system.getPEGravity(pNA) - system.getPESprings() energy_output = Output([KE-PE],system) energy_output.calc(states) energy_output.plot_time()
eq_d = [item.time_derivative() for item in eq] eq_dd = [item.time_derivative() for item in eq_d] eq_dd_scalar = [] eq_dd_scalar.append(eq_dd[0].dot(N.x)) eq_dd_scalar.append(eq_dd[0].dot(N.y)) eq_dd_scalar.append(eq_dd[1].dot(N.y)) c = AccelerationConstraint(eq_dd_scalar) # c.linearize(0) system.add_constraint(c) # f, ma = system.getdynamics() func1 = system.state_space_post_invert(f, ma, constants=constants, variable_functions={my_signal: ft2}) states = pynamics.integration.integrate(func1, ini, t, rtol=tol, atol=tol) KE = system.get_KE() PE = system.getPEGravity(0 * N.x) - system.getPESprings() energy = Output([KE - PE], constant_values=constants) energy.calc(states, t) energy.plot_time() #torque_plot = Output([torque]) #torque_plot.calc(states,t) #torque_plot.plot_time() points = [pDtip, pCD, pOC, pOA, pAB, pBtip] points = PointsOutput(points, constant_values=constants)
f, ma = system.getdynamics() # # ## Solve for Acceleration # # # # The next line of code solves the system of equations F=ma plus any constraint equations that have been added above. It returns one or two variables. func1 is the function that computes the velocity and acceleration given a certain state, and lambda1(optional) supplies the function that computes the constraint forces as a function of the resulting states # # # # There are a few ways of solveing for a. The below function inverts the mass matrix numerically every time step. This can be slower because the matrix solution has to be solved for, but is sometimes more tractable than solving the highly nonlinear symbolic expressions that can be generated from the previous step. The other options would be to use ```state_space_pre_invert```, which pre-inverts the equations symbolically before generating a numerical function, or ```state_space_post_invert2```, which adds Baumgarte's method for intermittent constraints. static_constants = [ rho, lA, lB, lC, lS, mA, mB, mC, mS, r2, Ixx_A, Iyy_A, Izz_A, Ixx_B, Iyy_B, Izz_B, Ixx_A, Iyy_B, Izz_B, Ixx_C, Iyy_C, Izz_C, Ixx_S, Iyy_S, Izz_S ] static_constants = dict([(key, system.constant_values[key]) for key in static_constants]) func1 = system.state_space_post_invert(f, ma, return_lambda=False, constants=static_constants) # %% statevariables = system.get_state_variables() ini = [initialvalues[item] for item in statevariables] # %% # # ## Integrate # # # # The next line of code integrates the function calculated tol = 1e-7 tinitial = 0 tfinal = 3 fps = 30
def Cal_robot(direction, given_l, given_arm_l, omega1, t1, t2, ini_states, name1, video_on, x1, x2): system1 = System() time_a = time.time() pynamics.set_system(__name__, system1) given_k, given_b = x1 f1, f2, f3 = x2 global_q = True damping_r = 0 tinitial = 0 tfinal = (t1 - t2) / omega1 tstep = 1 / 50 t = numpy.r_[tinitial:tfinal:tstep] tol_1 = 1e-6 tol_2 = 1e-6 lO = Constant(27.5 / 1000, 'lO', system1) # given_arm_l = 65 lR = Constant(given_arm_l / 1000, 'lR', system1) # lR = Constant(40.5/1000,'lR',system11) lA = Constant(given_l / 1000, 'lA', system1) lB = Constant(given_l / 1000, 'lB', system1) lC = Constant(given_l / 1000, 'lC', system1) mO = Constant(1e0 * 154.5 / 1000, 'mO', system1) # mR = Constant(9.282/1000 ,'mR',system1) mR = Constant((1.158 + 0.1445 * given_arm_l) / 1000, 'mR', system1) mA = Constant(given_l * 2.75 * 0.14450000000000002 / 1000, 'mA', system1) mB = Constant(given_l * 2.75 * 0.14450000000000002 / 1000, 'mB', system1) mC = Constant(given_l * 2.75 * 0.14450000000000002 / 1000, 'mC', system1) k = Constant(given_k, 'k', system1) friction_perp = Constant(f1, 'f_perp', system1) friction_par = Constant(-1, 'f_par', system1) friction_arm_perp = Constant(2 + given_arm_l * f3, 'fr_perp', system1) friction_arm_par = Constant(-0.3, 'fr_par', system1) b_damping = Constant(given_b, 'b_damping', system1) preload0 = Constant(0 * pi / 180, 'preload0', system1) preload1 = Constant(0 * pi / 180, 'preload1', system1) preload2 = Constant(0 * pi / 180, 'preload2', system1) preload3 = Constant(0 * pi / 180, 'preload3', system1) Ixx_O = Constant(1, 'Ixx_O', system1) Iyy_O = Constant(1, 'Iyy_O', system1) Izz_O = Constant(1, 'Izz_O', system1) Ixx_R = Constant(1, 'Ixx_R', system1) Iyy_R = Constant(1, 'Iyy_R', system1) Izz_R = Constant(1, 'Izz_R', system1) Ixx_A = Constant(1, 'Ixx_A', system1) Iyy_A = Constant(1, 'Iyy_A', system1) Izz_A = Constant(1, 'Izz_A', system1) Ixx_B = Constant(1, 'Ixx_B', system1) Iyy_B = Constant(1, 'Iyy_B', system1) Izz_B = Constant(1, 'Izz_B', system1) Ixx_C = Constant(1, 'Ixx_C', system1) Iyy_C = Constant(1, 'Iyy_C', system1) Izz_C = Constant(1, 'Izz_C', system1) y, y_d, y_dd = Differentiable('y', system1) qO, qO_d, qO_dd = Differentiable('qO', system1) qR, qR_d, qR_dd = Differentiable('qR', system1) qA, qA_d, qA_dd = Differentiable('qA', system1) qB, qB_d, qB_dd = Differentiable('qB', system1) qC, qC_d, qC_dd = Differentiable('qC', system1) initialvalues = {} initialvalues[y] = ini_states[0] + tol_1 initialvalues[qO] = ini_states[1] + tol_1 initialvalues[qR] = ini_states[2] + tol_1 initialvalues[qA] = ini_states[3] + tol_1 initialvalues[qB] = ini_states[4] + tol_1 initialvalues[qC] = ini_states[5] + tol_1 initialvalues[y_d] = ini_states[6] + tol_1 initialvalues[qO_d] = ini_states[7] + tol_1 initialvalues[qR_d] = ini_states[8] + tol_1 initialvalues[qA_d] = ini_states[9] + tol_1 initialvalues[qB_d] = ini_states[10] + tol_1 initialvalues[qC_d] = ini_states[11] + tol_1 statevariables = system1.get_state_variables() ini = [initialvalues[item] for item in statevariables] N = Frame('N') O = Frame('O') R = Frame('R') A = Frame('A') B = Frame('B') C = Frame('C') system1.set_newtonian(N) if not global_q: O.rotate_fixed_axis_directed(N, [0, 0, 1], qO, system1) R.rotate_fixed_axis_directed(O, [0, 0, 1], qR, system1) A.rotate_fixed_axis_directed(O, [0, 0, 1], qA, system1) B.rotate_fixed_axis_directed(A, [0, 0, 1], qB, system1) C.rotate_fixed_axis_directed(B, [0, 0, 1], qC, system1) else: O.rotate_fixed_axis_directed(N, [0, 0, 1], qO, system1) R.rotate_fixed_axis_directed(N, [0, 0, 1], qR, system1) A.rotate_fixed_axis_directed(N, [0, 0, 1], qA, system1) B.rotate_fixed_axis_directed(N, [0, 0, 1], qB, system1) C.rotate_fixed_axis_directed(N, [0, 0, 1], qC, system1) pNO = 0 * N.x + y * N.y pOR = pNO + lO * N.x # pOR= pNO +lO*N.x pRA = pOR + lR * R.x pAB = pRA + lA * A.x pBC = pAB + lB * B.x pCtip = pBC + lC * C.x # pOcm=pNO +lO/2*N.x pOcm = pNO pRcm = pOR + lR / 2 * R.x pAcm = pRA + lA / 2 * A.x pBcm = pAB + lB / 2 * B.x pCcm = pBC + lC / 2 * C.x wNO = N.getw_(O) wOR = N.getw_(R) wRA = R.getw_(A) wAB = A.getw_(B) wBC = B.getw_(C) IO = Dyadic.build(O, Ixx_O, Iyy_O, Izz_O) IR = Dyadic.build(R, Ixx_R, Iyy_R, Izz_R) 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, system1) BodyR = Body('BodyR', R, pRcm, mR, IR, system1) BodyA = Body('BodyA', A, pAcm, mA, IA, system1) BodyB = Body('BodyB', B, pBcm, mB, IB, system1) BodyC = Body('BodyC', C, pCcm, mC, IC, system1) j_tol = 0 * pi / 180 inv_k = 1e2 alw = 1 system1.add_spring_force1( k + inv_k * (qA - qR + alw * abs(qA - qR - j_tol)), (qA - qR - preload1) * N.z, wRA) system1.add_spring_force1( k + inv_k * (qB - qA + alw * abs(qB - qA - j_tol)), (qB - qA - preload2) * N.z, wAB) system1.add_spring_force1( k + inv_k * (qC - qB + alw * abs(qC - qB - j_tol)), (qC - qB - preload3) * N.z, wBC) # system1.add_spring_force1(k,(qA-qR-preload1)*N.z,wRA) # system1.add_spring_force1(k,(qB-qA-preload2)*N.z,wAB) # system1.add_spring_force1(k,(qC-qB-preload3)*N.z,wBC) vOcm = y_d * N.y # vOcm = pOcm.time_derivative() vRcm = pRcm.time_derivative() vAcm = pAcm.time_derivative() vBcm = pBcm.time_derivative() vCcm = pCcm.time_derivative() nvRcm = 1 / (vRcm.length() + tol_1) * vRcm nvAcm = 1 / (vAcm.length() + tol_1) * vAcm nvBcm = 1 / (vBcm.length() + tol_1) * vBcm nvCcm = 1 / (vCcm.length() + tol_1) * vCcm vSoil = -direction * 1 * N.y nSoil = 1 / vSoil.length() * vSoil # reff = abs( abs(y_d+0.01)-abs(y_d-0.01))*1/0.02*9.75 # foperp = reff*nSoil foperp = f2 * nSoil system1.addforce(foperp, vOcm) # system1.addforce(9.75*1*nSoil,vOcm) # system1.addforce(9.75*1*nSoil*y_d,vOcm) # system1.addforce(-100*N.x, y_d*N.x) frperp = friction_arm_perp * nvRcm.dot(R.y) * R.y frpar = friction_arm_par * nvRcm.dot(R.x) * R.x system1.addforce(-(frperp + frpar), vRcm) faperp = friction_perp * nvAcm.dot(A.y) * A.y fapar = friction_par * nvAcm.dot(A.x) * A.x system1.addforce(-(faperp + fapar), vAcm) fbperp = friction_perp * nvBcm.dot(B.y) * B.y fbpar = friction_par * nvBcm.dot(B.x) * B.x system1.addforce(-(fbperp + fbpar), vBcm) fcperp = friction_perp * nvCcm.dot(C.y) * C.y fcpar = friction_par * nvCcm.dot(C.x) * C.x system1.addforce(-(fcperp + fcpar), vCcm) system1.addforce(-b_damping * 1 * wRA, wRA) system1.addforce(-b_damping * 1 * wAB, wAB) system1.addforce(-b_damping * 1 * wBC, wBC) eq = [] eq_d = [(system1.derivative(item)) for item in eq] eq_d.append(qR_d - omega1) # eq_dd= [(system1.derivative(item)) for item in eq_d] eq_dd = [system1.derivative(eq_d[0])] f, ma = system1.getdynamics() func1 = system1.state_space_post_invert(f, ma, eq_dd) points = [pNO, pOR, pRA, pAB, pBC, pCtip] constants = system1.constant_values states = pynamics.integration.integrate_odeint(func1, ini, t, args=({ 'constants': constants }, )) final = numpy.asarray(states[-1, :]) logger1 = logging.getLogger('pynamics.system1') logger2 = logging.getLogger('pynamics.integration') logger3 = logging.getLogger('pynamics.output') logger1.disabled = True logger2.disabled = True logger3.disabled = True points_output = PointsOutput(points, system1, constant_values=constants) y1 = points_output.calc(states) if video_on == 1: plt.figure() plt.plot(*(y1[::int(len(y1) / 20)].T) * 1000) plt.axis('equal') plt.axis('equal') plt.title("Plate Configuration vs Distance") plt.xlabel("Configuration") plt.ylabel("Distance (mm)") plt.figure() plt.plot(t, numpy.rad2deg(states[:, 2])) plt.plot(t, numpy.rad2deg(states[:, 8])) plt.legend(["qR", "qR_d"]) plt.hlines(numpy.rad2deg(t1), tinitial, tfinal) plt.hlines(numpy.rad2deg(t2), tinitial, tfinal) plt.title("Robot Arm angle and velocitues (qR and qR_d) over Time") plt.xlabel("Time (s)") plt.ylabel("Angles,Velocities (deg, deg/s)") plt.figure() q_states = numpy.c_[(states[:, 2], states[:, 3], states[:, 4], states[:, 5])] plt.plot(t, numpy.rad2deg(q_states)) plt.title("Joint Angule over Time") plt.xlabel("Time (s)") plt.ylabel("Joint Angles (deg)") plt.legend(["Arm", "Joint 1", "Joint 2", "Joint 3"]) plt.figure() qd_states = numpy.c_[(states[:, 8], states[:, 9], states[:, 10], states[:, 11])] plt.plot(t, numpy.rad2deg(qd_states)) plt.legend(["qR_d", "qA_d", "qB_d", "qC_d"]) plt.title("Joint Angular Velocities over Time") plt.xlabel("Time (s)") plt.ylabel("Joint Angular Velocities (deg/s)") plt.legend(["Arm", "Joint 1", "Joint 2", "Joint 3"]) plt.figure() plt.plot(t, states[:, 0], '--') plt.plot(t, states[:, 6]) y_d1 = states[:, 6] # force1 = abs( abs(y_d1+0.1)-abs(y_d1-0.1))*40 # plt.plot(t,force1) plt.title("Robot Distance and Velocity over time") plt.xlabel("Time (s)") plt.ylabel("Distance (mm)") plt.legend(["Distance", "Velocity of the robot"]) points_output.animate(fps=1 / tstep, movie_name=name1, lw=2, marker='o', color=(1, 0, 0, 1), linestyle='-') else: pass return final, states, y1
sol = sympy.solve(zero,torques) sol2 = [sol[item] for item in torques] # sol2 = sympy.Matrix([sol[item] for item in torques]) f_torques = sympy.lambdify(system.get_q(0)+system.get_q(1)+system.get_q(2),sol2) res = numpy.array(f_torques(*(states_exp.T))).T ft1 = scipy.interpolate.interp1d(t,res[:,0],fill_value = 'extrapolate', kind='quadratic') ft2 = scipy.interpolate.interp1d(t,res[:,1],fill_value = 'extrapolate', kind='quadratic') ft3 = scipy.interpolate.interp1d(t,res[:,2],fill_value = 'extrapolate', kind='quadratic') plt.figure() plt.plot(t,numpy.array([ft1(t),ft2(t),ft3(t)]).T,'-o') variable_functions = {t1:ft1,t2:ft2,t3:ft3} func1 = system.state_space_post_invert(f,ma,variable_functions=variable_functions) # # func1,lambda1 = system.state_space_post_invert(f,ma,eq_dd,return_lambda = True) states=pynamics.integration.integrate_odeint(func1,ini,t,rtol=tol,atol=tol,args=({'constants':system.constant_values},) ) # lambda2 = numpy.array([lambda1(item1,item2,system.constant_values) for item1,item2 in zip(t,states)]) KE = system.get_KE() PE = system.getPEGravity(pNA) - system.getPESprings() points = [pNA,pAB,pBC,pCtip] #points = [item for item2 in points for item in [item2.dot(system.newtonian.x),item2.dot(system.newtonian.y)]] points_output = PointsOutput(points,system) y = points_output.calc(states) #y.resize(y.shape[0],int(y.shape[1]/2),2) plt.figure()