Merge pull request #69 from janbruedigam/fix_linearization

Fix linearization

Author: janbruedigam

Project.toml

@@ -18,7 +18,7 @@ StaticArrays = "90137ffa-7385-5640-81b9-e52037218182"
 Colors = "<0.12.10, 0.12.10"
 DocStringExtensions = "<0.9.3, 0.9.3"
 ForwardDiff = "<0.10.34, 0.10.34"
-GraphBasedSystems = "1.0, 1.1"
+GraphBasedSystems = "1.0, 1.2"
 LightXML = "<0.9.0, 0.9.0"
 Quaternions = "0.7.0, 0.7.4"
 Rotations = "1.0, 1.3"

src/main_components/system.jl

@@ -3,7 +3,7 @@ function create_system(origin::Origin{T}, eqconstraints::Vector{<:EqualityConstr
     ) where T
 
     adjacency, dims = adjacencyMatrix(eqconstraints, bodies, frictions, ineqconstraints)
-    system = System{T}(adjacency, dims)
+    system = System{T}(adjacency, dims, force_static = true)
 
     for eqc in eqconstraints
         eqc.parentid == origin.id && (eqc.parentid = nothing)

src/optional_components/linearization.jl

@@ -31,18 +31,18 @@ function linearsystem(mechanism::Mechanism{T,Nn,Ne,Nb}, xθd, vωd, Fτd, contro
     ωd = [szeros(T,3) for i=1:Nb]
 
     # store old state and set new initial state
-    for (id,body) in pairs(mechanism.bodies)
-        statesold[id] = deepcopy(body.state)
+    for (i,body) in enumerate(mechanism.bodies)
+        statesold[i] = deepcopy(body.state)
     end
     for (i,id) in enumerate(controlledids)
         setPosition!(mechanism, geteqconstraint(mechanism, id), [xθd[i]])
     end
-    for (id,body) in pairs(mechanism.bodies)
+    for (i,body) in enumerate(mechanism.bodies)
         state = body.state
-        xd[id] = state.xc
-        vd[id] = state.vc
-        qd[id] = state.qc
-        ωd[id] = state.ωc
+        xd[i] = state.xc
+        vd[i] = state.vc
+        qd[i] = state.qc
+        ωd[i] = state.ωc
     end
     for (i,id) in enumerate(controlids)
         setForce!(mechanism, geteqconstraint(mechanism, id), [Fτd[i]])
@@ -51,8 +51,8 @@ function linearsystem(mechanism::Mechanism{T,Nn,Ne,Nb}, xθd, vωd, Fτd, contro
     A, Bu, Bλ, G = lineardynamics(mechanism, controlids)
 
     # restore old state
-    for (id,body) in pairs(mechanism.bodies)
-        body.state = statesold[id]
+    for (i,body) in enumerate(mechanism.bodies)
+        body.state = statesold[i]
     end
 
     return A, Bu, Bλ, G, xd, vd, qd, ωd