def Cal_robot(direction, given_l, omega1, t1, t2, ini_states, name1, system,
video_on, x1):
time_a = time.time()
pynamics.set_system(__name__, system)
given_k, given_b = x1
global_q = True
damping_r = 0
tinitial = 0
tfinal = (t1 - t2) / omega1
tstep = 1 / 30
t = numpy.r_[tinitial:tfinal:tstep]
tol_1 = 1e-6
tol_2 = 1e-6
lO = Constant(27.5 / 1000, 'lO', system)
lR = Constant(40.5 / 1000, 'lR', system)
lA = Constant(given_l / 1000, 'lA', system)
lB = Constant(given_l / 1000, 'lB', system)
lC = Constant(given_l / 1000, 'lC', system)
mO = Constant(154.5 / 1000, 'mO', system)
mR = Constant(9.282 / 1000, 'mR', system)
mA = Constant(given_l * 2.75 * 0.14450000000000002 / 1000, 'mA', system)
mB = Constant(given_l * 2.75 * 0.14450000000000002 / 1000, 'mB', system)
mC = Constant(given_l * 2.75 * 0.14450000000000002 / 1000, 'mC', system)
k = Constant(given_k, 'k', system)
friction_perp = Constant(13 / 3, 'f_perp', system)
friction_par = Constant(-2 / 3, 'f_par', system)
friction_arm_perp = Constant(5.6, 'fr_perp', system)
friction_arm_par = Constant(-0.2, 'fr_par', system)
b_damping = Constant(given_b, 'b_damping', system)
preload0 = Constant(0 * pi / 180, 'preload0', system)
preload1 = Constant(0 * pi / 180, 'preload1', system)
preload2 = Constant(0 * pi / 180, 'preload2', system)
preload3 = Constant(0 * pi / 180, 'preload3', system)
Ixx_O = Constant(1, 'Ixx_O', system)
Iyy_O = Constant(1, 'Iyy_O', system)
Izz_O = Constant(1, 'Izz_O', system)
Ixx_R = Constant(1, 'Ixx_R', system)
Iyy_R = Constant(1, 'Iyy_R', system)
Izz_R = Constant(1, 'Izz_R', system)
Ixx_A = Constant(1, 'Ixx_A', system)
Iyy_A = Constant(1, 'Iyy_A', system)
Izz_A = Constant(1, 'Izz_A', system)
Ixx_B = Constant(1, 'Ixx_B', system)
Iyy_B = Constant(1, 'Iyy_B', system)
Izz_B = Constant(1, 'Izz_B', system)
Ixx_C = Constant(1, 'Ixx_C', system)
Iyy_C = Constant(1, 'Iyy_C', system)
Izz_C = Constant(1, 'Izz_C', system)
y, y_d, y_dd = Differentiable('y', system)
qO, qO_d, qO_dd = Differentiable('qO', system)
qR, qR_d, qR_dd = Differentiable('qR', system)
qA, qA_d, qA_dd = Differentiable('qA', system)
qB, qB_d, qB_dd = Differentiable('qB', system)
qC, qC_d, qC_dd = Differentiable('qC', system)
initialvalues = {}
initialvalues[y] = ini_states[0] + tol_1
initialvalues[qO] = ini_states[1] + tol_1
initialvalues[qR] = ini_states[2] + tol_1
initialvalues[qA] = ini_states[3] + tol_1
initialvalues[qB] = ini_states[4] + tol_1
initialvalues[qC] = ini_states[5] + tol_1
initialvalues[y_d] = ini_states[6] + tol_1
initialvalues[qO_d] = ini_states[7] + tol_1
initialvalues[qR_d] = ini_states[8] + tol_1
initialvalues[qA_d] = ini_states[9] + tol_1
initialvalues[qB_d] = ini_states[10] + tol_1
initialvalues[qC_d] = ini_states[11] + tol_1
statevariables = system.get_state_variables()
ini = [initialvalues[item] for item in statevariables]
N = Frame('N')
O = Frame('O')
R = Frame('R')
A = Frame('A')
B = Frame('B')
C = Frame('C')
system.set_newtonian(N)
if not global_q:
O.rotate_fixed_axis_directed(N, [0, 0, 1], qO, system)
R.rotate_fixed_axis_directed(O, [0, 0, 1], qR, system)
A.rotate_fixed_axis_directed(R, [0, 0, 1], qA, system)
B.rotate_fixed_axis_directed(A, [0, 0, 1], qB, system)
C.rotate_fixed_axis_directed(B, [0, 0, 1], qC, system)
else:
O.rotate_fixed_axis_directed(N, [0, 0, 1], qO, system)
R.rotate_fixed_axis_directed(N, [0, 0, 1], qR, system)
A.rotate_fixed_axis_directed(N, [0, 0, 1], qA, system)
B.rotate_fixed_axis_directed(N, [0, 0, 1], qB, system)
C.rotate_fixed_axis_directed(N, [0, 0, 1], qC, system)
pNO = 0 * N.x + y * N.y
pOR = pNO + lO * N.x
pRA = pOR + lR * R.x
pAB = pRA + lA * A.x
pBC = pAB + lB * B.x
pCtip = pBC + lC * C.x
pOcm = pNO + lO / 2 * N.x
pRcm = pOR + lR / 2 * R.x
pAcm = pRA + lA / 2 * A.x
pBcm = pAB + lB / 2 * B.x
pCcm = pBC + lC / 2 * C.x
wNO = N.getw_(O)
wOR = N.getw_(R)
wRA = R.getw_(A)
wAB = A.getw_(B)
wBC = B.getw_(C)
IO = Dyadic.build(O, Ixx_O, Iyy_O, Izz_O)
IR = Dyadic.build(R, Ixx_R, Iyy_R, Izz_R)
IA = Dyadic.build(A, Ixx_A, Iyy_A, Izz_A)
IB = Dyadic.build(B, Ixx_B, Iyy_B, Izz_B)
IC = Dyadic.build(C, Ixx_C, Iyy_C, Izz_C)
BodyO = Body('BodyO', O, pOcm, mO, IO, system)
BodyR = Body('BodyR', R, pRcm, mR, IR, system)
BodyA = Body('BodyA', A, pAcm, mA, IA, system)
BodyB = Body('BodyB', B, pBcm, mB, IB, system)
BodyC = Body('BodyC', C, pCcm, mC, IC, system)
j_tol = 3 * pi / 180
inv_k = 10
system.add_spring_force1(k + inv_k * (qA - qR + abs(qA - qR - j_tol)),
(qA - qR - preload1) * N.z, wRA)
system.add_spring_force1(k + inv_k * (qB - qA + abs(qB - qA - j_tol)),
(qB - qA - preload2) * N.z, wAB)
system.add_spring_force1(k + inv_k * (qC - qB + abs(qC - qB - j_tol)),
(qC - qB - preload3) * N.z, wBC)
vOcm = y_d * N.y
vRcm = pRcm.time_derivative()
vAcm = pAcm.time_derivative()
vBcm = pBcm.time_derivative()
vCcm = pCcm.time_derivative()
nvRcm = 1 / (vRcm.length() + tol_1) * vRcm
nvAcm = 1 / (vAcm.length() + tol_1) * vAcm
nvBcm = 1 / (vBcm.length() + tol_1) * vBcm
nvCcm = 1 / (vCcm.length() + tol_1) * vCcm
vSoil = -direction * 1 * N.y
nSoil = 1 / vSoil.length() * vSoil
foperp = 8 * nSoil
system.addforce(-foperp, vOcm)
frperp = friction_arm_perp * nvRcm.dot(R.y) * R.y
frpar = friction_arm_par * nvRcm.dot(R.x) * R.x
system.addforce(-(frperp + frpar), vRcm)
faperp = friction_perp * nvAcm.dot(A.y) * A.y
fapar = friction_par * nvAcm.dot(A.x) * A.x
system.addforce(-(faperp + fapar), vAcm)
fbperp = friction_perp * nvBcm.dot(B.y) * B.y
fbpar = friction_par * nvBcm.dot(B.x) * B.x
system.addforce(-(fbperp + fbpar), vBcm)
fcperp = friction_perp * nvCcm.dot(C.y) * C.y
fcpar = friction_par * nvCcm.dot(C.x) * C.x
system.addforce(-(fcperp + fcpar), vCcm)
system.addforce(-b_damping * 1 * wRA, wRA)
system.addforce(-b_damping * 1 * wAB, wAB)
system.addforce(-b_damping * 1 * wBC, wBC)
eq = []
eq_d = [(system.derivative(item)) for item in eq]
eq_d.append(qR_d - omega1)
eq_dd = [(system.derivative(item)) for item in eq_d]
f, ma = system.getdynamics()
func1 = system.state_space_post_invert(f, ma, eq_dd)
points = [pNO, pOR, pRA, pAB, pBC, pCtip]
constants = system.constant_values
states = pynamics.integration.integrate_odeint(func1,
ini,
t,
args=({
'constants': constants
}, ))
final = numpy.asarray(states[-1, :])
logger1 = logging.getLogger('pynamics.system')
logger2 = logging.getLogger('pynamics.integration')
logger3 = logging.getLogger('pynamics.output')
logger1.disabled = True
logger2.disabled = True
logger3.disabled = True
points_output = PointsOutput(points, system, constant_values=constants)
y1 = points_output.calc(states)
if video_on == 1:
plt.figure()
plt.plot(*(y1[::int(len(y1) / 20)].T) * 1000)
plt.axis('equal')
plt.axis('equal')
plt.title("Plate Configuration vs Distance")
plt.xlabel("Configuration")
plt.ylabel("Distance (mm)")
plt.figure()
plt.plot(t, numpy.rad2deg(states[:, 2]))
plt.plot(t, numpy.rad2deg(states[:, 8]))
plt.legend(["qR", "qR_d"])
plt.hlines(numpy.rad2deg(t1), tinitial, tfinal)
plt.hlines(numpy.rad2deg(t2), tinitial, tfinal)
plt.title("Robot Arm angle and velocitues (qR and qR_d) over Time")
plt.xlabel("Time (s)")
plt.ylabel("Angles,Velocities (deg, deg/s)")
plt.figure()
q_states = numpy.c_[(states[:, 2], states[:, 3], states[:,
4], states[:,
5])]
plt.plot(t, numpy.rad2deg(q_states))
plt.title("Joint Angule over Time")
plt.xlabel("Time (s)")
plt.ylabel("Joint Angles (deg)")
plt.legend(["Arm", "Joint 1", "Joint 2", "Joint 3"])
plt.figure()
qd_states = numpy.c_[(states[:, 8], states[:,
9], states[:,
10], states[:,
11])]
plt.plot(t, numpy.rad2deg(qd_states))
plt.legend(["qR_d", "qA_d", "qB_d", "qC_d"])
plt.title("Joint Angular Velocities over Time")
plt.xlabel("Time (s)")
plt.ylabel("Joint Angular Velocities (deg/s)")
plt.legend(["Arm", "Joint 1", "Joint 2", "Joint 3"])
plt.figure()
plt.plot(t, states[:, 0], '--')
plt.plot(t, states[:, 6])
plt.title("Robot Distance and Velocity over time")
plt.xlabel("Time (s)")
plt.ylabel("Distance (mm)")
plt.legend(["Distance", "Velocity of the robot"])
points_output.animate(fps=1 / tstep,
movie_name=name1,
lw=2,
marker='o',
color=(1, 0, 0, 1),
linestyle='-')
else:
pass
return final, states, y1
# These two lines of code order the initial values in a list in such a way that the integrator can use it in the same order that it expects the variables to be supplied
# In[10]:
statevariables = system.get_state_variables()
ini = [initialvalues[item] for item in statevariables]
# ## Kinematics
#
# ### Frames
# Define the reference frames of the system
# In[11]:
N = Frame('N')
A = Frame('A')
B = Frame('B')
C = Frame('C')
# ### Newtonian Frame
#
# It is important to define the Newtonian reference frame as a reference frame that is not accelerating, otherwise the dynamic equations will not be correct
# In[12]:
system.set_newtonian(N)
# This is the first time that the "global_q" variable is used. If you choose to rotate each frame with reference to the base frame, there is the potential for a representational simplification. If you use a relative rotation, this can also be simpler in some cases. Try running the code either way to see which one is simpler in this case.
# In[13]:
mC = Constant(1,'mC')
Ixx = Constant(2,'Ixx')
Iyy = Constant(3,'Iyy')
Izz = Constant(1,'Izz')
initialvalues = {}
initialvalues[qA]=0*math.pi/180
initialvalues[qA_d]=1
initialvalues[qB]=0*math.pi/180
initialvalues[qB_d]=1
initialvalues[qC]=0*math.pi/180
initialvalues[qC_d]=0
N = Frame('N',system)
A = Frame('A',system)
B = Frame('B',system)
C = Frame('C',system)
system.set_newtonian(N)
A.rotate_fixed_axis(N,[1,0,0],qA,system)
B.rotate_fixed_axis(A,[0,1,0],qB,system)
C.rotate_fixed_axis(B,[0,0,1],qC,system)
pCcm=0*N.x
w1 = N.get_w_to(C)
IC = Dyadic.build(C,Ixx,Iyy,Izz)
qA, qA_d, qA_dd = Differentiable(system, 'qA')
qB, qB_d, qB_dd = Differentiable(system, 'qB')
qC, qC_d, qC_dd = Differentiable(system, 'qC')
initialvalues = {}
initialvalues[qA] = 0 * pi / 180
initialvalues[qA_d] = 0 * pi / 180
initialvalues[qB] = 0 * pi / 180
initialvalues[qB_d] = 0 * pi / 180
initialvalues[qC] = 0 * pi / 180
initialvalues[qC_d] = 0 * pi / 180
statevariables = system.get_state_variables()
ini = [initialvalues[item] for item in statevariables]
N = Frame('N', system)
A = Frame('A', system)
B = Frame('B', system)
C = Frame('C', system)
system.set_newtonian(N)
A.rotate_fixed_axis(N, [0, 0, 1], qA, system)
B.rotate_fixed_axis(A, [0, 0, 1], qB, system)
C.rotate_fixed_axis(B, [0, 0, 1], qC, system)
pNA = 0 * N.x
pAB = pNA + lA * A.x
pBC = pAB + lB * B.x
pCtip = pBC + lC * C.x
pAcm = pNA + lA / 2 * A.x
qA, qA_d, qA_dd = Differentiable('qA', system)
x, x_d, x_dd = Differentiable('x', system)
y, y_d, y_dd = Differentiable('y', system)
initialvalues = {}
initialvalues[qA] = 0 * pi / 180
initialvalues[qA_d] = 0 * pi / 180
initialvalues[x] = 1
initialvalues[y] = 0
initialvalues[x_d] = 0
initialvalues[y_d] = 0
statevariables = system.get_state_variables()
ini = [initialvalues[item] for item in statevariables]
N = Frame('N')
A = Frame('A')
system.set_newtonian(N)
A.rotate_fixed_axis_directed(N, [0, 0, 1], qA, system)
pNA = 0 * N.x
pAB = pNA + lA * A.x
vAB = pAB.time_derivative(N, system)
pNA2 = 2 * N.x
pAB2 = pNA2 + x * N.x + y * N.y
vAB2 = pAB2.time_derivative(N, system)
ParticleA = Particle(pAB, mA, 'ParticleA', system)
ParticleB = Particle(pAB2, mA, 'ParticleB', system)
mC = Constant(1, 'mC')
Ixx = Constant(2, 'Ixx')
Iyy = Constant(3, 'Iyy')
Izz = Constant(1, 'Izz')
initialvalues = {}
initialvalues[qA] = 0 * math.pi / 180
initialvalues[qB] = 0 * math.pi / 180
initialvalues[qC] = 0 * math.pi / 180
initialvalues[wx] = 1.
initialvalues[wy] = 1.
initialvalues[wz] = 0.
N = Frame('N')
A = Frame('A')
B = Frame('B')
C = Frame('C')
system.set_newtonian(N)
A.rotate_fixed_axis_directed(N, [1, 0, 0], qA, system)
B.rotate_fixed_axis_directed(A, [0, 1, 0], qB, system)
C.rotate_fixed_axis_directed(B, [0, 0, 1], qC, system)
pCcm = 0 * N.x
IC = Dyadic.build(C, Ixx, Iyy, Izz)
w1 = N.getw_(C)
w2 = wx * C.x + wy * C.y + wz * C.z
qD1: -pi + 2.64,
qD1_d: 0,
qA2: -0.89,
qA2_d: 0,
qB2: -2.64,
qB2_d: 0,
qC2: -pi + 0.89,
qC2_d: 0,
qD2: -pi + 2.64,
qD2_d: 0
}
statevariables = system.get_state_variables()
ini = [initialvalues[item] for item in statevariables]
N = Frame('N', system)
O = Frame('O', system)
A1 = Frame('A1', system)
B1 = Frame('B1', system)
C1 = Frame('C1', system)
D1 = Frame('D1', system)
A2 = Frame('A2', system)
B2 = Frame('B2', system)
C2 = Frame('C2', system)
D2 = Frame('D2', system)
system.set_newtonian(N)
O.rotate_fixed_axis(N, [0, 0, 1], qO, system)
A1.rotate_fixed_axis(N, [0, 0, 1], qA1, system)
B1.rotate_fixed_axis(N, [0, 0, 1], qB1, system)
q2,q2_d,q2_dd = Differentiable(system,'q2')
initialvalues = {}
initialvalues[x]=0
initialvalues[y]=1
initialvalues[x_d]=0
initialvalues[y_d]=0
initialvalues[q1]=0
initialvalues[q1_d]=0
initialvalues[q2]=0
initialvalues[q2_d]=0
statevariables = system.get_q(0)+system.get_q(1)
ini = [initialvalues[item] for item in statevariables]
N = Frame('N')
A = Frame('A')
B = Frame('B')
system.set_newtonian(N)
A.rotate_fixed_axis_directed(N,[0,0,1],q1,system)
B.rotate_fixed_axis_directed(A,[0,0,1],q2,system)
pNA=0*N.x
#pAB=pNA+x*N.x+y*N.y
#vAB=pAB.time_derivative(N,system)
pm1 = x*N.x+y*N.y
vm1 = pm1.time_derivative(N,system)
pm2 = pm1 + l3*B.x
pk1 = pm1-l1*A.x
initialvalues[qA3] = 1 * pi / 180
initialvalues[qB1] = -1 * pi / 180
initialvalues[qB2] = -1 * pi / 180
initialvalues[qA1_d] = 0 * pi / 180
initialvalues[qA2_d] = 0 * pi / 180
initialvalues[qA3_d] = 0 * pi / 180
initialvalues[qB1_d] = 0 * pi / 180
initialvalues[qB2_d] = 0 * pi / 180
statevariables = system.get_state_variables()
ini = [initialvalues[item] for item in statevariables]
################################################
#Create Frames
N = Frame('N', system)
A1 = Frame('A1', system)
A12 = Frame('A12', system)
A2 = Frame('A2', system)
A23 = Frame('A23', system)
A3 = Frame('A3', system)
A34 = Frame('A34', system)
NB1 = Frame('NB1', system)
B1 = Frame('B1', system)
B12 = Frame('B12', system)
B2 = Frame('B2', system)
B23 = Frame('B23', system)
################################################
#Relative frame rotations from newtonian out to distal frames
qB_d: 0,
qD: -pi + 2.64,
qD_d: 0
}
initialvalues[iA] = 0
initialvalues[iC] = 0
initialvalues[qMA] = 0
initialvalues[qMA_d] = 0
initialvalues[qMC] = 0
initialvalues[qMC_d] = 0
statevariables = system.get_state_variables()
ini = [initialvalues[item] for item in statevariables]
N = Frame('N', system)
O = Frame('O', system)
A1 = Frame('A1', system)
C1 = Frame('C1', system)
A2 = Frame('A2', system)
C2 = Frame('C2', system)
MA = Frame('MA', system)
A = Frame('A', system)
B = Frame('B', system)
MC = Frame('MC', system)
C = Frame('C', system)
D = Frame('D', system)
system.set_newtonian(N)
O.rotate_fixed_axis(N, [0, 0, 1], qO, system)
Differentiable(system, 'qA')
Differentiable(system, 'qB')
#Differentiable('qC',system)
initialvalues = {}
initialvalues[qA] = 0 * pi / 180
initialvalues[qA_d] = 0 * pi / 180
initialvalues[qB] = 0 * pi / 180
initialvalues[qB_d] = 0 * pi / 180
#initialvalues[qC]=0*pi/180
#initialvalues[qC_d]=0*pi/180
statevariables = system.get_q(0) + system.get_q(1)
ini = [initialvalues[item] for item in statevariables]
Frame('N')
Frame('A')
Frame('B')
#Frame('C')
system.set_newtonian(N)
A.rotate_fixed_axis_directed(N, [0, 0, 1], qA, system)
B.rotate_fixed_axis_directed(A, [0, 0, 1], qB, system)
pNA = 0 * N.x
pAB = pNA + lA * A.x
pBC = pAB + lB * B.x
#pCtip = pBC + lC*C.x
pAcm = pNA + lA / 2 * A.x
pBcm = pAB + lB / 2 * B.x
from pynamics.variable_types import Differentiable, Constant
from pynamics.system import System
from pynamics.body import Body
from pynamics.dyadic import Dyadic
from pynamics.output import Output, PointsOutput
from pynamics.particle import Particle
import pynamics.integration
import numpy
import matplotlib.pyplot as plt
plt.ion()
from math import pi
system = System()
pynamics.set_system(__name__, system)
e0, e0_d, e0_dd = Differentiable('e0')
e1, e1_d, e1_dd = Differentiable('e1')
e2, e2_d, e2_dd = Differentiable('e2')
e3, e3_d, e3_dd = Differentiable('e3')
qA, qA_d, qA_dd = Differentiable('qA')
qB, qB_d, qB_dd = Differentiable('qB')
qC, qC_d, qC_dd = Differentiable('qC')
N = Frame('N', system)
A = Frame('A', system)
B = Frame('B', system)
v = e1 * N.x + e2 * N.y + e3 * N.z
eq = e0**2 + e1**2 + +e2**2 + e3**2 - 1
pynamics.set_system(__name__, system)
mp1 = Constant(1, 'mp1')
g = Constant(9.81, 'g')
l1 = Constant(1, 'l1')
b = Constant(1, 'b')
k = Constant(1e1, 'k', system)
x, x_d, x_dd = Differentiable('x', ini=[1, 0])
y, y_d, y_dd = Differentiable('y', ini=[0, 0])
z, z_d, z_dd = Differentiable('z', ini=[0, 0])
q1, q1_d, q1_dd = Differentiable('q1', ini=[0.01, 1])
q2, q2_d, q2_dd = Differentiable('q2', ini=[0.01, 0])
q3, q3_d, q3_dd = Differentiable('q3', ini=[0.01, 0])
f1 = Frame()
f2 = Frame()
f3 = Frame()
f4 = Frame()
#f5 = Frame()
system.set_newtonian(f1)
f2.rotate_fixed_axis_directed(f1, [0, 0, 1], q1)
f3.rotate_fixed_axis_directed(f2, [1, 0, 0], q2)
f4.rotate_fixed_axis_directed(f3, [0, 1, 0], q3)
p1 = x * f1.x + y * f1.y + z * f1.z
p2 = -l1 * f4.x
#v1=p1.time_derivative(f1)
wNA = f1.getw_(f2)
def Cal_system(initial_states, drag_direction, tinitial, tstep, tfinal,
fit_vel, f1, f2):
g_k, g_b_damping, g_b_damping1 = [0.30867935, 1.42946955, 1.08464536]
system = System()
pynamics.set_system(__name__, system)
global_q = True
lO = Constant(7 / 1000, 'lO', system)
lA = Constant(33 / 1000, 'lA', system)
lB = Constant(33 / 1000, 'lB', system)
lC = Constant(33 / 1000, 'lC', system)
mO = Constant(10 / 1000, 'mA', system)
mA = Constant(2.89 / 1000, 'mA', system)
mB = Constant(2.89 / 1000, 'mB', system)
mC = Constant(2.89 / 1000, 'mC', system)
k = Constant(g_k, 'k', system)
k1 = Constant(0.4, 'k1', system)
friction_perp = Constant(f1, 'f_perp', system)
friction_par = Constant(f2, 'f_par', system)
b_damping = Constant(g_b_damping, 'b_damping', system)
b_damping1 = Constant(g_b_damping1, 'b_damping1', system)
preload0 = Constant(0 * pi / 180, 'preload0', system)
preload1 = Constant(0 * pi / 180, 'preload1', system)
preload2 = Constant(0 * pi / 180, 'preload2', system)
preload3 = Constant(0 * pi / 180, 'preload3', system)
Ixx_O = Constant(1, 'Ixx_O', system)
Iyy_O = Constant(1, 'Iyy_O', system)
Izz_O = Constant(1, 'Izz_O', system)
Ixx_A = Constant(1, 'Ixx_A', system)
Iyy_A = Constant(1, 'Iyy_A', system)
Izz_A = Constant(1, 'Izz_A', system)
Ixx_B = Constant(1, 'Ixx_B', system)
Iyy_B = Constant(1, 'Iyy_B', system)
Izz_B = Constant(1, 'Izz_B', system)
Ixx_C = Constant(1, 'Ixx_C', system)
Iyy_C = Constant(1, 'Iyy_C', system)
Izz_C = Constant(1, 'Izz_C', system)
y, y_d, y_dd = Differentiable('y', system)
qO, qO_d, qO_dd = Differentiable('qO', system)
qA, qA_d, qA_dd = Differentiable('qA', system)
qB, qB_d, qB_dd = Differentiable('qB', system)
qC, qC_d, qC_dd = Differentiable('qC', system)
fit_states = initial_states
initialvalues = {}
initialvalues[y] = fit_states[0]
initialvalues[y_d] = fit_states[5]
initialvalues[qO] = 0
initialvalues[qO_d] = 0
initialvalues[qA] = fit_states[2]
initialvalues[qA_d] = fit_states[7]
initialvalues[qB] = fit_states[3]
initialvalues[qB_d] = fit_states[8]
initialvalues[qC] = fit_states[4]
initialvalues[qC_d] = fit_states[9]
statevariables = system.get_state_variables()
ini = [initialvalues[item] for item in statevariables]
N = Frame('N')
O = Frame('O')
A = Frame('A')
B = Frame('B')
C = Frame('C')
system.set_newtonian(N)
if not global_q:
O.rotate_fixed_axis_directed(N, [0, 0, 1], qO, system)
A.rotate_fixed_axis_directed(O, [0, 0, 1], qA, system)
B.rotate_fixed_axis_directed(A, [0, 0, 1], qB, system)
C.rotate_fixed_axis_directed(B, [0, 0, 1], qC, system)
else:
O.rotate_fixed_axis_directed(N, [0, 0, 1], qO, system)
A.rotate_fixed_axis_directed(N, [0, 0, 1], qA, system)
B.rotate_fixed_axis_directed(N, [0, 0, 1], qB, system)
C.rotate_fixed_axis_directed(N, [0, 0, 1], qC, system)
pNO = 0 * N.x + y * N.y
pOA = lO * N.x + y * N.y
pAB = pOA + lA * A.x
pBC = pAB + lB * B.x
pCtip = pBC + lC * C.x
pOcm = pNO + lO / 2 * N.x
pAcm = pOA + lA / 2 * A.x
pBcm = pAB + lB / 2 * B.x
pCcm = pBC + lC / 2 * C.x
wNO = N.getw_(O)
wOA = N.getw_(A)
wAB = A.getw_(B)
wBC = B.getw_(C)
IO = Dyadic.build(O, Ixx_O, Iyy_O, Izz_O)
IA = Dyadic.build(A, Ixx_A, Iyy_A, Izz_A)
IB = Dyadic.build(B, Ixx_B, Iyy_B, Izz_B)
IC = Dyadic.build(C, Ixx_C, Iyy_C, Izz_C)
BodyO = Body('BodyO', O, pOcm, mO, IO, system)
BodyA = Body('BodyA', A, pAcm, mA, IA, system)
BodyB = Body('BodyB', B, pBcm, mB, IB, system)
BodyC = Body('BodyC', C, pCcm, mC, IC, system)
# vOcm = pOcm.time_derivative()
vAcm = pAcm.time_derivative()
vBcm = pBcm.time_derivative()
vCcm = pCcm.time_derivative()
# system.add_spring_force1(k1+10000*(qA+abs(qA)),(qA-qO-preload1)*N.z,wOA)
# system.add_spring_force1(k+10000*(qB+abs(qB)),(qB-qA-preload2)*N.z,wAB)
# system.add_spring_force1(k+10000*(qC+abs(qC)),(qC-qB-preload3)*N.z,wBC)
system.add_spring_force1(k1, (qA - qO - preload1) * N.z, wOA)
system.add_spring_force1(k, (qB - qA - preload2) * N.z, wAB)
system.add_spring_force1(k, (qC - qB - preload3) * N.z, wBC)
#new Method use nJoint
nvAcm = 1 / vAcm.length() * vAcm
nvBcm = 1 / vBcm.length() * vBcm
nvCcm = 1 / vCcm.length() * vCcm
vSoil = drag_direction * 1 * N.y
nSoil = 1 / vSoil.length() * vSoil
if fit_vel == 0:
vSoil = 1 * 1 * N.y
nSoil = 1 / vSoil.length() * vSoil
faperp = friction_perp * nSoil.dot(A.y) * A.y
fapar = friction_par * nSoil.dot(A.x) * A.x
system.addforce(-(faperp + fapar), vAcm)
fbperp = friction_perp * nSoil.dot(B.y) * B.y
fbpar = friction_par * nSoil.dot(B.x) * B.x
system.addforce(-(fbperp + fbpar), vBcm)
fcperp = friction_perp * nSoil.dot(C.y) * C.y
fcpar = friction_par * nSoil.dot(C.x) * C.x
system.addforce(-(fcperp + fcpar), vCcm)
else:
faperp = friction_perp * nvAcm.dot(A.y) * A.y
fapar = friction_par * nvAcm.dot(A.x) * A.x
system.addforce(-(faperp + fapar), vAcm)
fbperp = friction_perp * nvBcm.dot(B.y) * B.y
fbpar = friction_par * nvBcm.dot(B.x) * B.x
system.addforce(-(fbperp + fbpar), vBcm)
fcperp = friction_perp * nvCcm.dot(C.y) * C.y
fcpar = friction_par * nvCcm.dot(C.x) * C.x
system.addforce(-(fcperp + fcpar), vCcm)
system.addforce(-b_damping1 * wOA, wOA)
system.addforce(-b_damping * wAB, wAB)
system.addforce(-b_damping * wBC, wBC)
eq = []
eq_d = [(system.derivative(item)) for item in eq]
eq_d.append(y_d - fit_vel)
eq_dd = [(system.derivative(item)) for item in eq_d]
f, ma = system.getdynamics()
func1 = system.state_space_post_invert(f, ma, eq_dd)
points = [pNO, pOA, pAB, pBC, pCtip]
constants = system.constant_values
states = pynamics.integration.integrate_odeint(func1,
ini,
t,
args=({
'constants': constants
}, ))
points_output = PointsOutput(points, system, constant_values=constants)
y = points_output.calc(states)
final = numpy.asarray(states[-1, :])
time1 = time.time()
points_output.animate(fps=30,
movie_name=str(time1) + 'video_1.mp4',
lw=2,
marker='o',
color=(1, 0, 0, 1),
linestyle='-')
return final, states, y, system
initialvalues[Q_d] = 0 * pi / 180
initialvalues[x] = 0
initialvalues[x_d] = 0
initialvalues[y] = 0
initialvalues[y_d] = 0
initialvalues[wx] = 0
initialvalues[wx_d] = 0
initialvalues[wy] = 0
initialvalues[wy_d] = 0
initialvalues[wz] = 0
initialvalues[wz_d] = 0
statevariables = system.get_state_variables()
ini = [initialvalues[item] for item in statevariables]
A = Frame('A', system)
B = Frame('B', system)
C = Frame('C', system)
D = Frame('D', system)
system.set_newtonian(A)
B.rotate_fixed_axis(A, [0, 0, 1], H, system)
C.rotate_fixed_axis(B, [1, 0, 0], -L, system)
D.rotate_fixed_axis(C, [0, 1, 0], Q, system)
pNA = 0 * A.x
pAD = pNA + x * A.x + y * A.y
pBcm = pAD + r * C.z
pDA = pBcm - r * D.z
wAD = A.get_w_to(D)
def init_system(v, drag_direction, time_step):
import pynamics
from pynamics.frame import Frame
from pynamics.variable_types import Differentiable, Constant
from pynamics.system import System
from pynamics.body import Body
from pynamics.dyadic import Dyadic
from pynamics.output import Output, PointsOutput
from pynamics.particle import Particle
import pynamics.integration
import logging
import sympy
import numpy
import matplotlib.pyplot as plt
from math import pi
from scipy import optimize
from sympy import sin
import pynamics.tanh as tanh
from fit_qs import exp_fit
import fit_qs
# time_step = tstep
x = numpy.zeros((7, 1))
friction_perp = x[0]
friction_par = x[1]
given_b = x[2]
given_k = x[3]
given_k1 = x[4]
given_b1 = x[4]
system = System()
pynamics.set_system(__name__, system)
global_q = True
lO = Constant(7 / 1000, 'lO', system)
lA = Constant(33 / 1000, 'lA', system)
lB = Constant(33 / 1000, 'lB', system)
lC = Constant(33 / 1000, 'lC', system)
mO = Constant(10 / 1000, 'mA', system)
mA = Constant(2.89 / 1000, 'mA', system)
mB = Constant(2.89 / 1000, 'mB', system)
mC = Constant(2.89 / 1000, 'mC', system)
k = Constant(0.209, 'k', system)
k1 = Constant(0.209, 'k1', system)
friction_perp = Constant(1.2, 'f_perp', system)
friction_par = Constant(-0.2, 'f_par', system)
b_damping = Constant(given_b, 'b_damping', system)
# time_step = 1/00
if v == 0:
[
t, tinitial, tfinal, tstep, qAa1, qAb1, qAc1, qAa2, qAb2, qAc2,
qAa3, qAb3, qAc3, qBa1, qBb1, qBc1, qBa2, qBb2, qBc2, qBa3, qBb3,
qBc3, qCa1, qCb1, qCc1, qCa2, qCb2, qCc2, qCa3, qCb3, qCc3
] = fit_qs.fit_0_amount(time_step)
elif v == 10:
[
t, tinitial, tfinal, tstep, qAa1, qAb1, qAc1, qAa2, qAb2, qAc2,
qAa3, qAb3, qAc3, qBa1, qBb1, qBc1, qBa2, qBb2, qBc2, qBa3, qBb3,
qBc3, qCa1, qCb1, qCc1, qCa2, qCb2, qCc2, qCa3, qCb3, qCc3
] = fit_qs.fit_10_amount(time_step)
elif v == 20:
[
t, tinitial, tfinal, tstep, qAa1, qAb1, qAc1, qAa2, qAb2, qAc2,
qAa3, qAb3, qAc3, qBa1, qBb1, qBc1, qBa2, qBb2, qBc2, qBa3, qBb3,
qBc3, qCa1, qCb1, qCc1, qCa2, qCb2, qCc2, qCa3, qCb3, qCc3
] = fit_qs.fit_20_amount(time_step)
elif v == 30:
[
t, tinitial, tfinal, tstep, qAa1, qAb1, qAc1, qAa2, qAb2, qAc2,
qAa3, qAb3, qAc3, qBa1, qBb1, qBc1, qBa2, qBb2, qBc2, qBa3, qBb3,
qBc3, qCa1, qCb1, qCc1, qCa2, qCb2, qCc2, qCa3, qCb3, qCc3
] = fit_qs.fit_30_amount(time_step)
elif v == 40:
[
t, tinitial, tfinal, tstep, qAa1, qAb1, qAc1, qAa2, qAb2, qAc2,
qAa3, qAb3, qAc3, qBa1, qBb1, qBc1, qBa2, qBb2, qBc2, qBa3, qBb3,
qBc3, qCa1, qCb1, qCc1, qCa2, qCb2, qCc2, qCa3, qCb3, qCc3
] = fit_qs.fit_40_amount(time_step)
elif v == 50:
[
t, tinitial, tfinal, tstep, qAa1, qAb1, qAc1, qAa2, qAb2, qAc2,
qAa3, qAb3, qAc3, qBa1, qBb1, qBc1, qBa2, qBb2, qBc2, qBa3, qBb3,
qBc3, qCa1, qCb1, qCc1, qCa2, qCb2, qCc2, qCa3, qCb3, qCc3
] = fit_qs.fit_50_amount(time_step)
distance = 200 / 1000
nums = int(tfinal / tstep)
array_num = numpy.arange(0, nums)
array_num1 = numpy.repeat(array_num, nums, axis=0)
array_num1.shape = (nums, nums)
error_k = array_num1 / 8000 + numpy.ones((nums, nums))
fit_t = t
fit_qA = exp_fit(fit_t, qAa1, qAb1, qAc1, qAa2, qAb2, qAc2, qAa3, qAb3,
qAc3)
fit_qB = exp_fit(fit_t, qBa1, qBb1, qBc1, qBa2, qBb2, qBc2, qBa3, qBb3,
qBc3)
fit_qC = exp_fit(fit_t, qCa1, qCb1, qCc1, qCa2, qCb2, qCc2, qCa3, qCb3,
qCc3)
fit_qAd1 = numpy.diff(fit_qA) / numpy.diff(fit_t)
fit_qAd = numpy.append(fit_qAd1[0], fit_qAd1)
fit_qBd1 = numpy.diff(fit_qB) / numpy.diff(fit_t)
fit_qBd = numpy.append(fit_qBd1[0], fit_qBd1)
fit_qCd1 = numpy.diff(fit_qC) / numpy.diff(fit_t)
fit_qCd = numpy.append(fit_qCd1[0], fit_qCd1)
fit_states1 = numpy.stack(
(fit_qA, fit_qB, fit_qC, fit_qAd, fit_qBd, fit_qCd), axis=1)
fit_states1[:, 0:3] = fit_states1[:, 0:3] - fit_states1[0, 0:3]
fit_states = -drag_direction * numpy.deg2rad(fit_states1)
# plt.plot(t,fit_states)
if drag_direction == -1:
zero_shape = fit_states.shape
fit_states = numpy.zeros(zero_shape)
fit_vel = drag_direction * distance / (tfinal)
if qAa1 == 0:
fit_vel = 0
fit_v = numpy.ones(t.shape) * fit_vel
if qAa1 == 0:
fit_d = numpy.ones(t.shape) * fit_vel
else:
fit_d = drag_direction * numpy.r_[tinitial:distance:tstep *
abs(fit_vel)]
preload0 = Constant(0 * pi / 180, 'preload0', system)
preload1 = Constant(0 * pi / 180, 'preload1', system)
preload2 = Constant(0 * pi / 180, 'preload2', system)
preload3 = Constant(0 * pi / 180, 'preload3', system)
Ixx_O = Constant(1, 'Ixx_O', system)
Iyy_O = Constant(1, 'Iyy_O', system)
Izz_O = Constant(1, 'Izz_O', system)
Ixx_A = Constant(1, 'Ixx_A', system)
Iyy_A = Constant(1, 'Iyy_A', system)
Izz_A = Constant(1, 'Izz_A', system)
Ixx_B = Constant(1, 'Ixx_B', system)
Iyy_B = Constant(1, 'Iyy_B', system)
Izz_B = Constant(1, 'Izz_B', system)
Ixx_C = Constant(1, 'Ixx_C', system)
Iyy_C = Constant(1, 'Iyy_C', system)
Izz_C = Constant(1, 'Izz_C', system)
y, y_d, y_dd = Differentiable('y', system)
qO, qO_d, qO_dd = Differentiable('qO', system)
qA, qA_d, qA_dd = Differentiable('qA', system)
qB, qB_d, qB_dd = Differentiable('qB', system)
qC, qC_d, qC_dd = Differentiable('qC', system)
initialvalues = {}
initialvalues[y] = 0 + 1e-14
initialvalues[y_d] = fit_vel + 1e-14
initialvalues[qO] = 0 + 1e-14
initialvalues[qO_d] = 0 + 1e-14
initialvalues[qA] = fit_states[0, 0] + 1e-14
initialvalues[qA_d] = fit_states[0, 3] + 1e-14
initialvalues[qB] = fit_states[0, 1] + 1e-14
initialvalues[qB_d] = fit_states[0, 4] + 1e-14
initialvalues[qC] = fit_states[0, 2] + 1e-14
initialvalues[qC_d] = fit_states[0, 5] + 1e-14
statevariables = system.get_state_variables()
ini = [initialvalues[item] for item in statevariables]
N = Frame('N')
O = Frame('O')
A = Frame('A')
B = Frame('B')
C = Frame('C')
drag_direction = drag_direction
velocity = 200 / tfinal / 1000
vSoil = drag_direction * velocity * N.y
nSoil = 1 / vSoil.length() * vSoil
system.set_newtonian(N)
if not global_q:
O.rotate_fixed_axis_directed(N, [0, 0, 1], qO, system)
A.rotate_fixed_axis_directed(O, [0, 0, 1], qA, system)
B.rotate_fixed_axis_directed(A, [0, 0, 1], qB, system)
C.rotate_fixed_axis_directed(B, [0, 0, 1], qC, system)
else:
O.rotate_fixed_axis_directed(N, [0, 0, 1], qO, system)
A.rotate_fixed_axis_directed(N, [0, 0, 1], qA, system)
B.rotate_fixed_axis_directed(N, [0, 0, 1], qB, system)
C.rotate_fixed_axis_directed(N, [0, 0, 1], qC, system)
pNO = 0 * N.x + y * N.y
pOA = lO * N.x + y * N.y
pAB = pOA + lA * A.x
pBC = pAB + lB * B.x
pCtip = pBC + lC * C.x
pOcm = pNO + lO / 2 * N.x
pAcm = pOA + lA / 2 * A.x
pBcm = pAB + lB / 2 * B.x
pCcm = pBC + lC / 2 * C.x
wNO = N.getw_(O)
wOA = N.getw_(A)
wAB = A.getw_(B)
wBC = B.getw_(C)
IO = Dyadic.build(O, Ixx_O, Iyy_O, Izz_O)
IA = Dyadic.build(A, Ixx_A, Iyy_A, Izz_A)
IB = Dyadic.build(B, Ixx_B, Iyy_B, Izz_B)
IC = Dyadic.build(C, Ixx_C, Iyy_C, Izz_C)
BodyO = Body('BodyO', O, pOcm, mO, IO, system)
BodyA = Body('BodyA', A, pAcm, mA, IA, system)
BodyB = Body('BodyB', B, pBcm, mB, IB, system)
BodyC = Body('BodyC', C, pCcm, mC, IC, system)
# BodyC = Particle(pCcm,mC,'ParticleC',system)
vOcm = pOcm.time_derivative()
vAcm = pAcm.time_derivative()
vBcm = pBcm.time_derivative()
vCcm = pCcm.time_derivative()
system.add_spring_force1(k1 + 10000 * (qA + abs(qA)),
(qA - qO - preload1) * N.z, wOA)
system.add_spring_force1(k + 10000 * (qB + abs(qB)),
(qB - qA - preload2) * N.z, wAB)
system.add_spring_force1(k + 10000 * (qC + abs(qC)),
(qC - qB - preload3) * N.z, wBC)
#new Method use nJoint
nvAcm = 1 / vAcm.length() * vAcm
nvBcm = 1 / vBcm.length() * vBcm
nvCcm = 1 / vCcm.length() * vCcm
faperp = friction_perp * nvAcm.dot(A.y) * A.y
fapar = friction_par * nvAcm.dot(A.x) * A.x
system.addforce(-(faperp + fapar), vAcm)
fbperp = friction_perp * nvBcm.dot(B.y) * B.y
fbpar = friction_par * nvBcm.dot(B.x) * B.x
system.addforce(-(fbperp + fbpar), vBcm)
fcperp = friction_perp * nvCcm.dot(C.y) * C.y
fcpar = friction_par * nvCcm.dot(C.x) * C.x
system.addforce(-(fcperp + fcpar), vCcm)
system.addforce(-b_damping * wOA, wOA)
system.addforce(-b_damping * wAB, wAB)
system.addforce(-b_damping * wBC, wBC)
eq = []
eq_d = [(system.derivative(item)) for item in eq]
eq_d.append(y_d - fit_vel)
eq_dd = [(system.derivative(item)) for item in eq_d]
f, ma = system.getdynamics()
func1 = system.state_space_post_invert(f, ma, eq_dd)
points = [pNO, pOA, pAB, pBC, pCtip]
constants = system.constant_values
return system, f, ma, func1, points, t, ini, constants, b_damping, k, k1, tstep, fit_states
qC_d: 0,
qD: -pi + 2.64,
qD_d: 0
}
initialvalues[iA] = 0
initialvalues[iC] = 0
initialvalues[qMA] = 0
initialvalues[qMA_d] = 0
initialvalues[qMC] = 0
initialvalues[qMC_d] = 0
statevariables = system.get_state_variables()
ini = [initialvalues[item] for item in statevariables]
N = Frame('N', system)
O = Frame('O', system)
MA = Frame('MA', system)
A = Frame('A', system)
B = Frame('B', system)
MC = Frame('MC', system)
C = Frame('C', system)
D = Frame('D', system)
system.set_newtonian(N)
O.rotate_fixed_axis(N, [0, 0, 1], qO, system)
MA.rotate_fixed_axis(N, [0, 0, 1], qMA, system)
A.rotate_fixed_axis(N, [0, 0, 1], qA, system)
B.rotate_fixed_axis(N, [0, 0, 1], qB, system)
MC.rotate_fixed_axis(N, [0, 0, 1], qMC, system)
C.rotate_fixed_axis(N, [0, 0, 1], qC, system)
qC,qC_d,qC_dd = Differentiable('qC',system)
qD,qD_d,qD_dd = Differentiable('qD',system)
initialvalues = {}
initialvalues[qA]=30*pi/180
initialvalues[qA_d]=0*pi/180
initialvalues[qB]=30*pi/180
initialvalues[qB_d]=0*pi/180
initialvalues[qC]=150*pi/180
initialvalues[qC_d]=0*pi/180
initialvalues[qD]=-30*pi/180
initialvalues[qD_d]=0*pi/180
statevariables = system.get_state_variables()
N = Frame('N',system)
A = Frame('A',system)
B = Frame('B',system)
C = Frame('C',system)
D = Frame('D',system)
system.set_newtonian(N)
A.rotate_fixed_axis(N,[0,0,1],qA,system)
B.rotate_fixed_axis(A,[0,0,1],qB,system)
C.rotate_fixed_axis(N,[0,0,1],qC,system)
D.rotate_fixed_axis(C,[0,0,1],qD,system)
pNA = 0*N.x
pAB = pNA + lA*A.x
pBD = pAB + lB*B.x
pCD = pNA + lC*C.x
constants[m] = 1
constants[g] = 9.81
initialvalues = {}
#initialvalues[qA]=0*pi/180
#initialvalues[qA_d]=0*pi/180
initialvalues[qB] = 0 * pi / 180
initialvalues[qB_d] = 0 * pi / 180
#initialvalues[wB]=0*pi/180
#initialvalues[a]=0
initialvalues[i] = 0
statevariables = system.get_state_variables()
ini = [initialvalues[item] for item in statevariables]
N = Frame('N', system)
#A = Frame('A',system)
B = Frame('B', system)
M = Frame('M', system)
system.set_newtonian(N)
#A.rotate_fixed_axis(N,[0,0,1],qA,system)
B.rotate_fixed_axis(N, [0, 0, 1], qB, system)
pO = 0 * N.x
#wNA = N.get_w_to(A)
wNB = N.get_w_to(B)
wNA = G * wNB
aNA = wNA.time_derivative()
#wNB = wB*B.z
#aNB = wB_d*B.z
t = numpy.r_[tinitial:tfinal:tstep]
x, x_d, x_dd = Differentiable('x', system)
q, q_d, q_dd = Differentiable('q', system)
initialvalues = {}
initialvalues[x] = .5
initialvalues[x_d] = 0
initialvalues[q] = 30 * pi / 180
initialvalues[q_d] = 0 * pi / 180
statevariables = system.get_state_variables()
ini = [initialvalues[item] for item in statevariables]
N = Frame('N', system)
A = Frame('A', system)
system.set_newtonian(N)
A.rotate_fixed_axis(N, [0, 0, 1], q, system)
p1 = x * N.x
p2 = p1 - l * A.y
v1 = p1.time_derivative(N, system)
v2 = p2.time_derivative(N, system)
I = Dyadic.build(A, I_xx, I_yy, I_zz)
BodyA = Body('BodyA', A, p2, m, I, system)
ParticleO = Particle(p2, M, 'ParticleO', system)
import pynamics.variable_types
from pynamics.frame import Frame
from pynamics.system import System
import pynamics
from pynamics.frame import Frame
from pynamics.variable_types import Differentiable,Constant
from pynamics.system import System
from pynamics.body import Body
from pynamics.dyadic import Dyadic
from pynamics.output import Output
from pynamics.particle import Particle
s = System()
pynamics.set_system(__name__,s)
x,x_d,x_dd=pynamics.variable_types.Differentiable('x',s)
q1,q1_d,q1_dd=pynamics.variable_types.Differentiable('q1',s)
eq = x**2+2*x
eq_d = s.derivative(eq)
N = Frame('N')
A = Frame('A')
s.set_newtonian(N)
A.rotate_fixed_axis_directed(N,[0,0,1],q1,s)
p1 = 3*A.x+2*N.y
v1=p1.time_derivative(N,s)
initialvalues = {}
#initialvalues[xA]=.02
#initialvalues[xA_d]=0
#initialvalues[yA]=0
#initialvalues[yA_d]=0
initialvalues[qA] = 0 * pi / 180
initialvalues[qA_d] = 0 * pi / 180
initialvalues[xB] = .06
initialvalues[xB_d] = 0
initialvalues[yB] = 0
initialvalues[yB_d] = 0
initialvalues[qB] = 0 * pi / 180
initialvalues[qB_d] = 0 * pi / 180
N = Frame('N')
A = Frame('A')
B = Frame('B')
system.set_newtonian(N)
A.rotate_fixed_axis_directed(N, [0, 0, 1], qA, system)
B.rotate_fixed_axis_directed(A, [0, 0, 1], qB, system)
#A.setpathtonewtonian(['A','N'])
#B.setpathtonewtonian(['B','A','N'])
zero = sympy.Number(0)
pNA = zero * N.x + zero * N.y + zero * N.z
#pAcm=xA*N.x*yA*N.y
pAN = pNA
qA: -0.89,
qA_d: 0,
qB: -2.64,
qB_d: 0,
qC: -pi + 0.89,
qC_d: 0,
qD: -pi + 2.64,
qD_d: 0,
qE: 0,
qE_d: 0,
}
statevariables = system.get_state_variables()
ini = [initialvalues[item] for item in statevariables]
N = Frame('N')
O = Frame('O')
A = Frame('A')
B = Frame('B')
C = Frame('C')
D = Frame('D')
E = Frame('E')
system.set_newtonian(N)
O.rotate_fixed_axis_directed(N, [0, 0, 1], qO, system)
A.rotate_fixed_axis_directed(N, [0, 0, 1], qA, system)
B.rotate_fixed_axis_directed(N, [0, 0, 1], qB, system)
C.rotate_fixed_axis_directed(N, [0, 0, 1], qC, system)
D.rotate_fixed_axis_directed(N, [0, 0, 1], qD, system)
E.rotate_fixed_axis_directed(N, [0, 0, 1], qE, system)
qC, qC_d, qC_dd = Differentiable('qC', ini=[ang_ini * pi / 180, 0])
mC = Constant(.05, 'mC')
g = Constant(9.81, 'g')
I_11 = Constant(6e-3, 'I_11')
rho = Constant(1.292, 'rho')
Sw = Constant(.1, 'Sw')
Se = Constant(.025, 'Se')
l = Constant(.35, 'l')
lw = Constant(-.03, 'lw')
le = Constant(.04, 'le')
qE = Constant(3 * pi / 180, 'qE')
ini = system.get_ini()
N = Frame('N')
A = Frame('A')
B = Frame('B')
C = Frame('C')
E = Frame('E')
system.set_newtonian(N)
A.rotate_fixed_axis_directed(N, [1, 0, 0], qA)
B.rotate_fixed_axis_directed(A, [0, 1, 0], qB)
C.rotate_fixed_axis_directed(B, [0, 0, 1], qC)
E.rotate_fixed_axis_directed(C, [0, 0, 1], -qE)
pCcm = x * N.x + y * N.y + z * N.z
#pCcm=x*N.x+y*N.y
pCcp = pCcm - lw * C.x
qA, qA_d, qA_dd = Differentiable('qA', system)
qB, qB_d, qB_dd = Differentiable('qB', system)
qC, qC_d, qC_dd = Differentiable('qC', system)
initialvalues = {}
initialvalues[qA] = -90 * pi / 180
initialvalues[qA_d] = 0 * pi / 180
initialvalues[qB] = 90 * pi / 180
initialvalues[qB_d] = 0 * pi / 180
initialvalues[qC] = 90 * pi / 180
initialvalues[qC_d] = 0 * pi / 180
statevariables = system.get_state_variables()
N = Frame('N')
A = Frame('A')
B = Frame('B')
C = Frame('C')
system.set_newtonian(N)
A.rotate_fixed_axis_directed(N, [0, 0, 1], qA, system)
B.rotate_fixed_axis_directed(A, [0, 0, 1], qB, system)
C.rotate_fixed_axis_directed(B, [0, 0, 1], qC, system)
pNA = 0 * N.x
pAB = pNA + lA * A.x
pBC = pAB + lB * B.x
pCtip = pBC + lC * C.x
pD = lD * N.x
tstep = 1/30
t = numpy.r_[tinitial:tfinal:tstep]
x1,x1_d,x1_dd = Differentiable('x1',system)
x2,x2_d,x2_dd = Differentiable('x2',system)
initialvalues = {}
initialvalues[x1]=2
initialvalues[x1_d]=0
initialvalues[x2]=0
initialvalues[x2_d]=1
statevariables = system.get_state_variables()
ini = [initialvalues[item] for item in statevariables]
N = Frame('N',system)
system.set_newtonian(N)
pNA=0*N.x
pm1 = x1*N.y
pm2 = pm1 - x2*N.y
#BodyA = Body('BodyA',A,pm1,m1,IA,system)
Particle1 = Particle(pm1,m1,'Particle1',system)
Particle2 = Particle(pm2,m2,'Particle2',system)
vpm1 = pm1.time_derivative(N,system)
vpm2 = pm2.time_derivative(N,system)
l_ = pm1-pm2
l = (l_.dot(l_))**.5
tstep = 1/100
t = numpy.r_[tinitial:tfinal:tstep]
x,x_d,x_dd = Differentiable('x',system)
y,y_d,y_dd = Differentiable('y',system)
initialvalues = {}
initialvalues[x]=0
initialvalues[x_d]=1
initialvalues[y]=.1
initialvalues[y_d]=0
statevariables = system.get_state_variables()
ini = [initialvalues[item] for item in statevariables]
N = Frame('N')
system.set_newtonian(N)
pNA=0*N.x
pAcm=pNA+x*N.x+y*N.y
vAcm = pAcm.time_derivative(N,system)
ParticleA = Particle(pAcm,mA,'ParticleA',system)
system.addforce(-b*vAcm,vAcm)
stretch = y
stretched1 = (stretch+abs(stretch))/2
stretched2 = -(-stretch+abs(-stretch))/2
initialvalues = {}
#initialvalues[xA]=.02
#initialvalues[xA_d]=0
#initialvalues[yA]=0
#initialvalues[yA_d]=0
initialvalues[qA] = 0 * pi / 180
initialvalues[qA_d] = 0 * pi / 180
initialvalues[xB] = .06
initialvalues[xB_d] = 0
initialvalues[yB] = 0
initialvalues[yB_d] = 0
initialvalues[qB] = 0 * pi / 180
initialvalues[qB_d] = 0 * pi / 180
N = Frame('N', system)
A = Frame('A', system)
B = Frame('B', system)
system.set_newtonian(N)
A.rotate_fixed_axis(N, [0, 0, 1], qA, system)
B.rotate_fixed_axis(A, [0, 0, 1], qB, system)
#A.setpathtonewtonian(['A','N'])
#B.setpathtonewtonian(['B','A','N'])
zero = sympy.Number(0)
pNA = zero * N.x + zero * N.y + zero * N.z
#pAcm=xA*N.x*yA*N.y
pAN = pNA
t = numpy.r_[tinitial:tfinal:tstep]
x, x_d, x_dd = Differentiable('x', system)
q, q_d, q_dd = Differentiable('q', system)
initialvalues = {}
initialvalues[x] = 1
initialvalues[x_d] = 0
initialvalues[q] = 30 * pi / 180
initialvalues[q_d] = 0 * pi / 180
statevariables = system.get_state_variables()
ini = [initialvalues[item] for item in statevariables]
N = Frame('N', system)
A = Frame('A', system)
system.set_newtonian(N)
A.rotate_fixed_axis(N, [0, 0, 1], q, system)
p1 = x * N.x
p2 = p1 - l * A.y
wNA = N.get_w_to(A)
v1 = p1.time_derivative(N, system)
v2 = p2.time_derivative(N, system)
I = Dyadic.build(A, I_xx, I_yy, I_zz)
qO: 0,
qO_d: 0,
qA: -0.89,
qA_d: 0,
qB: -2.64,
qB_d: 0,
qC: -pi + 0.89,
qC_d: 0,
qD: -pi + 2.64,
qD_d: 0
}
statevariables = system.get_state_variables()
ini0 = [initialvalues[item] for item in statevariables]
N = Frame('N', system)
O = Frame('O', system)
A = Frame('A', system)
B = Frame('B', system)
C = Frame('C', system)
D = Frame('D', system)
system.set_newtonian(N)
O.rotate_fixed_axis(N, [0, 0, 1], qO, system)
A.rotate_fixed_axis(N, [0, 0, 1], qA, system)
B.rotate_fixed_axis(N, [0, 0, 1], qB, system)
C.rotate_fixed_axis(N, [0, 0, 1], qC, system)
D.rotate_fixed_axis(N, [0, 0, 1], qD, system)
pOrigin = 0 * N.x + 0 * N.y
pOcm = x * N.x + y * N.y
