Skip to content

Commit 08ee9ac

Browse files
janbruedigamjanbruedigam
authored
Merge pull request #21 from janbruedigam/minimal_ui
Minimal coordinate ui improvement
2 parents 4ab4e98 + eb5f40b commit 08ee9ac

14 files changed

+151
-24
lines changed

‎src/ConstrainedDynamics.jl

+1
Original file line numberDiff line numberDiff line change
@@ -104,6 +104,7 @@ include(joinpath("components", "controller.jl"))
104104
include(joinpath("solver", "sparseldu.jl"))
105105
include(joinpath("components", "mechanism.jl"))
106106
include(joinpath("components", "mechanism_functions.jl"))
107+
include(joinpath("components", "mechanism_ui.jl"))
107108
include(joinpath("components", "simulate.jl"))
108109
include(joinpath("components", "initialize.jl"))
109110
include(joinpath("solver", "solverfunctions.jl"))

‎src/components/component.jl

+15
Original file line numberDiff line numberDiff line change
@@ -17,3 +17,18 @@ deactivate!(component::Component) = (component.active = false; return)
1717

1818
isactive(component::Component) = component.active
1919
isinactive(component::Component) = !isactive(component)
20+
21+
function Base.getindex(dict::UnitDict{Base.OneTo{K},<:Component}, key::String) where K
22+
for component in dict.values
23+
component.name == key && return component
24+
end
25+
26+
return
27+
end
28+
function Base.getindex(dict::UnitDict{UnitRange{K},<:Component}, key::String) where K
29+
for component in dict.values
30+
component.name == key && return component
31+
end
32+
33+
return
34+
end

‎src/components/equalityconstraint.jl

+19-2
Original file line numberDiff line numberDiff line change
@@ -13,7 +13,7 @@ mutable struct EqualityConstraint{T,N,Nc,Cs} <: AbstractConstraint{T,N}
1313
function EqualityConstraint(data...; name::String="")
1414
jointdata = Tuple{Joint,Int64,Int64}[]
1515
for info in data
16-
if typeof(info[1]) <: Joint
16+
if info[1] isa Joint
1717
push!(jointdata, info)
1818
else
1919
for subinfo in info
@@ -53,9 +53,26 @@ mutable struct EqualityConstraint{T,N,Nc,Cs} <: AbstractConstraint{T,N}
5353
end
5454

5555

56+
function setPosition!(mechanism, eqc::EqualityConstraint, xθ; iter::Bool = true)
57+
if !iter
58+
_setPosition!(mechanism, eqc, xθ)
59+
else
60+
currentvals = minimalCoordinates(mechanism)
61+
_setPosition!(mechanism, eqc, xθ)
62+
for id in recursivedirectchildren!(mechanism.graph, eqc.id)
63+
component = getcomponent(mechanism, id)
64+
if component isa EqualityConstraint
65+
_setPosition!(mechanism, component, currentvals[id])
66+
end
67+
end
68+
end
69+
70+
return
71+
end
72+
5673
# TODO make zero alloc
5774
# TODO currently assumed constraints are in order and only joints which is the case unless very low level constraint setting
58-
function setPosition!(mechanism, eqc::EqualityConstraint{T,N,Nc}, xθ) where {T,N,Nc}
75+
function _setPosition!(mechanism, eqc::EqualityConstraint{T,N,Nc}, xθ) where {T,N,Nc}
5976
@assert length(xθ)==3*Nc-N
6077
n = Int64(Nc/2)
6178
body1 = getbody(mechanism, eqc.parentid)

‎src/components/inequalityconstraint.jl

+2-2
Original file line numberDiff line numberDiff line change
@@ -14,7 +14,7 @@ mutable struct InequalityConstraint{T,N,Cs} <: AbstractConstraint{T,N}
1414
function InequalityConstraint(data...; name::String="")
1515
bounddata = Tuple{Bound,Int64}[]
1616
for info in data
17-
if typeof(info[1]) <: Bound
17+
if info[1] isa Bound
1818
push!(bounddata, info)
1919
else
2020
for subinfo in info
@@ -121,7 +121,7 @@ end
121121
function calcFrictionForce!(mechanism, ineqc::InequalityConstraint{T,N}) where {T,N}
122122
for i = 1:N
123123
constraint = ineqc.constraints[i]
124-
if typeof(constraint) <: Friction
124+
if constraint isa Friction
125125
calcFrictionForce!(mechanism, ineqc, constraint, i, getbody(mechanism, ineqc.parentid))
126126
end
127127
end

‎src/components/mechanism.jl

+1-1
Original file line numberDiff line numberDiff line change
@@ -149,7 +149,7 @@ mutable struct Mechanism{T,N,Nb,Ne,Ni}
149149

150150
for id in graph.rdfslist # from root to leaves
151151
component = getcomponent(mechanism, id)
152-
if typeof(component) <: Body
152+
if component isa Body
153153
body = component
154154
xbodylocal = body.state.xc
155155
qbodylocal = body.state.qc

‎src/components/mechanism_functions.jl

-1
Original file line numberDiff line numberDiff line change
@@ -144,7 +144,6 @@ end
144144
return
145145
end
146146

147-
# Currently only for eqcs
148147
@inline function activate!(mechanism::Mechanism, id::Integer)
149148
component = getcomponent(mechanism, id)
150149
activate!(component)

‎src/components/mechanism_ui.jl

+95
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,95 @@
1+
function verifyConstraints!(mechanism::Mechanism)
2+
for eqc in mechanism.eqconstraints
3+
if norm(g(mechanism, eqc)) > 1e-3
4+
@info string("Bad constraint satisfaction at constraint: ", eqc.id, ", |g| = ", norm(g(mechanism, eqc)))
5+
end
6+
end
7+
return
8+
end
9+
10+
function activateConstraints!(mechanism::Mechanism)
11+
graph = mechanism.graph
12+
13+
for (id,eqc) in pairs(mechanism.eqconstraints)
14+
activate!(eqc)
15+
activate!(mechanism.graph,id)
16+
end
17+
for (id,ineqc) in pairs(mechanism.ineqconstraints)
18+
activate!(ineqc)
19+
activate!(mechanism.graph,id)
20+
end
21+
22+
return
23+
end
24+
25+
function deactivateConstraints!(mechanism::Mechanism)
26+
graph = mechanism.graph
27+
28+
for (id,eqc) in pairs(mechanism.eqconstraints)
29+
deactivate!(eqc)
30+
deactivate!(mechanism.graph,id)
31+
end
32+
for (id,ineqc) in pairs(mechanism.ineqconstraints)
33+
deactivate!(ineqc)
34+
deactivate!(mechanism.graph,id)
35+
end
36+
37+
return
38+
end
39+
40+
function nameiddict(mechanism::Mechanism)
41+
dict = Dict{String,Int64}()
42+
for (id,body) in pairs(mechanism.bodies)
43+
if body.name != ""
44+
dict[body.name] = id
45+
end
46+
end
47+
for (id,eqc) in pairs(mechanism.eqconstraints)
48+
if eqc.name != ""
49+
dict[eqc.name] = id
50+
end
51+
end
52+
for (id,ineqc) in pairs(mechanism.ineqconstraints)
53+
if ineqc.name != ""
54+
dict[ineqc.name] = id
55+
end
56+
end
57+
58+
return dict
59+
end
60+
61+
function minimalCoordinates(mechanism::Mechanism)
62+
keys = mechanism.eqconstraints.keys
63+
values = Vector{SVector}()
64+
65+
for eqc in mechanism.eqconstraints
66+
push!(values, minimalCoordinates(mechanism, eqc))
67+
end
68+
69+
return UnitDict(keys, values)
70+
71+
end
72+
73+
function setPosition!(mechanism::Mechanism, dict::UnitDict)
74+
for (id,eqc) in pairs(mechanism.eqconstraints)
75+
setPosition!(mechanism, eqc, dict[id])
76+
end
77+
78+
return
79+
end
80+
81+
function setVelocity!(mechanism::Mechanism, dict::UnitDict)
82+
for (id,eqc) in pairs(mechanism.eqconstraints)
83+
setVelocity!(mechanism, eqc, dict[id])
84+
end
85+
86+
return
87+
end
88+
89+
function setForce!(mechanism::Mechanism, dict::UnitDict)
90+
for (id,eqc) in pairs(mechanism.eqconstraints)
91+
setVelocity!(mechanism, eqc, dict[id])
92+
end
93+
94+
return
95+
end

‎src/components/simulate.jl

-9
Original file line numberDiff line numberDiff line change
@@ -10,15 +10,6 @@ function saveToStorage!(mechanism::Mechanism, storage::Storage, i)
1010
return
1111
end
1212

13-
function verifyConstraints!(mechanism::Mechanism)
14-
for eqc in mechanism.eqconstraints
15-
if norm(g(mechanism, eqc)) > 1e-3
16-
@info string("Bad constraint satisfaction at constraint: ", eqc.id, ", |g| = ", norm(g(mechanism, eqc)))
17-
end
18-
end
19-
return
20-
end
21-
2213
function initializeSimulation!(mechanism::Mechanism, debug::Bool)
2314
discretizestate!(mechanism)
2415
debug && verifyConstraints!(mechanism)

‎src/solver/solverfunctions.jl

+1-1
Original file line numberDiff line numberDiff line change
@@ -176,7 +176,7 @@ function factor!(graph::Graph, ldu::SparseLDU)
176176
return
177177
end
178178

179-
function solve!(mechanism)
179+
function solve!(mechanism::Mechanism)
180180
ldu = mechanism.ldu
181181
graph = mechanism.graph
182182
dfslist = graph.dfslist

‎src/solver/system.jl

+5-5
Original file line numberDiff line numberDiff line change
@@ -41,7 +41,7 @@ function densesystem(mechanism::Mechanism{T,N,Nb}) where {T,N,Nb}
4141
x[range] = diagonal.Δs
4242

4343
# b
44-
if typeof(component) <:Body
44+
if component isa Body
4545
b[range] = dynamics(mechanism, component)
4646
else
4747
b[range] = g(mechanism, component)
@@ -106,7 +106,7 @@ function ∂g∂ʳextension(mechanism::Mechanism{T,N,Nb}) where {T,N,Nb}
106106
cGr = ∂g∂ʳposb(eqc.constraints[i], posargssol(pstate)..., posargssol(cstate)...) # x2
107107
cXr, cQr = cGr[:,1:3], cGr[:,4:6]
108108

109-
if typeof(eqc.constraints[i])<:Joint{T,2}
109+
if eqc.constraints[i] isa Joint{T,2}
110110
pXl = eqc.constraints[i].V12 * pXl
111111
pQl = eqc.constraints[i].V12 * pQl
112112
pXr = eqc.constraints[i].V12 * pXr
@@ -115,7 +115,7 @@ function ∂g∂ʳextension(mechanism::Mechanism{T,N,Nb}) where {T,N,Nb}
115115
cQl = eqc.constraints[i].V12 * cQl
116116
cXr = eqc.constraints[i].V12 * cXr
117117
cQr = eqc.constraints[i].V12 * cQr
118-
elseif typeof(eqc.constraints[i])<:Joint{T,3}
118+
elseif eqc.constraints[i] isa Joint{T,3}
119119
pXl = convert(SMatrix{3,3,T,9}, pXl)
120120
pQl = convert(SMatrix{3,4,T,12}, pQl)
121121
cXl = convert(SMatrix{3,3,T,9}, cXl)
@@ -170,12 +170,12 @@ function ∂g∂ʳextension(mechanism::Mechanism{T,N,Nb}) where {T,N,Nb}
170170
cGr = ∂g∂ʳposb(eqc.constraints[i], posargssol(cstate)...) # x2
171171
cXr, cQr = cGr[:,1:3], cGr[:,4:6]
172172

173-
if typeof(eqc.constraints[i])<:Joint{T,2}
173+
if eqc.constraints[i] isa Joint{T,2}
174174
cXl = eqc.constraints[i].V12 * cXl
175175
cQl = eqc.constraints[i].V12 * cQl
176176
cXr = eqc.constraints[i].V12 * cXr
177177
cQr = eqc.constraints[i].V12 * cQr
178-
elseif typeof(eqc.constraints[i])<:Joint{T,3}
178+
elseif eqc.constraints[i] isa Joint{T,3}
179179
cXl = convert(SMatrix{3,3,T,9}, cXl)
180180
cQl = convert(SMatrix{3,4,T,12}, cQl)
181181
else

‎src/util/graph.jl

+9
Original file line numberDiff line numberDiff line change
@@ -255,6 +255,15 @@ function connections(dfslist, adjacency, dict::Dict)
255255
return cons
256256
end
257257

258+
function recursivedirectchildren!(graph, id::Integer)
259+
dirs = copy(directchildren(graph, id))
260+
dirslocal = copy(dirs)
261+
for childid in dirslocal
262+
append!(dirs, recursivedirectchildren!(graph, childid))
263+
end
264+
return dirs
265+
end
266+
258267

259268
@inline directchildren(graph, id::Integer) = graph.directchildren[graph.dict[id]]
260269
@inline loopchildren(graph, id::Integer) = graph.loopchildren[graph.dict[id]]

‎test/initialization/fixed_joint_test.jl

+1-1
Original file line numberDiff line numberDiff line change
@@ -31,7 +31,7 @@ for i=1:10
3131

3232
mech = Mechanism(origin, links, constraints, g = 0., shapes = shapes)
3333

34-
setPosition!(mech,joint1,SA_F64[])
34+
setPosition!(mech,joint1,SA_F64[]; iter=false)
3535
setVelocity!(mech,joint1,SA_F64[])
3636
setForce!(mech,joint1,SA_F64[])
3737

‎test/initialization/prismatic_joint_test.jl

+1-1
Original file line numberDiff line numberDiff line change
@@ -37,7 +37,7 @@ for i=1:10
3737
vω = rand(1)
3838
Fτ = rand(1)
3939

40-
setPosition!(mech,joint1,xθ)
40+
setPosition!(mech,joint1,xθ;iter=false)
4141
setVelocity!(mech,joint1,vω)
4242
setForce!(mech,joint1,Fτ)
4343

‎test/initialization/revolute_joint_test.jl

+1-1
Original file line numberDiff line numberDiff line change
@@ -35,7 +35,7 @@ for i=1:10
3535
vω = rand(1)
3636
Fτ = rand(1)
3737

38-
setPosition!(mech,joint1,xθ)
38+
setPosition!(mech,joint1,xθ;iter=false)
3939
setVelocity!(mech,joint1,vω)
4040
setForce!(mech,joint1,Fτ)
4141

0 commit comments

Comments
 (0)