def post_process(states10, constants10, points10, system10, t10, fit_states10,
vel):
states = states10
constants = constants10
points = points10
system = system10
t = t10
fit_states = fit_states10
points_output = PointsOutput(points, system, constant_values=constants)
y1 = points_output.calc(states)
plt.figure()
plt.plot(*(y1[::int(len(y1) / 20)].T) * 1000)
plt.axis('equal')
plt.title(str(vel))
plt.figure()
q_states = numpy.c_[(states[:, 2], states[:, 3], states[:, 4],
states[:, 7], states[:, 8], states[:, 9])]
plt.plot(t, numpy.rad2deg(q_states))
plt.plot(t, numpy.rad2deg(fit_states), '--')
print('final states:' + str(numpy.rad2deg(q_states[-1, :])))
plt.title(str(vel))
# plt.figure()
points_output.animate(fps=1 / tstep,
movie_name='render'
'.mp4',
lw=2,
marker='o',
color=(1, 0, 0, 1),
linestyle='-')
return y1
eq_dd=[(system.derivative(item)) for item in eq_d]
c=KinematicConstraint(eq)
variables = [qA,qB,qC,qD]
guess = [initialvalues[item] for item in variables]
result = c.solve_numeric(variables,guess,system.constant_values)
ini = []
for item in system.get_state_variables():
if item in variables:
ini.append(result[item])
else:
ini.append(initialvalues[item])
po1 = PointsOutput(points, constant_values=system.constant_values)
po1.calc(numpy.array([ini0,ini]),[0,1])
po1.plot_time()
# po1.calc(numpy.array([ini,ini]),[0,1])
# po1.plot_time()
pAcm = pNA+lA/2*A.x
pBcm = pAB+lA/2*B.x
pCcm = pNC+lA/2*C.x
pDcm = pCD+lA/2*D.x
vBD = pBD.time_derivative()
vDB = pDB.time_derivative()
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()]
states=pynamics.integration.integrate_odeint(func1,ini,t,rtol = tol, atol=tol,args=({'constants':system.constant_values},))
po = PointsOutput(points,system)
po.calc(states,t)
po.animate(fps = 30,lw=2)
so = Output([qA,qB,qC])
so.calc(states,t)
so.plot_time()
# ### Energy
# In[28]:
# KE = system.get_KE()
# PE = system.getPEGravity(pNA) - system.getPESprings()
# energy_output = Output([KE-PE],system)
# energy_output.calc(states)
# energy_output.plot_time()
# ### Motion
# In[29]:
points = [pm1, pNA, pAB, pBC, pC1, pC2]
points_output = PointsOutput(points, system)
y = points_output.calc(states)
points_output.plot_time(5)
# #### Motion Animation
# in normal Python the next lines of code produce an animation using matplotlib
# In[30]:
points_output.animate(fps=fps,
movie_name='render.mp4',
lw=2,
marker='o',
color=(1, 0, 0, 1),
linestyle='-')
#a()
ParticleA = Particle(pAB, mA, 'ParticleA', system)
ParticleB = Particle(pAB2, mA, 'ParticleB', system)
system.addforce(-b * vAB, vAB)
system.addforce(-b * vAB2, vAB2)
system.addforcegravity(-g * N.y)
v = pAB2 - pNA2
u = (v.dot(v))**.5
eq1 = [(v.dot(v)) - lA**2]
eq1_dd = system.derivative(system.derivative(eq1[0]))
eq = [eq1_dd]
f, ma = system.getdynamics()
func = system.state_space_post_invert(f,
ma,
eq,
constants=system.constant_values)
states = pynamics.integration.integrate(func,
ini,
t,
rtol=1e-12,
atol=1e-12,
hmin=1e-14)
points = [pNA, pAB, pNA, pNA2, pAB2]
points_output = PointsOutput(points, system)
points_output.calc(states)
points_output.animate(fps=30, lw=2)
d = (c**2).sum()
e = system.get_state_variables()
#e = sorted(list(d.atoms(Differentiable)),key=lambda x:str(x))
f = sympy.lambdify(e, d)
g = lambda args: f(*args)
return g
fun = gen_init()
import scipy.optimize
result = scipy.optimize.minimize(fun, ini)
if result.fun < 1e-7:
points = [pDtip, pD1D2, pCD, pC1C2, pOC, pOA, pA1A2, pAB, pB1B2, pBtip]
points_output = PointsOutput(points, constant_values=constants)
state = numpy.array([ini, result.x])
ini1 = list(result.x)
y = points_output.calc(state)
y = y.reshape((-1, len(points), 2))
plt.figure()
for item in y:
plt.plot(*(item.T))
# for item,value in zip(system.get_state_variables(),result.x):
# initialvalues[item]=value
pA1cm = pOA + lA / 4 * A1.x
pB1cm = pAB + lB / 4 * B1.x
pC1cm = pOC + lC / 4 * C1.x
pD1cm = pCD + lD / 4 * D1.x
#eq1_dd=[system.derivative(system.derivative(item)) for item in eq1]
eq = []
#a = [0-pm2.dot(N.y)]
#b = [(item+abs(item)) for item in a]
f, ma = system.getdynamics()
func = system.state_space_post_invert(f, ma)
#func = system.state_space_post_invert2(f,ma,eq1_dd,eq1_d,eq1,eq_active = b)
states = pynamics.integration.integrate_odeint(func,
ini,
t,
rtol=error,
atol=error,
args=({
'constants':
system.constant_values
}, ))
#states = states[0]
points = [pm1, pm2]
po = PointsOutput(points, system, constant_values=system.constant_values)
y = po.calc(states)
plt.figure()
for item in y:
plt.plot(*(item.T), lw=2, marker='o')
plt.axis('equal')
#
po.animate(fps=30, movie_name='bouncy-mod.mp4', lw=2, marker='o')
system.addforce(f_aero_C, vcp)
system.addforce(f_aero_E, ve)
points = [pC1, pC2]
#ang = [wNC.dot(C.x),wNC.dot(C.y),wNC.dot(C.z)]
f, ma = system.getdynamics()
func1 = system.state_space_post_invert(f, ma)
states = pynamics.integration.integrate_odeint(func1,
ini,
t,
args=({
'constants':
system.constant_values
}, ))
#output = Output(ang,system)
#output.calc(states)
#output.plot_time()
po = PointsOutput(points, system)
y = po.calc(states)
#po.plot_time()
#y = y.reshape((-1,2,2))
plt.figure()
for item in y:
plt.plot(*(item.T), lw=2, marker='o')
#
po.animate(fps=30, movie_name='glider.mp4', lw=2, marker='o')
plt.figure() artists = plt.plot(t,states[:,:3]) plt.legend(artists,['qA','qB','qC']) # Plot --- energy KE = system.get_KE() PE = system.getPEGravity(pNA) - system.getPESprings() energy_output = Output([KE-PE],system) energy_output.calc(states) energy_output.plot_time() # Motion points = [pNA,pAB,pBtip,pBC, pCD] points_output = PointsOutput(points,system) y = points_output.calc(states) points_output.plot_time(20) points_output.animate(fps = 30,lw=2) from matplotlib import animation, rc from IPython.display import HTML HTML(points_output.anim.to_html5_video()) """From the animation above, we see that because we set the neutral position of the device to stand straight (vertical sides for the four-bar mechanism), our device would not just freely drop to the ground. # 6. Tuning: Now adjust the damper value to something nonzero, that over 10s shows that the system is settling. """ b = Constant(1e0,'b',system)
eqs.append((pBtip - pDtip).dot(N.y))
constraint_system = KinematicConstraint(eqs)
variables = [qO, qA, qB, qC, qD]
guess = [initialvalues[item] for item in variables]
result = constraint_system.solve_numeric(variables, guess, constants)
ini = []
for item in system.get_state_variables():
if item in variables:
ini.append(result[item])
else:
ini.append(initialvalues[item])
points = PointsOutput(points, constant_values=constants)
points.calc(numpy.array([ini0, ini]), [0, 1])
points.plot_time()
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.get_w_to(A)
wAB = A.get_w_to(B)
wOC = O.get_w_to(C)
wCD = C.get_w_to(D)
wBD = B.get_w_to(D)
BodyO = Body('BodyO', O, pOcm, mO, Dyadic.build(O, I_main, I_main, I_main),
#eq1_dd=[system.derivative(system.derivative(item)) for item in eq1]
eq = []
#a = [0-pm2.dot(N.y)]
#b = [(item+abs(item)) for item in a]
f, ma = system.getdynamics()
func = system.state_space_post_invert(f, ma)
#func = system.state_space_post_invert2(f,ma,eq1_dd,eq1_d,eq1,eq_active = b)
states = pynamics.integration.integrate_odeint(func,
ini,
t,
rtol=error,
atol=error,
args=({
'constants':
system.constant_values
}, ))
#states = states[0]
points = [pm1, pm2]
po = PointsOutput(points, system, constant_values=system.constant_values)
y = po.calc(states, t)
plt.figure()
for item in y:
plt.plot(*(item.T), lw=2, marker='o')
plt.axis('equal')
#
#po.animate(fps = 30, movie_name='bouncy-mod.mp4',lw=2,marker='o')
atol=1e-12,
hmin=1e-14,
args=({
'constants':
system.constant_values
}, ))
KE = system.get_KE()
PE = system.getPEGravity(
pNA) - system.getPESprings() - 1 / 2 * k * (stretch)**2
output = Output([x1, y1, q, KE - PE], system)
y = output.calc(states, t)
plt.figure(1)
plt.plot(y[:, 0], y[:, 1])
plt.axis('equal')
plt.figure(2)
plt.plot(y[:, 3])
#plt.figure(3)
#plt.plot(t,y[:,0])
#plt.show()
points = [pNA, pAB]
#points = [item for item2 in points for item in [item2.dot(system.newtonian.x),item2.dot(system.newtonian.y)]]
points_output = PointsOutput(points, system)
y = points_output.calc(states, t)
#points_output.animate(fps = 100,movie_name = 'render.mp4',lw=2,marker='o',color=(1,0,0,1),linestyle='-')
states=pynamics.integration.integrate_odeint(func1,ini,t, args=({'constants':constants},))
return states
def measure_perf(args):
print('r: ',args[0])
if args[0]>1:
return 1000
if args[0]<=0:
return 1000
try:
states = run(args)
perf = 1/states[-1,0]
return perf
except scipy.linalg.LinAlgError:
return 1000
yy = []
xx = numpy.r_[0.1:1:5j]
for ii in xx:
yy.append(measure_perf([ii]))
yy = numpy.array(yy)
plt.plot(xx,yy)
states = run([.1])
po = PointsOutput(points,system)
po.calc(states,t)
po.plot_time()
# po.animate(fps = 30, movie_name='glider.mp4',lw=2,marker='o')
beta,
'constants':
system.constant_values
}, ),
full_output=1,
mxstep=int(1e5))
states = states[0]
KE = system.get_KE()
PE = system.getPEGravity(pOrigin) - system.getPESprings()
output = Output([x1, x2, l.length(), KE - PE], system)
y = output.calc(states, t)
plt.figure(0)
plt.plot(t, y[:, 0])
plt.plot(t, y[:, 1])
plt.axis('equal')
plt.figure(1)
plt.plot(t, y[:, 2])
plt.axis('equal')
plt.figure(2)
plt.plot(t, y[:, 3])
#plt.axis('equal')
points = [BodyA.pCM, Particle2.pCM]
points = PointsOutput(points)
points.calc(states, t)
# points.animate(fps = 30, movie_name='bouncy2.mp4',lw=2)
# =============================================================================
KE = system.get_KE()
PE = system.getPEGravity(0 * N.x) - system.getPESprings()
energy = Output([KE - PE])
energy.calc(states, t)
energy.plot_time()
# =============================================================================
points_list = [p1, p2]
#points_list = [item2 for item in points_list for item2 in [item.dot(N.x),item.dot(N.y)]]
#points = Output(points_list)
#y = points.calc(states,t)
#y = y.reshape((-1,2,2))
#plt.figure()
#plt.plot(y[:,1,0],y[:,1,1])
#plt.axis('equal')
states2 = Output([x, q])
states2.calc(states, t)
plt.figure()
plt.plot(states[:, 0])
plt.figure()
plt.plot(states[:, 1])
points2 = PointsOutput(points_list)
points2.calc(states, t)
#points2.plot_time()
#points2.animate(fps = 30, movie_name='cart_pendulum.mp4',lw=2)
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]
points = [pNA, pAcm, pAtip]
points_output = PointsOutput(points, system)
out1 = Output([tin, qA])
my_constants = {}
freq_sweep = numpy.r_[-1.5:1:20j]
freq_sweep = 1 * 10**freq_sweep
amps = []
for ff in freq_sweep:
tol = 1e-4
tinitial = 0
tfinal = 10 / ff
tstep = 1 / 30
t = numpy.r_[tinitial:tfinal:tstep]
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
f,ma = system.getdynamics()
#func = system.state_space_post_invert(f,ma,eq)
func = system.state_space_post_invert2(f,ma,eq1_dd,eq1_d,eq1,eq_active = b)
states=pynamics.integration.integrate_odeint(func,ini,t,rtol = error, atol = error, args=({'alpha':alpha,'beta':beta, 'constants':system.constant_values},),full_output = 1,mxstep = int(1e5))
states = states[0]
KE = system.get_KE()
PE = system.getPEGravity(pNA) - system.getPESprings()
output = Output([x1,x2,l, KE-PE],system)
y = output.calc(states)
pynamics.toc()
plt.figure(0)
plt.plot(t,y[:,0])
plt.plot(t,y[:,1])
plt.axis('equal')
plt.figure(1)
plt.plot(t,y[:,2])
plt.axis('equal')
plt.figure(2)
plt.plot(t,y[:,3])
#plt.axis('equal')
points = [Particle1.pCM,Particle2.pCM]
points = PointsOutput(points)
points.calc(states)
points.animate(fps = 30, movie_name='bouncy.mp4',lw=2)
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
hmin=tol,
args=({
'constants': constants
}, ))
return states
input_data_all = run_sim([1.1e2, 9e2])
input_positions = input_data_all[:, :3]
# input_positions = input_positions.copy()
# pynamics.integration.logger.setLevel(logging.ERROR)
pynamics.system.logger.setLevel(logging.ERROR)
points = [pNA, pAB, pBC, pCtip]
points_output = PointsOutput(points, system)
y = points_output.calc(input_data_all)
points_output.plot_time()
r = numpy.random.randn(*(y.shape)) * .01
y_rand = y + r
fy = scipy.interpolate.interp1d(t, y_rand.transpose((1, 2, 0)))
fyt = fy(t)
# points_output.animate(fps = fps,movie_name = 'render.mp4',lw=2,marker='o',color=(1,0,0,1),linestyle='-')
def calc_error(args):
states_guess = run_sim(args)
y_guess = points_output.calc(states_guess).transpose((1, 2, 0))
error = fyt - y_guess
f, ma = system.getdynamics()
func = system.state_space_post_invert(f, ma)
constants = system.constant_values.copy()
# constants[b_constraint]=0
states = pynamics.integration.integrate_odeint(func,
ini,
t,
rtol=tol,
atol=tol,
args=({
'constants': constants
}, ))
points = [pm1, pm2]
po = PointsOutput(points, system, constant_values=system.constant_values)
po.calc(states)
constants = system.constant_values.copy()
constants[b_constraint] = 1e3
states = pynamics.integration.integrate_odeint(func,
ini,
t,
rtol=tol,
atol=tol,
args=({
'constants': constants
}, ))
points = [pm1, pm2]
po2 = PointsOutput(points, system, constant_values=system.constant_values)
po2.calc(states)
d = (c**2).sum()
e = system.get_state_variables()
#e = sorted(list(d.atoms(Differentiable)),key=lambda x:str(x))
f = sympy.lambdify(e, d)
g = lambda args: f(*args)
return g
fun = gen_init()
import scipy.optimize
result = scipy.optimize.minimize(fun, ini)
if result.fun < 1e-7:
points = [pDtip, pCD, pOC, pOA, pAB, pBtip]
points = PointsOutput(points)
state = numpy.array([ini, result.x])
ini1 = list(result.x)
y = points.calc(state)
y = y.reshape((-1, 6, 2))
plt.figure()
for item in y:
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
pEcm = pBtip - .1 * E.y
# # =============================================================================
# KE = system.get_KE()
# PE = system.getPEGravity(0*N.x) - system.getPESprings()
# energy = Output([KE-PE], constant_values = constants)
# energy.calc(states,t)
# energy.plot_time()
# # =============================================================================
# positions = Output(system.get_q(0), constant_values = constants)
# positions.calc(states,t)
# positions.plot_time()
# speeds = Output(system.get_q(1), constant_values = constants)
# speeds.calc(states,t)
# speeds.plot_time()
# y= Output([G*qB_d], constant_values=constants)
# y.calc(states,t)
# y.plot_time()
points = [pAcm, pBcm]
po = PointsOutput(points, constant_values=constants)
po.calc(states, t)
po.plot_time()
po.animate(fps=fps,
movie_name='triple_pendulum.mp4',
lw=2,
marker='o',
color=(1, 0, 0, 1),
linestyle='-')
c = Constraint(eq)
variables = [qA, qB, qC]
guess = [initialvalues[item] for item in variables]
result = c.solve_numeric(variables, guess, system.constant_values)
guess = [initialvalues[item] for item in variables]
ini = []
for item in system.get_state_variables():
if item in variables:
ini.append(result.x[variables.index(item)])
else:
ini.append(initialvalues[item])
points = PointsOutput(points, constant_values=system.constant_values)
points.calc(numpy.array([ini0, ini]))
points.plot_time()
eq_d = sympy.Matrix(eq_d)
qi = sympy.Matrix([qA_d])
qd = sympy.Matrix([qB_d, qC_d])
C = eq_d.jacobian(qi)
D = eq_d.jacobian(qd)
J = -D.inv() * C
J.simplify()
qd2 = J * qi
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):
b1,k1 = x
constants = system.constant_values
constants[b] = b1
constants[k] = k1
states=pynamics.integration.integrate_odeint(func1,ini,t,rtol=tol,atol=tol, args=({'constants':constants},))
return states, constants
points = [pNA,pAB,pBC,pCtip]
states0,constants0 = myfunc([1e1,1e2])
points_output = PointsOutput(points,system,constant_values = constants0)
y = points_output.calc(states0,t)
points_output.plot_time()
KE = system.get_KE()
PE = system.getPEGravity(pNA)-system.getPESprings()
output = Output([x1,y1,KE-PE,x,y],system)
y = output.calc(states)
plt.figure(1)
plt.plot(y[:,0],y[:,1])
plt.axis('equal')
plt.figure(2)
plt.plot(y[:,2])
plt.figure(3)
plt.plot(t,y[:,4])
plt.show()
plt.figure(4)
plt.plot(t,states[:,2])
plt.show()
points = [pNA,pAcm]
#points = [item for item2 in points for item in [item2.dot(system.newtonian.x),item2.dot(system.newtonian.y)]]
points_output = PointsOutput(points,system)
y = points_output.calc(states)
points_output.animate(fps = 100,movie_name = 'single_dof_bouncer.mp4',lw=2,marker='o',color=(1,0,0,1),linestyle='')
eq1 = v.dot(v) - lA**2 eq1_d=system.derivative(eq1) eq1_dd=system.derivative(eq1_d) eq = [eq1_dd] constraint = AccelerationConstraint([eq1_dd]) system.add_constraint(constraint) tol = 1e-4 tinitial = 0 tfinal = 5 tstep = 1/30 t = numpy.r_[tinitial:tfinal:tstep] f,ma = system.getdynamics() func = system.state_space_post_invert(f,ma,constants=system.constant_values) statevariables = system.get_state_variables() ini = [initialvalues[item] for item in statevariables] states=pynamics.integration.integrate(func,ini,t,rtol=tol,atol=tol) points = [pNA,pAB,pNA,pNA2,pAB2] points_output = PointsOutput(points,system) points_output.calc(states,t) points_output.plot_time() #points_output.animate(fps = 30,lw=2)
# =============================================================================
KE = system.get_KE()
PE = system.getPEGravity(0 * N.x) - system.getPESprings()
energy = Output([KE - PE])
energy.calc(states, t)
energy.plot_time()
# =============================================================================
points_list = [p1, p2]
#points_list = [item2 for item in points_list for item2 in [item.dot(N.x),item.dot(N.y)]]
#points = Output(points_list)
#y = points.calc(states,t)
#y = y.reshape((-1,2,2))
#plt.figure()
#plt.plot(y[:,1,0],y[:,1,1])
#plt.axis('equal')
states2 = Output([x, q])
states2.calc(states, t)
plt.figure()
plt.plot(states[:, 0])
plt.figure()
plt.plot(states[:, 1])
points2 = PointsOutput(points_list)
points2.calc(states, t)
#points2.plot_time()
points2.animate(fps=30, movie_name='cart_pendulum.mp4', lw=2)
f, ma = system.getdynamics()
func = system.state_space_post_invert(f, ma)
constants = system.constant_values.copy()
# constants[b_constraint]=0
states = pynamics.integration.integrate_odeint(func,
ini,
t,
rtol=tol,
atol=tol,
args=({
'constants': constants
}, ))
points = [pm1, pm2]
po = PointsOutput(points, system, constant_values=system.constant_values)
po.calc(states, t)
constants = system.constant_values.copy()
constants[b_constraint] = 1e3
states = pynamics.integration.integrate_odeint(func,
ini,
t,
rtol=tol,
atol=tol,
args=({
'constants': constants
}, ))
points = [pm1, pm2]
po2 = PointsOutput(points, system, constant_values=system.constant_values)
po2.calc(states, t)