Kinematische Modellierung ist nicht schwierig

    Ich wollte schon lange Roboter bauen, aber es gibt immer nicht genug freies Geld, Zeit oder Raum. Deshalb wollte ich ihre virtuellen Modelle schreiben!

    Leistungsstarke Tools, mit denen Sie dies tun können, sind entweder schwer mit Programmen von Drittanbietern ( Modelica ) oder proprietären Programmen ( Wolfram Mathematica, verschiedene CAD-Systeme) zu verbinden, und ich habe beschlossen, ein Fahrrad für Julia zu bauen . Dann kann die gesamte Betriebszeit mit ROS- Diensten angedockt werden .

    Wir gehen davon aus, dass sich unsere Roboter ziemlich langsam bewegen und ihre Mechanik sich in einem Zustand mit der geringsten Energie befindet, abhängig von den Einschränkungen, die durch die Konstruktion und die Servos vorgegeben sind. Daher reicht es für uns, das Optimierungsproblem in Einschränkungen zu lösen, für die wir JuMP-Pakete benötigen"(Für die nichtlineare Optimierung benötigt er das Paket" Ipopt ", das in den Abhängigkeiten nicht angegeben ist (Sie können stattdessen proprietäre Bibliotheken verwenden, ich möchte es jedoch auf" frei "beschränken) und muss separat installiert werden. Wir werden Differentialgleichungen nicht lösen, wie in Modelica, obwohl es dafür schon einige Pakete gibt, zum Beispiel " DASSL ".

    Wir werden das System mit reaktiver Programmierung (die " Reactive " -Bibliothek ) steuern . In "Notepad" (Jupyter) zeichnen , was " IJulia ", " Interact " und " Compose " erfordert. Zur Vereinfachung benötigen Sie weiterhin " MacroTools ".

    Um Pakete zu installieren, müssen Sie den Befehl in Julia REPL ausführen

    foreach(Pkg.add, ["IJulia", "Ipopt", "Interact", "Reactive", "JuMP", "Compose", "MacroTools"])
    

    Man betrachte ein einfaches zweidimensionales System, das in der Figur schematisch dargestellt ist.
    Die roten Linien kennzeichnen die Federn, die schwarzen Linien die nicht dehnbaren Seile, die kleine Schnur - der schwerelose Block, die große - die Last. Seile haben eine Länge, eine Feder hat eine Länge und Steifheit. Wir werden die variablen Koordinaten nennen:

    (x, y) - Koordinaten der Last (großer Kreis)
    (xctl, yctl) - Koordinaten des "Steuerendes" eines Seils mit einer Länge von 1,7, das an die Last gebunden ist.
    (xp, yp) - Koordinaten eines schwerelosen Punktblocks, der an einem Seil entlang gleiten kann.

    Eine Feder der Länge 0,1 und der Steifheit 1 ist bei (0,3) festgelegt und an der Last angebracht. Die zweite Feder mit einer Länge von 0,15 und einer Steifigkeit von 5 verbindet die Last und den Block, der entlang eines Seils der Länge 6 gleitet, das an den Punkten (5.1) und (4.5.5) befestigt ist.

    (Die Parameter wurden evolutionär ausgewählt, damit ich das Verhalten beim Hinzufügen neuer Funktionen klar und verständlich nachvollziehen kann.)

        @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])
    

    vollständiger Gleichungslösungsfunktionscode
    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
    


    Das Drahtmakro setzt die Einschränkung "nicht dehnbare Seilbindung" einer bestimmten Länge auf zwei Objekte oder zwei Objekte und einen Block.

    Das Energiemakro beschreibt die minimierte Energiefunktion - der spezielle Term w beschreibt die Feder (mit einer gegebenen Steifheit und Länge) und 0,4 * y ist die Belastungsenergie im Gravitationsfeld, die zu einfach ist, um eine spezielle Syntax zu finden.

    Diese Makros werden durch die Bibliothek NLconstraint und NLobjective ausgedrückt (lineare Einschränkung und Zielsetzung sind effektiver, aber sie können unsere Funktionen nicht erfüllen):

    @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)
    

    Die Verwendung von Einschränkungen von weniger als oder gleich anstelle von gleich bedeutet, dass das Seil durchhängen, aber nicht dehnen kann. Für zwei Punkte kann eine strikte Gleichheit verwendet werden, wenn es erforderlich ist, sie mit einer „starren Stange“ zu verbinden und das entsprechende Makro zu ersetzen.

    Makrocode
    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
    


    Nun beschreiben wir die Kontrollen:

    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)
    

    Verbinden wir sie mit unserem System:

    ops = map(myModel, xctlsig, yctlsig)
    

    Und dies im Notizbuch anzeigen:

    Paar Hilfsfunktionen
    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
    

    Vollversion des Codes
    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
    


    Eine einfache Möglichkeit, Notebook auszuführen: Geben Sie in Julia REPL Folgendes ein using IJulia notebook(). Dies sollte sich im Browser " http: // localhost: 8888 / tree " öffnen . Dort müssen Sie „Neu“ / „Julia“ auswählen, den Code in das Eingabefeld kopieren und die Umschalt-Eingabetaste drücken.

    Jetzt auch beliebt: