Merge pull request #17 from janbruedigam/switch_to_quaternions

Fix examples, CI

Author: janbruedigam

.github/workflows/TagBot.yml

@@ -1,11 +1,15 @@
 name: TagBot
 on:
-  schedule:
-    - cron: 0 0 * * *
+  issue_comment:
+    types:
+      - created
+  workflow_dispatch:
 jobs:
   TagBot:
+    if: github.event_name == 'workflow_dispatch' || github.actor == 'JuliaTagBot'
     runs-on: ubuntu-latest
     steps:
       - uses: JuliaRegistries/TagBot@v1
         with:
-          token: ${{ secrets.GITHUB_TOKEN }}
+          token: ${{ secrets.GITHUB_TOKEN }}
+          ssh: ${{ secrets.DOCUMENTER_KEY }}

.github/workflows/ci.yml

@@ -14,7 +14,7 @@ jobs:
       matrix:
         version:
           - '1.6'
-          - '1.7'
+          - '1'
           - 'nightly'
         os:
           - ubuntu-latest

Project.toml

@@ -6,14 +6,12 @@ version = "0.2.0"
 [deps]
 ConstrainedDynamics = "65b0d8a9-e9fd-4f02-8653-a7163c490b26"
 LinearAlgebra = "37e2e46d-f89d-539d-b4ee-838fcccc9c8e"
-Rotations = "6038ab10-8711-5258-84ad-4b1120ba62dc"
 StaticArrays = "90137ffa-7385-5640-81b9-e52037218182"
 
 [compat]
-ConstrainedDynamics = "0.8"
-Rotations = "1.1"
-StaticArrays = "0.12, 1.0"
-julia = "1.6, 1.7"
+ConstrainedDynamics = "0.9.1, 0.9.1"
+StaticArrays = "1.0, 1.5"
+julia = "1.6, 1.8"
 
 [extras]
 BenchmarkTools = "6e4b80f9-dd63-53aa-95a3-0cdb28fa8baf"

examples/lqr_acrobot.jl

@@ -2,7 +2,6 @@ using ConstrainedDynamics
 using ConstrainedDynamicsVis
 using ConstrainedControl
 using LinearAlgebra
-using Rotations
 
 
 # Parameters
@@ -32,11 +31,11 @@ constraints = [joint1;joint2]
 
 
 mech = Mechanism(origin, links, constraints, g=-9.81)
-setPosition!(origin,link1,p2 = p2a,Δq = QuatRotation(RotX(ϕ1-0.1)))
-setPosition!(link1,link2,p1=-p2a,p2 = p2b,Δq = QuatRotation(RotX(0.1)))
+setPosition!(origin,link1,p2 = p2a,Δq = RotX(ϕ1-0.1))
+setPosition!(link1,link2,p1=-p2a,p2 = p2b,Δq = RotX(0.1))
 
 xd = [[[0;0;0.5]];[[0;0;2.0]]]
-qd=[[QuatRotation(RotX(ϕ1))];[QuatRotation(RotX(ϕ2))]]
+qd=[[RotX(ϕ1)];[RotX(ϕ2)]]
 
 Q = [diagm(ones(12))*0.0 for i=1:2]
 Q[1][7,7]=4.0

examples/lqr_cartpole.jl

@@ -2,7 +2,6 @@ using ConstrainedDynamics
 using ConstrainedDynamicsVis
 using ConstrainedControl
 using LinearAlgebra
-using Rotations
 
 
 # Parameters
@@ -32,7 +31,7 @@ constraints = [joint1;joint2]
 
 mech = Mechanism(origin, links, constraints, g=-9.81)
 setPosition!(origin,cart,Δx = [0;0.5;0])
-setPosition!(cart,pole,p2 = -p2,Δq = QuatRotation(RotX(ϕ+0.2)))
+setPosition!(cart,pole,p2 = -p2,Δq = Quaternion(RotX(ϕ+0.2)))
 
 xd = [[[0;0;0.0]];[[0;0;0.5]]]
 

examples/lqr_cartpole_n_pendulum.jl

@@ -2,7 +2,6 @@ using ConstrainedDynamics
 using ConstrainedDynamicsVis
 using ConstrainedControl
 using LinearAlgebra
-using Rotations
 
 
 # Parameters
@@ -38,9 +37,9 @@ end
 
 mech = Mechanism(origin, bodies, constraints, g=-9.81)
 setPosition!(origin,cart,Δx = [0;yinit;0])
-setPosition!(cart,bodies[2],p2 = -p2,Δq = QuatRotation(RotX(ϕinit[1])))
+setPosition!(cart,bodies[2],p2 = -p2,Δq = Quaternion(RotX(ϕinit[1])))
 for i=2:N
-    setPosition!(bodies[i],bodies[i+1],p1=p2,p2=-p2,Δq=QuatRotation(RotX(ϕinit[i])))
+    setPosition!(bodies[i],bodies[i+1],p1=p2,p2=-p2,Δq=Quaternion(RotX(ϕinit[i])))
 end
 
 xd = [[[0;0;0.0]]; [[0;0;i-1+0.5] for i=1:N]]

examples/lqr_deltabot.jl

@@ -2,7 +2,6 @@ using ConstrainedDynamics
 using ConstrainedDynamicsVis
 using ConstrainedControl
 using LinearAlgebra
-using Rotations
 
 
 # Parameters
@@ -23,7 +22,7 @@ upperlegr = deepcopy(upperlegl)
 platform = Box(0.1, 0.1, length1/2*sqrt(2), length1/2*sqrt(2))
 
 # Constraints
-floorlr = EqualityConstraint(Revolute(origin, lowerlegl, joint_axis; p2=-pll),Revolute(origin, lowerlegr, joint_axis; p2=-pll),FixedOrientation(origin,platform;qoffset = QuatRotation(RotX(pi/2))))
+floorlr = EqualityConstraint(Revolute(origin, lowerlegl, joint_axis; p2=-pll),Revolute(origin, lowerlegr, joint_axis; p2=-pll),FixedOrientation(origin,platform;qoffset = Quaternion(RotX(pi/2))))
 kneel = EqualityConstraint(Revolute(lowerlegl, upperlegl, joint_axis; p1=pll, p2=-pul))
 kneer = EqualityConstraint(Revolute(lowerlegr, upperlegr, joint_axis; p1=pll, p2=-pul))
 platl = EqualityConstraint(Revolute(platform, upperlegl, joint_axis; p2=pul, p1=pp))
@@ -35,11 +34,11 @@ constraints = [platl;platr;floorlr;kneel;kneer]
 
 
 mech = Mechanism(origin, links, constraints, g=-9.81, Δt = 0.01)
-setPosition!(origin,lowerlegl,p2 = -pll,Δq = QuatRotation(RotX(pi/4)))
-setPosition!(origin,lowerlegr,p2 = -pll,Δq = QuatRotation(RotX(-pi/4)))
-setPosition!(lowerlegl,upperlegl,p1 = pll, p2 = -pul,Δq = QuatRotation(RotX(-pi/2)))
-setPosition!(lowerlegr,upperlegr,p1 = pll, p2 = -pul,Δq = QuatRotation(RotX(pi/2)))
-setPosition!(upperlegl,platform,p1 = pul, p2 = pp,Δq = QuatRotation(RotX(3pi/4)))
+setPosition!(origin,lowerlegl,p2 = -pll,Δq = Quaternion(RotX(pi/4)))
+setPosition!(origin,lowerlegr,p2 = -pll,Δq = Quaternion(RotX(-pi/4)))
+setPosition!(lowerlegl,upperlegl,p1 = pll, p2 = -pul,Δq = Quaternion(RotX(-pi/2)))
+setPosition!(lowerlegr,upperlegr,p1 = pll, p2 = -pul,Δq = Quaternion(RotX(pi/2)))
+setPosition!(upperlegl,platform,p1 = pul, p2 = pp,Δq = Quaternion(RotX(3pi/4)))
 
 xd = [links[i].state.xc for i=1:5]
 qd=[links[i].state.qc for i=1:5]
@@ -139,11 +138,11 @@ end
 ### End
 
 i = 97
-setPosition!(origin,lowerlegl,p2 = -pll,Δq = QuatRotation(RotX(anglesl[i][1])))
-setPosition!(origin,lowerlegr,p2 = -pll,Δq = QuatRotation(RotX(anglesr[i][1])))
-setPosition!(lowerlegl,upperlegl,p1 = pll, p2 = -pul,Δq = QuatRotation(RotX(anglesl[i][2])))
-setPosition!(lowerlegr,upperlegr,p1 = pll, p2 = -pul,Δq = QuatRotation(RotX(anglesr[i][2])))
-setPosition!(origin,platform,p1 = [0;A[i,:]],Δq = QuatRotation(RotX(pi/2)))
+setPosition!(origin,lowerlegl,p2 = -pll,Δq = Quaternion(RotX(anglesl[i][1])))
+setPosition!(origin,lowerlegr,p2 = -pll,Δq = Quaternion(RotX(anglesr[i][1])))
+setPosition!(lowerlegl,upperlegl,p1 = pll, p2 = -pul,Δq = Quaternion(RotX(anglesl[i][2])))
+setPosition!(lowerlegr,upperlegr,p1 = pll, p2 = -pul,Δq = Quaternion(RotX(anglesr[i][2])))
+setPosition!(origin,platform,p1 = [0;A[i,:]],Δq = Quaternion(RotX(pi/2)))
 
 storage = simulate!(mech,10,lqr,record = true)
 visualize(mech,storage)

examples/lqr_pendulum.jl

@@ -27,10 +27,10 @@ constraints = [joint_between_origin_and_link1]
 
 
 mech = Mechanism(origin, links, constraints)
-setPosition!(origin,link1,p2 = p2,Δq = QuatRotation(RotX(ϕ+pi-0.4)))
+setPosition!(origin,link1,p2 = p2,Δq = Quaternion(RotX(ϕ+pi-0.4)))
 
 xd=[[0;0.;0.5]]
-qd=[QuatRotation(RotX(ϕ+pi))]
+qd=[Quaternion(RotX(ϕ+pi))]
 
 Q = [diagm(ones(12))*0.0]
 Q[1][7,7] = 1000.0

examples/lqr_sawyer.jl

@@ -1,3 +1,4 @@
+# Currently somewhat broken
 using ConstrainedDynamics
 using ConstrainedDynamicsVis
 using ConstrainedControl
@@ -8,12 +9,12 @@ path = "examples/examples_files/sawyer_arm.urdf"
 mech = Mechanism(path, floating=false, g = 0.0)
 
 setPosition!(mech,mech.eqconstraints["right_j0"],[0.0])
-setPosition!(mech,mech.eqconstraints["right_j1"],[-pi/2])
-setPosition!(mech,mech.eqconstraints["right_j2"],[-pi/2])
-setPosition!(mech,mech.eqconstraints["right_j3"],[pi/2])
+setPosition!(mech,mech.eqconstraints["right_j1"],[0])
+setPosition!(mech,mech.eqconstraints["right_j2"],[0])
+setPosition!(mech,mech.eqconstraints["right_j3"],[0])
 
-xd=[mech.bodies[i].state.xc for i=1:length(mech.bodies)]
-qd=[mech.bodies[i].state.qc for i=1:length(mech.bodies)]
+xd=[body.state.xc for body in mech.bodies]
+qd=[body.state.qc for body in mech.bodies]
 
 setPosition!(mech,mech.eqconstraints["right_j0"],[0.0])
 setPosition!(mech,mech.eqconstraints["right_j1"],[0.0])
@@ -24,9 +25,9 @@ setPosition!(mech,mech.eqconstraints["right_j3"],[0.0])
 Q = [diagm(ones(12))*1000.0 for i=1:7]
 R = [ones(1,1) for i=1:7]
 
-ConstrainedDynamics.deactivateConstraints!(mech)
+# ConstrainedDynamics.deactivateConstraints!(mech)
 lqr = LQR(mech, getid.(mech.bodies), getid.(mech.eqconstraints), Q, R, 20., xd=xd, qd=qd)
-ConstrainedDynamics.activateConstraints!(mech)
+# ConstrainedDynamics.activateConstraints!(mech)
 
 
 storage = simulate!(mech,20,lqr,record = true)

examples/pid_doublependulum.jl

@@ -12,9 +12,9 @@ p2 = [0.0;0.0;length1 / 2] # joint connection point
 
 # Initial orientation
 ϕ1 = 0
-q1 = QuatRotation(RotX(ϕ1))
+q1 = Quaternion(RotX(ϕ1))
 ϕ2 = 0
-q2 = QuatRotation(RotX(ϕ2))
+q2 = Quaternion(RotX(ϕ2))
 
 # Links
 origin = Origin{Float64}()

examples/pid_pendulum.jl

@@ -12,7 +12,7 @@ p2 = [0.0;0.0;length1 / 2] # joint connection point
 
 # Initial orientation
 ϕ1 = 0
-q1 = QuatRotation(RotX(ϕ1))
+q1 = Quaternion(RotX(ϕ1))
 
 # Links
 origin = Origin{Float64}()

examples/trackingLQR_triple_cartpole.jl

@@ -5,8 +5,6 @@ using ConstrainedDynamics
 using ConstrainedDynamicsVis
 using ConstrainedControl
 using LinearAlgebra
-using Rotations
-using Rotations: rotation_error
 
 # Trajectory Generation from U
 
@@ -41,12 +39,12 @@ constraints = [joint1;joint2;joint3;joint4]
 
 mech = Mechanism(origin, links, constraints, g=-9.81,Δt = 0.01)
 setPosition!(origin,cart,Δx = [0;0.0;0])
-setPosition!(cart,pole1,p2 = p2,Δq = QuatRotation(RotX(ϕ+0)))
-setPosition!(pole1,pole2,p1 = -p2,p2 = p2,Δq = QuatRotation(RotX(0.)))
-setPosition!(pole2,pole3,p1 = -p2,p2 = p2,Δq = QuatRotation(RotX(0.)))
+setPosition!(cart,pole1,p2 = p2,Δq = Quaternion(RotX(ϕ+0)))
+setPosition!(pole1,pole2,p1 = -p2,p2 = p2,Δq = Quaternion(RotX(0.)))
+setPosition!(pole2,pole3,p1 = -p2,p2 = p2,Δq = Quaternion(RotX(0.)))
 
 function control!(mechanism, k)
-    setForce!(mechanism, geteqconstraint(mechanism,5), [U[k]])
+    setForce!(mechanism, joint1, [U[k]])
 end
 
 steps = Base.OneTo(1000)
@@ -59,9 +57,9 @@ simulate!(mech,storage0,control!,record = true);
 # Tracking Control
 
 setPosition!(origin,cart,Δx = [0;0.0;0])
-setPosition!(cart,pole1,p2 = p2,Δq = QuatRotation(RotX(0.)))
-setPosition!(pole1,pole2,p1 = -p2,p2 = p2,Δq = QuatRotation(RotX(0.)))
-setPosition!(pole2,pole3,p1 = -p2,p2 = p2,Δq = QuatRotation(RotX(0.)))
+setPosition!(cart,pole1,p2 = p2,Δq = Quaternion(RotX(0.)))
+setPosition!(pole1,pole2,p1 = -p2,p2 = p2,Δq = Quaternion(RotX(0.)))
+setPosition!(pole2,pole3,p1 = -p2,p2 = p2,Δq = Quaternion(RotX(0.)))
 
 Q = [diagm(ones(12))*0.0 for i=1:4]
 Q[1][2,2] = 10
@@ -77,7 +75,6 @@ R = [ones(1,1)*0.1]
 
 function owncontrol_trackinglqr!(mechanism::Mechanism{T,Nn,Nb}, lqr::TrackingLQR{T,N}, k) where {T,Nn,Nb,N}
     Δz = zeros(T,Nb*12)
-    qvm = QuatVecMap()
     for (id,body) in pairs(mechanism.bodies)
         colx = (id-1)*12+1:(id-1)*12+3
         colv = (id-1)*12+4:(id-1)*12+6
@@ -87,7 +84,9 @@ function owncontrol_trackinglqr!(mechanism::Mechanism{T,Nn,Nb}, lqr::TrackingLQR
         state = body.state
         Δz[colx] = state.xsol[2]-lqr.xd[k][id]
         Δz[colv] = state.vsol[2]-lqr.vd[k][id]
-        Δz[colq] = rotation_error(state.qsol[2],lqr.qd[k][id],qvm)
+        # Δz[colq] = rotation_error(state.qsol[2],lqr.qd[k][id],qvm)
+        qerr = lqr.qd[k][id]\state.qsol[2]
+        Δz[colq] = imag(qerr) #* sign(qerr.s)
         Δz[colω] = state.ωsol[2]-lqr.ωd[k][id]
     end
 
@@ -115,23 +114,23 @@ function owncontrol_trackinglqr!(mechanism::Mechanism{T,Nn,Nb}, lqr::TrackingLQR
     return
 end
 
-lqr = TrackingLQR(mech, storage0, [[[U[k]]] for k=1:1000], [5], Q, R, controlfunction = owncontrol_trackinglqr!)
+lqr = TrackingLQR(mech, storage0, [[[U[k]]] for k=1:1000], [joint1.id], Q, R, controlfunction = owncontrol_trackinglqr!)
 
 function uncontrol!(mechanism, k)
-    v1 = mechanism.bodies[1].state.vc[2]
-    ω2 = mechanism.bodies[2].state.ωc[1]
-    ω3 = mechanism.bodies[3].state.ωc[1] - ω2
-    ω4 = mechanism.bodies[4].state.ωc[1] - ω2 - ω3
+    v1 = cart.state.vc[2]
+    ω2 = pole1.state.ωc[1]
+    ω3 = pole2.state.ωc[1] - ω2
+    ω4 = pole3.state.ωc[1] - ω2 - ω3
 
     ucart = U[k] - sign(v1)*0.1*abs(v1) + randn()*2
     up2 = -sign(ω2)*0.1*abs(ω2)
     up3 = -sign(ω3)*0.1*abs(ω3)
     up4 = -sign(ω4)*0.1*abs(ω4)
 
-    setForce!(mechanism, geteqconstraint(mechanism,5), [ucart])
-    setForce!(mechanism, geteqconstraint(mechanism,6), [up2])
-    setForce!(mechanism, geteqconstraint(mechanism,7), [up3])
-    setForce!(mechanism, geteqconstraint(mechanism,8), [up4])
+    setForce!(mechanism, joint1, [ucart])
+    setForce!(mechanism, joint2, [up2])
+    setForce!(mechanism, joint3, [up3])
+    setForce!(mechanism, joint4, [up4])
 end
 
 

src/ConstrainedControl.jl

@@ -1,11 +1,9 @@
 module ConstrainedControl
 
 using ConstrainedDynamics
-using ConstrainedDynamics: svcat, szeros
+using ConstrainedDynamics: svcat, szeros, imag
 using LinearAlgebra
 using StaticArrays
-using Rotations
-using Rotations: rotation_error
 
 export PID,
     LQR,

src/control/lqr.jl

@@ -5,7 +5,7 @@ mutable struct LQR{T,N,NK} <: Controller
 
     xd::Vector{SVector{3,Float64}}
     vd::Vector{SVector{3,Float64}}
-    qd::Vector{QuatRotation{T}}
+    qd::Vector{Quaternion{T}}
     ωd::Vector{SVector{3,Float64}}
 
     eqcids::Vector{Integer}
@@ -50,7 +50,7 @@ mutable struct LQR{T,N,NK} <: Controller
             Q::Vector{<:AbstractMatrix{T}}, R::Vector{<:AbstractMatrix{T}}, horizon;
             xd::Vector{<:AbstractVector{T}} = [SA{T}[0; 0; 0] for i=1:Nb], 
             vd::Vector{<:AbstractVector{T}} = [SA{T}[0; 0; 0] for i=1:Nb],
-            qd::Vector{QuatRotation{T}} = [one(QuatRotation{T}) for i=1:Nb], 
+            qd::Vector{Quaternion{T}} = [one(Quaternion{T}) for i=1:Nb], 
             ωd::Vector{<:AbstractVector{T}} = [SA{T}[0; 0; 0] for i=1:Nb],
             Fτd::Vector{<:AbstractVector{T}} = [SA{T}[0] for i=1:length(eqcids)],
             controlfunction::Function = control_lqr!
@@ -88,8 +88,7 @@ end
 
 function control_lqr!(mechanism::Mechanism{T,Nn,Nb}, lqr::LQR{T,N}, k) where {T,Nn,Nb,N}
     Δz = zeros(T,Nb*12)
-    qvm = QuatVecMap()
-    for (id,body) in pairs(mechanism.bodies)
+    for (id,body) in enumerate(mechanism.bodies)
         colx = (id-1)*12+1:(id-1)*12+3
         colv = (id-1)*12+4:(id-1)*12+6
         colq = (id-1)*12+7:(id-1)*12+9
@@ -98,7 +97,9 @@ function control_lqr!(mechanism::Mechanism{T,Nn,Nb}, lqr::LQR{T,N}, k) where {T,
         state = body.state
         Δz[colx] = state.xsol[2]-lqr.xd[id]
         Δz[colv] = state.vsol[2]-lqr.vd[id]
-        Δz[colq] = rotation_error(state.qsol[2],lqr.qd[id],qvm)
+        # Δz[colq] = rotation_error(state.qsol[2],lqr.qd[id],qvm)
+        qerr = lqr.qd[id]\state.qsol[2]
+        Δz[colq] = imag(qerr) #* sign(qerr.s)
         Δz[colω] = state.ωsol[2]-lqr.ωd[id]
     end
 
@@ -114,8 +115,7 @@ end
 
 function control_lqr!(mechanism::Mechanism{T,Nn,Nb}, lqr::LQR{T,Inf}, k) where {T,Nn,Nb}
     Δz = zeros(T,Nb*12)
-    qvm = QuatVecMap()
-    for (id,body) in pairs(mechanism.bodies)
+    for (id,body) in enumerate(mechanism.bodies)
         colx = (id-1)*12+1:(id-1)*12+3
         colv = (id-1)*12+4:(id-1)*12+6
         colq = (id-1)*12+7:(id-1)*12+9
@@ -124,7 +124,9 @@ function control_lqr!(mechanism::Mechanism{T,Nn,Nb}, lqr::LQR{T,Inf}, k) where {
         state = body.state
         Δz[colx] = state.xsol[2]-lqr.xd[id]
         Δz[colv] = state.vsol[2]-lqr.vd[id]
-        Δz[colq] = rotation_error(state.qsol[2],lqr.qd[id],qvm)
+        # Δz[colq] = rotation_error(state.qsol[2],lqr.qd[id],qvm)
+        qerr = lqr.qd[id]\state.qsol[2]
+        Δz[colq] = imag(qerr) #* sign(qerr.s)
         Δz[colω] = state.ωsol[2]-lqr.ωd[id]
     end