Skip to content

Commit 3232f7a

Browse files
Larbino1Larbino1
committed
Updated RNE support for more coordinates/joints. Improved testing. Not all tests pass
1 parent 361c4cd commit 3232f7a

File tree

5 files changed

+167
-29
lines changed

5 files changed

+167
-29
lines changed

‎src/coordinates/coordinate_implementations.jl‎

Lines changed: 95 additions & 16 deletions
Original file line numberDiff line numberDiff line change
@@ -62,6 +62,13 @@ end
6262

6363
@inline __acceleration!(cache::CacheBundle, c::CMC{<:CoordDifference}) = nothing
6464

65+
function __propagate_opspace_force!(cache::CacheBundle, mc::CMC{<:CoordDifference})
66+
f_view = f_cache_view(cache, mc)
67+
f_cache_view(cache, cache[mc.coord_data.parent]) .+= f_view
68+
f_cache_view(cache, cache[mc.coord_data.child]) .-= f_view
69+
nothing
70+
end
71+
6572
function _configuration(cache::CacheBundle, c::CMC{<:CoordDifference})
6673
z_p = _configuration(cache, c.coord_data.parent)
6774
z_c = _configuration(cache, c.coord_data.child)
@@ -106,6 +113,14 @@ end
106113

107114
@inline __acceleration!(cache::CacheBundle, c::CMC{<:CoordSum}) = nothing
108115

116+
function __propagate_opspace_force!(cache::CacheBundle, mc::CMC{<:CoordSum})
117+
f_view = f_cache_view(cache, mc)
118+
f_cache_view(cache, cache[mc.coord_data.c1]) .+= f_view
119+
f_cache_view(cache, cache[mc.coord_data.c2]) .+= f_view
120+
nothing
121+
end
122+
123+
109124
function _configuration(cache::CacheBundle, c::CMC{<:CoordSum})
110125
z_1 = _configuration(cache, c.coord_data.c1)
111126
z_2 = _configuration(cache, c.coord_data.c2)
@@ -131,15 +146,16 @@ end
131146
# CoordStack
132147
####################################################################################################
133148

134-
@inline __configuration!(cache::CacheBundle, c::CMC{<:CoordStack}) = nothing
135-
@inline __velocity!(cache::CacheBundle, c::CMC{<:CoordStack}) = nothing
136-
function __jacobian!(cache::CacheBundle, c::CMC{<:CoordStack})
137-
J_ret = J_cache_view(cache, c)
149+
@inline __configuration!(cache::CacheBundle, mc::CMC{<:CoordStack}) = nothing
150+
@inline __velocity!(cache::CacheBundle, mc::CMC{<:CoordStack}) = nothing
151+
function __jacobian!(cache::CacheBundle, mc::CMC{<:CoordStack})
152+
c = mc.coord_data
153+
J_ret = J_cache_view(cache, mc)
138154
fill!(J_ret, zero(eltype(J_ret)))
139-
J_ret_1 = _vms_jacobian_result_view(cache, J_ret, c.coord_data.c1)
140-
J_ret_2 = _vms_jacobian_result_view(cache, J_ret, c.coord_data.c2)
141-
J1 = _jacobian(cache, c.coord_data.c1)
142-
J2 = _jacobian(cache, c.coord_data.c2)
155+
J_ret_1 = _vms_jacobian_result_view(cache, J_ret, c.c1)
156+
J_ret_2 = _vms_jacobian_result_view(cache, J_ret, c.c2)
157+
J1 = _jacobian(cache, c.c1)
158+
J2 = _jacobian(cache, c.c2)
143159
# Instead, do it manually
144160
Nc1 = size(J1, 1)
145161
J_ret_1[1:Nc1, :] .= J1
@@ -148,6 +164,19 @@ function __jacobian!(cache::CacheBundle, c::CMC{<:CoordStack})
148164
end
149165
@inline __acceleration!(cache::CacheBundle, c::CMC{<:CoordStack}) = nothing
150166

167+
function __propagate_opspace_force!(cache::CacheBundle, mc::CMC{<:CoordStack})
168+
f_view = f_cache_view(cache, mc)
169+
f2 = f_cache_view(cache, cache[mc.coord_data.c2])
170+
f1 = f_cache_view(cache, cache[mc.coord_data.c1])
171+
for i in 1:length(f1)
172+
f1[i] += f_view[i]
173+
end
174+
for i in 1:length(f2)
175+
f2[i] += f_view[i+length(f1)]
176+
end
177+
nothing
178+
end
179+
151180
function _configuration(cache::CacheBundle, c::CMC{<:CoordStack})
152181
z1 = _configuration(cache, c.coord_data.c1)
153182
z2 = _configuration(cache, c.coord_data.c2)
@@ -179,6 +208,15 @@ end
179208
@inline __jacobian!(cache::CacheBundle, c::CMC{<:CoordSlice}) = nothing
180209
@inline __acceleration!(cache::CacheBundle, c::CMC{<:CoordSlice}) = nothing
181210

211+
function __propagate_opspace_force!(cache::CacheBundle, mc::CMC{<:CoordSlice})
212+
f_applied = f_cache_view(cache, mc)
213+
f_ret = f_cache_view(cache, mc.coord_data.coord)
214+
for (i, idx) in enumerate(mc.coord_data.idxs)
215+
f_ret[idx] += f_applied[i]
216+
end
217+
nothing
218+
end
219+
182220
function _configuration(cache::CacheBundle, c::CMC{<:CoordSlice})
183221
_configuration(cache, c.coord_data.coord)[c.coord_data.idxs]
184222
end
@@ -206,6 +244,7 @@ function __jacobian!(cache::CacheBundle, c::CMC{<:ConstCoord})
206244
nothing
207245
end
208246
@inline __acceleration!(cache::CacheBundle, c::CMC{<:ConstCoord}) = nothing
247+
@inline __propagate_opspace_force!(cache::CacheBundle, mc::CMC{<:ConstCoord}) = nothing
209248

210249
_configuration(cache::CacheBundle, c::CMC{<:ConstCoord{Nc, Tc}}) where {Tc, Nc} = SVector{Nc, eltype(cache)}(c.coord_data.val)
211250
_velocity(cache::CacheBundle, c::CMC{<:ConstCoord{Nc, Tc}}) where {Tc, Nc} = zero(SVector{Nc, eltype(cache)})
@@ -224,6 +263,7 @@ function __jacobian!(cache::CacheBundle, c::CMC{<:ReferenceCoord})
224263
nothing
225264
end
226265
@inline __acceleration!(cache::CacheBundle, c::CMC{<:ReferenceCoord}) = nothing
266+
@inline __propagate_opspace_force!(cache::CacheBundle, mc::CMC{<:ReferenceCoord}) = nothing
227267

228268
_configuration( cache::CacheBundle, c::CMC{<:ReferenceCoord{Nc, Tc}}) where {Tc, Nc} = SVector{Nc, eltype(cache)}(c.coord_data.val[])
229269
_velocity( cache::CacheBundle, c::CMC{<:ReferenceCoord{Nc, Tc}}) where {Tc, Nc} = SVector{Nc, eltype(cache)}(c.coord_data.vel[])
@@ -246,6 +286,13 @@ function __jacobian!(cache::CacheBundle, c::CMC{<:JointSubspace})
246286
nothing
247287
end
248288
@inline __acceleration!(cache::CacheBundle, c::CMC{<:JointSubspace}) = nothing
289+
function __propagate_opspace_force!(cache::CacheBundle, mc::CMC{<:JointSubspace})
290+
f_view = f_cache_view(cache, mc)
291+
@assert length(f_view) == 1
292+
_apply_joint_force_to_frames!(cache, mc.coord_data.joint, f_view[1])
293+
nothing
294+
end
295+
249296

250297
_configuration(cache::CacheBundle, c::CMC{<:JointSubspace}) = get_q(cache, c.coord_data.joint)
251298
_velocity(cache::CacheBundle, c::CMC{<:JointSubspace}) = get_q̇(cache, c.coord_data.joint)
@@ -364,7 +411,10 @@ function __jacobian!(cache::CacheBundle, c::CMC{<:FrameOrigin})
364411
nothing
365412
end
366413
__acceleration!(cache::CacheBundle, c::CMC{<:FrameOrigin}) = nothing
367-
__propagate_opspace_force!(cache::CacheBundle, c::CMC{<:FrameOrigin}) = get_frame_forces(cache)[c.coord_data.frameID] += f_cache_view(cache, c)
414+
function __propagate_opspace_force!(cache::CacheBundle, c::CMC{<:FrameOrigin})
415+
get_frame_forces(cache)[c.coord_data.frameID] += f_cache_view(cache, c)
416+
nothing
417+
end
368418

369419
_configuration(cache::CacheBundle, c::CMC{<:FrameOrigin}) = origin(get_transform(cache, c.coord_data.frameID))
370420
_velocity(cache::CacheBundle, c::CMC{<:FrameOrigin}) = get_linear_vel(cache, c.coord_data.frameID)
@@ -439,7 +489,7 @@ function __acceleration!(cache::CacheBundle, cmc::CMC{<:QuaternionAttitude})
439489
α_cache_view(cache, cmc) .= bivector(ddrotor_tf)
440490
end
441491

442-
# function __propagate_opspace_force!(cache::CacheBundle, cmc::CMC{<:QuaternionAttitude}, f)
492+
function __propagate_opspace_force!(cache::CacheBundle, cmc::CMC{<:QuaternionAttitude})
443493
# c = cmc.coord_data
444494
# fID = c.frameID
445495

@@ -462,7 +512,7 @@ end
462512
# τ = cross(configuration(cache, cmc), f)
463513
# get_frame_torques(cache)[fID] .+= τ
464514
# nothing
465-
# end
515+
end
466516

467517
_configuration(cache::CacheBundle, cmc::CMC{<:QuaternionAttitude}) = SVector{length(cmc), eltype(cache)}(z_cache_view(cache, cmc))
468518
_velocity(cache::CacheBundle, cmc::CMC{<:QuaternionAttitude}) = SVector{length(cmc), eltype(cache)}(ż_cache_view(cache, cmc))
@@ -484,11 +534,6 @@ function __jacobian!(cache::CacheBundle, c::CMC{<:FrameAngularVelocity})
484534
end
485535
__acceleration!(cache::CacheBundle, c::CMC{<:FrameAngularVelocity}) = nothing
486536

487-
# _configuration(cache::MechanismCacheBundle, c::CMC{<:FrameAngularVelocity}) = rotor(transform(cache, c.coord_data.frameID))
488-
_velocity(cache::CacheBundle, c::CMC{<:FrameAngularVelocity}) = get_angular_vel(cache, c.coord_data.frameID)
489-
_jacobian(cache::CacheBundle, c::CMC{<:FrameAngularVelocity}) = J_cache_view(cache, c)
490-
_acceleration(cache::CacheBundle, c::CMC{<:FrameAngularVelocity}) = _get_angular_acc(cache, c.coord_data.frameID)
491-
492537
function __propagate_opspace_force!(cache::CacheBundle, mc::CMC{<:FrameAngularVelocity})
493538
c = mc.coord_data
494539
f_view = f_cache_view(cache, mc)
@@ -499,6 +544,10 @@ function __propagate_opspace_force!(cache::CacheBundle, mc::CMC{<:FrameAngularVe
499544
nothing
500545
end
501546

547+
# _configuration(cache::MechanismCacheBundle, c::CMC{<:FrameAngularVelocity}) = rotor(transform(cache, c.coord_data.frameID))
548+
_velocity(cache::CacheBundle, c::CMC{<:FrameAngularVelocity}) = get_angular_vel(cache, c.coord_data.frameID)
549+
_jacobian(cache::CacheBundle, c::CMC{<:FrameAngularVelocity}) = J_cache_view(cache, c)
550+
_acceleration(cache::CacheBundle, c::CMC{<:FrameAngularVelocity}) = _get_angular_acc(cache, c.coord_data.frameID)
502551

503552
####################################################################################################
504553
# CoordNorm
@@ -546,6 +595,20 @@ function __acceleration!(cache::CacheBundle, c::CMC{<:CoordNorm})
546595
α_cache_view(cache, c) .= αᵢ
547596
end
548597

598+
function __propagate_opspace_force!(cache::CacheBundle, mc::CMC{<:CoordNorm})
599+
c = mc.coord_data
600+
f_view = f_cache_view(cache, mc)
601+
@assert length(f_view) == 1
602+
f_ret = f_cache_view(cache, cache[c.coord])
603+
zₐ = _configuration(cache, c.coord)
604+
zᵢ = _configuration(cache, mc)
605+
for i in 1:length(f_ret)
606+
f_ret[i] += f_view[1] * zₐ[i] / zᵢ[1]
607+
end
608+
nothing
609+
end
610+
611+
549612
_configuration(cache::CacheBundle, c::CMC{<:CoordNorm}) = SVector(z_cache_view(cache, c)[1])
550613
_velocity(cache::CacheBundle, c::CMC{<:CoordNorm}) = SVector(ż_cache_view(cache, c)[1])
551614
_jacobian(cache::CacheBundle, c::CMC{<:CoordNorm}) = J_cache_view(cache, c)
@@ -626,6 +689,15 @@ function __acceleration!(cache::CacheBundle, cmc::CMC{<:RotatedCoord})
626689
nothing
627690
end
628691

692+
function __propagate_opspace_force!(cache::CacheBundle, mc::CMC{<:RotatedCoord})
693+
c = mc.coord_data
694+
f_view = f_cache_view(cache, mc)
695+
f_ret = f_cache_view(cache, cache[c.world_frame_coord])
696+
nothing
697+
end
698+
699+
700+
629701
_configuration(cache::CacheBundle, cmc::CMC{<:RotatedCoord}) = SVector{length(cmc)}(z_cache_view(cache, cmc))
630702
_velocity(cache::CacheBundle, cmc::CMC{<:RotatedCoord}) = SVector{length(cmc)}(ż_cache_view(cache, cmc))
631703
_jacobian(cache::CacheBundle, cmc::CMC{<:RotatedCoord}) = J_cache_view(cache, cmc)
@@ -711,6 +783,13 @@ function __acceleration!(cache::CacheBundle, cmc::CMC{<:UnrotatedCoord})
711783
nothing
712784
end
713785

786+
function __propagate_opspace_force!(cache::CacheBundle, mc::CMC{<:UnrotatedCoord})
787+
c = mc.coord_data
788+
f_view = f_cache_view(cache, mc)
789+
f_ret = f_cache_view(cache, cache[c.link_frame_coord])
790+
nothing
791+
end
792+
714793
_configuration(cache::CacheBundle, cmc::CMC{<:UnrotatedCoord}) = SVector{length(cmc)}(z_cache_view(cache, cmc))
715794
_velocity(cache::CacheBundle, cmc::CMC{<:UnrotatedCoord}) = SVector{length(cmc)}(ż_cache_view(cache, cmc))
716795
_jacobian(cache::MechanismCacheBundle, cmc::CMC{<:UnrotatedCoord}) = J_cache_view(cache, cmc)

‎src/coordinates/coordinates.jl‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -19,7 +19,7 @@ struct CompiledCoord{C<:CoordinateData}
1919
cache_idxs::UnitRange{Int}
2020
end
2121

22-
Base.show(io::IO, ::Type{<:CompiledCoord{C}}) where C = print(io, "CompiledCoord{$C}")
22+
# Base.show(io::IO, ::Type{<:CompiledCoord{C}}) where C = print(io, "CompiledCoord{$C}")
2323

2424
"""
2525
CompiledCoordID{C}

‎src/joints/joint_definitions.jl‎

Lines changed: 9 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -623,7 +623,12 @@ function _project_forces_to_jointspace(j::RevoluteData, tf_p::Transform{T}, tf_c
623623

624624
# The minus sign is due to the fact that the torque is expressed on the right side of the
625625
# equation
626-
τ = -dot(rotor(tf_p) * rotor(j.transform) * j.axis, τ_c)
627-
628-
return SVector(τ)
629-
end
626+
u = -dot(rotor(tf_p) * rotor(j.transform) * j.axis, τ_c)
627+
return SVector(u)
628+
end
629+
630+
function _project_forces_to_jointspace(j::PrismaticData, tf_p::Transform{T}, tf_c::Transform{T}, f_p::S, f_c::S, τ_p::S, τ_c::S) where {T, S<: SVector{3, T}}
631+
# Rotate axis to world frame, then dot with force
632+
u = -dot(rotor(tf_c) * j.axis, f_c)
633+
return SVector(u)
634+
end

‎src/joints/mechanism_joint.jl‎

Lines changed: 30 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -141,3 +141,33 @@ function q̇_idxs(j::CompiledMechanismJoint)
141141
end
142142

143143
Base.show(io::IO, ::Type{CompiledMechanismJoint{T, JD}}) where {T, JD} = print(io, "CompiledMechanismJoint($T, $JD)")
144+
145+
146+
147+
148+
######################
149+
# Apply forces to frames
150+
######################
151+
152+
# This must come after the definition of CompiledJointID... hence living here
153+
154+
function _apply_joint_force_to_frames!(bundle, jID::CompiledJointID, u::Real)
155+
# Axis in world frame
156+
mj = bundle[jID]
157+
_apply_joint_force_to_frames!(bundle, mj.jointData, mj.parentFrameID, mj.childFrameID, u)
158+
nothing
159+
end
160+
161+
162+
function _apply_joint_force_to_frames!(
163+
bundle::CacheBundle,
164+
jointData::RevoluteData,
165+
parentFrameID::CompiledFrameID,
166+
childFrameID::CompiledFrameID,
167+
u::Real)
168+
# Axis in world frame
169+
axis = rotor(get_transform(bundle, parentFrameID)) * jointData.axis
170+
get_frame_torques(bundle)[parentFrameID] -= axis * u
171+
get_frame_torques(bundle)[childFrameID] += axis * u
172+
nothing
173+
end

‎test/inverse_dynamics_test.jl‎

Lines changed: 32 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -5,26 +5,50 @@ function test_inverse_dynamics(m; N=20)
55
t = 0.0
66
q = rand!(rng, zero_q(Float64, m))
77
q̇ = rand!(rng, zero_q̇(Float64, m))
8+
q̈ = rand!(rng, zero_q̈(Float64, m))
89
gravity = randn(rng, SVector{3, Float64})
9-
_test_inverse_dynamics(m, t, q, q̇, gravity)
10+
_test_inverse_dynamics(m, t, q, q̇, q̈, gravity; rng)
1011
end
1112
end
1213

13-
function _test_inverse_dynamics(m, time, q, q̇, gravity)
14+
function _test_inverse_dynamics(m, time, q, q̇, q̈, gravity; rng=MersenneTwister(1234))
1415
u = zero_u(Float64, m)
1516

1617
icache = new_inverse_dynamics_cache(m)
17-
dcache = new_dynamics_cache(m)
18+
dcache = new_dynamics_cache(m)
1819

20+
############################
21+
# Test 1
22+
# Run forward dynamics then inverse dynamics, and compare the control input
1923
dynamics!(dcache, time, q, q̇, gravity, u)
20-
q̈ = get_q̈(dcache)
21-
inverse_dynamics!(icache, time, q, q̇, q̈, gravity)
22-
23-
# u is the control input used to compute forward dynamics, and is correct
24-
# get_u(icache) is the result of the inverse dynamics computation, and should be equal to u
24+
inverse_dynamics!(icache, time, q, q̇, get_q̈(dcache), gravity)
2525
@test get_u(icache) ≈ u atol=1e-7 rtol=1e-7
2626

2727

28+
############################
29+
# Test 2
30+
# Run inverse dynamics then forward dynamics, and compare the acceleration
31+
inverse_dynamics!(icache, time, q, q̇, q̈, gravity)
32+
dynamics!(dcache, time, q, q̇, gravity, get_u(icache))
33+
@test get_q̈(dcache) ≈ q̈ atol=1e-7 rtol=1e-7
34+
35+
############################
36+
# Test 3
37+
# Run inverse dynamics with a single coordinate and compare to jacobian transpose method
38+
VMRobotControl._inverse_dynamics_set_inputs!(icache, time, q, 0 .* q̇, 0 .* q̈, 0 * gravity)
39+
VMRobotControl._inverse_dynamics_forward_pass!(icache)
40+
for coordID in values(m.rbtree.coord_id_map)
41+
VMRobotControl._inverse_dynamics_zero!(icache)
42+
# Apply random force to the coordinate
43+
f = randn(rng, length(icache[coordID]))
44+
VMRobotControl.f_cache_view(icache, icache[coordID]) .+= f
45+
VMRobotControl._inverse_dynamics_backward_pass_a!(icache)
46+
VMRobotControl._inverse_dynamics_backward_pass_b!(icache)
47+
VMRobotControl._inverse_dynamics_backward_pass_c!(icache)
48+
VMRobotControl._inverse_dynamics_backward_pass_d!(icache)
49+
J = jacobian(dcache, coordID)
50+
@test get_u(icache) ≈ -J' * f
51+
end
2852
# if isa(m, CompiledMechanism)
2953
# elseif isa(m, CompiledVirtualMechanismSystem)
3054
# end

0 commit comments

Comments
 (0)