def Cal_robot(direction, given_l, omega1, t1, t2, ini_states, name1, system, video_on, x1): time_a = time.time() pynamics.set_system(__name__, system) given_k, given_b = x1 global_q = True damping_r = 0 tinitial = 0 tfinal = (t1 - t2) / omega1 tstep = 1 / 30 t = numpy.r_[tinitial:tfinal:tstep] tol_1 = 1e-6 tol_2 = 1e-6 lO = Constant(27.5 / 1000, 'lO', system) lR = Constant(40.5 / 1000, 'lR', system) lA = Constant(given_l / 1000, 'lA', system) lB = Constant(given_l / 1000, 'lB', system) lC = Constant(given_l / 1000, 'lC', system) mO = Constant(154.5 / 1000, 'mO', system) mR = Constant(9.282 / 1000, 'mR', system) mA = Constant(given_l * 2.75 * 0.14450000000000002 / 1000, 'mA', system) mB = Constant(given_l * 2.75 * 0.14450000000000002 / 1000, 'mB', system) mC = Constant(given_l * 2.75 * 0.14450000000000002 / 1000, 'mC', system) k = Constant(given_k, 'k', system) friction_perp = Constant(13 / 3, 'f_perp', system) friction_par = Constant(-2 / 3, 'f_par', system) friction_arm_perp = Constant(5.6, 'fr_perp', system) friction_arm_par = Constant(-0.2, 'fr_par', system) b_damping = Constant(given_b, 'b_damping', 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_R = Constant(1, 'Ixx_R', system) Iyy_R = Constant(1, 'Iyy_R', system) Izz_R = Constant(1, 'Izz_R', 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) qR, qR_d, qR_dd = Differentiable('qR', 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] = 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 = system.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') system.set_newtonian(N) if not global_q: O.rotate_fixed_axis_directed(N, [0, 0, 1], qO, system) R.rotate_fixed_axis_directed(O, [0, 0, 1], qR, system) A.rotate_fixed_axis_directed(R, [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) R.rotate_fixed_axis_directed(N, [0, 0, 1], qR, 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 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 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, system) BodyR = Body('BodyR', R, pRcm, mR, IR, 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) j_tol = 3 * pi / 180 inv_k = 10 system.add_spring_force1(k + inv_k * (qA - qR + abs(qA - qR - j_tol)), (qA - qR - preload1) * N.z, wRA) system.add_spring_force1(k + inv_k * (qB - qA + abs(qB - qA - j_tol)), (qB - qA - preload2) * N.z, wAB) system.add_spring_force1(k + inv_k * (qC - qB + abs(qC - qB - j_tol)), (qC - qB - preload3) * N.z, wBC) vOcm = y_d * N.y 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 foperp = 8 * nSoil system.addforce(-foperp, vOcm) frperp = friction_arm_perp * nvRcm.dot(R.y) * R.y frpar = friction_arm_par * nvRcm.dot(R.x) * R.x system.addforce(-(frperp + frpar), vRcm) 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 * 1 * wRA, wRA) system.addforce(-b_damping * 1 * wAB, wAB) system.addforce(-b_damping * 1 * wBC, wBC) eq = [] eq_d = [(system.derivative(item)) for item in eq] eq_d.append(qR_d - omega1) 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, pOR, pRA, pAB, pBC, pCtip] constants = system.constant_values states = pynamics.integration.integrate_odeint(func1, ini, t, args=({ 'constants': constants }, )) final = numpy.asarray(states[-1, :]) logger1 = logging.getLogger('pynamics.system') logger2 = logging.getLogger('pynamics.integration') logger3 = logging.getLogger('pynamics.output') logger1.disabled = True logger2.disabled = True logger3.disabled = True points_output = PointsOutput(points, system, 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]) 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
# These two lines of code order the initial values in a list in such a way that the integrator can use it in the same order that it expects the variables to be supplied # In[10]: statevariables = system.get_state_variables() ini = [initialvalues[item] for item in statevariables] # ## Kinematics # # ### Frames # Define the reference frames of the system # In[11]: N = Frame('N') A = Frame('A') B = Frame('B') C = Frame('C') # ### Newtonian Frame # # It is important to define the Newtonian reference frame as a reference frame that is not accelerating, otherwise the dynamic equations will not be correct # In[12]: system.set_newtonian(N) # This is the first time that the "global_q" variable is used. If you choose to rotate each frame with reference to the base frame, there is the potential for a representational simplification. If you use a relative rotation, this can also be simpler in some cases. Try running the code either way to see which one is simpler in this case. # In[13]:
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 initialvalues[qC_d]=0 N = Frame('N',system) A = Frame('A',system) B = Frame('B',system) C = Frame('C',system) system.set_newtonian(N) A.rotate_fixed_axis(N,[1,0,0],qA,system) B.rotate_fixed_axis(A,[0,1,0],qB,system) C.rotate_fixed_axis(B,[0,0,1],qC,system) pCcm=0*N.x w1 = N.get_w_to(C) IC = Dyadic.build(C,Ixx,Iyy,Izz)
qA, qA_d, qA_dd = Differentiable(system, 'qA') qB, qB_d, qB_dd = Differentiable(system, 'qB') qC, qC_d, qC_dd = Differentiable(system, 'qC') 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_state_variables() ini = [initialvalues[item] for item in statevariables] N = Frame('N', system) A = Frame('A', system) B = Frame('B', system) C = Frame('C', system) system.set_newtonian(N) A.rotate_fixed_axis(N, [0, 0, 1], qA, system) B.rotate_fixed_axis(A, [0, 0, 1], qB, system) C.rotate_fixed_axis(B, [0, 0, 1], qC, system) pNA = 0 * N.x pAB = pNA + lA * A.x pBC = pAB + lB * B.x pCtip = pBC + lC * C.x pAcm = pNA + lA / 2 * A.x
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') A = Frame('A') system.set_newtonian(N) A.rotate_fixed_axis_directed(N, [0, 0, 1], qA, system) pNA = 0 * N.x pAB = pNA + lA * A.x vAB = pAB.time_derivative(N, system) pNA2 = 2 * N.x pAB2 = pNA2 + x * N.x + y * N.y vAB2 = pAB2.time_derivative(N, system) ParticleA = Particle(pAB, mA, 'ParticleA', system) ParticleB = Particle(pAB2, mA, 'ParticleB', system)
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 initialvalues[qC] = 0 * math.pi / 180 initialvalues[wx] = 1. initialvalues[wy] = 1. initialvalues[wz] = 0. N = Frame('N') A = Frame('A') B = Frame('B') C = Frame('C') system.set_newtonian(N) A.rotate_fixed_axis_directed(N, [1, 0, 0], qA, system) B.rotate_fixed_axis_directed(A, [0, 1, 0], qB, system) C.rotate_fixed_axis_directed(B, [0, 0, 1], qC, system) pCcm = 0 * N.x IC = Dyadic.build(C, Ixx, Iyy, Izz) w1 = N.getw_(C) w2 = wx * C.x + wy * C.y + wz * C.z
qD1: -pi + 2.64, qD1_d: 0, qA2: -0.89, qA2_d: 0, qB2: -2.64, qB2_d: 0, qC2: -pi + 0.89, qC2_d: 0, qD2: -pi + 2.64, qD2_d: 0 } statevariables = system.get_state_variables() ini = [initialvalues[item] for item in statevariables] N = Frame('N', system) O = Frame('O', system) A1 = Frame('A1', system) B1 = Frame('B1', system) C1 = Frame('C1', system) D1 = Frame('D1', system) A2 = Frame('A2', system) B2 = Frame('B2', system) C2 = Frame('C2', system) D2 = Frame('D2', system) system.set_newtonian(N) O.rotate_fixed_axis(N, [0, 0, 1], qO, system) A1.rotate_fixed_axis(N, [0, 0, 1], qA1, system) B1.rotate_fixed_axis(N, [0, 0, 1], qB1, system)
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) ini = [initialvalues[item] for item in statevariables] N = Frame('N') A = Frame('A') B = Frame('B') system.set_newtonian(N) A.rotate_fixed_axis_directed(N,[0,0,1],q1,system) B.rotate_fixed_axis_directed(A,[0,0,1],q2,system) pNA=0*N.x #pAB=pNA+x*N.x+y*N.y #vAB=pAB.time_derivative(N,system) pm1 = x*N.x+y*N.y vm1 = pm1.time_derivative(N,system) pm2 = pm1 + l3*B.x pk1 = pm1-l1*A.x
initialvalues[qA3] = 1 * pi / 180 initialvalues[qB1] = -1 * pi / 180 initialvalues[qB2] = -1 * pi / 180 initialvalues[qA1_d] = 0 * pi / 180 initialvalues[qA2_d] = 0 * pi / 180 initialvalues[qA3_d] = 0 * pi / 180 initialvalues[qB1_d] = 0 * pi / 180 initialvalues[qB2_d] = 0 * pi / 180 statevariables = system.get_state_variables() ini = [initialvalues[item] for item in statevariables] ################################################ #Create Frames N = Frame('N', system) A1 = Frame('A1', system) A12 = Frame('A12', system) A2 = Frame('A2', system) A23 = Frame('A23', system) A3 = Frame('A3', system) A34 = Frame('A34', system) NB1 = Frame('NB1', system) B1 = Frame('B1', system) B12 = Frame('B12', system) B2 = Frame('B2', system) B23 = Frame('B23', system) ################################################ #Relative frame rotations from newtonian out to distal frames
qB_d: 0, qD: -pi + 2.64, qD_d: 0 } initialvalues[iA] = 0 initialvalues[iC] = 0 initialvalues[qMA] = 0 initialvalues[qMA_d] = 0 initialvalues[qMC] = 0 initialvalues[qMC_d] = 0 statevariables = system.get_state_variables() ini = [initialvalues[item] for item in statevariables] N = Frame('N', system) O = Frame('O', system) A1 = Frame('A1', system) C1 = Frame('C1', system) A2 = Frame('A2', system) C2 = Frame('C2', system) MA = Frame('MA', system) A = Frame('A', system) B = Frame('B', system) MC = Frame('MC', system) C = Frame('C', system) D = Frame('D', system) system.set_newtonian(N) O.rotate_fixed_axis(N, [0, 0, 1], qO, 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') Frame('A') Frame('B') #Frame('C') system.set_newtonian(N) A.rotate_fixed_axis_directed(N, [0, 0, 1], qA, system) B.rotate_fixed_axis_directed(A, [0, 0, 1], qB, system) pNA = 0 * N.x pAB = pNA + lA * A.x pBC = pAB + lB * B.x #pCtip = pBC + lC*C.x pAcm = pNA + lA / 2 * A.x pBcm = pAB + lB / 2 * B.x
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
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) f4.rotate_fixed_axis_directed(f3, [0, 1, 0], q3) p1 = x * f1.x + y * f1.y + z * f1.z p2 = -l1 * f4.x #v1=p1.time_derivative(f1) wNA = f1.getw_(f2)
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
initialvalues[Q_d] = 0 * pi / 180 initialvalues[x] = 0 initialvalues[x_d] = 0 initialvalues[y] = 0 initialvalues[y_d] = 0 initialvalues[wx] = 0 initialvalues[wx_d] = 0 initialvalues[wy] = 0 initialvalues[wy_d] = 0 initialvalues[wz] = 0 initialvalues[wz_d] = 0 statevariables = system.get_state_variables() ini = [initialvalues[item] for item in statevariables] A = Frame('A', system) B = Frame('B', system) C = Frame('C', system) D = Frame('D', system) system.set_newtonian(A) B.rotate_fixed_axis(A, [0, 0, 1], H, system) C.rotate_fixed_axis(B, [1, 0, 0], -L, system) D.rotate_fixed_axis(C, [0, 1, 0], Q, system) pNA = 0 * A.x pAD = pNA + x * A.x + y * A.y pBcm = pAD + r * C.z pDA = pBcm - r * D.z wAD = A.get_w_to(D)
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
qC_d: 0, qD: -pi + 2.64, qD_d: 0 } initialvalues[iA] = 0 initialvalues[iC] = 0 initialvalues[qMA] = 0 initialvalues[qMA_d] = 0 initialvalues[qMC] = 0 initialvalues[qMC_d] = 0 statevariables = system.get_state_variables() ini = [initialvalues[item] for item in statevariables] N = Frame('N', system) O = Frame('O', system) MA = Frame('MA', system) A = Frame('A', system) B = Frame('B', system) MC = Frame('MC', system) C = Frame('C', system) D = Frame('D', system) system.set_newtonian(N) O.rotate_fixed_axis(N, [0, 0, 1], qO, system) MA.rotate_fixed_axis(N, [0, 0, 1], qMA, system) A.rotate_fixed_axis(N, [0, 0, 1], qA, system) B.rotate_fixed_axis(N, [0, 0, 1], qB, system) MC.rotate_fixed_axis(N, [0, 0, 1], qMC, system) C.rotate_fixed_axis(N, [0, 0, 1], qC, system)
qC,qC_d,qC_dd = Differentiable('qC',system) qD,qD_d,qD_dd = Differentiable('qD',system) initialvalues = {} initialvalues[qA]=30*pi/180 initialvalues[qA_d]=0*pi/180 initialvalues[qB]=30*pi/180 initialvalues[qB_d]=0*pi/180 initialvalues[qC]=150*pi/180 initialvalues[qC_d]=0*pi/180 initialvalues[qD]=-30*pi/180 initialvalues[qD_d]=0*pi/180 statevariables = system.get_state_variables() N = Frame('N',system) A = Frame('A',system) B = Frame('B',system) C = Frame('C',system) D = Frame('D',system) system.set_newtonian(N) A.rotate_fixed_axis(N,[0,0,1],qA,system) B.rotate_fixed_axis(A,[0,0,1],qB,system) C.rotate_fixed_axis(N,[0,0,1],qC,system) D.rotate_fixed_axis(C,[0,0,1],qD,system) pNA = 0*N.x pAB = pNA + lA*A.x pBD = pAB + lB*B.x pCD = pNA + lC*C.x
constants[m] = 1 constants[g] = 9.81 initialvalues = {} #initialvalues[qA]=0*pi/180 #initialvalues[qA_d]=0*pi/180 initialvalues[qB] = 0 * pi / 180 initialvalues[qB_d] = 0 * pi / 180 #initialvalues[wB]=0*pi/180 #initialvalues[a]=0 initialvalues[i] = 0 statevariables = system.get_state_variables() ini = [initialvalues[item] for item in statevariables] N = Frame('N', system) #A = Frame('A',system) B = Frame('B', system) M = Frame('M', system) system.set_newtonian(N) #A.rotate_fixed_axis(N,[0,0,1],qA,system) B.rotate_fixed_axis(N, [0, 0, 1], qB, system) pO = 0 * N.x #wNA = N.get_w_to(A) wNB = N.get_w_to(B) wNA = G * wNB aNA = wNA.time_derivative() #wNB = wB*B.z #aNB = wB_d*B.z
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] = .5 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) system.set_newtonian(N) A.rotate_fixed_axis(N, [0, 0, 1], q, system) p1 = x * N.x p2 = p1 - l * A.y v1 = p1.time_derivative(N, system) v2 = p2.time_derivative(N, system) I = Dyadic.build(A, I_xx, I_yy, I_zz) BodyA = Body('BodyA', A, p2, m, I, system) ParticleO = Particle(p2, M, 'ParticleO', system)
import pynamics.variable_types from pynamics.frame import Frame from pynamics.system import System 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 from pynamics.particle import Particle s = System() pynamics.set_system(__name__,s) x,x_d,x_dd=pynamics.variable_types.Differentiable('x',s) q1,q1_d,q1_dd=pynamics.variable_types.Differentiable('q1',s) eq = x**2+2*x eq_d = s.derivative(eq) N = Frame('N') A = Frame('A') s.set_newtonian(N) A.rotate_fixed_axis_directed(N,[0,0,1],q1,s) p1 = 3*A.x+2*N.y v1=p1.time_derivative(N,s)
initialvalues = {} #initialvalues[xA]=.02 #initialvalues[xA_d]=0 #initialvalues[yA]=0 #initialvalues[yA_d]=0 initialvalues[qA] = 0 * pi / 180 initialvalues[qA_d] = 0 * pi / 180 initialvalues[xB] = .06 initialvalues[xB_d] = 0 initialvalues[yB] = 0 initialvalues[yB_d] = 0 initialvalues[qB] = 0 * pi / 180 initialvalues[qB_d] = 0 * pi / 180 N = Frame('N') A = Frame('A') B = Frame('B') system.set_newtonian(N) A.rotate_fixed_axis_directed(N, [0, 0, 1], qA, system) B.rotate_fixed_axis_directed(A, [0, 0, 1], qB, system) #A.setpathtonewtonian(['A','N']) #B.setpathtonewtonian(['B','A','N']) zero = sympy.Number(0) pNA = zero * N.x + zero * N.y + zero * N.z #pAcm=xA*N.x*yA*N.y pAN = pNA
qA: -0.89, qA_d: 0, qB: -2.64, qB_d: 0, qC: -pi + 0.89, qC_d: 0, qD: -pi + 2.64, qD_d: 0, qE: 0, qE_d: 0, } 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') D = Frame('D') E = Frame('E') system.set_newtonian(N) 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) D.rotate_fixed_axis_directed(N, [0, 0, 1], qD, system) E.rotate_fixed_axis_directed(N, [0, 0, 1], qE, system)
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') le = Constant(.04, 'le') qE = Constant(3 * pi / 180, 'qE') ini = system.get_ini() N = Frame('N') A = Frame('A') B = Frame('B') C = Frame('C') E = Frame('E') system.set_newtonian(N) A.rotate_fixed_axis_directed(N, [1, 0, 0], qA) B.rotate_fixed_axis_directed(A, [0, 1, 0], qB) C.rotate_fixed_axis_directed(B, [0, 0, 1], qC) E.rotate_fixed_axis_directed(C, [0, 0, 1], -qE) pCcm = x * N.x + y * N.y + z * N.z #pCcm=x*N.x+y*N.y pCcp = pCcm - lw * C.x
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') B = Frame('B') C = Frame('C') system.set_newtonian(N) A.rotate_fixed_axis_directed(N, [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) pNA = 0 * N.x pAB = pNA + lA * A.x pBC = pAB + lB * B.x pCtip = pBC + lC * C.x pD = lD * N.x
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 pm1 = x1*N.y pm2 = pm1 - x2*N.y #BodyA = Body('BodyA',A,pm1,m1,IA,system) Particle1 = Particle(pm1,m1,'Particle1',system) Particle2 = Particle(pm2,m2,'Particle2',system) vpm1 = pm1.time_derivative(N,system) vpm2 = pm2.time_derivative(N,system) l_ = pm1-pm2 l = (l_.dot(l_))**.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) pNA=0*N.x pAcm=pNA+x*N.x+y*N.y vAcm = pAcm.time_derivative(N,system) ParticleA = Particle(pAcm,mA,'ParticleA',system) system.addforce(-b*vAcm,vAcm) stretch = y stretched1 = (stretch+abs(stretch))/2 stretched2 = -(-stretch+abs(-stretch))/2
initialvalues = {} #initialvalues[xA]=.02 #initialvalues[xA_d]=0 #initialvalues[yA]=0 #initialvalues[yA_d]=0 initialvalues[qA] = 0 * pi / 180 initialvalues[qA_d] = 0 * pi / 180 initialvalues[xB] = .06 initialvalues[xB_d] = 0 initialvalues[yB] = 0 initialvalues[yB_d] = 0 initialvalues[qB] = 0 * pi / 180 initialvalues[qB_d] = 0 * pi / 180 N = Frame('N', system) A = Frame('A', system) B = Frame('B', system) system.set_newtonian(N) A.rotate_fixed_axis(N, [0, 0, 1], qA, system) B.rotate_fixed_axis(A, [0, 0, 1], qB, system) #A.setpathtonewtonian(['A','N']) #B.setpathtonewtonian(['B','A','N']) zero = sympy.Number(0) pNA = zero * N.x + zero * N.y + zero * N.z #pAcm=xA*N.x*yA*N.y pAN = pNA
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) system.set_newtonian(N) A.rotate_fixed_axis(N, [0, 0, 1], q, system) p1 = x * N.x p2 = p1 - l * A.y wNA = N.get_w_to(A) v1 = p1.time_derivative(N, system) v2 = p2.time_derivative(N, system) I = Dyadic.build(A, I_xx, I_yy, I_zz)
qO: 0, qO_d: 0, qA: -0.89, qA_d: 0, qB: -2.64, qB_d: 0, qC: -pi + 0.89, qC_d: 0, qD: -pi + 2.64, qD_d: 0 } statevariables = system.get_state_variables() ini0 = [initialvalues[item] for item in statevariables] N = Frame('N', system) O = Frame('O', system) A = Frame('A', system) B = Frame('B', system) C = Frame('C', system) D = Frame('D', system) system.set_newtonian(N) O.rotate_fixed_axis(N, [0, 0, 1], qO, system) A.rotate_fixed_axis(N, [0, 0, 1], qA, system) B.rotate_fixed_axis(N, [0, 0, 1], qB, system) C.rotate_fixed_axis(N, [0, 0, 1], qC, system) D.rotate_fixed_axis(N, [0, 0, 1], qD, system) pOrigin = 0 * N.x + 0 * N.y pOcm = x * N.x + y * N.y