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
tstep = 1 / fps
t = numpy.r_[tinitial:tfinal:tstep]
force = t * 0
ii = (t == 3).nonzero()[0][0]
jj = (t == 5).nonzero()[0][0]
force[ii:jj] = 10
f_force = scipy.interpolate.interp1d(t, force, fill_value='extrapolate')
# ### Differentiable State Variables
#
# Define your differentiable state variables that you will use to model the state of the system. In this case $qA$, $qB$, and $qC$ are the rotation angles of a three-link mechanism
# In[8]:
x, x_d, x_dd = Differentiable('x', system)
y, y_d, y_dd = Differentiable('y', 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)
x2, x2_d, x2_dd = Differentiable('x2', system)
# ### Initial Values
# Define a set of initial values for the position and velocity of each of your state variables. It is necessary to define a known. This code create a dictionary of initial values.
# In[9]:
initialvalues = {}
initialvalues[x] = 0
initialvalues[x_d] = 0
initialvalues[y] = 2.01
import numpy
import matplotlib.pyplot as plt
plt.ion()
import math
system = System()
pynamics.set_system(__name__,system)
g = Constant(9.81,'g',system)
tol = 1e-5
tinitial = 0
tfinal = 5
tstep = 1/30
t = numpy.r_[tinitial:tfinal:tstep]
qA,qA_d,qA_dd = Differentiable('qA')
qB,qB_d,qB_dd = Differentiable('qB')
qC,qC_d,qC_dd = Differentiable('qC')
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
lA = Constant(1, 'lA', system)
mA = Constant(1, 'mA', system)
g = Constant(9.81, 'g', system)
b = Constant(1e0, 'b', system)
k = Constant(1e1, 'k', system)
tinitial = 0
tfinal = 5
tstep = 1 / 30
t = numpy.r_[tinitial:tfinal:tstep]
preload1 = Constant(0 * pi / 180, 'preload1', system)
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')
mB = Constant(.0145,'mB',system)
zero = Constant(0,'zero',system)
Ixx_A = Constant(8.6e-007,'Ixx_A',system)
Iyy_A = Constant(2.2e-006,'Iyy_A',system)
Izz_A = Constant(2.2e-006,'Izz_A',system)
Ixx_B = Constant(8.6e-007,'Ixx_B',system)
Iyy_B = Constant(2.2e-006,'Iyy_B',system)
Izz_B = Constant(2.2e-006,'Izz_B',system)
b = Constant(0.00001,'b',system)
k = Constant(0.1,'k',system)
#accelerationvariable('xA')
#accelerationvariable('yA')
qA,qA_d,qA_dd = Differentiable(system,'qA')
xB,xB_d,xB_dd = Differentiable(system,'xB')
yB,yB_d,yB_dd = Differentiable(system,'yB')
qB,qB_d,qB_dd = Differentiable(system,'qB')
fNAx = Variable('fNAx')
fNAy = Variable('fNAy')
fABx = Variable('fABx')
fABy = Variable('fABy')
initialvalues = {}
#initialvalues[xA]=.02
#initialvalues[xA_d]=0
#initialvalues[yA]=0
#initialvalues[yA_d]=0
initialvalues[qA]=0*pi/180
lA = Constant('lA', 1, system)
mA = Constant('mA', 1, system)
g = Constant('g', 9.81, system)
b = Constant('b', 1e0, system)
k = Constant('k', 1e1, system)
tinitial = 0
tfinal = 5
tstep = .001
t = numpy.r_[tinitial:tfinal:tstep]
preload1 = Constant('preload1', 0 * pi / 180, system)
qA, qA_d, qA_dd = Differentiable(system, 'qA')
initialvalues = {}
initialvalues[qA] = 0 * pi / 180
initialvalues[qA_d] = 0 * pi / 180
statevariables = system.get_q(0) + system.get_q(1)
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
#constants[I_main] = 1
constants[g] = 9.81
constants[b_joint] = .00025
constants[k_joint] = .0229183
constants[b_beam] = 0
constants[k_beam] = 14.0224
constants[stall_torque] = .00016 * 5.7 / 4.3586
constants[k_constraint] = 1e3
constants[b_constraint] = 1e1
constants[preload1] = 0 * pi / 180
constants[preload2] = 0 * pi / 180
constants[preload3] = -180 * pi / 180
constants[preload4] = 0 * pi / 180
constants[preload5] = 180 * pi / 180
x, x_d, x_dd = Differentiable(name='x', system=system)
y, y_d, y_dd = Differentiable(name='y', system=system)
qO, qO_d, qO_dd = Differentiable(name='qO', system=system)
qA1, qA1_d, qA1_dd = Differentiable(name='qA1', system=system)
qB1, qB1_d, qB1_dd = Differentiable(name='qB1', system=system)
qC1, qC1_d, qC1_dd = Differentiable(name='qC1', system=system)
qD1, qD1_d, qD1_dd = Differentiable(name='qD1', system=system)
qA2, qA2_d, qA2_dd = Differentiable(name='qA2', system=system)
qB2, qB2_d, qB2_dd = Differentiable(name='qB2', system=system)
qC2, qC2_d, qC2_dd = Differentiable(name='qC2', system=system)
qD2, qD2_d, qD2_dd = Differentiable(name='qD2', system=system)
initialvalues = {
x: 0,
x_d: 0,
y: .5,
V = Constant(name='V', system=system)
R = Constant(name='R', system=system)
Im = Constant(name='Im', system=system)
Il = Constant(name='Il', system=system)
Ib = Constant(name='Ib', system=system)
G = Constant(name='G', system=system)
b = Constant(name='b', system=system)
kv = Constant(name='kv', system=system)
kt = Constant(name='kt', system=system)
m_pendulum = Constant(name='m_pendulum', system=system)
m_motor = Constant(name='m_motor', system=system)
m_body = Constant(name='m_body', system=system)
g = Constant(name='g', system=system)
l = Constant(name='l', system=system)
x, x_d, x_dd = Differentiable('x', system)
y, y_d, y_dd = Differentiable('y', system)
qA, qA_d, qA_dd = Differentiable('qA', system)
qB, qB_d, qB_dd = Differentiable('qB', system)
qM, qM_d, qM_dd = Differentiable('qM', system)
#wB,wB_d= Differentiable('wB',ii=1,limit=3,system=system)
i, i_d = Differentiable('i', ii=1, system=system)
constants = {}
constants[L] = .541e-3
constants[V] = 12
constants[R] = 2.29
constants[G] = 10
constants[Im] = 52.3 * 1e-3 * (1e-2)**2
constants[Il] = .1
constants[Ib] = .1
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
m2 = Constant('m1',1,system)
m1 = Constant('m2',1,system)
g = Constant('g',9.81,system)
b = Constant('b',1e1,system)
k = Constant('k',1e1,system)
#preload1 = Constant('preload1',0*pi/180,system)
k_controller = Constant('k_controller',1e2,system)
q2_command = Constant('q2_command',90*pi/180,system)
#q2_command= Variable('q2_command')
tinitial = 0
tfinal = 1
tstep = .01
t = numpy.r_[tinitial:tfinal:tstep]
x,x_d,x_dd = Differentiable(system,'x')
y,y_d,y_dd = Differentiable(system,'y')
q1,q1_d,q1_dd = Differentiable(system,'q1')
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)
################################################
#other constants
g = Constant(0, 'g', system)
b = Constant(1e0, 'b', system)
k = Constant(1e2, 'k', system)
################################################
#time parameters
tinitial = 0
tfinal = 5
tstep = 1 / 30
t = numpy.r_[tinitial:tfinal:tstep]
################################################
#State variables
qA1, qA1_d, qA1_dd = Differentiable('qA1', system)
qA2, qA2_d, qA2_dd = Differentiable('qA2', system)
qA3, qA3_d, qA3_dd = Differentiable('qA3', system)
qB1, qB1_d, qB1_dd = Differentiable('qB1', system)
qB2, qB2_d, qB2_dd = Differentiable('qB2', system)
################################################
#Define initial values for state variables
initialvalues = {}
initialvalues[qA1] = 1 * pi / 180
initialvalues[qA2] = 1 * pi / 180
initialvalues[qA3] = 1 * pi / 180
initialvalues[qB1] = -1 * pi / 180
initialvalues[qB2] = -1 * pi / 180
initialvalues[qA1_d] = 0 * pi / 180
Constant('Iyy_A', 5.31645644183654e-06, system)
Constant('Izz_A', 5.31645644183654e-06, system)
Constant('Ixx_B', 6.27600676796613e-07, system)
Constant('Iyy_B', 1.98358014762822e-06, system)
Constant('Izz_B', 1.98358014762822e-06, system)
#Constant('Ixx_C',4.39320316677997e-07,system)
#Constant('Iyy_C',7.9239401855911e-07,system)
#Constant('Izz_C',7.9239401855911e-07,system)
Constant('b', 1e-4, system)
Constant('k', 1.0, system)
Constant('preload1', -90 * pi / 180, system)
Constant('preload2', 0 * pi / 180, system)
#Constant('preload3',0*pi/180,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')
constants[preload5] = 180 * pi / 180
constants[L] = .5
#constants[V] = 1
constants[R] = 1
constants[G] = 10
constants[Im] = .01
#constants[Il] = .1
#constants[b] = .1
constants[kv] = .01
constants[kt] = .01
constants[Tl] = 0
constants[m_motor] = .1
#constants[g] = 9.81
x, x_d, x_dd = Differentiable(name='x', system=system)
y, y_d, y_dd = Differentiable(name='y', system=system)
qO, qO_d, qO_dd = Differentiable(name='qO', system=system)
qMA, qMA_d, qMA_dd = Differentiable(name='qMA', system=system)
qA1, qA1_d, qA1_dd = Differentiable(name='qA1', system=system)
qA2, qA2_d, qA2_dd = Differentiable(name='qA2', system=system)
qB, qB_d, qB_dd = Differentiable(name='qB', system=system)
qMC, qMC_d, qMC_dd = Differentiable(name='qMC', system=system)
qC1, qC1_d, qC1_dd = Differentiable(name='qC1', system=system)
qC2, qC2_d, qC2_dd = Differentiable(name='qC2', system=system)
qD, qD_d, qD_dd = Differentiable(name='qD', system=system)
iA, iA_d = Differentiable('iA', ii=1, system=system)
iC, iC_d = Differentiable('iC', ii=1, system=system)
initialvalues = {
g = Constant('g', 9.81, system)
tinitial = 0
tfinal = 10
tstep = .001
t = numpy.r_[tinitial:tfinal:tstep]
Ixx_A = Constant('Ixx_A', 50 / 1000 / 100 / 100, system)
Iyy_A = Variable('Iyy_A')
Izz_A = Variable('Izz_A')
Ixx_B = Constant('Ixx_B', 2500 / 1000 / 100 / 100, system)
Iyy_B = Constant('Iyy_B', 500 / 1000 / 100 / 100, system)
Izz_B = Constant('Izz_B', 2000 / 1000 / 100 / 100, system)
qA, qA_d, qA_dd = Differentiable(system, 'qA')
qB, qB_d, qB_dd = Differentiable(system, 'qB')
initialvalues = {}
initialvalues[qA] = 90 * pi / 180
initialvalues[qA_d] = 0 * pi / 180
initialvalues[qB] = .5 * pi / 180
initialvalues[qB_d] = 0 * pi / 180
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')
import sympy
import numpy
import matplotlib.pyplot as plt
plt.ion()
import math
system = System()
pynamics.set_system(__name__, system)
g = Constant(9.81, 'g', system)
tinitial = 0
tfinal = 5
tstep = 1 / 30
t = numpy.r_[tinitial:tfinal:tstep]
qA, qA_d = Differentiable('qA', limit=2)
qB, qB_d = Differentiable('qB', limit=2)
qC, qC_d = Differentiable('qC', limit=2)
wx, wx_d = Differentiable('wx', ii=1, limit=3)
wy, wy_d = Differentiable('wy', ii=1, limit=3)
wz, wz_d = Differentiable('wz', ii=1, limit=3)
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
from mpl_toolkits.mplot3d import Axes3D
import sympy
import numpy
plt.ion()
from math import pi
system = System()
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)
q1,q1_d,q1_dd = Differentiable('q1',ini = [0,1])
q2,q2_d,q2_dd = Differentiable('q2',ini = [0,0])
q3,q3_d,q3_dd = Differentiable('q3',ini = [0,0])
f1 = Frame(,system)
f2 = Frame(,system)
f3 = Frame(,system)
f4 = Frame(,system)
#f5 = Frame(,system)
system.set_newtonian(f1)
f2.rotate_fixed_axis(f1,[0,0,1],q1,system)
f3.rotate_fixed_axis(f2,[1,0,0],q2,system)
f4.rotate_fixed_axis(f3,[0,1,0],q3,system)
p0 = 0*f1.x
#l1 = Constant(1,'l1',system)
m1 = Constant(1e-1, 'm1', system)
m2 = Constant(1e-3, 'm2', system)
k = Constant(1.5e2, 'k', system)
l0 = Constant(1, 'l0', system)
b = Constant(1e0, 'b', system)
g = Constant(9.81, 'g', system)
k_constraint = Constant(1e4, 'k_constraint', system)
b_constraint = Constant(1e5, 'b_constraint', system)
tinitial = 0
tfinal = 2
tstep = 1 / 30
t = numpy.r_[tinitial:tfinal:tstep]
x1, x1_d, x1_dd = Differentiable('x1', system)
y1, y1_d, y1_dd = Differentiable('y1', system)
#x2,x2_d,x2_dd = Differentiable('x2',system)
#y2,y2_d,y2_dd = Differentiable('y2',system)
q1, q1_d, q1_dd = Differentiable('q1', system)
l1, l1_d, l1_dd = Differentiable('l1', system)
vini = 5
aini = -60 * pi / 180
initialvalues = {}
initialvalues[x1] = 0
initialvalues[x1_d] = vini * cos(aini)
initialvalues[y1] = 1.2
initialvalues[y1_d] = vini * sin(aini)
constants[mD] = .1
constants[I_main] = 1
constants[g] = 9.81
constants[b] = 1e0
constants[k] = 1e2
constants[stall_torque] = 1e2
constants[k_constraint] = 1e5
constants[b_constraint] = 1e3
constants[preload1] = 0*pi/180
constants[preload2] = 0*pi/180
constants[preload3] = -180*pi/180
constants[preload4] = 0*pi/180
constants[preload5] = 180*pi/180
x,x_d,x_dd = Differentiable(name='x',system=system)
y,y_d,y_dd = Differentiable(name='y',system=system)
qO,qO_d,qO_dd = Differentiable(name='qO',system=system)
qA,qA_d,qA_dd = Differentiable(name='qA',system=system)
qB,qB_d,qB_dd = Differentiable(name='qB',system=system)
qC,qC_d,qC_dd = Differentiable(name='qC',system=system)
qD,qD_d,qD_dd = Differentiable(name='qD',system=system)
initialvalues={
x: 0,
x_d: 0,
y: 1.25,
y_d: 0,
qO: 0,
qO_d: 0,
qA: -0.89,
Il = Constant(name='Il', system=system)
G = Constant(name='G', system=system)
b = Constant(name='b', system=system)
kv = Constant(name='kv', system=system)
kt = Constant(name='kt', system=system)
Tl = Constant(name='Tl', system=system)
m = Constant(name='m', system=system)
g = Constant(name='g', system=system)
tinitial = 0
tfinal = 3
tstep = .01
t = numpy.r_[tinitial:tfinal:tstep]
#qA,qA_d,qA_dd = Differentiable('qA',system)
qB, qB_d, qB_dd = Differentiable('qB', system)
#wB,wB_d= Differentiable('wB',ii=1,limit=3,system=system)
i, i_d = Differentiable('i', ii=1, system=system)
constants = {}
constants[L] = .5
constants[V] = 1
constants[R] = 1
constants[G] = 10
constants[Im] = .01
constants[Il] = .1
constants[b] = .1
constants[kv] = .01
constants[kt] = .01
constants[Tl] = 0
constants[m] = 1
freq = Constant(.1, 'freq', system)
torque = Constant(10, 'torque', system)
Area = Constant(.1, 'Area', system)
# b = Constant(1e0,'b',system)
k = Constant(1e1, 'k', system)
rho = Constant(1000, 'rho')
Ixx_motor = Constant(.1, 'Ixx_motor')
Iyy_motor = Constant(.1, 'Iyy_motor')
Izz_motor = Constant(1, 'Izz_motor')
Ixx_plate = Constant(.1, 'Ixx_plate')
Iyy_plate = Constant(.1, 'Iyy_plate')
Izz_plate = Constant(1, 'Izz_plate')
qA, qA_d, qA_dd = Differentiable('qA', system)
initialvalues = {}
initialvalues[qA] = 0 * pi / 180
initialvalues[qA_d] = 0 * pi / 180
N = Frame('N', system)
A = Frame('A', system)
system.set_newtonian(N)
A.rotate_fixed_axis(N, [0, 0, 1], qA, system)
pNA = 0 * N.x
pAcm = pNA + lA / 2 * A.x
pAtip = pNA + lA * A.x
vAcm = pAcm.time_derivative(N, system)
k_constraint = Constant(1e4, 'k_constraint', system)
b_constraint = Constant(1e2, 'b_constraint', system)
tinitial = 0
tfinal = 10
tstep = 1 / 30
t = numpy.r_[tinitial:tfinal:tstep]
preload1 = Constant(0 * pi / 180, 'preload1', system)
preload2 = Constant(0 * pi / 180, 'preload2', system)
preload3 = Constant(-180 * pi / 180, 'preload3', system)
preload4 = Constant(0 * pi / 180, 'preload4', system)
preload5 = Constant(180 * pi / 180, 'preload5', system)
preload6 = Constant(0 * pi / 180, 'preload6', system)
x, x_d, x_dd = Differentiable('x', 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)
qD, qD_d, qD_dd = Differentiable('qD', system)
qE, qE_d, qE_dd = Differentiable('qE', system)
initialvalues = {
x: 0,
x_d: .5,
y: 2,
y_d: 0,
qO: 5 * pi / 180,
qO_d: 0,
import matplotlib.pyplot as plt
plt.ion()
from math import pi, sin, cos
system = System()
pynamics.set_system(__name__, system)
tinitial = 0
tfinal = 5
tstep = 1 / 30
t = numpy.r_[tinitial:tfinal:tstep]
ang_ini = 0
v = 1
x, x_d, x_dd = Differentiable('x', ini=[0, v * cos(ang_ini * pi / 180)])
y, y_d, y_dd = Differentiable('y', ini=[1, v * sin(ang_ini * pi / 180)])
z, z_d, z_dd = Differentiable('z', ini=[0, 0])
qA, qA_d, qA_dd = Differentiable('qA', ini=[0, 0])
qB, qB_d, qB_dd = Differentiable('qB', ini=[0, 0])
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')
# preload1 = Constant(0*pi/180,'preload1',system)
# preload2 = Constant(0*pi/180,'preload2',system)
# preload3 = Constant(0*pi/180,'preload3',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)
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')
#preload1 = Constant('preload1',0*pi/180,system)
l1 = Constant(1,'l1',system)
m1 = Constant(1e1,'m1',system)
m2 = Constant(1e0,'m2',system)
k = Constant(1e4,'k',system)
l0 = Constant(1,'l0',system)
b = Constant(5e0,'b',system)
g = Constant(9.81,'g',system)
tinitial = 0
tfinal = 10
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
system = System()
lA = Constant('lA', 2, system)
mA = Constant('mA', 1, system)
g = Constant('g', 9.81, system)
b = Constant('b', 1e0, system)
k = Constant('k', 1e1, system)
tinitial = 0
tfinal = 5
tstep = .001
t = numpy.r_[tinitial:tfinal:tstep]
preload1 = Constant('preload1', 0 * pi / 180, system)
x, x_d, x_dd = Differentiable(system, 'x')
y, y_d, y_dd = Differentiable(system, 'y')
initialvalues = {}
initialvalues[x] = 1
initialvalues[y] = 0
initialvalues[x_d] = 0
initialvalues[y_d] = 0
statevariables = system.get_q(0) + system.get_q(1)
ini = [initialvalues[item] for item in statevariables]
N = Frame('N')
system.set_newtonian(N)
pNA = 0 * N.x
error = 1e-12
tinitial = 0
tfinal = 10
tstep = .001
t = numpy.r_[tinitial:tfinal:tstep]
m = Constant(1, 'm', system)
g = Constant(9.81, 'g', system)
I = Constant(1, 'I', system)
J = Constant(1, 'J', system)
r = Constant(1, 'L', system)
H, H_d = Differentiable(
'H',
system,
limit=2,
)
L, L_d = Differentiable(
'L',
system,
limit=2,
)
Q, Q_d = Differentiable(
'Q',
system,
limit=2,
)
x, x_d, x_dd = Differentiable('x', system)
y, y_d, y_dd = Differentiable('y', system)
from math import pi
system = System()
pynamics.set_system(__name__,system)
mA = Constant(1,'mA',system)
g = Constant(9.81,'g',system)
b = Constant(1e0,'b',system)
k = Constant(1e6,'k',system)
tinitial = 0
tfinal = 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)
constants[preload5] = 180 * pi / 180
constants[L] = .5
#constants[V] = 1
constants[R] = 1
constants[G] = 10
constants[Im] = .01
#constants[Il] = .1
#constants[b] = .1
constants[kv] = .01
constants[kt] = .01
constants[Tl] = 0
constants[m_motor] = 1
#constants[g] = 9.81
x, x_d, x_dd = Differentiable(name='x', system=system)
y, y_d, y_dd = Differentiable(name='y', system=system)
qO, qO_d, qO_dd = Differentiable(name='qO', system=system)
qMA, qMA_d, qMA_dd = Differentiable(name='qMA', system=system)
qA, qA_d, qA_dd = Differentiable(name='qA', system=system)
qB, qB_d, qB_dd = Differentiable(name='qB', system=system)
qMC, qMC_d, qMC_dd = Differentiable(name='qMC', system=system)
qC, qC_d, qC_dd = Differentiable(name='qC', system=system)
qD, qD_d, qD_dd = Differentiable(name='qD', system=system)
iA, iA_d = Differentiable('iA', ii=1, system=system)
iC, iC_d = Differentiable('iC', ii=1, system=system)
initialvalues = {
x: 0,
x_d: 0,
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)
k = Constant(1e2, 'k', system)
tinitial = 0
tfinal = 10
tstep = 1 / 30
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)
from mpl_toolkits.mplot3d import Axes3D
import sympy
import numpy
plt.ion()
from math import pi
system = System()
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)