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

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

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

    Similar posts

    AdBlock has stolen the banner, but banners are not teeth — they will be back

    More
    Ads

    Comments 3

      +1
      Мощные инструменты, позволяющие это делать, либо сложно стыкуются со сторонними программами (Modelica), либо проприетарны (Wolfram Mathematica, различные САПР)

      А Gazebo рассматривали? Почти родной симулятор для ROS.
        0
        Собирался, но ROS оказалась очень капризной в настройке и руки пока не дошли. Julia оказалась проще.
          0
          Ну да, порог вхождения там высокий, но за то получаешь множество разных возможностей. Там визуализация, физика, зрение и т.п.

      Only users with full accounts can post comments. Log in, please.