Мне давно хочется заняться созданием роботов, но всегда не хватает свободных денег, времени или места. По этому я собрался писать их виртуальные модели!

Мощные инструменты, позволяющие это делать, либо сложно стыкуются со сторонними программами (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.