Моделювання кінематики — це не складно

Мені давно хочеться зайнятися створенням роботів, але завжди не вистачає вільних грошей, часу або місця. З цього я зібрався писати їх віртуальні моделі!

Потужні інструменти, що дозволяють це робити, або складно стикуються зі сторонніми програмами (Modelica), або проприетарны (Wolfram Mathematica, різні САПР), і я вирішив робити велосипед на Julia. Потім всі напрацювання можна буде зістикувати з сервісами ROS.

Будемо вважати, що наші роботи рухаються досить повільно і їх механіка знаходиться в стан з найменшою енергією, при обмеженнях, заданих конструкцією і сервоприводами. Таким чином нам досить вирішити задачу оптимізації в обмеженнях, для чого знадобляться пакети "JuMP" (для нелінійної оптимізації йому знадобиться пакет "Ipopt", який не вказаний в залежностях (замість нього можна використовувати власні бібліотеки, але я хочу обмежиться вільними) і повинен бути встановлений окремо). Вирішувати диференціальні рівняння, як в Modelica, ми не будемо, хоча для цього є достатньо розвинені пакети, наприклад "DASSL".

Управляти системою ми будемо використовуючи реактивне програмування (бібліотеки "Reactive"). Малювати у блокноті (Jupyter), для чого потрібні "IJulia", "Interact" і "Compose". Для зручності ще знадобиться "MacroTools".

Для інсталяції пакетів треба виконати в Julia REPL команду

foreach(Pkg.add, ["IJulia", "Ipopt", "Interact", "Reactive", "JuMP", "Compose", "MacroTools"])

Розглянемо просту двовимірну систему, схематично зображена на малюнку.
Червоними лініями там позначені пружини, чорними — нерастяжимые мотузки, маленький кружечек — невагомий блок, великий вантаж. У мотузок є довжина, пружини — довжина і жорсткість. Змінні координати назвемо:

(x,y) — координати вантажу (великого кола)
(xctl, yctl) — координати «керуючого кінця мотузки, довжиною 1.7 прив'язаною до вантажу.
(xp, yp) — координати невагомого точкового блоку, здатного ковзати по мотузці.
Одна пружина довжини 0.1 і жорсткості 1 закріплена в точці (0,3) і причеплена до вантажу. Друга пружина довжиною 0.15 і жорсткістю 5 з'єднує вантаж і блок, який ковзає по мотузці довжиною 6 закріпленої в точках (5,1) і (4.5,5).

(параметри підбиралися еволюційно, що б мені було наочно і зрозуміло як вона себе веде при додаванні нових фіч)

@wire(x,y, xctl,yctl, 1.7)
@wire(xp, yp, 5.0,1.0, 4.5,5.0, 6.0)
@energy([w(x,y, 0,3, 1, 0.1), w(x,y, xp,yp, 5, 0.15), 0.4*y])

повний код функції, вирішальною рівняння
function myModel(xctl, yctl)
m = Model()
@variable(m, y >= 0)
@variable(m, x)
@variable(m, yp)
@variable(m, xp)

@wire(x,y, xctl,yctl, 1.7)
@wire(xp, yp, 5.0,1.0, 4.5,5.0, 6.0)

@energy([w(x,y, 0,3, 1, 0.1), w(x,y, xp,yp, 5, 0.15), 0.4*y])

status = solve(m)

xval = getvalue(x)
yval = getvalue(y)
xpval = getvalue(xp)
ypval = getvalue(yp)

# print("calculate $status $xval $yval for $xctl, $yctl\n")
(status, xval, yval, xpval, ypval)
end


Макрос wire задає обмеження «зв'язування нерозтяжній мотузкою» заданої довжини два об'єкта або два об'єкта і блок.

Макрос energy описує минимизируемую функцію енергії — спеціальний терм w описує пружину (із заданою жорсткістю і завдовжки), а 0.4*y — енергія вантажу в гравітаційному полі, занадто проста, що для її придумувати спеціальний синтаксис.

Ці макроси виражаються через бібліотечними NLconstraint і NLobjective (лінійні constraint і objective ефективніше, але з нашими функціями не справляються):

@NLconstraint(m, (x-xctl)^2+(y-yctl)^2 <= 1.7)
@NLconstraint(m, sqrt((5.0-xp)^2+(1.0-yp)^2) + sqrt((4.5-xp)^2+(5.0-yp)^2) <= 6.0)
@NLobjective(m, Min, 1*(sqrt(((x-0)^2 + (y - 3)^2)) - 0.1)^2
+ 5*(sqrt((x - xp)^2 + (y - yp)^2) - 0.15)^2
+ 0.4*y)

Використання в обмеженнях «менше або дорівнює» замість «дорівнює» означає що мотузка може провисати, але не розтягуватися. Для двох точок можна використовувати суворе рівність, якщо треба з'єднати їх «жорстким стрижнем», замінивши відповідний макрос.

код макросу
macro wire(x,y,x0,y0, l)
v = l^2
:(@NLconstraint(m, ($x0-$x)^2+($y0-$y)^2 <= $v))
end

macro wire(x,y, x0,y0, x1,y1, l)
:(@NLconstraint(m, sqrt(($x0-$x)^2+($y0-$y)^2) + sqrt(($x1-$x)^2+($y1-$y)^2) <= $l))
end


calcenergy(d) = MacroTools.@match begin d
[t__] => :(+$(map(energy1,t)...))
v_ => energy1(v)
end

energy1(v) = MacroTools.@match v begin
w(x1_,y1_, x2_,y2_, k_, l_) => :($k*(sqrt(($x1 - $x2)^2 + ($y1 - $y2)^2) - $l)^2)
x_ => x
end

macro energy(v)
e = calcenergy(v)
:(@NLobjective(m, Min, $e))
end


Тепер опишемо елементи управління:

xctl = slider(-2:0.01:5, label="X Control")
xctlsig = signal(xctl)
yctl = slider(-1:0.01:0.5, label="Y Control")
yctlsig = signal(yctl)

приєднаємо їх до нашої системи:

ops = map(myModel, xctlsig, yctlsig)

І відобразимо це в notebook:

пара допоміжних функцій
xscale = 3
yscale = 3
shift = 1.5

pscale(x,y) = ((x*xscale+shift)cm, (y*yscale)cm)

function l(x1,y1, x2,y2; w=0.3 mm, color="black")
compose(context(),
linewidth(w), stroke(color),
line([pscale(x1, y1), pscale(x2, y2)]))
end


@manipulate for xc in xctl, yc in yctl, op in ops
compose(context(),
# text(150px, 220px, "m = $(op[2]) $(op[3])"),
# text(150px, 200px, "p = $(op[4]) $(op[5])"),
circle(pscale(op[2], op[3])..., 0.03),
circle(pscale(op[4], op[5])..., 0.01),

l(op[2], op[3], op[4], op[5], color="red"),
l(op[2], op[3], 0, 3, color="red"),

l(op[2], op[3], xc, yc),
l(op[4], op[5], 5, 1),
l(op[4], op[5], 4.5,5)
)
end

повний варіант коду
using Interact, Reactive, JuMP, Compose, MacroTools

macro wire(x,y,x0,y0, l)
v = l^2
:(@NLconstraint(m, ($x0-$x)^2+($y0-$y)^2 <= $v))
end

macro wire(x,y, x0,y0, x1,y1, l)
:(@NLconstraint(m, sqrt(($x0-$x)^2+($y0-$y)^2) + sqrt(($x1-$x)^2+($y1-$y)^2) <= $l))
end


calcenergy(d) = MacroTools.@match begin d
[t__] => :(+$(map(energy1,t)...))
v_ => energy1(v)
end

energy1(v) = MacroTools.@match v begin
w(x1_,y1_, x2_,y2_, k_, l_) => :($k*(sqrt(($x1 - $x2)^2 + ($y1 - $y2)^2) - $l)^2)
x_ => x
end

macro energy(v)
e = calcenergy(v)
:(@NLobjective(m, Min, $e))
end

function myModel(xctl, yctl)
m = Model()
@variable(m, y >= 0)
@variable(m, x)
@variable(m, yp)
@variable(m, xp)

@wire(x,y, xctl,yctl, 1.7)
@wire(xp, yp, 5.0,1.0, 4.5,5.0, 6.0)

@energy([w(x,y, 0,3, 1, 0.1), w(x,y, xp,yp, 5, 0.15), 0.4*y])

status = solve(m)
xval = getvalue(x)
yval = getvalue(y)
xpval = getvalue(xp)
ypval = getvalue(yp)
# print("calculate $status $xval $yval for $xctl, $yctl\n")

(status, xval, yval, xpval, ypval)
end

xctl = slider(-2:0.01:5, label="X Control")
xctlsig = signal(xctl)
yctl = slider(-1:0.01:0.5, label="Y Control")
yctlsig = signal(yctl)

ops = map(myModel, xctlsig, yctlsig)

xscale = 3
yscale = 3
shift = 1.5

pscale(x,y) = ((x*xscale+shift)cm, (y*yscale)cm)

function l(x1,y1, x2,y2; w=0.3 mm, color="black")
compose(context(),
linewidth(w), stroke(color),
line([pscale(x1, y1), pscale(x2, y2)]))
end


@manipulate for xc in xctl, yc in yctl, op in ops
compose(context(),
# text(150px, 220px, "m = $(op[2]) $(op[3])"),
# text(150px, 200px, "p = $(op[4]) $(op[5])"),
circle(pscale(op[2], op[3])..., 0.03),
circle(pscale(op[4], op[5])..., 0.01),

l(op[2], op[3], op[4], op[5], color="red"),
l(op[2], op[3], 0, 3, color="red"),

l(op[2], op[3], xc, yc),
l(op[4], op[5], 5, 1),
l(op[4], op[5], 4.5,5)
)
end


Простий спосіб запустити notebook: Julia REPL набрати
using IJulia notebook()
. Це має відкрити в браузері "http://localhost:8888/tree". Там треба вибрати «new»/«Julia», там у полі введення скопіювати код і натиснути Shift-Enter.
Джерело: Хабрахабр

0 коментарів

Тільки зареєстровані та авторизовані користувачі можуть залишати коментарі.