Мне давно хочется заняться созданием роботов, но всегда не хватает свободных денег, времени или места. По этому я собрался писать их виртуальные модели!Мощные инструменты, позволяющие это делать, либо сложно стыкуются со сторонними программами (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 d begin
[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.3mm, 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 d begin
[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.3mm, 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.Поделиться с друзьями
LonelyCruiser
А Gazebo рассматривали? Почти родной симулятор для ROS.
potan
Собирался, но ROS оказалась очень капризной в настройке и руки пока не дошли. Julia оказалась проще.
LonelyCruiser
Ну да, порог вхождения там высокий, но за то получаешь множество разных возможностей. Там визуализация, физика, зрение и т.п.