pEcm = pBtip - .1 * E.y
pE1 = pEcm + lE / 2 * E.x
vE1 = pE1.time_derivative(N, system)
pE2 = pEcm - lE / 2 * E.x
vE2 = pE2.time_derivative(N, system)
wOA = O.getw_(A)
wAB = A.getw_(B)
wOC = O.getw_(C)
wCD = C.getw_(D)
wBD = B.getw_(D)
wOE = O.getw_(E)
BodyO = Body('BodyO', O, pOcm, mO, Dyadic.build(O, I_main, I_main, I_main),
system)
#BodyA = Body('BodyA',A,pAcm,mA,Dyadic.build(A,I_leg,I_leg,I_leg),system)
#BodyB = Body('BodyB',B,pBcm,mB,Dyadic.build(B,I_leg,I_leg,I_leg),system)
#BodyC = Body('BodyC',C,pCcm,mC,Dyadic.build(C,I_leg,I_leg,I_leg),system)
#BodyD = Body('BodyD',D,pDcm,mD,Dyadic.build(D,I_leg,I_leg,I_leg),system)
BodyE = Body('BodyE', E, pEcm, mE, Dyadic.build(D, I_leg, I_leg, I_leg),
system)
ParticleA = Particle(pAcm, mA, 'ParticleA')
ParticleB = Particle(pBcm, mB, 'ParticleB')
ParticleC = Particle(pCcm, mC, 'ParticleC')
ParticleD = Particle(pDcm, mD, 'ParticleD')
#ParticleE = Particle(pEcm,mE,'ParticleE')
system.addforce(-b * wOA, wOA)
pDcm = pCD + lD / 2 * D.x
wOMA = O.get_w_to(MA)
wOA = O.get_w_to(A)
wAB = A.get_w_to(B)
wOMC = O.get_w_to(MC)
wOC = O.get_w_to(C)
wCD = C.get_w_to(D)
wBD = B.get_w_to(D)
wNMA = N.get_w_to(MA)
aNMA = wNMA.time_derivative()
wNMC = N.get_w_to(MC)
aNMC = wNMC.time_derivative()
I_motorA = Dyadic.build(MA, Im, Im, Im)
I_motorC = Dyadic.build(MC, Im, Im, Im)
BodyO = Body('BodyO', O, pOcm, mO, Dyadic.build(O, I_main, I_main, I_main),
system)
#BodyA = Body('BodyA',A,pAcm,mA,Dyadic.build(A,I_leg,I_leg,I_leg),system)
#BodyB = Body('BodyB',B,pBcm,mB,Dyadic.build(B,I_leg,I_leg,I_leg),system)
#BodyC = Body('BodyC',C,pCcm,mC,Dyadic.build(C,I_leg,I_leg,I_leg),system)
#BodyD = Body('BodyD',D,pDcm,mD,Dyadic.build(D,I_leg,I_leg,I_leg),system)
MotorA = Body('MotorA', MA, pOA, m_motor, I_motorA, system)
MotorA = Body('MotorC', MC, pOC, m_motor, I_motorC, system)
InductorA = PseudoParticle(0 * MA.x,
L,
name='InductorA',
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
I_motor = Dyadic.build(B, Im, Im, Im)
I_load = Dyadic.build(B, Il, Il, Il)
#Motor = Body('Motor',A,pO,0,I_motor,system)
Motor = Body('Motor', B, pO, 0, I_motor, system, wNBody=wNA, alNBody=aNA)
Inductor = PseudoParticle(0 * M.x,
L,
name='Inductor',
vCM=i * M.x,
aCM=i_d * M.x)
#Load = Body('Load',B,pO,0,I_load,system,wNBody = wNB,alNBody = aNB)
Load = Body('Load', B, pO, m, I_load, system)
#T = kt*(V/R)-kv*G*qB_d
T = kt * i
pCcp = pCcm - lw * C.x
pC1 = pCcm
pC2 = pCcm - l * C.x
pE = pC2 - le * E.x
#wNC = N.getw_(C)
vcm = pCcm.time_derivative()
vcp = pCcp.time_derivative()
vcp2 = vcp.dot(vcp)
ve = pE.time_derivative()
ve2 = ve.dot(ve)
IC = Dyadic.build(C, I_11, I_11, I_11)
Body('BodyC', C, pCcm, mC, IC)
vcx = vcp.dot(C.x)
vcy = vcp.dot(-C.y)
angle_of_attack_C = sympy.atan2(vcy, vcx)
vex = ve.dot(E.x)
vey = ve.dot(-E.y)
angle_of_attack_E = sympy.atan2(vey, vex)
#cl = 2*sin(angle_of_attack)*cos(angle_of_attack)
#cd = 2*sin(angle_of_attack)**2
#fl = .5*rho*vcp2*cl*A
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)
II = Dyadic.build(B, J, I, J)
BodyD = Body('BodyD', D, pBcm, m, II, system)
#ParticleA = Particle(pAcm,mA,'ParticleA',system)
#ParticleB = Particle(pBcm,mB,'ParticleB',system)
#ParticleC = Particle(pCcm,mC,'ParticleC',system)
system.addforcegravity(-g * A.z)
f, ma = system.getdynamics()
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)
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()]
system.addforce(-f_aero_C, vctip)
# ## Calculating Velocity
#
# The angular velocity between frames, and the time derivatives of vectors are extremely useful in calculating the equations of motion and for determining many of the forces that need to be applied to your system (damping, drag, etc). Thus, it is useful, once kinematics have been defined, to take or find the derivatives of some of those vectors for calculating linear or angular velocity vectors
#
# ### Angular Velocity
wA3B3 = A3.get_w_to(B3)
wB3C3 = B3.get_w_to(C3)
wA3S = A3.get_w_to(S)
# ### Define Inertias and Bodies
# The next several lines compute the inertia dyadics of each body and define a rigid body on each frame. In the case of frame C, we represent the mass as a particle located at point pCcm.
IA = Dyadic.build(A3, Ixx_A, Iyy_A, Izz_A)
IB = Dyadic.build(B3, Ixx_B, Iyy_B, Izz_B)
IC = Dyadic.build(C3, Ixx_C, Iyy_C, Izz_C)
IS = Dyadic.build(S, Ixx_S, Iyy_S, Izz_S)
BodyA = Body('BodyA', A3, pAcm, mA, IA, system)
BodyB = Body('BodyB', B3, pBcm, mB, IB, system)
BodyC = Body('BodyC', C3, pCcm, mC, IC, system)
BodyS = Body('BodyS', S, pScm, mS, IS, system)
# ## Forces and Torques
# Forces and torques are added to the system with the generic ```addforce``` method. The first parameter supplied is a vector describing the force applied at a point or the torque applied along a given rotational axis. The second parameter is the vector describing the linear speed (for an applied force) or the angular velocity(for an applied torque)
system.addforce(torque * sympy.sin(freq * 2 * sympy.pi * system.t) * A3.z,
wA3S)
#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
p1 = p0-l1*f4.x
v1=p1.time_derivative(f1)
wNA = f1.get_w_to(f2)
particle1 = Particle(p1,mp1)
body1 = Body('body1',f4,p1,mp1,Dyadic.build(f4,1,1,1),system = None)
#system.addforce(-b*v1,v1)
system.addforcegravity(-g*f1.z)
#system.add_spring_force1(k,(q1)*f1.z,wNA)
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)
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], q, system)
pNA = 0 * N.x
pAB = pNA - x * A.y
vNA = pNA.time_derivative(N, system)
vAB = pAB.time_derivative(N, system)
aAB = vAB.time_derivative(N, system)
#ParticleA = Particle(pAB,mA,'ParticleA',system)
IA = Dyadic.build(A, Ixx_A, Iyy_A, Izz_A)
BodyA = Body('BodyA', A, pAB, mA, IA, system)
stretch = x - lA
direction = -A.y
#system.addforce(-b*vAB,vAB)
system.addforce(-k * stretch * A.y, vNA)
system.addforce(k * stretch * A.y, vAB)
#system.add_springforce(k*stretch*A.y,vAB)
#system.addforce(b*x_d*A.y,vAB)
system.addforcegravity(-g * N.y)
x1 = BodyA.pCM.dot(N.x)
y1 = BodyA.pCM.dot(N.y)
f, ma = system.getdynamics()
M.rotate_fixed_axis(A, [0, 0, 1], qM, system)
pO = 0 * N.x
pAcm = x * N.x + y * N.y
wNA = N.get_w_to(A)
wAB = A.get_w_to(B)
wAM = A.get_w_to(M)
pBcm = pAcm + l * B.x
vAcm = pAcm.time_derivative()
#wNA = G*wNB
#aNA = wNA.time_derivative()
#wNB = wB*B.z
#aNB = wB_d*B.z
I_motor = Dyadic.build(M, Im, Im, Im)
I_body = Dyadic.build(A, Ib, Ib, Ib)
I_load = Dyadic.build(B, Il, Il, Il)
Motor = Body('Motor', M, pAcm, 0, I_motor, system)
main_body = Body('main_body', A, pAcm, m_body, I_body, system)
Load = Body('Load', B, pBcm, m_pendulum, I_load, system)
Inductor = PseudoParticle(0 * Z.x,
L,
name='Inductor',
vCM=i * Z.x,
aCM=i_d * Z.x)
#Load = Body('Load',B,pO,0,I_load,system,wNBody = wNB,alNBody = aNB)
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
wOA1 = O.get_w_to(A1)
wA1A2 = A1.get_w_to(A2)
wA2B = A2.get_w_to(B)
#wB1B2 = B1.get_w_to(B2)
wOC1 = O.get_w_to(C1)
wC1C2 = C1.get_w_to(C2)
wC2D = C2.get_w_to(D)
#wD1D2 = D1.get_w_to(D2)
wBD = B.get_w_to(D)
wNMA = N.get_w_to(MA)
aNMA = wNMA.time_derivative()
wNMC = N.get_w_to(MC)
aNMC = wNMC.time_derivative()
I_motorA = Dyadic.build(MA, Im, Im, Im)
I_motorC = Dyadic.build(MC, Im, Im, Im)
#BodyO = Body('BodyO',O,pOcm,mO,Dyadic.build(O,I_main,I_main,I_main),system)
#BodyA = Body('BodyA',A,pAcm,mA,Dyadic.build(A,I_leg,I_leg,I_leg),system)
#BodyB = Body('BodyB',B,pBcm,mB,Dyadic.build(B,I_leg,I_leg,I_leg),system)
#BodyC = Body('BodyC',C,pCcm,mC,Dyadic.build(C,I_leg,I_leg,I_leg),system)
#BodyD = Body('BodyD',D,pDcm,mD,Dyadic.build(D,I_leg,I_leg,I_leg),system)
Particle0 = Particle(pOcm, mO, 'ParticleO')
MotorA = Body('MotorA', MA, pOA, m_motor, I_motorA, system)
MotorA = Body('MotorC', MC, pOC, m_motor, I_motorC, system)
ParticleA1 = Particle(pA1cm, mA / 2, 'ParticleA1')
ParticleC1 = Particle(pC1cm, mC / 2, 'ParticleC1')
InductorA = PseudoParticle(0 * MA.x,
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
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
vBA = pBA.time_derivative(N,system)
aBA = vBA.time_derivative(N,system)
#constraint1 = pNA-pAN
#constraint1_d = vectorderivative(constraint1,N)
#constraint1_dd = vectorderivative(constraint1_d,N)
#
constraint2 = pAB-pBA
constraint2_d = constraint2.time_derivative(N,system)
constraint2_dd = constraint2_d.time_derivative(N,system)
wNA = N.getw_(A)
wAB = A.getw_(B)
IA = Dyadic.build(A,Ixx_A,Iyy_A,Izz_A)
IB = Dyadic.build(A,Ixx_B,Iyy_B,Izz_B)
Body('BodyA',A,pAcm,mA,IA,system)
Body('BodyB',B,pBcm,mB,IB,system)
system.addforce(-b*wNA,wNA)
system.addforce(-b*wAB,wAB)
system.addforce(-k*qA*N.z,wNA)
system.addforce(-k*qB*A.z,wAB)
#system.addforce(fNAx*N.x+fNAy*N.y,vNA)
#system.addforce(-fNAx*N.x+-fNAy*N.y,vAN)
system.addforce(fABx*N.x+fABy*N.y,vBA)
plt.plot(*(item.T))
# for item,value in zip(system.get_state_variables(),result.x):
# initialvalues[item]=value
pAcm=pOA+lA/2*A.x
pBcm=pAB+lB/2*B.x
pCcm=pOC+lC/2*C.x
pDcm=pCD+lD/2*D.x
wOA = O.getw_(A)
wAB = A.getw_(B)
wOC = O.getw_(C)
wCD = C.getw_(D)
wBD = B.getw_(D)
BodyO = Body('BodyO',O,pOcm,mO,Dyadic.build(O,I_main,I_main,I_main),system)
#BodyA = Body('BodyA',A,pAcm,mA,Dyadic.build(A,I_leg,I_leg,I_leg),system)
#BodyB = Body('BodyB',B,pBcm,mB,Dyadic.build(B,I_leg,I_leg,I_leg),system)
#BodyC = Body('BodyC',C,pCcm,mC,Dyadic.build(C,I_leg,I_leg,I_leg),system)
#BodyD = Body('BodyD',D,pDcm,mD,Dyadic.build(D,I_leg,I_leg,I_leg),system)
ParticleA = Particle(pAcm,mA,'ParticleA')
ParticleB = Particle(pBcm,mB,'ParticleB')
ParticleC = Particle(pCcm,mC,'ParticleC')
ParticleD = Particle(pDcm,mD,'ParticleD')
system.addforce(-b*wOA,wOA)
system.addforce(-b*wAB,wAB)
system.addforce(-b*wOC,wOC)
system.addforce(-b*wCD,wCD)
system.addforce(-b*wBD,wBD)
wNA = N.getw_(A)
wAB = A.getw_(B)
wBC = B.getw_(C)
# ### Vector derivatives
# The time derivatives of vectors may also be
# vCtip = pCtip.time_derivative(N,system)
# ### Define Inertias and Bodies
# The next several lines compute the inertia dyadics of each body and define a rigid body on each frame. In the case of frame C, we represent the mass as a particle located at point pCcm.
# In[17]:
IA = Dyadic.build(A, Ixx_A, Iyy_A, Izz_A)
IB = Dyadic.build(B, Ixx_B, Iyy_B, Izz_B)
IC = Dyadic.build(B, Ixx_C, Iyy_C, Izz_C)
BodyA = Body('BodyA', A, pAcm, mA, IA, system)
BodyB = Body('BodyB', B, pBcm, mB, IB, system)
BodyC = Body('BodyC', C, pCcm, mC, IC, system)
ParticleM = Particle(pm1, m1, 'ParticleM', system)
# ## Forces and Torques
# Forces and torques are added to the system with the generic ```addforce``` method. The first parameter supplied is a vector describing the force applied at a point or the torque applied along a given rotational axis. The second parameter is the vector describing the linear speed (for an applied force) or the angular velocity(for an applied torque)
# In[18]:
stretch1 = -pC1.dot(N.y)
stretch1_s = (stretch1 + abs(stretch1))
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)
wNA = N.get_w_to(A)
IA_motor = Dyadic.build(A, Ixx_motor, Iyy_motor, Izz_motor)
IA_plate = Dyadic.build(A, Ixx_plate, Iyy_plate, Izz_plate)
BodyMotor = Body('BodyMotor', A, pNA, mA, IA_motor)
BodyPlate = Body('BodyPlate', A, pAcm, mA, IA_plate)
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()
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)
BodyA = Body('BodyA', A, p2, m, I, system)
ParticleO = Particle(p2, M, 'ParticleO', system)
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))
statevariables = [qA, qA_d]
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
pAcm = pNA + lA / 2 * A.x
wNA = N.getw_(A)
BodyA = Body('BodyA', A, pAcm, mA, Dyadic.build(A, Ixx_A, Iyy_A, Izz_A),
system)
system.addforce(-b * wNA, wNA)
system.addforce(-k * qA * N.z, wNA)
system.addforcegravity(-g * N.y)
t = scipy.arange(0, 10, .01)
pynamics.tic()
print('solving dynamics...')
var_dd = system.solvedynamics('LU', False)
pynamics.toc()
print('integrating...')
var_dd = var_dd.subs(system.constants)
func1 = system.createsecondorderfunction(var_dd,
statevariables,
