system.addforce(-b_joint * wOA1, wOA1) system.addforce(-b_joint * wA2B1, wA2B1) system.addforce(-b_joint * wOC1, wOC1) system.addforce(-b_joint * wC2D1, wC2D1) system.addforce(-b_joint * wB2D2, wB2D2) #system.addforce(-b_beam*wA1A2,wA1A2) #system.addforce(-b_beam*wB1B2,wB1B2) #system.addforce(-b_beam*wC1C2,wC1C2) #system.addforce(-b_beam*wD1D2,wD1D2) # stretch = -pBtip.dot(N.y) stretch_s = (stretch + abs(stretch)) on = stretch_s / (2 * stretch + 1e-5) system.add_spring_force1(k_constraint, -stretch_s * N.y, vBtip) system.addforce(-b_constraint * vBtip * on, vBtip) system.add_spring_force1(k_joint, (qA1 - qO - preload1) * N.z, wOA1) system.add_spring_force1(k_joint, (qB1 - qA2 - preload2) * N.z, wA2B1) system.add_spring_force1(k_joint, (qC1 - qO - preload3) * N.z, wOC1) system.add_spring_force1(k_joint, (qD1 - qC2 - preload4) * N.z, wC2D1) system.add_spring_force1(k_joint, (qD2 - qB2 - preload5) * N.z, wB2D2) system.add_spring_force1(k_beam, (qA2 - qA1) * N.z, wA1A2) system.add_spring_force1(k_beam, (qB2 - qB1) * N.z, wB1B2) system.add_spring_force1(k_beam, (qC2 - qC1) * N.z, wC1C2) system.add_spring_force1(k_beam, (qD2 - qD1) * N.z, wD1D2) system.addforcegravity(-g * N.y)
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)
BodyA = Body('BodyA', A, pAcm, mA, IA, system)
BodyB = Body('BodyB', B, pBcm, mB, IB, system)
BodyC = Body('BodyC', C, pCcm, mC, IC, system)
system.addforce(-b * wNA, wNA)
system.addforce(-b * wAB, wAB)
system.addforce(-b * wBC, wBC)
#system.addforce(-k*(qA-preload1)*N.z,wNA)
#system.addforce(-k*(qB-preload2)*A.z,wAB)
#system.addforce(-k*(qC-preload3)*B.z,wBC)
system.add_spring_force1(k, (qA - preload1) * N.z, wNA)
system.add_spring_force1(k, (qB - preload2) * N.z, wAB)
system.add_spring_force1(k, (qC - preload3) * N.z, wBC)
system.addforcegravity(-g * N.y)
x1 = BodyA.pCM.dot(N.x)
y1 = BodyA.pCM.dot(N.y)
x2 = BodyB.pCM.dot(N.x)
y2 = BodyB.pCM.dot(N.y)
x3 = BodyC.pCM.dot(N.x)
y3 = BodyC.pCM.dot(N.y)
KE = system.KE
PE = system.getPEGravity(pNA) - system.getPESprings()
t = numpy.r_[0:5:.001]
#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
l_d =system.derivative(l)
stretch = l - l0
ul_ = l_*((l+error_tol)**-1)
vl = l_.time_derivative(N,system)
system.add_spring_force1(k,stretch*ul_,vl)
#system.addforce(-k*stretch*ul_,vpm1)
#system.addforce(k*stretch*ul_,vpm2)
system.addforce(-b*l_d*ul_,vpm1)
system.addforce(b*l_d*ul_,vpm2)
#system.addforce(k*l*ul_,vpm2)
#system.addforce(-b*vl,vl)
#system.addforce(-b*vl,vl)
#system.addforce(-b*vl,vl)
system.addforcegravity(-g*N.y)
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)
#
stretch = -pBtip.dot(N.y)
stretch_s = (stretch + abs(stretch))
on = stretch_s / (2 * stretch + 1e-10)
system.add_spring_force1(k_constraint, -stretch_s * N.y, vBtip)
system.addforce(-b_constraint * vBtip * on, vBtip)
system.add_spring_force1(k, (qA - qO - preload1) * N.z, wOA)
system.add_spring_force1(k, (qB - qA - preload2) * N.z, wAB)
system.add_spring_force1(k, (qC - qO - preload3) * N.z, wOC)
system.add_spring_force1(k, (qD - qC - preload4) * N.z, wCD)
system.add_spring_force1(k, (qD - qB - preload5) * N.z, wBD)
system.addforcegravity(-g * N.y)
import pynamics.time_series
x = [0, 2, 2, 5, 5, 6, 6, 10]
y = [0, 0, 1, 1, -1, -1, 0, 0]
my_signal, ft2 = pynamics.time_series.build_smoothed_time_signal(
x, y, t, 'my_signal', window_time_width=.1)
def Cal_robot(direction, given_l, given_arm_l, omega1, t1, t2, ini_states,
name1, video_on, x1, x2):
system1 = System()
time_a = time.time()
pynamics.set_system(__name__, system1)
given_k, given_b = x1
f1, f2, f3 = x2
global_q = True
damping_r = 0
tinitial = 0
tfinal = (t1 - t2) / omega1
tstep = 1 / 50
t = numpy.r_[tinitial:tfinal:tstep]
tol_1 = 1e-6
tol_2 = 1e-6
lO = Constant(27.5 / 1000, 'lO', system1)
# given_arm_l = 65
lR = Constant(given_arm_l / 1000, 'lR', system1)
# lR = Constant(40.5/1000,'lR',system11)
lA = Constant(given_l / 1000, 'lA', system1)
lB = Constant(given_l / 1000, 'lB', system1)
lC = Constant(given_l / 1000, 'lC', system1)
mO = Constant(1e0 * 154.5 / 1000, 'mO', system1)
# mR = Constant(9.282/1000 ,'mR',system1)
mR = Constant((1.158 + 0.1445 * given_arm_l) / 1000, 'mR', system1)
mA = Constant(given_l * 2.75 * 0.14450000000000002 / 1000, 'mA', system1)
mB = Constant(given_l * 2.75 * 0.14450000000000002 / 1000, 'mB', system1)
mC = Constant(given_l * 2.75 * 0.14450000000000002 / 1000, 'mC', system1)
k = Constant(given_k, 'k', system1)
friction_perp = Constant(f1, 'f_perp', system1)
friction_par = Constant(-1, 'f_par', system1)
friction_arm_perp = Constant(2 + given_arm_l * f3, 'fr_perp', system1)
friction_arm_par = Constant(-0.3, 'fr_par', system1)
b_damping = Constant(given_b, 'b_damping', system1)
preload0 = Constant(0 * pi / 180, 'preload0', system1)
preload1 = Constant(0 * pi / 180, 'preload1', system1)
preload2 = Constant(0 * pi / 180, 'preload2', system1)
preload3 = Constant(0 * pi / 180, 'preload3', system1)
Ixx_O = Constant(1, 'Ixx_O', system1)
Iyy_O = Constant(1, 'Iyy_O', system1)
Izz_O = Constant(1, 'Izz_O', system1)
Ixx_R = Constant(1, 'Ixx_R', system1)
Iyy_R = Constant(1, 'Iyy_R', system1)
Izz_R = Constant(1, 'Izz_R', system1)
Ixx_A = Constant(1, 'Ixx_A', system1)
Iyy_A = Constant(1, 'Iyy_A', system1)
Izz_A = Constant(1, 'Izz_A', system1)
Ixx_B = Constant(1, 'Ixx_B', system1)
Iyy_B = Constant(1, 'Iyy_B', system1)
Izz_B = Constant(1, 'Izz_B', system1)
Ixx_C = Constant(1, 'Ixx_C', system1)
Iyy_C = Constant(1, 'Iyy_C', system1)
Izz_C = Constant(1, 'Izz_C', system1)
y, y_d, y_dd = Differentiable('y', system1)
qO, qO_d, qO_dd = Differentiable('qO', system1)
qR, qR_d, qR_dd = Differentiable('qR', system1)
qA, qA_d, qA_dd = Differentiable('qA', system1)
qB, qB_d, qB_dd = Differentiable('qB', system1)
qC, qC_d, qC_dd = Differentiable('qC', system1)
initialvalues = {}
initialvalues[y] = ini_states[0] + tol_1
initialvalues[qO] = ini_states[1] + tol_1
initialvalues[qR] = ini_states[2] + tol_1
initialvalues[qA] = ini_states[3] + tol_1
initialvalues[qB] = ini_states[4] + tol_1
initialvalues[qC] = ini_states[5] + tol_1
initialvalues[y_d] = ini_states[6] + tol_1
initialvalues[qO_d] = ini_states[7] + tol_1
initialvalues[qR_d] = ini_states[8] + tol_1
initialvalues[qA_d] = ini_states[9] + tol_1
initialvalues[qB_d] = ini_states[10] + tol_1
initialvalues[qC_d] = ini_states[11] + tol_1
statevariables = system1.get_state_variables()
ini = [initialvalues[item] for item in statevariables]
N = Frame('N')
O = Frame('O')
R = Frame('R')
A = Frame('A')
B = Frame('B')
C = Frame('C')
system1.set_newtonian(N)
if not global_q:
O.rotate_fixed_axis_directed(N, [0, 0, 1], qO, system1)
R.rotate_fixed_axis_directed(O, [0, 0, 1], qR, system1)
A.rotate_fixed_axis_directed(O, [0, 0, 1], qA, system1)
B.rotate_fixed_axis_directed(A, [0, 0, 1], qB, system1)
C.rotate_fixed_axis_directed(B, [0, 0, 1], qC, system1)
else:
O.rotate_fixed_axis_directed(N, [0, 0, 1], qO, system1)
R.rotate_fixed_axis_directed(N, [0, 0, 1], qR, system1)
A.rotate_fixed_axis_directed(N, [0, 0, 1], qA, system1)
B.rotate_fixed_axis_directed(N, [0, 0, 1], qB, system1)
C.rotate_fixed_axis_directed(N, [0, 0, 1], qC, system1)
pNO = 0 * N.x + y * N.y
pOR = pNO + lO * N.x
# pOR= pNO +lO*N.x
pRA = pOR + lR * R.x
pAB = pRA + lA * A.x
pBC = pAB + lB * B.x
pCtip = pBC + lC * C.x
# pOcm=pNO +lO/2*N.x
pOcm = pNO
pRcm = pOR + lR / 2 * R.x
pAcm = pRA + lA / 2 * A.x
pBcm = pAB + lB / 2 * B.x
pCcm = pBC + lC / 2 * C.x
wNO = N.getw_(O)
wOR = N.getw_(R)
wRA = R.getw_(A)
wAB = A.getw_(B)
wBC = B.getw_(C)
IO = Dyadic.build(O, Ixx_O, Iyy_O, Izz_O)
IR = Dyadic.build(R, Ixx_R, Iyy_R, Izz_R)
IA = Dyadic.build(A, Ixx_A, Iyy_A, Izz_A)
IB = Dyadic.build(B, Ixx_B, Iyy_B, Izz_B)
IC = Dyadic.build(C, Ixx_C, Iyy_C, Izz_C)
BodyO = Body('BodyO', O, pOcm, mO, IO, system1)
BodyR = Body('BodyR', R, pRcm, mR, IR, system1)
BodyA = Body('BodyA', A, pAcm, mA, IA, system1)
BodyB = Body('BodyB', B, pBcm, mB, IB, system1)
BodyC = Body('BodyC', C, pCcm, mC, IC, system1)
j_tol = 0 * pi / 180
inv_k = 1e2
alw = 1
system1.add_spring_force1(
k + inv_k * (qA - qR + alw * abs(qA - qR - j_tol)),
(qA - qR - preload1) * N.z, wRA)
system1.add_spring_force1(
k + inv_k * (qB - qA + alw * abs(qB - qA - j_tol)),
(qB - qA - preload2) * N.z, wAB)
system1.add_spring_force1(
k + inv_k * (qC - qB + alw * abs(qC - qB - j_tol)),
(qC - qB - preload3) * N.z, wBC)
# system1.add_spring_force1(k,(qA-qR-preload1)*N.z,wRA)
# system1.add_spring_force1(k,(qB-qA-preload2)*N.z,wAB)
# system1.add_spring_force1(k,(qC-qB-preload3)*N.z,wBC)
vOcm = y_d * N.y
# vOcm = pOcm.time_derivative()
vRcm = pRcm.time_derivative()
vAcm = pAcm.time_derivative()
vBcm = pBcm.time_derivative()
vCcm = pCcm.time_derivative()
nvRcm = 1 / (vRcm.length() + tol_1) * vRcm
nvAcm = 1 / (vAcm.length() + tol_1) * vAcm
nvBcm = 1 / (vBcm.length() + tol_1) * vBcm
nvCcm = 1 / (vCcm.length() + tol_1) * vCcm
vSoil = -direction * 1 * N.y
nSoil = 1 / vSoil.length() * vSoil
# reff = abs( abs(y_d+0.01)-abs(y_d-0.01))*1/0.02*9.75
# foperp = reff*nSoil
foperp = f2 * nSoil
system1.addforce(foperp, vOcm)
# system1.addforce(9.75*1*nSoil,vOcm)
# system1.addforce(9.75*1*nSoil*y_d,vOcm)
# system1.addforce(-100*N.x, y_d*N.x)
frperp = friction_arm_perp * nvRcm.dot(R.y) * R.y
frpar = friction_arm_par * nvRcm.dot(R.x) * R.x
system1.addforce(-(frperp + frpar), vRcm)
faperp = friction_perp * nvAcm.dot(A.y) * A.y
fapar = friction_par * nvAcm.dot(A.x) * A.x
system1.addforce(-(faperp + fapar), vAcm)
fbperp = friction_perp * nvBcm.dot(B.y) * B.y
fbpar = friction_par * nvBcm.dot(B.x) * B.x
system1.addforce(-(fbperp + fbpar), vBcm)
fcperp = friction_perp * nvCcm.dot(C.y) * C.y
fcpar = friction_par * nvCcm.dot(C.x) * C.x
system1.addforce(-(fcperp + fcpar), vCcm)
system1.addforce(-b_damping * 1 * wRA, wRA)
system1.addforce(-b_damping * 1 * wAB, wAB)
system1.addforce(-b_damping * 1 * wBC, wBC)
eq = []
eq_d = [(system1.derivative(item)) for item in eq]
eq_d.append(qR_d - omega1)
# eq_dd= [(system1.derivative(item)) for item in eq_d]
eq_dd = [system1.derivative(eq_d[0])]
f, ma = system1.getdynamics()
func1 = system1.state_space_post_invert(f, ma, eq_dd)
points = [pNO, pOR, pRA, pAB, pBC, pCtip]
constants = system1.constant_values
states = pynamics.integration.integrate_odeint(func1,
ini,
t,
args=({
'constants': constants
}, ))
final = numpy.asarray(states[-1, :])
logger1 = logging.getLogger('pynamics.system1')
logger2 = logging.getLogger('pynamics.integration')
logger3 = logging.getLogger('pynamics.output')
logger1.disabled = True
logger2.disabled = True
logger3.disabled = True
points_output = PointsOutput(points, system1, constant_values=constants)
y1 = points_output.calc(states)
if video_on == 1:
plt.figure()
plt.plot(*(y1[::int(len(y1) / 20)].T) * 1000)
plt.axis('equal')
plt.axis('equal')
plt.title("Plate Configuration vs Distance")
plt.xlabel("Configuration")
plt.ylabel("Distance (mm)")
plt.figure()
plt.plot(t, numpy.rad2deg(states[:, 2]))
plt.plot(t, numpy.rad2deg(states[:, 8]))
plt.legend(["qR", "qR_d"])
plt.hlines(numpy.rad2deg(t1), tinitial, tfinal)
plt.hlines(numpy.rad2deg(t2), tinitial, tfinal)
plt.title("Robot Arm angle and velocitues (qR and qR_d) over Time")
plt.xlabel("Time (s)")
plt.ylabel("Angles,Velocities (deg, deg/s)")
plt.figure()
q_states = numpy.c_[(states[:, 2], states[:, 3], states[:,
4], states[:,
5])]
plt.plot(t, numpy.rad2deg(q_states))
plt.title("Joint Angule over Time")
plt.xlabel("Time (s)")
plt.ylabel("Joint Angles (deg)")
plt.legend(["Arm", "Joint 1", "Joint 2", "Joint 3"])
plt.figure()
qd_states = numpy.c_[(states[:, 8], states[:,
9], states[:,
10], states[:,
11])]
plt.plot(t, numpy.rad2deg(qd_states))
plt.legend(["qR_d", "qA_d", "qB_d", "qC_d"])
plt.title("Joint Angular Velocities over Time")
plt.xlabel("Time (s)")
plt.ylabel("Joint Angular Velocities (deg/s)")
plt.legend(["Arm", "Joint 1", "Joint 2", "Joint 3"])
plt.figure()
plt.plot(t, states[:, 0], '--')
plt.plot(t, states[:, 6])
y_d1 = states[:, 6]
# force1 = abs( abs(y_d1+0.1)-abs(y_d1-0.1))*40
# plt.plot(t,force1)
plt.title("Robot Distance and Velocity over time")
plt.xlabel("Time (s)")
plt.ylabel("Distance (mm)")
plt.legend(["Distance", "Velocity of the robot"])
points_output.animate(fps=1 / tstep,
movie_name=name1,
lw=2,
marker='o',
color=(1, 0, 0, 1),
linestyle='-')
else:
pass
return final, states, y1
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()
for key in changing_constants:
del static_constants[key]
func = system.state_space_post_invert(f, ma, constants=static_constants)
statevariables = system.get_state_variables()
ini = [initialvalues[item] for item in statevariables]
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
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
#system.add_spring_force1(k,(stretched1)*N.y,vAcm)
system.add_spring_force1(k,(stretched2)*N.y,vAcm)
system.addforcegravity(-g*N.y)
x1 = ParticleA.pCM.dot(N.x)
y1 = ParticleA.pCM.dot(N.y)
f,ma = system.getdynamics()
func1 = system.state_space_post_invert(f,ma)
states=pynamics.integration.integrate_odeint(func1,ini,t,rtol=1e-12,atol=1e-12,hmin=1e-14, args=({'constants':system.constant_values},))
KE = system.get_KE()
PE = system.getPEGravity(pNA)-system.getPESprings()
output = Output([x1,y1,KE-PE,x,y],system)
y = output.calc(states)
#system.addforce(-b*wOA,wOA)
system.addforce(-b_joint * wA2B, wA2B)
#system.addforce(-b*wOC,wOC)
system.addforce(-b_joint * wC2D, wC2D)
system.addforce(-b_joint * wBD, wBD)
#
system.addforce(-b_beam * wA1A2, wA1A2)
#system.addforce(-b_beam*wB1B2,wB1B2)
system.addforce(-b_beam * wC1C2, wC1C2)
#system.addforce(-b_beam*wD1D2,wD1D2)
stretch = -pBtip.dot(N.y)
stretch_s = (stretch + abs(stretch))
on = stretch_s / (2 * stretch + 1e-5)
system.add_spring_force1(k_constraint, -stretch_s * N.y, vBtip)
system.addforce(-b_constraint * vBtip * on, vBtip)
kA1 = (-sympy.tanh(((qA1 - qO) * 180 / pi + 180) * 1) / 2 + .5 + sympy.tanh(
((qA1 - qO) * 180 / pi - 180) * 1) / 2 - .5) * k_joint
kC1 = (-sympy.tanh(((qC1 - qO) * 180 / pi + 180) * 1) / 2 + .5 + sympy.tanh(
((qC1 - qO) * 180 / pi - 180) * 1) / 2 - .5) * k_joint
system.add_spring_force1(kA1, (qA1 - qO - preload1) * N.z, wOA1)
#system.add_spring_force1(k_joint,(qB-qA2-preload2)*N.z,wA2B)
system.add_spring_force1(kC1, (qC1 - qO - preload3) * N.z, wOC1)
#system.add_spring_force1(k_joint,(qD-qC2-preload4)*N.z,wC2D)
#system.add_spring_force1(k_joint,(qD-qB-preload5)*N.z,wBD)
system.add_spring_force1(k_beam, (qA2 - qA1) * N.z, wA1A2)
#system.add_spring_force1(k_beam,(qB2-qB1)*N.z,wB1B2)
wB1 = NB1.get_w_to(B1) wB2 = B12.get_w_to(B2) ################################################ #Add damping between joints system.addforce(-b * wA1, wA1) system.addforce(-b * wA2, wA2) system.addforce(-b * wA3, wA3) system.addforce(-b * wB1, wB1) system.addforce(-b * wB2, wB2) #system.addforce(1*A1.x,wA1) ################################################ #Add spring forces to two joints system.add_spring_force1(k, (qA1 - pi / 180 * 45) * A1.x, wA1) system.add_spring_force1(k, (qA2) * A2.x, wA2) system.add_spring_force1(k, (qA3) * A3.x, wA3) system.add_spring_force1(k, (qB1 + pi / 180 * 45) * B1.x, wB1) system.add_spring_force1(k, (qB2) * B2.x, wB2) ################################################ #Add gravity system.addforcegravity(-g * N.z) ################################################ #variables for constraint equation solving v1 = B23.x - A3.x v2 = B23.y - A3.y v3 = B23.z - A3.z
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
# ## 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)
# ### Spring Forces
#
# Spring forces are a special case because the energy stored in springs is conservative and should be considered when calculating the system's potential energy. To do this, use the ```add_spring_force``` command. In this method, the first value is the linear spring constant. The second value is the "stretch" vector, indicating the amount of deflection from the neutral point of the spring. The final parameter is, as above, the linear or angluar velocity vector (depending on whether your spring is a linear or torsional spring)
#
# In this case, the torques applied to each joint are dependent upon whether qA, qB, and qC are absolute or relative rotations, as defined above.
# system.add_spring_force1(k,(qA-preload1)*N.z,wNA)
qAB = -sympy.atan2(A3.x.dot(B3.y), A3.x.dot(B3.x))
system.add_spring_force1(k, (qAB) * A3.z, wA3B3)
qBC = -sympy.atan2(B3.x.dot(C3.y), B3.x.dot(C3.x))
system.add_spring_force1(k, (qBC) * B3.z, wB3C3)
system.add_spring_force1(k2, (qS) * S.z, wA3S)
# ### Gravity
# Again, like springs, the force of gravity is conservative and should be applied to all bodies. To globally apply the force of gravity to all particles and bodies, you can use the special ```addforcegravity``` method, by supplying the acceleration due to gravity as a vector. This will get applied to all bodies defined in your system.
#system.addforcegravity(-g*N.y)
# ## Constraints
# Constraints may be defined that prevent the motion of certain elements. Try uncommenting the commented out line to see what happens.
eq = []
eq.append(A3.z - B3.z)
wNA3 = N.get_w_to(A3)
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)
system.addforce(torque * sympy.sin(freq * 2 * sympy.pi * system.t) * A3.z,
wNA3)
qNA = -sympy.atan2(N.x.dot(A3.y), N.x.dot(A3.x))
system.add_spring_force1(k, (qNA - preload1) * N.z, wNA3)
qAB = -sympy.atan2(A3.x.dot(B3.y), A3.x.dot(B3.x))
system.add_spring_force1(k, (qAB - preload2) * A3.z, wA3B3)
qBC = -sympy.atan2(B3.x.dot(C3.y), B3.x.dot(C3.x))
system.add_spring_force1(k, (qBC - preload3) * B3.z, wB3C3)
system.addforce(-b * wNA3, wNA3)
system.addforce(-b * wA3B3, wA3B3)
system.addforce(-b * wB3C3, wB3C3)
system.addforcegravity(-g * N.y)
eq = []
eq.append(A3.z - B3.z)
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) system.addforce(-b * wAB, wAB) system.addforce(-b * wOC, wOC) system.addforce(-b * wCD, wCD) system.addforce(-b_ankle * wOE, wOE) # stretch1 = -pE1.dot(N.y) stretch1_s = (stretch1 + abs(stretch1)) on = stretch1_s / (2 * stretch1 + 1e-10) system.add_spring_force1(k_constraint, -stretch1_s * N.y, vE1) system.addforce(-b_constraint * vE1 * on, vE1) toeforce = k_constraint * -stretch1_s stretch2 = -pE2.dot(N.y) stretch2_s = (stretch2 + abs(stretch2)) on = stretch2_s / (2 * stretch2 + 1e-10) system.add_spring_force1(k_constraint, -stretch2_s * N.y, vE2) system.addforce(-b_constraint * vE2 * on, vE2) heelforce = k_constraint * -stretch2_s system.add_spring_force1(k, (qA - qO - preload1) * N.z, wOA) system.add_spring_force1(k, (qB - qA - preload2) * N.z, wAB) system.add_spring_force1(k, (qC - qO - preload3) * N.z, wOC)
faperp = k_perp*vSoil.dot(A.y)*A.y fapar= k_par*vSoil.dot(A.x)*A.x fbperp = k_perp*vSoil.dot(B.y)*B.y fbpar= k_par*vSoil.dot(B.x)*B.x fcperp = k_perp*vSoil.dot(C.y)*C.y fcpar= k_par*vSoil.dot(C.x)*C.x system.addforce((faperp+fapar),vAcm) system.addforce((fbperp+fbpar),vBcm) system.addforce((fcperp+fcpar),vCcm) system.addforce(-b*wNA,wNA) system.addforce(-b*wAB,wAB) system.addforce(-b*wBC,wBC) system.add_spring_force1(k,(qA-preload1)*N.z,wNA) system.add_spring_force1(k,(qB-qA-preload2)*N.z,wAB) system.add_spring_force1(k,(qC-qB-preload3)*N.z,wBC) # system.addforcegravity(-g*N.y) f,ma = system.getdynamics() func1 = system.state_space_post_invert(f,ma) tol = 1e-5 tinitial = 0 tfinal = 5 tstep = 1/30 t = numpy.r_[tinitial:tfinal:tstep] def myfunc(x):
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))
on = stretch1_s / (2 * stretch1 + 1e-10)
system.add_spring_force1(k_constraint, -stretch1_s * N.y, vC1)
system.addforce(-b_constraint * vC1 * on, vC1)
toeforce = k_constraint * -stretch1_s
stretch2 = -pC2.dot(N.y)
stretch2_s = (stretch2 + abs(stretch2))
on = stretch2_s / (2 * stretch2 + 1e-10)
system.add_spring_force1(k_constraint, -stretch2_s * N.y, vC2)
system.addforce(-b_constraint * vC2 * on, vC2)
system.addforce(-b * wNA, wNA)
system.addforce(-b * wAB, wAB)
system.addforce(-b * wBC, wBC)
# system.addforce(force_var*N.x,vNA)
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)
stretch = x - xO
system.add_spring_force1(k, (stretch) * N.x, v1)
system.addforce(-b * v1, v1)
system.addforcegravity(-g * N.y)
f, ma = system.getdynamics()
func1 = system.state_space_post_invert(f, ma, constants=system.constant_values)
states = pynamics.integration.integrate_odeint(func1,
ini,
t,
rtol=tol,
atol=tol,
args=({
'constants': {},
'alpha': 1e2,
'beta': 1e1
}, ))
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))
system.add_constraint(AccelerationConstraint(eq_dd_scalar))
f, ma = system.getdynamics()
func1, lambda1 = system.state_space_post_invert(
f, ma, constants=system.constant_values, return_lambda=True)
# system.addforce(-b*wNA,wNA) # system.addforce(-b*wAB,wAB) # system.addforce(-b*wBC,wBC) system.addforce(-b * vCcm, vCcm) # system.addforce(-b*vAcm,vAcm) # ### Spring Forces # # Spring forces are a special case because the energy stored in springs is conservative and should be considered when calculating the system's potential energy. To do this, use the ```add_spring_force``` command. In this method, the first value is the linear spring constant. The second value is the "stretch" vector, indicating the amount of deflection from the neutral point of the spring. The final parameter is, as above, the linear or angluar velocity vector (depending on whether your spring is a linear or torsional spring) # # In this case, the torques applied to each joint are dependent upon whether qA, qB, and qC are absolute or relative rotations, as defined above. # In[19]: # system.add_spring_force1(k,(qA-preload1)*N.z,wNA) system.add_spring_force1(k, (qB - preload2) * A3.z, wA3B1) system.add_spring_force1(k, (qC - preload3) * B2.z, wB2C) # ### Gravity # Again, like springs, the force of gravity is conservative and should be applied to all bodies. To globally apply the force of gravity to all particles and bodies, you can use the special ```addforcegravity``` method, by supplying the acceleration due to gravity as a vector. This will get applied to all bodies defined in your system. # In[20]: system.addforcegravity(-g * N.y) # ## Constraints # Constraints may be defined that prevent the motion of certain elements. Try uncommenting the commented out line to see what happens. # In[21]: eq = []
