Skip to content

Commit 34cfa35

Browse files
janbruedigamjanbruedigam
authored
Merge pull request #48 from janbruedigam/update_wbar
Make ωbar more consistent
2 parents 64ff2c7 + a0f1af8 commit 34cfa35

File tree

11 files changed

+142
-27
lines changed

11 files changed

+142
-27
lines changed

‎src/bounds/contact.jl‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -49,7 +49,7 @@ end
4949
@inline function ∂g∂ʳvel(contact::Contact,x2::AbstractVector, q2::UnitQuaternion, x1::AbstractVector,v1::AbstractVector, q1::UnitQuaternion,w1::AbstractVector, Δt)
5050
X, Q = ∂g∂pos(contact, x2, q2)
5151
V = X * Δt
52-
Ω = Q * Lmat(q1) * derivωbar(w1, Δt)
52+
Ω = Q * Lmat(q1) * derivωbar(w1, Δt) * Δt / 2
5353
return [V Ω]
5454
end
5555

‎src/discretization/implicit_trapezoid/integrator.jl‎

Lines changed: 5 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -9,7 +9,7 @@ getGlobalOrder() = (global METHODORDER; return METHODORDER)
99

1010
# Convenience functions
1111
@inline getx3(state::State, Δt) = state.xk[2] + state.vsol[2]*Δt
12-
@inline getq3(state::State, Δt) = state.qk[2] * ωbar(state.ωsol[2],Δt)
12+
@inline getq3(state::State, Δt) = state.qk[2] * ωbar(state.ωsol[2],Δt) * Δt / 2
1313

1414
@inline posargsc(state::State) = (state.xc, state.qc)
1515
@inline fullargsc(state::State) = (state.xc, state.vc, state.qc, state.ωc)
@@ -20,11 +20,11 @@ getGlobalOrder() = (global METHODORDER; return METHODORDER)
2020

2121
@inline function derivωbar(ω::SVector{3}, Δt)
2222
msq = -sqrt(4 / Δt^2 - dot(ω, ω))
23-
return Δt / 2 * [ω' / msq; I]
23+
return [ω' / msq; I]
2424
end
2525

2626
@inline function ωbar(ω, Δt)
27-
return Δt / 2 * UnitQuaternion(sqrt(4 / Δt^2 - dot(ω, ω)), ω, false)
27+
return UnitQuaternion(sqrt(4 / Δt^2 - dot(ω, ω)), ω, false)
2828
end
2929

3030
@inline function setForce!(state::State, F, τ)
@@ -44,7 +44,7 @@ end
4444
state.xk[1] = xc
4545
state.xk[2] = xc + vc*Δt
4646
state.qk[1] = qc
47-
state.qk[2] = qc * ωbar(ωc,Δt)
47+
state.qk[2] = qc * ωbar(ωc,Δt) * Δt / 2
4848

4949
state.Fk[1] = szeros(T,3)
5050
state.Fk[2] = szeros(T,3)
@@ -74,7 +74,7 @@ end
7474
state.xk[1] = state.xk[2]
7575
state.xk[2] = state.xk[2] + state.vsol[2]*Δt
7676
state.qk[1] = state.qk[2]
77-
state.qk[2] = state.qk[2] * ωbar(state.ωsol[2],Δt)
77+
state.qk[2] = state.qk[2] * ωbar(state.ωsol[2],Δt) * Δt / 2
7878

7979
state.xsol[2] = state.xk[2]
8080
state.qsol[2] = state.qk[2]

‎src/discretization/implicit_trapezoid/test.jl‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -11,7 +11,7 @@
1111
state.xk[1] = x1
1212
state.xk[2] = x1 + v1*Δt
1313
state.qk[1] = q1
14-
state.qk[2] = q1 * ωbar(ω1,Δt)
14+
state.qk[2] = q1 * ωbar(ω1,Δt) * Δt / 2
1515

1616
state.xsol[2] = state.xk[2]
1717
state.qsol[2] = state.qk[2]

‎src/discretization/linear/integrator.jl‎

Lines changed: 110 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,110 @@
1+
## Uses linear interpolation between the knot points. For a = b = 1/2 the integration follows the trapezoid rule.
2+
## This results in the Symplectic Euler method or Stoermer-Verlet, depending on how exactly one sets it up.
3+
# u(t) = a t + b = (xdk+1 - xdk)/Δt t + xdk
4+
# du(t) = a = (xdk+1 - xdk)/Δt
5+
# ωc(t) = 2 V L(qc)ᵀ du(t)
6+
# L(xck,vck) -> Δt (a Ld(u(0),du(0)) + b Ld(u(h),u(h)), where a + b = 1
7+
# L(qck,ωck) -> Δt (a Ld(u(0),2 V qdk† (qdk+1-qdk)/Δt) + b Ld(qdk+1,2 V qdk† (qdk+1-qdk)/Δt), where a + b = 1
8+
# ωckw = sqrt((2/Δt)^2 - ωckᵀωck) - 2/Δt
9+
# Fckᵀxck -> a Fdkᵀxdk + b Fdk+1ᵀxdk, where a + b = 1
10+
11+
METHODORDER = 1 # This refers to the interpolating spline
12+
getGlobalOrder() = (global METHODORDER; return METHODORDER)
13+
14+
# Convenience functions
15+
@inline getx3(state::State, Δt) = state.xk[1] + state.vsol[2]*Δt
16+
@inline getq3(state::State, Δt) = state.qk[1] * ωbar(state.ωsol[2],Δt) * Δt / 2
17+
18+
@inline posargsc(state::State) = (state.xc, state.qc)
19+
@inline fullargsc(state::State) = (state.xc, state.vc, state.qc, state.ωc)
20+
@inline posargsk(state::State; k=1) = (state.xk[k], state.qk[k])
21+
@inline posargssol(state::State) = (state.xsol[2], state.qsol[2])
22+
@inline fullargssol(state::State) = (state.xsol[2], state.vsol[2], state.qsol[2], state.ωsol[2])
23+
@inline posargsnext(state::State, Δt) = (getx3(state, Δt), getq3(state, Δt))
24+
25+
26+
@inline function derivωbar(ω::SVector{3}, Δt)
27+
msq = -sqrt(4 / Δt^2 - dot(ω, ω))
28+
return [ω' / msq; I]
29+
end
30+
31+
@inline function ωbar(ω, Δt)
32+
return UnitQuaternion(sqrt(4 / Δt^2 - dot(ω, ω)), ω, false)
33+
end
34+
35+
@inline function setForce!(state::State, F, τ)
36+
state.Fk[1] = F
37+
state.τk[1] = τ
38+
return
39+
end
40+
41+
42+
@inline function discretizestate!(body::Body{T}, Δt) where T
43+
state = body.state
44+
xc = state.xc
45+
qc = state.qc
46+
vc = state.vc
47+
ωc = state.ωc
48+
49+
state.xk[1] = xc + vc*Δt
50+
state.qk[1] = qc * ωbar(ωc,Δt) * Δt / 2
51+
52+
state.Fk[1] = szeros(T,3)
53+
state.τk[1] = szeros(T,3)
54+
55+
return
56+
end
57+
58+
@inline function currentasknot!(body::Body)
59+
state = body.state
60+
61+
state.xk[1] = state.xc
62+
state.qk[1] = state.qc
63+
64+
return
65+
end
66+
67+
@inline function updatestate!(body::Body{T}, Δt) where T
68+
state = body.state
69+
70+
state.xc = state.xsol[2]
71+
state.qc = state.qsol[2]
72+
state.vc = state.vsol[2]
73+
state.ωc = state.ωsol[2]
74+
75+
state.xk[1] = state.xk[1] + state.vsol[2]*Δt
76+
state.qk[1] = state.qk[1] * ωbar(state.ωsol[2],Δt) * Δt / 2
77+
78+
state.xsol[2] = state.xk[1]
79+
state.qsol[2] = state.qk[1]
80+
81+
state.Fk[1] = szeros(T,3)
82+
state.τk[1] = szeros(T,3)
83+
return
84+
end
85+
86+
@inline function setsolution!(body::Body)
87+
state = body.state
88+
state.xsol[2] = state.xk[1]
89+
state.qsol[2] = state.qk[1]
90+
state.vsol[1] = state.vc
91+
state.vsol[2] = state.vc
92+
state.ωsol[1] = state.ωc
93+
state.ωsol[2] = state.ωc
94+
return
95+
end
96+
97+
@inline function settempvars!(body::Body{T}, x, v, F, q, ω, τ, d) where T
98+
state = body.state
99+
stateold = deepcopy(state)
100+
101+
state.xc = x
102+
state.qc = q
103+
state.vc = v
104+
state.ωc = ω
105+
state.Fk[1] = F
106+
state.τk[1] = τ
107+
state.d = d
108+
109+
return stateold
110+
end

‎src/discretization/symplectic_euler/integrator.jl‎

Lines changed: 5 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -8,7 +8,7 @@ getGlobalOrder() = (global METHODORDER; return METHODORDER)
88

99
# Convenience functions
1010
@inline getx3(state::State, Δt) = state.xk[1] + state.vsol[2]*Δt
11-
@inline getq3(state::State, Δt) = state.qk[1] * ωbar(state.ωsol[2],Δt)
11+
@inline getq3(state::State, Δt) = state.qk[1] * ωbar(state.ωsol[2],Δt) * Δt / 2
1212

1313
@inline posargsc(state::State) = (state.xc, state.qc)
1414
@inline fullargsc(state::State) = (state.xc, state.vc, state.qc, state.ωc)
@@ -20,11 +20,11 @@ getGlobalOrder() = (global METHODORDER; return METHODORDER)
2020

2121
@inline function derivωbar(ω::SVector{3}, Δt)
2222
msq = -sqrt(4 / Δt^2 - dot(ω, ω))
23-
return Δt / 2 * [ω' / msq; I]
23+
return [ω' / msq; I]
2424
end
2525

2626
@inline function ωbar(ω, Δt)
27-
return Δt / 2 * UnitQuaternion(sqrt(4 / Δt^2 - dot(ω, ω)), ω, false)
27+
return UnitQuaternion(sqrt(4 / Δt^2 - dot(ω, ω)), ω, false)
2828
end
2929

3030
@inline function setForce!(state::State, F, τ)
@@ -42,7 +42,7 @@ end
4242
ωc = state.ωc
4343

4444
state.xk[1] = xc + vc*Δt
45-
state.qk[1] = qc * ωbar(ωc,Δt)
45+
state.qk[1] = qc * ωbar(ωc,Δt) * Δt / 2
4646

4747
state.Fk[1] = szeros(T,3)
4848
state.τk[1] = szeros(T,3)
@@ -68,7 +68,7 @@ end
6868
state.ωc = state.ωsol[2]
6969

7070
state.xk[1] = state.xk[1] + state.vsol[2]*Δt
71-
state.qk[1] = state.qk[1] * ωbar(state.ωsol[2],Δt)
71+
state.qk[1] = state.qk[1] * ωbar(state.ωsol[2],Δt) * Δt / 2
7272

7373
state.xsol[2] = state.xk[1]
7474
state.qsol[2] = state.qk[1]

‎src/discretization/symplectic_euler/test.jl‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -9,7 +9,7 @@
99
state.ωsol[2] = ω2
1010

1111
state.xk[1] = x1 + v1*Δt
12-
state.qk[1] = q1 * ωbar(ω1,Δt)
12+
state.qk[1] = q1 * ωbar(ω1,Δt) * Δt / 2
1313

1414
state.xsol[2] = state.xk[1]
1515
state.qsol[2] = state.qk[1]

‎src/joints/abstract_joint.jl‎

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -105,7 +105,7 @@ end
105105

106106
X, Q = ∂g∂posa(joint, x2a, q2a, x2b, q2b)
107107
V = X * Δt
108-
Ω = Q * Lmat(q1a) * derivωbar(ω1a, Δt)
108+
Ω = Q * Lmat(q1a) * derivωbar(ω1a, Δt) * Δt / 2
109109

110110
return [V Ω]
111111
end
@@ -115,7 +115,7 @@ end
115115

116116
X, Q = ∂g∂posb(joint, x2a, q2a, x2b, q2b)
117117
V = X * Δt
118-
Ω = Q * Lmat(q1b) * derivωbar(ω1b, Δt)
118+
Ω = Q * Lmat(q1b) * derivωbar(ω1b, Δt) * Δt / 2
119119

120120
return [V Ω]
121121
end
@@ -125,7 +125,7 @@ end
125125

126126
X, Q = ∂g∂posb(joint, x2b, q2b)
127127
V = X * Δt
128-
Ω = Q * Lmat(q1b) * derivωbar(ω1b, Δt)
128+
Ω = Q * Lmat(q1b) * derivωbar(ω1b, Δt) * Δt / 2
129129

130130
return [V Ω]
131131
end

‎src/main_components/body.jl‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -95,7 +95,7 @@ function ∂F∂z(body::Body{T}, Δt) where T
9595

9696
AvelT = [Z3 -I*body.m/Δt]
9797

98-
AposR = [-Rmat(ωbar(state.ωc, Δt))*LVᵀmat(state.qc) -Lmat(state.qc)*derivωbar(state.ωc, Δt)]
98+
AposR = [-Rmat(ωbar(state.ωc, Δt)*Δt/2)*LVᵀmat(state.qc) -Lmat(state.qc)*derivωbar(state.ωc, Δt)*Δt/2]
9999

100100
J = body.J
101101
ω1 = state.ωc

‎src/solver/system.jl‎

Lines changed: 6 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -266,15 +266,15 @@ function linearconstraints(mechanism::Mechanism{T,Nn,Nb}) where {T,Nn,Nb}
266266

267267
Gl[range,pcol3a12] = pGlx
268268
Gl[range,pcol3b12] = pGlx*Δt
269-
Gl[range,pcol3c12] = pGlq*Rmat(ωbar(pstate.ωsol[2],Δt))*LVᵀmat(pstate.qsol[2])
270-
Gl[range,pcol3d12] = pGlq*Lmat(pstate.qsol[2])*derivωbar(pstate.ωsol[2],Δt)
269+
Gl[range,pcol3c12] = pGlq*Rmat(ωbar(pstate.ωsol[2],Δt)*Δt/2)*LVᵀmat(pstate.qsol[2])
270+
Gl[range,pcol3d12] = pGlq*Lmat(pstate.qsol[2])*derivωbar(pstate.ωsol[2],Δt)*Δt/2
271271
Gr[range,pcol3b] = pGrx
272272
Gr[range,pcol3d] = pGrq
273273

274274
Gl[range,ccol3a12] = cGlx
275275
Gl[range,ccol3b12] = cGlx*Δt
276-
Gl[range,ccol3c12] = cGlq*Rmat(ωbar(cstate.ωsol[2],Δt))*LVᵀmat(cstate.qsol[2])
277-
Gl[range,ccol3d12] = cGlq*Lmat(cstate.qsol[2])*derivωbar(cstate.ωsol[2],Δt)
276+
Gl[range,ccol3c12] = cGlq*Rmat(ωbar(cstate.ωsol[2],Δt)*Δt/2)*LVᵀmat(cstate.qsol[2])
277+
Gl[range,ccol3d12] = cGlq*Lmat(cstate.qsol[2])*derivωbar(cstate.ωsol[2],Δt)*Δt/2
278278
Gr[range,ccol3b] = cGrx
279279
Gr[range,ccol3d] = cGrq
280280

@@ -310,8 +310,8 @@ function linearconstraints(mechanism::Mechanism{T,Nn,Nb}) where {T,Nn,Nb}
310310

311311
Gl[range,ccol3a12] = cGlx
312312
Gl[range,ccol3b12] = cGlx*Δt
313-
Gl[range,ccol3c12] = cGlq*Rmat(ωbar(cstate.ωsol[2],Δt))*LVᵀmat(cstate.qsol[2])
314-
Gl[range,ccol3d12] = cGlq*Lmat(cstate.qsol[2])*derivωbar(cstate.ωsol[2],Δt)
313+
Gl[range,ccol3c12] = cGlq*Rmat(ωbar(cstate.ωsol[2],Δt)*Δt/2)*LVᵀmat(cstate.qsol[2])
314+
Gl[range,ccol3d12] = cGlq*Lmat(cstate.qsol[2])*derivωbar(cstate.ωsol[2],Δt)*Δt/2
315315
Gr[range,ccol3b] = cGrx
316316
Gr[range,ccol3d] = cGrq
317317

‎src/util/quaternion.jl‎

Lines changed: 5 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -16,6 +16,11 @@ Lᵀmat(q) = lmult(q)'
1616
Rmat(q) = rmult(q)
1717
Rᵀmat(q) = rmult(q)'
1818

19+
# Remove once added to Rotations.jl
20+
function Base.:/(q::UnitQuaternion, w::Real)
21+
return UnitQuaternion(q.w/w, q.x/w, q.y/w, q.z/w, false)
22+
end
23+
1924
Tmat(::Type{T}=Float64) where T = tmat(T)
2025
Tᵀmat(::Type{T}=Float64) where T = tmat(T)
2126
Vmat(::Type{T}=Float64) where T = vmat(T)

0 commit comments

Comments
 (0)