Моделирование кинематики — это не сложно

Мне давно хочется заняться созданием роботов, но всегда не хватает свободных денег, времени или места. По этому я собрался писать их виртуальные модели!
Мощные инструменты, позволяющие это делать, либо сложно стыкуются со сторонними программами (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.
ссылка на оригинал статьи https://habrahabr.ru/post/313138/

Добавить комментарий

Ваш адрес email не будет опубликован. Обязательные поля помечены *