System Analysis and Design
CAD/CAM
Mathematics
Julia
Robotics development
October 2016 25

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

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

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

+11
8.7k 67
Comments 3
Top of the day