def optimal_weights(k, xi, **kwargs):
r"""Compute the optimal weights for a 2k-1 order WENO scheme
corresponding to the reconstruction points in *xi*.
The coefficients are stored as SymPy variables in a dictionary
indexed according to ``w[l,r]``. That is
.. math::
f(\xi^l) \approx \sum_r w^r f^r
for each :math:`l` from 0 to ``len(xi)``.
"""
n = len(xi)
c = reconstruction_coefficients(k, xi, **kwargs)
c2k = reconstruction_coefficients(2*k-1, xi, **kwargs)
omega = [ sym('omega%d' % r) for r in range(k) ]
varpi = { 'n': n, 'k': k }
split = { 'n': n }
for l in range(n):
# use first k eqns since weight system is overdetermined
eqns = []
for j in range(k):
rmin, rmax = max(0, (k-1)-j), min(k-1, 2*(k-1)-j)
terms = [ omega[r] * c[l, r, r-(k-1)+j] for r in range(rmin, rmax+1) ]
eqns.append(sum(terms) - c2k[l, k-1, j])
# XXX: Should make sure that when mpmath.mpf's are passed in
# (in xi), that the solution obtained above is a high
# precision solution.
sol = sympy.solve(eqns, omega)
# now check all 2*k-1 eqns to make sure the weights work out properly
for j in range(2*k-1):
rmin, rmax = max(0, (k-1)-j), min(k-1, 2*(k-1)-j)
terms = [ omega[r] * c[l, r, r-(k-1)+j] for r in range(rmin, rmax+1) ]
eqn = sum(terms) - c2k[l, k-1, j]
err = eqn.subs(sol)
if abs(err) > 1e-10:
raise ValueError("optimal weight %d failed with error %s" % (j, err))
# set weight or split as appropriate
split[l] = min(sol.values()) < 0
for r in range(k):
if split[l]:
w = sol[omega[r]]
wp = (w + 3*abs(w))/2
wm = wp - w
varpi[l, r] = (wp, wm)
else:
varpi[l, r] = sol[omega[r]]
return varpi, split
def symbolic_equation(self, n):
T = sym(' '.join(f"T_{i}" for i in range(n)))
J = sym(' '.join(f"J_{i}" for i in range(n)))
A = [
sym('A_0'),
]
for i in range(n):
A.append(A[-1] + J[i] * T[i])
S = [
sym('S_0'),
]
for i in range(n):
S.append(S[-1] + A[i] * T[i] + J[i] * T[i]**2 / 2)
return T, J, A, S
def jiang_shu_smoothness_coefficients(k):
r"""Compute the Jiang-Shu smoothness coefficients for a 2k-1 order
WENO scheme.
The coefficients are stored as SymPy variables in a dictionary
indexed according to ``beta[r, m, n]``. That is
.. math::
\sigma^r = \sum_{m=1}^{2k-1} \sum_{n=1}^{2k-1}
\beta_{r,m,n}\, \overline{f}_{i-k+m}\, \overline{f}_{i-k+n}.
"""
x, dx, xi = sym('x'), sym('dx'), sym('x')
# build arrays of cell boundaries and cell averages
xs = [ j*dx for j in range(-k+1, k+1) ]
fs = [ sym('f[i%+d]' % j) for j in range(-k+1, k) ]
beta = { 'k': k }
# compute smoothness coefficients for each left shift r
for r in range(k):
p = ppi(xs[k-1-r:2*k-r], fs[k-1-r:2*k-1-r]).diff(x)
# sum of L^2 norms of derivatives
s = 0
for j in range(1, k):
pp = sympy.diff(p, xi, j)**2
pp = pp.as_poly(x)
pp = pp.integrate(x)
pp = dx**(2*j-1) * ( pp.subs(x, xs[k]) - pp.subs(x, xs[k-1]) )
s += pp.expand()
# pick out coefficients
for m in range(k):
for n in range(m, k):
c = s.coeff(fs[k-1-r+m]*fs[k-1-r+n])
if c is not None:
beta[r, m, n] = c
return beta
def jiang_shu_smoothness_coefficients(k):
r"""Compute the Jiang-Shu smoothness coefficients for a 2k-1 order
WENO scheme.
The coefficients are stored as SymPy variables in a dictionary
indexed according to ``beta[r, m, n]``. That is
.. math::
\sigma^r = \sum_{m=1}^{2k-1} \sum_{n=1}^{2k-1}
\beta_{r,m,n}\, \overline{f}_{i-k+m}\, \overline{f}_{i-k+n}.
"""
x, dx, xi = sym('x'), sym('dx'), sym('x')
# build arrays of cell boundaries and cell averages
xs = [j * dx for j in range(-k + 1, k + 1)]
fs = [sym('f[i%+d]' % j) for j in range(-k + 1, k)]
beta = {'k': k}
# compute smoothness coefficients for each left shift r
for r in range(k):
p = ppi(xs[k - 1 - r:2 * k - r], fs[k - 1 - r:2 * k - 1 - r]).diff(x)
# sum of L^2 norms of derivatives
s = 0
for j in range(1, k):
pp = sympy.diff(p, xi, j)**2
pp = pp.as_poly(x)
pp = pp.integrate(x)
pp = dx**(2 * j - 1) * (pp.subs(x, xs[k]) - pp.subs(x, xs[k - 1]))
s += pp.expand()
# pick out coefficients
for m in range(k):
for n in range(m, k):
c = s.coeff(fs[k - 1 - r + m] * fs[k - 1 - r + n])
if c is not None:
beta[r, m, n] = c
return beta
def reconstruction_coefficients(k, xi, d=0):
r"""Compute the reconstruction coefficients for a 2k-1 order WENO
scheme corresponding to the reconstruction points in *xi*.
The reconstruction points in *xi* should in :math:`[-1, 1]`. This
interval is then mapped to the cell :math:`[x_{i-1/2}, x_{i+1/2}]`.
The returned coefficients are stored as SymPy variables in a
dictionary indexed according to ``c[l, r, j]``. That is
.. math::
f^r(\xi_l) \approx \sum_j c^{l,r}_j \, f_{i-r+j}
for each :math:`l` from 0 to ``len(xi)``.
"""
i, n, x, dx = k-1, len(xi), sym('x'), sym('dx')
# build arrays of cell boundaries and cell averages
xs = [ j*dx for j in range(-k+1, k+1) ]
fs = [ sym('f[i%+d]' % j) for j in range(-k+1, k) ]
c = { 'n': n, 'k': k, 'd': d }
# compute reconstruction coefficients for each left shift r
for l, r in product(range(n), range(k)):
p = ppi(xs[i-r:i-r+k+1], fs[i-r:i-r+k]).diff(x, d+1)
for j in range(k):
z = _pt(xs[i], xs[i+1], xi[l])
c[l, r, j] = p.subs(x, z).expand().coeff(fs[i-r+j])
if c[l, r, j] is None:
c[l, r, j] = 0
c[l, r, j] *= dx**d
return c
def reconstruction_coefficients(k, xi, d=0):
r"""Compute the reconstruction coefficients for a 2k-1 order WENO
scheme corresponding to the reconstruction points in *xi*.
The reconstruction points in *xi* should in :math:`[-1, 1]`. This
interval is then mapped to the cell :math:`[x_{i-1/2}, x_{i+1/2}]`.
The returned coefficients are stored as SymPy variables in a
dictionary indexed according to ``c[l, r, j]``. That is
.. math::
f^r(\xi_l) \approx \sum_j c^{l,r}_j \, f_{i-r+j}
for each :math:`l` from 0 to ``len(xi)``.
"""
i, n, x, dx = k - 1, len(xi), sym('x'), sym('dx')
# build arrays of cell boundaries and cell averages
xs = [j * dx for j in range(-k + 1, k + 1)]
fs = [sym('f[i%+d]' % j) for j in range(-k + 1, k)]
c = {'n': n, 'k': k, 'd': d}
# compute reconstruction coefficients for each left shift r
for l, r in product(range(n), range(k)):
p = ppi(xs[i - r:i - r + k + 1], fs[i - r:i - r + k]).diff(x, d + 1)
for j in range(k):
z = _pt(xs[i], xs[i + 1], xi[l])
c[l, r, j] = p.subs(x, z).expand().coeff(fs[i - r + j])
if c[l, r, j] is None:
c[l, r, j] = 0
c[l, r, j] *= dx**d
return c
def theta_penalty(p):
"""
unweighted
"""
a0, a1, a2, a3 = get_coeff(p)
start, goal = posture()
tht0 = start[2]
sf = p[2]
s = sym('s')
thtsf = tht0 + (a3 / 4) * (s**4) + (a2 / 3) * (s**3) + (a1 / 2) * (
s**2) + a0 * s
thtf = goal[2]
tht_penalty = abs(thtsf.subs(s, sf) - thtf)
return (thtsf, tht_penalty)
def get_sympy(self):
n = self.n
px_lst = sym(' '.join(f'P^{i}_x' for i in range(n)))
py_lst = sym(' '.join(f'P^{i}_y' for i in range(n)))
q_lst = self.p_coef(n)
x0 = 0
y0 = 0
for i in range(n):
x0 += q_lst[i] * px_lst[i] * (t)**i * (1 - t)**(n - i - 1)
y0 += q_lst[i] * py_lst[i] * (t)**i * (1 - t)**(n - i - 1)
x1 = sympy.diff(x0, t)
y1 = sympy.diff(y0, t)
x2 = sympy.diff(x1, t)
y2 = sympy.diff(y1, t)
s0 = (x1 * y2 - y1 * x2) / (x1**2 + y1**2)**sympy.Rational(3, 2)
return x0, y0, s0
def y_penalty(p):
a0, a1, a2, a3 = get_coeff(p)
start, goal = posture()
s = sym('s')
#need to call theta_penalty
thtsf, th_penalty = theta_penalty(p)
sf = p[2]
y0 = start[1]
ysf = y0 + (sf/24)*( \
sin(thtsf.subs(s,0)) + 4*sin(thtsf.subs(s,sf/8)) + \
2*sin(thtsf.subs(s,(2*sf)/8)) + 4*sin(thtsf.subs(s,(3*sf)/8)) + \
2*sin(thtsf.subs(s,(4*sf)/8)) + 4*sin(thtsf.subs(s,(5*sf)/8)) + \
2*sin(thtsf.subs(s,(6*sf)/8)) + 4*sin(thtsf.subs(s,(7*sf)/8)) + \
sin(thtsf.subs(s,(8*sf)/8)))
yf = goal[1]
y_p = abs(ysf - yf)
return y_p
def x_penalty(p):
a0, a1, a2, a3 = get_coeff(p)
start, goal = posture()
s = sym('s')
#need to call theta_penalty
thtsf, th_penalty = theta_penalty(p)
sf = p[2]
x0 = start[0]
xsf = x0 + (sf/24)*( \
cos(thtsf.subs(s,0)) + 4*cos(thtsf.subs(s,sf/8)) + \
2*cos(thtsf.subs(s,(2*sf)/8)) + 4*cos(thtsf.subs(s,(3*sf)/8)) + \
2*cos(thtsf.subs(s,(4*sf)/8)) + 4*cos(thtsf.subs(s,(5*sf)/8)) + \
2*cos(thtsf.subs(s,(6*sf)/8)) + 4*cos(thtsf.subs(s,(7*sf)/8)) + \
cos(thtsf.subs(s,(8*sf)/8)))
xf = goal[0]
x_p = abs(xsf - xf)
return x_p
def polynomial_interpolator(x, y):
"""Build a symbolic polynomial that interpolates the points (x_i, y_i).
The returned polynomial is a function of the SymPy variable x.
"""
k, xi = len(x), sym('x')
sum_i = 0
for i in range(k):
ns = range(k)
ns.remove(i)
num, den = 1, 1
for n in ns:
num *= xi - x[n]
den *= x[i] - x[n]
sum_i += num / den * y[i]
return sum_i
def target_pos(self):
"""
compute the expected position reached at the end of the trajbang3
"""
t = sym('t')
pos_n = 0
spd_n = self.s0
acc_n = self.a0
for c, d in zip(self.cmd, self.dur):
jrk_s = c
acc_s = sympy.integrate(jrk_s, t) + acc_n
spd_s = sympy.integrate(acc_s, t) + spd_n
pos_s = sympy.integrate(spd_s, t) + pos_n
acc_n = acc_s.subs({'t': d})
spd_n = spd_s.subs({'t': d})
pos_n = pos_s.subs({'t': d})
self.pg = pos_n
return self.pg
def polynomial_interpolator(x, y):
"""Build a symbolic polynomial that interpolates the points (x_i, y_i).
The returned polynomial is a function of the SymPy variable x.
"""
k, xi = len(x), sym('x')
sum_i = 0
for i in range(k):
ns = range(k)
ns.remove(i)
num, den = 1, 1
for n in ns:
num *= xi - x[n]
den *= x[i] - x[n]
sum_i += num / den * y[i]
return sum_i
def optimal_weights(k, xi, **kwargs):
r"""Compute the optimal weights for a 2k-1 order WENO scheme
corresponding to the reconstruction points in *xi*.
The coefficients are stored as SymPy variables in a dictionary
indexed according to ``w[l,r]``. That is
.. math::
f(\xi^l) \approx \sum_r w^r f^r
for each :math:`l` from 0 to ``len(xi)``.
"""
n = len(xi)
c = reconstruction_coefficients(k, xi, **kwargs)
c2k = reconstruction_coefficients(2 * k - 1, xi, **kwargs)
omega = [sym('omega%d' % r) for r in range(k)]
varpi = {'n': n, 'k': k}
split = {'n': n}
for l in range(n):
# use first k eqns since weight system is overdetermined
eqns = []
for j in range(k):
rmin, rmax = max(0, (k - 1) - j), min(k - 1, 2 * (k - 1) - j)
terms = [
omega[r] * c[l, r, r - (k - 1) + j]
for r in range(rmin, rmax + 1)
]
eqns.append(sum(terms) - c2k[l, k - 1, j])
# XXX: Should make sure that when mpmath.mpf's are passed in
# (in xi), that the solution obtained above is a high
# precision solution.
sol = sympy.solve(eqns, omega)
# now check all 2*k-1 eqns to make sure the weights work out properly
for j in range(2 * k - 1):
rmin, rmax = max(0, (k - 1) - j), min(k - 1, 2 * (k - 1) - j)
terms = [
omega[r] * c[l, r, r - (k - 1) + j]
for r in range(rmin, rmax + 1)
]
eqn = sum(terms) - c2k[l, k - 1, j]
err = eqn.subs(sol)
if abs(err) > 1e-10:
raise ValueError("optimal weight %d failed with error %s" %
(j, err))
# set weight or split as appropriate
split[l] = min(sol.values()) < 0
for r in range(k):
if split[l]:
w = sol[omega[r]]
wp = (w + 3 * abs(w)) / 2
wm = wp - w
varpi[l, r] = (wp, wm)
else:
varpi[l, r] = sol[omega[r]]
return varpi, split
def bending_energy(p):
a0, a1, a2, a3 = get_coeff(p)
s = sym('s')
result = integrate((a0 + a1 * s + a2 * (s**2) + a3 * (s**3))**2, s)
return result
#!/usr/bin/env python3
import math
import numpy as np
import matplotlib.pyplot as plt
import sympy
from sympy import symbols as sym
t = sym('t')
class Bezier():
def __init__(self, arg=None):
self.p_lst = None
if arg is None:
self.n = None
try:
self.set_p(arg)
except TypeError:
self.n = int(arg)
def set_p(self, p_lst):
self.p_lst = list(p_lst)
self.n = len(p_lst)
self.val = dict()
for i, (a, b) in enumerate(self.p_lst):
obj = be + tht_p + x_p + y_p
first = diff(obj, p1)
second = diff(obj, p2)
third = diff(obj, p4)
return (first, second, third)
def spiral_planner():
start, goal = posture()
p = [1, 1, 3] #the p values will originally come from optimization
# objective_function(p)
be = bending_energy(p)
_, tht_p = theta_penalty(p)
x_p = x_penalty(p)
y_p = y_penalty(p)
objective_value = be + tht_p + x_p + y_p
print(objective_value)
print(be)
tic = time.time()
p1 = sym('p1')
p2 = sym('p2')
p4 = sym('p4')
p = [p1, p2, p4]
jacobian(p)
toc = time.time()
print(toc - tic)
def compute_3(jm, am, a0, s0, sg, debug=False):
# calcul la commande en jrk nécessaire pour:
# * rejoindre la vitesse sg à partir de la vitesse s0
# * avec une accélération finale nulle
J_m, A_m, A_0, S_0, S_g = sym('J_m A_m A_0 S_0 S_g')
val = {
'J_m': jm,
'A_m': am,
'A_0': a0,
'S_0': s0,
'S_g': sg,
}
# l'aire totale de l'accélération doit être égale au delta de vitesse
Q_d = S_g - S_0
if float(A_0.subs(val)) == 0.0:
# départ à accélération nulle
s = math.copysign(1.0, sg - s0)
Q_m = A_m**2 / J_m
if float(abs(Q_d).subs(val)) <= float(Q_m.subs(val)):
# CASE 2
# on a pas le temps d'atteindre l'accélération max
# time_for_acc = math.sqrt(abs(aire_target) / jm)
T_a = sympy.sqrt(abs(Q_d) / J_m)
bch = "B" + sign_var(s)
cmd = [s, -s]
dur = [T_a, T_a]
else:
# CASE 1
# on atteind l'accélération maximale
# time_triangle = am / jm
T_t = A_m / J_m
# aire_restante_pour_rectangle = abs(aire_target) - aire_maxi_triangle
Q_r = abs(Q_d) - Q_m
# time_rectangle = aire_restante_pour_rectangle / am
T_r = Q_r / A_m
bch = "A" + sign_var(s)
cmd = [s, 0, -s]
dur = [T_t, T_r, T_t]
else:
# mais pour revenir à zero, il nous faut au moins un triangle, dont l'aire est égale à
T_n = abs(A_0) / J_m
Q_n = T_n * A_0 / 2
Q_c = Q_d - Q_n
s = math.copysign(1.0, a0)
if 0 < float((Q_c * Q_n).subs(val)):
# l'aire restante est un triangle possiblement tronqué de taille maxi :
Q_u = abs(A_m**2 - A_0**2) / (J_m)
if float(abs(Q_c).subs(val)) <= float(Q_u.subs(val)):
D = 4 * A_0**2 + 4 * J_m * abs(Q_c)
T_b = (-2 * abs(A_0) + sympy.sqrt(D)) / (2 * J_m)
bch = "C" + sign_var(s)
cmd = [s, -s, -s]
dur = [T_b, T_b, T_n]
else:
T_s = (abs(Q_c) - Q_u) / A_m
bch = "D" + sign_var(s)
cmd = [s, 0, -s, -s]
dur = [
abs(A_m - abs(A_0)) / J_m, T_s,
abs(A_m - abs(A_0)) / J_m, T_n
]
elif float((Q_c * Q_n).subs(val)) < 0:
# le reste n'est pas dans le même sens que le petit triangle
# c'est un triangle et pas un trapèze, si l'aire restante est inférieure à :
Q_s = A_m * A_m / J_m
if float(Q_s.subs(val)) < float(abs(Q_c).subs(val)):
Q_g = abs(Q_c) - Q_s
bch = "E" + sign_var(s)
cmd = [-s, -s, 0, s]
dur = [T_n, A_m / J_m, Q_g / A_m, A_m / J_m]
else:
bch = "F" + sign_var(s)
cmd = [-s, -s, s]
dur = [
T_n,
sympy.sqrt(abs(Q_c) / J_m),
sympy.sqrt(abs(Q_c) / J_m)
]
else:
bch = "G0"
cmd = [
-s,
]
dur = [
T_n,
]
return bch, cmd, dur, val
from pwn import *
from sympy.parsing.sympy_parser import parse_expr
from sympy import Eq, Symbol as sym, solve
#Connect to server
r = remote("misc.chal.csaw.io", 9002)
while (True):
try:
#Get data from server
x = r.recv()
lines = x.split("\n")
equation = lines[-2]
#Split equation into L.H.S and R.H.S
parts = equation.split("=")
if len(parts) > 1:
X = sym('X')
lhs = parts[0].strip()
rhs = parts[-1].strip()
#Use parse_expr to convert the two sides of the equation from string to a format accepted by sympy
lhs = parse_expr(lhs)
rhs = parse_expr(rhs)
#Solve equation
eqa = Eq(lhs, rhs)
res = solve(eqa)
print "Result:", res
#sympy returns answers of 0 and 1 as Boolean type which causes error while sending, handling these use cases
if str(res) == "True":
r.send(str(float(1)) + "\n")
if str(res) == "False":
r.send(str(float(0)) + "\n")
#Sending equation
import numpy as np
import sympy
from sympy import RealNumber as Real, symbols as sym, diff as d
DIMENSION = 2
x = sym('x y')
CONTRAV = True
COV = False
class Index:
def __init__(self, name):
self.name = name.strip('-')
self.contrv = (name[0] != '-')
@staticmethod
def new(names):
return tuple([Index(i) for i in names.split()])
def __repr__(self):
return self.name if self.contrv else '-' + self.name
def __neg__(self):
if self.contrv:
return Index('-' + self.name)
else:
return Index(self.name)
def __eq__(self, other):
return self.name == other.name and self.contrv == other.contrv
def _compute_analytic(self, unfold=False):
"""
this implementation keep symbolic expressions as much as possible, and details all cases
"""
bch_lst = list()
jm, am, a0, s0, sg = self.jm, self.am, self.a0, self.s0, self.sg
val = self.val
def u_abs(exp):
if unfold:
p = exp.subs(val)
if 0 <= p:
bch_lst.append('+')
return exp
else:
bch_lst.append('-')
return -exp
else:
return abs(exp)
# creation des symboles
J_m, A_m, A_0, S_0, S_g = sym('J_m A_m A_0 S_0 S_g')
# l'intégrale de l'accélération doit être égale au delta de vitesse
Q_d = S_g - S_0
if float(A_0.subs(val)) == 0.0:
# cas d'un départ à accélération nulle, le sens est donné par le delta de vitesse
s = 1 if (Q_d).subs(val) >= 0 else -1
if unfold:
bch_lst.append(sign_var(s))
# l'integrale d'une acceleration triangulaire, partant de zero et atteignant l'accéleration maximale
Q_m = A_m**2 / J_m
if float(abs(Q_d).subs(val)) <= float(Q_m.subs(val)):
# BRANCH B
bch_lst = [
"B",
] + bch_lst
# on a pas le temps d'atteindre l'accélération maximale
# le temps de montée jusqu'à l'accélération requise (pour une aire inférieure ou égale à Q_m)
T_a = sympy.sqrt(u_abs(Q_d) / J_m)
cmd = [s, -s]
dur = [T_a, T_a]
else:
# BRANCH A
bch_lst = [
"A",
] + bch_lst
# on a le temps d'atteindre l'accélération maximale, il y a donc une phase plateau à jerk nul
# time_triangle = am / jm
T_t = A_m / J_m
# aire_restante_pour_rectangle = abs(aire_target) - aire_maxi_triangle
Q_r = u_abs(Q_d) - Q_m
# time_rectangle = aire_restante_pour_rectangle / am
T_r = Q_r / A_m
cmd = [s, 0, -s]
dur = [T_t, T_r, T_t]
else:
s = 1 if a0 >= 0 else -1
if unfold:
bch_lst.append(sign_var(s))
# mais pour revenir à zero, il nous faut au moins un triangle, dont l'aire est égale à
T_n = u_abs(A_0) / J_m
Q_n = T_n * A_0 / 2
Q_c = Q_d - Q_n
if 0 < float((Q_c * Q_n).subs(val)):
# l'aire restante est un triangle possiblement tronqué de taille maxi :
Q_u = u_abs(A_m**2 - A_0**2) / (J_m)
if float(abs(Q_c).subs(val)) <= float(Q_u.subs(val)):
bch_lst = [
"C",
] + bch_lst
D = 4 * A_0**2 + 4 * J_m * u_abs(Q_c)
T_b = (-2 * u_abs(A_0) + sympy.sqrt(D)) / (2 * J_m)
cmd = [s, -s]
dur = [T_b, T_b + T_n]
else:
bch_lst = [
"D",
] + bch_lst
T_s = (u_abs(Q_c) - Q_u) / A_m
Q_t = u_abs(A_m - u_abs(A_0))
cmd = [s, 0, -s]
dur = [Q_t / J_m, T_s, Q_t / J_m + T_n]
elif float((Q_c * Q_n).subs(val)) < 0:
# le reste n'est pas dans le même sens que le petit triangle
# c'est un triangle et pas un trapèze, si l'aire restante est inférieure à :
Q_s = A_m * A_m / J_m
if float(Q_s.subs(val)) < float(abs(Q_c).subs(val)):
bch_lst = [
"E",
] + bch_lst
Q_g = u_abs(Q_c) - Q_s
cmd = [-s, 0, s]
dur = [T_n + A_m / J_m, Q_g / A_m, A_m / J_m]
else:
bch_lst = [
"F",
] + bch_lst
T_e = sympy.sqrt(u_abs(Q_c) / J_m)
cmd = [-s, s]
dur = [T_n + T_e, T_e]
else:
bch_lst = [
f"G",
] + bch_lst
cmd = [
-s,
]
dur = [
T_n,
]
bch = ''.join(bch_lst)
return cmd, dur, bch
def objective_function(p):
"""
BENDING ENERGY + PENALTIES
"""
#bending energy
p1 = p[0]
p2 = p[1]
p4 = p[2]
start, goal = posture()
p0 = start[-1]
p3 = goal[-1]
a0 = p0
a1 = -(5.5 * p0 - 9 * p1 + 4.5 * p2 - p3) / p4
a2 = (9 * p0 - 22.5 * p1 + 18 * p2 - 4.5 * p3) / (p4**2)
a3 = (-4.5 * p0 - 13.5 * p1 + 13.5 * p2 - 4.5 * p3) / (p4**2)
s = sym('s')
result = integrate((a0 + a1 * s + a2 * (s**2) + a3 * (s**3))**2)
bending_energy = result.subs(s, p4) - result.subs(s, 0)
#weights
alpha = 0.1
beta = 0.1
gamma = 0.1
#theta penalty
tht0 = start[2]
sf = p4
thtsf = tht0 + (a3 / 4) * (s**4) + (a2 / 3) * (s**3) + (a1 / 2) * (
s**2) + a0 * s #obtained symbolic
thtf = goal[2]
theta_penalty = gamma * abs(thtsf.subs(s, sf) - thtf)
#x_penalty
x0 = start[0]
xsf = x0 + (sf/24)*( \
cos(float(thtsf.subs(s,0))) + 4*cos(float(thtsf.subs(s,sf/8))) + \
2*cos(float(thtsf.subs(s,(2*sf)/8))) + 4*cos(float(thtsf.subs(s,(3*sf)/8))) + \
2*cos(float(thtsf.subs(s,(4*sf)/8))) + 4*cos(float(thtsf.subs(s,(5*sf)/8))) + \
2*cos(float(thtsf.subs(s,(6*sf)/8))) + 4*cos(float(thtsf.subs(s,(7*sf)/8))) + \
cos(float(thtsf.subs(s,(8*sf)/8))))
xf = goal[0]
x_penalty = alpha * (abs(xsf - xf))
#y_penalty
y0 = start[1]
ysf = y0 + (sf/24)*( \
sin(float(thtsf.subs(s,0))) + 4*sin(float(thtsf.subs(s,sf/8))) + \
2*sin(float(thtsf.subs(s,(2*sf)/8))) + 4*sin(float(thtsf.subs(s,(3*sf)/8))) + \
2*sin(float(thtsf.subs(s,(4*sf)/8))) + 4*sin(float(thtsf.subs(s,(5*sf)/8))) + \
2*sin(float(thtsf.subs(s,(6*sf)/8))) + 4*sin(float(thtsf.subs(s,(7*sf)/8))) + \
sin(float(thtsf.subs(s,(8*sf)/8))))
yf = goal[1]
y_penalty = alpha * (abs(ysf - yf))
objective_value = bending_energy + x_penalty + y_penalty + theta_penalty
print("breakdown:")
print("total objective_value", objective_value)
print("bending energy", bending_energy)
print("x_penalty", x_penalty)
print("y_penalty", y_penalty)
print("theta_penalty", theta_penalty)
return objective_value
new_locals = {
sym.name: sympy.Symbol(sym.name, commutative=False)
for sym in parsed_expr.atoms(sympy.Symbol)
}
return sympy.sympify(expr_string, locals=new_locals)
init_session(quiet=True)
init_printing(use_unicode=False,
wrap_line=False,
no_global=True,
use_latex=True)
Nz = 5
dz, bmz, cmz, delta, dt, n, np1, np1ov2, np3ov2, nm1, al, bl, cl, Co, wl, bz, az = sym(
"Δ_z,b_mz, c_mz,δ, Δ_t,n, n+1, n+1/2, n+3/2, n-1, a_L, b_L, c_L, Co, omega_L, b_z, a_z"
)
# bz, az from cpml update
#al, bl, cl lorentz polarisation
sigmaE, sigmaH, De, K, Kt, Du, Ex, Hy, Je, P, Psi_exy, Psi_hxy = sym(
"σ_E, σ_H, D_E, K, K^t, D_μ, Ex, Hy, J_e, P, ψ_(exy), ψ_(hxy)",
commutative=False)
Exnp1 = Ex**np1
Exn = Ex**n
Hypn3ov2 = Hy**np3ov2
Hypn1ov2 = Hy**np1ov2
Pn1 = P**np1
Pn = P**n
Pnm1 = P**nm1
Jen32 = Je**np3ov2
return f"A {r:0.3f} {r:0.3f} 0 0 {'1' if 0 < self.w else '0'} {p.x:0.3f} {p.y:0.3f}"
if __name__ == '__main__':
import sympy
from sympy import symbols as sym
l1 = Line.from_originPoint_normalVector(Point(4, 1), Vector(4, -2))
l2 = Line.from_originPoint_normalVector(Point(1, 5), Vector(2, -6))
print(l1)
print(l1.debug_canonical())
print(l2.debug_canonical())
a1, a2, b1, b2, c1, c2 = sym('a_1 a_2 b_1 b_2 c_1 c_2')
a1x, a1y, b1x, b1y, a2x, a2y, b2x, b2y = sym(
'a^1_x a^1_y b^1_x b^1_y a^2_x a^2_y b^2_x b^2_y')
y = (a2 * c1 - a1 * c2) / (a1 * b2 - a2 * b1)
print(
sympy.latex(
y.subs(a1, a1y).subs(a2, a2y).subs(b1, -a1x).subs(b2, -a2x).subs(
c1, a1x * b1y - a1y * b1x).subs(c2, a2x * b2y - a2y * b2x)))
print(l1.intersection_with_line(l2))
sys.exit(0)
Arc.from_2_Point(Point(-2, 2), Point(2, 2), 1 / 5)
line_o = Line.from_2_Point(Point(-1, 0), Point(5, 3))
