90 lines
2.0 KiB
Python
90 lines
2.0 KiB
Python
from casadi import *
|
|
import matplotlib.pyplot as plt
|
|
|
|
file = open('tmp2019_cci_08_26_11_36_00.txt')
|
|
|
|
c = (0.0, 0.0) # center of big circle
|
|
R = 15.0 # radius of big circle
|
|
|
|
# center for smaller circles
|
|
p1 = (-5.55287862, -7.64288174)
|
|
p2 = (5.55287862, -7.64288174)
|
|
p3 = (-8.98474635, 2.91932105)
|
|
p4 = (8.98474635, 2.91932105)
|
|
p5 = (0.00000000, 9.44712138)
|
|
rtilde = 5.5 # radius of smaller circles
|
|
|
|
points = [p1, p2, p3, p4, p5]
|
|
|
|
# midpoint between center of two circles
|
|
m = np.mean([p1, p2], axis=0)
|
|
|
|
# vector in direction of midpoint
|
|
v = m - np.array(c)
|
|
v = v/np.linalg.norm(v)
|
|
|
|
plt.xlim((-16, 16))
|
|
plt.ylim((-16, 16))
|
|
plt.gca().set_aspect('equal', 'box')
|
|
|
|
|
|
for p in points:
|
|
plt.plot(p[0], p[1], 'o')
|
|
circle = plt.Circle(p, rtilde, fill=False)
|
|
plt.gca().add_artist(circle)
|
|
circle = plt.Circle(c, R, fill=False)
|
|
plt.gca().add_artist(circle)
|
|
#plt.show()
|
|
|
|
plt.plot(c[0], c[1], 'o')
|
|
plt.plot(m[0], m[1], 'o')
|
|
|
|
opti = casadi.Opti()
|
|
|
|
r = opti.variable(1) # radius of new circle
|
|
p = opti.variable(2) # center of new circle
|
|
#v1 = opti.variable(2) # direction vector from
|
|
#v2 = opti.variable(2)
|
|
lamb = opti.variable(1)
|
|
|
|
opti.minimize(-r)
|
|
opti.subject_to(p == c + v * lamb)
|
|
opti.subject_to((p[0] - p1[0])**2 + (p[1] - p1[1])**2 >= (rtilde + r)**2)
|
|
#opti.subject_to(p == (rtilde + r) * v1 + p1)
|
|
#opti.subject_to(p == (rtilde + r) * v2 + p2)
|
|
#opti.subject_to((v1[0]**2+v1[1]**2)**0.5 == 1)
|
|
#opti.subject_to((v2[0]**2+v2[1]**2)**0.5 == 1)
|
|
opti.subject_to(R == lamb + r)
|
|
opti.subject_to(r >= 0)
|
|
opti.subject_to(r <= R)
|
|
#opti.subject_to(lamb >= 0.6 * R)
|
|
|
|
opti.solver('ipopt')
|
|
|
|
init_r = 0.1
|
|
init_lamb = R - init_r
|
|
init_p = c + v * init_lamb
|
|
|
|
|
|
opti.set_initial(r, init_r)
|
|
opti.set_initial(p, init_p)
|
|
opti.set_initial(lamb, init_lamb)
|
|
#opti.set_initial(v1, )
|
|
|
|
sol = opti.solve()
|
|
|
|
p = sol.value(p)
|
|
r = sol.value(r)
|
|
lamb = sol.value(lamb)
|
|
|
|
print("p = {}".format(p))
|
|
print("r = {}".format(r))
|
|
print("lambda = {}".format(lamb))
|
|
print("v = {}".format(v))
|
|
|
|
plt.plot(p[0], p[1], 'o')
|
|
circle = plt.Circle(p, r, fill=False)
|
|
plt.gca().add_artist(circle)
|
|
plt.show()
|
|
|
|
pass |