Exemple #1
0
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)
Exemple #2
0
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',
Exemple #3
0
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
Exemple #4
0
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()]
Exemple #7
0
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)
Exemple #8
0
#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()
Exemple #10
0
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
Exemple #12
0
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,
Exemple #13
0
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))
Exemple #18
0
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,