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 from pynamics.constraint import AccelerationConstraint import pynamics.integration import sympy import numpy import matplotlib.pyplot as plt plt.ion() from math import pi system = System() pynamics.set_system(__name__, system) tol = 1e-7 l = Constant(.5, 'l', system) q0 = Constant(0, 'q0', system) M = Constant(10, 'M', system) m = Constant(10, 'm', system) I_xx = Constant(9, 'I_xx', system) I_yy = Constant(9, 'I_yy', system) I_zz = Constant(9, 'I_zz', system) g = Constant(9.81, 'g', system) b = Constant(1e3, 'b', system)
def Cal_system(initial_states, drag_direction, tinitial, tstep, tfinal,
fit_vel, f1, f2):
g_k, g_b_damping, g_b_damping1 = [0.30867935, 1.42946955, 1.08464536]
system = System()
pynamics.set_system(__name__, system)
global_q = True
lO = Constant(7 / 1000, 'lO', system)
lA = Constant(33 / 1000, 'lA', system)
lB = Constant(33 / 1000, 'lB', system)
lC = Constant(33 / 1000, 'lC', system)
mO = Constant(10 / 1000, 'mA', system)
mA = Constant(2.89 / 1000, 'mA', system)
mB = Constant(2.89 / 1000, 'mB', system)
mC = Constant(2.89 / 1000, 'mC', system)
k = Constant(g_k, 'k', system)
k1 = Constant(0.4, 'k1', system)
friction_perp = Constant(f1, 'f_perp', system)
friction_par = Constant(f2, 'f_par', system)
b_damping = Constant(g_b_damping, 'b_damping', system)
b_damping1 = Constant(g_b_damping1, 'b_damping1', system)
preload0 = Constant(0 * pi / 180, 'preload0', system)
preload1 = Constant(0 * pi / 180, 'preload1', system)
preload2 = Constant(0 * pi / 180, 'preload2', system)
preload3 = Constant(0 * pi / 180, 'preload3', system)
Ixx_O = Constant(1, 'Ixx_O', system)
Iyy_O = Constant(1, 'Iyy_O', system)
Izz_O = Constant(1, 'Izz_O', system)
Ixx_A = Constant(1, 'Ixx_A', system)
Iyy_A = Constant(1, 'Iyy_A', system)
Izz_A = Constant(1, 'Izz_A', system)
Ixx_B = Constant(1, 'Ixx_B', system)
Iyy_B = Constant(1, 'Iyy_B', system)
Izz_B = Constant(1, 'Izz_B', system)
Ixx_C = Constant(1, 'Ixx_C', system)
Iyy_C = Constant(1, 'Iyy_C', system)
Izz_C = Constant(1, 'Izz_C', system)
y, y_d, y_dd = Differentiable('y', system)
qO, qO_d, qO_dd = Differentiable('qO', system)
qA, qA_d, qA_dd = Differentiable('qA', system)
qB, qB_d, qB_dd = Differentiable('qB', system)
qC, qC_d, qC_dd = Differentiable('qC', system)
fit_states = initial_states
initialvalues = {}
initialvalues[y] = fit_states[0]
initialvalues[y_d] = fit_states[5]
initialvalues[qO] = 0
initialvalues[qO_d] = 0
initialvalues[qA] = fit_states[2]
initialvalues[qA_d] = fit_states[7]
initialvalues[qB] = fit_states[3]
initialvalues[qB_d] = fit_states[8]
initialvalues[qC] = fit_states[4]
initialvalues[qC_d] = fit_states[9]
statevariables = system.get_state_variables()
ini = [initialvalues[item] for item in statevariables]
N = Frame('N')
O = Frame('O')
A = Frame('A')
B = Frame('B')
C = Frame('C')
system.set_newtonian(N)
if not global_q:
O.rotate_fixed_axis_directed(N, [0, 0, 1], qO, system)
A.rotate_fixed_axis_directed(O, [0, 0, 1], qA, system)
B.rotate_fixed_axis_directed(A, [0, 0, 1], qB, system)
C.rotate_fixed_axis_directed(B, [0, 0, 1], qC, system)
else:
O.rotate_fixed_axis_directed(N, [0, 0, 1], qO, system)
A.rotate_fixed_axis_directed(N, [0, 0, 1], qA, system)
B.rotate_fixed_axis_directed(N, [0, 0, 1], qB, system)
C.rotate_fixed_axis_directed(N, [0, 0, 1], qC, system)
pNO = 0 * N.x + y * N.y
pOA = lO * N.x + y * N.y
pAB = pOA + lA * A.x
pBC = pAB + lB * B.x
pCtip = pBC + lC * C.x
pOcm = pNO + lO / 2 * N.x
pAcm = pOA + lA / 2 * A.x
pBcm = pAB + lB / 2 * B.x
pCcm = pBC + lC / 2 * C.x
wNO = N.getw_(O)
wOA = N.getw_(A)
wAB = A.getw_(B)
wBC = B.getw_(C)
IO = Dyadic.build(O, Ixx_O, Iyy_O, Izz_O)
IA = Dyadic.build(A, Ixx_A, Iyy_A, Izz_A)
IB = Dyadic.build(B, Ixx_B, Iyy_B, Izz_B)
IC = Dyadic.build(C, Ixx_C, Iyy_C, Izz_C)
BodyO = Body('BodyO', O, pOcm, mO, IO, system)
BodyA = Body('BodyA', A, pAcm, mA, IA, system)
BodyB = Body('BodyB', B, pBcm, mB, IB, system)
BodyC = Body('BodyC', C, pCcm, mC, IC, system)
# vOcm = pOcm.time_derivative()
vAcm = pAcm.time_derivative()
vBcm = pBcm.time_derivative()
vCcm = pCcm.time_derivative()
# system.add_spring_force1(k1+10000*(qA+abs(qA)),(qA-qO-preload1)*N.z,wOA)
# system.add_spring_force1(k+10000*(qB+abs(qB)),(qB-qA-preload2)*N.z,wAB)
# system.add_spring_force1(k+10000*(qC+abs(qC)),(qC-qB-preload3)*N.z,wBC)
system.add_spring_force1(k1, (qA - qO - preload1) * N.z, wOA)
system.add_spring_force1(k, (qB - qA - preload2) * N.z, wAB)
system.add_spring_force1(k, (qC - qB - preload3) * N.z, wBC)
#new Method use nJoint
nvAcm = 1 / vAcm.length() * vAcm
nvBcm = 1 / vBcm.length() * vBcm
nvCcm = 1 / vCcm.length() * vCcm
vSoil = drag_direction * 1 * N.y
nSoil = 1 / vSoil.length() * vSoil
if fit_vel == 0:
vSoil = 1 * 1 * N.y
nSoil = 1 / vSoil.length() * vSoil
faperp = friction_perp * nSoil.dot(A.y) * A.y
fapar = friction_par * nSoil.dot(A.x) * A.x
system.addforce(-(faperp + fapar), vAcm)
fbperp = friction_perp * nSoil.dot(B.y) * B.y
fbpar = friction_par * nSoil.dot(B.x) * B.x
system.addforce(-(fbperp + fbpar), vBcm)
fcperp = friction_perp * nSoil.dot(C.y) * C.y
fcpar = friction_par * nSoil.dot(C.x) * C.x
system.addforce(-(fcperp + fcpar), vCcm)
else:
faperp = friction_perp * nvAcm.dot(A.y) * A.y
fapar = friction_par * nvAcm.dot(A.x) * A.x
system.addforce(-(faperp + fapar), vAcm)
fbperp = friction_perp * nvBcm.dot(B.y) * B.y
fbpar = friction_par * nvBcm.dot(B.x) * B.x
system.addforce(-(fbperp + fbpar), vBcm)
fcperp = friction_perp * nvCcm.dot(C.y) * C.y
fcpar = friction_par * nvCcm.dot(C.x) * C.x
system.addforce(-(fcperp + fcpar), vCcm)
system.addforce(-b_damping1 * wOA, wOA)
system.addforce(-b_damping * wAB, wAB)
system.addforce(-b_damping * wBC, wBC)
eq = []
eq_d = [(system.derivative(item)) for item in eq]
eq_d.append(y_d - fit_vel)
eq_dd = [(system.derivative(item)) for item in eq_d]
f, ma = system.getdynamics()
func1 = system.state_space_post_invert(f, ma, eq_dd)
points = [pNO, pOA, pAB, pBC, pCtip]
constants = system.constant_values
states = pynamics.integration.integrate_odeint(func1,
ini,
t,
args=({
'constants': constants
}, ))
points_output = PointsOutput(points, system, constant_values=constants)
y = points_output.calc(states)
final = numpy.asarray(states[-1, :])
time1 = time.time()
points_output.animate(fps=30,
movie_name=str(time1) + 'video_1.mp4',
lw=2,
marker='o',
color=(1, 0, 0, 1),
linestyle='-')
return final, states, y, system
def 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
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
