Skip to content

Commit a75900f

Browse files
Larbino1Larbino1
committed
Zero all cache entries instead of using undef when constructed.
1 parent 04c1fa9 commit a75900f

File tree

5 files changed

+65
-80
lines changed

5 files changed

+65
-80
lines changed

‎src/coordinates/coordinate_cache.jl‎

Lines changed: 14 additions & 14 deletions
Original file line numberDiff line numberDiff line change
@@ -3,7 +3,7 @@ abstract type AbstractCoordinateCache{T} end
33
struct CoordKinematicsCache{T} <: AbstractCoordinateCache{T}
44
z::Vector{T}
55
function CoordKinematicsCache{T}(Nc) where T
6-
new{T}(Vector{T}(undef, Nc))
6+
new{T}(zeros(T, Nc))
77
end
88
end
99

@@ -12,8 +12,8 @@ struct CoordJacobiansCache{T} <: AbstractCoordinateCache{T}
1212
J::Matrix{T}
1313
function CoordJacobiansCache{T}(Nc, NDOF) where T
1414
new{T}(
15-
Vector{T}(undef, Nc),
16-
Matrix{T}(undef, Nc, NDOF)
15+
zeros(T, Nc),
16+
zeros(T, Nc, NDOF)
1717
)
1818
end
1919
end
@@ -24,9 +24,9 @@ struct CoordRBStatesCache{T} <: AbstractCoordinateCache{T}
2424
z̈::Vector{T}
2525
function CoordRBStatesCache{T}(Nc) where T
2626
new{T}(
27-
Vector{T}(undef, Nc),
28-
Vector{T}(undef, Nc),
29-
Vector{T}(undef, Nc)
27+
zeros(T, Nc),
28+
zeros(T, Nc),
29+
zeros(T, Nc)
3030
)
3131
end
3232
end
@@ -38,10 +38,10 @@ struct CoordRBStatesJacobianCache{T} <: AbstractCoordinateCache{T}
3838
J::Matrix{T}
3939
function CoordRBStatesJacobianCache{T}(Nc, NDOF) where T
4040
new{T}(
41-
Vector{T}(undef, Nc),
42-
Vector{T}(undef, Nc),
43-
Vector{T}(undef, Nc),
44-
Matrix{T}(undef, Nc, NDOF)
41+
zeros(T, Nc),
42+
zeros(T, Nc),
43+
zeros(T, Nc),
44+
zeros(T, Nc, NDOF)
4545
)
4646
end
4747
end
@@ -53,10 +53,10 @@ struct CoordRBStatesWrenchesCache{T} <: AbstractCoordinateCache{T}
5353
f::Vector{T}
5454
function CoordRBStatesWrenchesCache{T}(Nc) where T
5555
new{T}(
56-
Vector{T}(undef, Nc),
57-
Vector{T}(undef, Nc),
58-
Vector{T}(undef, Nc),
59-
Vector{T}(undef, Nc)
56+
zeros(T, Nc),
57+
zeros(T, Nc),
58+
zeros(T, Nc),
59+
zeros(T, Nc)
6060
)
6161
end
6262
end

‎src/coordinates/coordinate_implementations.jl‎

Lines changed: 6 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -283,11 +283,15 @@ end
283283

284284
function __jacobian!(cache::CacheBundle, mc::CMC{<:FramePoint})
285285
c = mc.coord_data
286-
Jz = _vms_jacobian_result_view(cache, J_cache_view(cache, mc), c.frameID)
286+
J_view = J_cache_view(cache, mc)
287+
288+
fill!(J_view, zero(eltype(J_view))) # TODO do this once rather than every time
289+
Jz = _vms_jacobian_result_view(cache, J_view, c.frameID)
287290
Jᵥ⁰ᵃ = get_linear_jacobian(cache, c.frameID)
288291
Jω⁰ᵃ = get_angular_jacobian(cache, c.frameID)
289292
T⁰ᵃ = get_transform(cache, c.frameID)
290293

294+
291295
pᵃ = c.point
292296
R⁰ᵃ = rotor(T⁰ᵃ)
293297
δᵒ = rotate(R⁰ᵃ, pᵃ)
@@ -305,6 +309,7 @@ function __jacobian!(cache::CacheBundle, mc::CMC{<:FramePoint})
305309
for i in axes(Jz, 1), j in axes(Jz, 2)
306310
Jz[i, j] = Jᵥ⁰ᵃ[i, j] - skew_p[i, :]' * Jω⁰ᵃ[SVector(1, 2, 3), j]
307311
end
312+
308313
nothing
309314
end
310315

‎src/mechanismcache.jl‎

Lines changed: 31 additions & 31 deletions
Original file line numberDiff line numberDiff line change
@@ -10,7 +10,7 @@ abstract type AbstractFrameCache{T} end
1010
struct FrameKinematicsCache{T} <: AbstractFrameCache{T}
1111
transforms::Vector{Transform{T}}
1212
function FrameKinematicsCache{T}(Nframes) where T
13-
transforms = Vector{Transform{T}}(undef, Nframes)
13+
transforms = zeros(Transform{T}, Nframes)
1414
new{T}(transforms)
1515
end
1616
end
@@ -19,7 +19,7 @@ struct FrameJacobiansCache{T} <: AbstractFrameCache{T}
1919
transforms::Vector{Transform{T}}
2020
jacobians::Array{T, 3}
2121
function FrameJacobiansCache{T}(Nframes, NDOF) where T
22-
transforms = Vector{Transform{T}}(undef, Nframes)
22+
transforms = zeros(Transform{T}, Nframes)
2323
jacobians = zeros(T, 6, NDOF, Nframes)
2424
new{T}(transforms, jacobians)
2525
end
@@ -28,7 +28,7 @@ end
2828
struct FrameRBStatesCache{T} <: AbstractFrameCache{T}
2929
rbstates::Vector{RigidBodyState{T}}
3030
function FrameRBStatesCache{T}(Nframes) where T
31-
rbstates = Vector{RigidBodyState{T}}(undef, Nframes)
31+
rbstates = zeros(RigidBodyState{T}, Nframes)
3232
new{T}(rbstates)
3333
end
3434
end
@@ -37,7 +37,7 @@ struct FrameRBStatesJacobianCache{T} <: AbstractFrameCache{T}
3737
rbstates::Vector{RigidBodyState{T}}
3838
jacobians::Array{T, 3}
3939
function FrameRBStatesJacobianCache{T}(Nframes, NDOF) where T
40-
rbstates = Vector{RigidBodyState{T}}(undef, Nframes)
40+
rbstates = zeros(RigidBodyState{T}, Nframes)
4141
jacobians = zeros(T, 6, NDOF, Nframes)
4242
new{T}(rbstates, jacobians)
4343
end
@@ -48,9 +48,9 @@ struct FrameRBStatesWrenchesCache{T} <: AbstractFrameCache{T}
4848
forces::Vector{SVector{3, T}}
4949
torques::Vector{SVector{3, T}}
5050
function FrameRBStatesWrenchesCache{T}(Nframes) where T
51-
rbstates = Vector{RigidBodyState{T}}(undef, Nframes)
52-
forces = Vector{SVector{3, T}}(undef, Nframes)
53-
torques = Vector{SVector{3, T}}(undef, Nframes)
51+
rbstates = zeros(RigidBodyState{T}, Nframes)
52+
forces = zeros(SVector{3, T}, Nframes)
53+
torques = zeros(SVector{3, T}, Nframes)
5454
new{T}(rbstates, forces, torques)
5555
end
5656
end
@@ -99,7 +99,7 @@ struct MechKinematicsCache{T} <: MechanismCache{T}
9999
end
100100
function MechKinematicsCache{T}(NDOF, Nf, Nc) where T
101101
t = Base.RefValue{T}(0.0)
102-
q = Vector{T}(undef, NDOF)
102+
q = zeros(T, NDOF)
103103
fcache = FrameKinematicsCache{T}(Nf)
104104
ccache = CoordKinematicsCache{T}(Nc)
105105
MechKinematicsCache(t, q, fcache, ccache)
@@ -126,7 +126,7 @@ struct MechJacobiansCache{T} <: MechanismCache{T}
126126
end
127127
function MechJacobiansCache{T}(NDOF, Nf, Nc) where T
128128
t = Base.RefValue{T}(0.0)
129-
q = Vector{T}(undef, NDOF)
129+
q = zeros(T, NDOF)
130130
fcache = FrameJacobiansCache{T}(Nf, NDOF)
131131
ccache = CoordJacobiansCache{T}(Nc, NDOF)
132132
MechJacobiansCache(t, q, fcache, ccache)
@@ -160,9 +160,9 @@ struct MechRBStatesCache{T} <: MechanismCache{T}
160160
end
161161
function MechRBStatesCache{T}(NDOF, Nf, Nc) where T
162162
t = Base.RefValue{T}(0.0)
163-
q = Vector{T}(undef, NDOF)
164-
q̇ = Vector{T}(undef, NDOF)
165-
q̈ = Vector{T}(undef, NDOF)
163+
q = zeros(T, NDOF)
164+
q̇ = zeros(T, NDOF)
165+
q̈ = zeros(T, NDOF)
166166
gravity = Base.RefValue(zero(SVector{3, T}))
167167
fcache = FrameRBStatesCache{T}(Nf)
168168
ccache = CoordRBStatesCache{T}(Nc)
@@ -213,15 +213,15 @@ struct MechDynamicsCache{T} <: MechanismCache{T}
213213
end
214214
function MechDynamicsCache{T}(NDOF, Nf, Nc) where T
215215
t = Base.RefValue{T}(0.0)
216-
q = Vector{T}(undef, NDOF)
217-
q̇ = Vector{T}(undef, NDOF)
218-
q̈ = Vector{T}(undef, NDOF)
219-
u = Vector{T}(undef, NDOF)
216+
q = zeros(T, NDOF)
217+
q̇ = zeros(T, NDOF)
218+
q̈ = zeros(T, NDOF)
219+
u = zeros(T, NDOF)
220220
gravity = Base.RefValue(zero(SVector{3, T}))
221221
inertance_matrix = zeros(T, NDOF, NDOF)
222222
inertance_matrix_workspace = zeros(T, NDOF, NDOF)
223-
generalized_force = Vector{T}(undef, NDOF)
224-
generalized_force_workspace = Vector{T}(undef, NDOF)
223+
generalized_force = zeros(T, NDOF)
224+
generalized_force_workspace = zeros(T, NDOF)
225225
fcache = FrameRBStatesJacobianCache{T}(Nf, NDOF)
226226
ccache = CoordRBStatesJacobianCache{T}(Nc, NDOF)
227227
MechDynamicsCache(t, q, q̇, q̈, gravity, u, inertance_matrix, inertance_matrix_workspace,
@@ -265,10 +265,10 @@ struct MechRNECache{T} <: MechanismCache{T}
265265
end
266266
function MechRNECache{T}(NDOF, Nf, Nc) where T
267267
t = Base.RefValue{T}(0.0)
268-
q = Vector{T}(undef, NDOF)
269-
q̇ = Vector{T}(undef, NDOF)
270-
q̈ = Vector{T}(undef, NDOF)
271-
u = Vector{T}(undef, NDOF)
268+
q = zeros(T, NDOF)
269+
q̇ = zeros(T, NDOF)
270+
q̈ = zeros(T, NDOF)
271+
u = zeros(T, NDOF)
272272
gravity = Base.RefValue(zero(SVector{3, T}))
273273
generalized_force = Vector{T}(undef, NDOF)
274274
generalized_force_workspace = Vector{T}(undef, NDOF)
@@ -722,8 +722,8 @@ struct VMSKinematicsCache{T} <: VirtualMechanismSystemCache{T}
722722
end
723723
function VMSKinematicsCache{T}(NDOF_robot, NDOF_vm, Nf_robot, Nf_vm, Nc_robot, Nc_vm, Nc) where T
724724
t = Base.RefValue{T}(0.0)
725-
qʳ = Vector{T}(undef, NDOF_robot)
726-
qᵛ = Vector{T}(undef, NDOF_vm)
725+
qʳ = zeros(T, NDOF_robot)
726+
qᵛ = zeros(T, NDOF_vm)
727727
q = (qʳ, qᵛ)
728728
robot_frame_cache = FrameKinematicsCache{T}(Nf_robot)
729729
robot_coord_cache = CoordKinematicsCache{T}(Nc_robot)
@@ -802,20 +802,20 @@ struct VMSDynamicsCache{T} <: VirtualMechanismSystemCache{T}
802802
function VMSDynamicsCache{T}(NDOF_robot, NDOF_vm, Nf_robot, Nf_vm, Nc_robot, Nc_vm, Nc) where T
803803
t = Base.RefValue{T}(0.0)
804804
NDOF = NDOF_robot + NDOF_vm
805-
q = (Vector{T}(undef, NDOF_robot), Vector{T}(undef, NDOF_vm))
806-
q̇ = (Vector{T}(undef, NDOF_robot), Vector{T}(undef, NDOF_vm))
807-
q̈ = (Vector{T}(undef, NDOF_robot), Vector{T}(undef, NDOF_vm))
808-
u = (Vector{T}(undef, NDOF_robot), Vector{T}(undef, NDOF_vm))
805+
q = (zeros(T, NDOF_robot), zeros(NDOF_vm))
806+
q̇ = (zeros(T, NDOF_robot), zeros(NDOF_vm))
807+
q̈ = (zeros(T, NDOF_robot), zeros(NDOF_vm))
808+
u = (zeros(T, NDOF_robot), zeros(NDOF_vm))
809809
gravity = Base.RefValue(zero(SVector{3, T}))
810810
robot_frame_cache = FrameRBStatesJacobianCache{T}(Nf_robot, NDOF_robot)
811811
robot_coord_cache = CoordRBStatesJacobianCache{T}(Nc_robot, NDOF_robot)
812812
vm_frame_cache = FrameRBStatesJacobianCache{T}(Nf_vm, NDOF_vm)
813813
vm_coord_cache = CoordRBStatesJacobianCache{T}(Nc_vm, NDOF_vm)
814814
coord_cache = CoordRBStatesJacobianCache{T}(Nc, NDOF) # TODO generalize
815-
inertance_matrix = (Matrix{T}(undef, NDOF_robot, NDOF_robot), Matrix{T}(undef, NDOF_vm, NDOF_vm))
815+
inertance_matrix = (zeros(T, NDOF_robot, NDOF_robot), zeros(T, NDOF_vm, NDOF_vm))
816816
inertance_matrix_workspace = (Matrix{T}(undef, NDOF_robot, NDOF_robot), Matrix{T}(undef, NDOF_vm, NDOF_vm))
817-
generalized_force = (Vector{T}(undef, NDOF_robot), Vector{T}(undef, NDOF_vm))
818-
generalized_force_workspace = (Vector{T}(undef, NDOF_robot), Vector{T}(undef, NDOF_vm))
817+
generalized_force = (zeros(T, NDOF_robot), zeros(T, NDOF_vm))
818+
generalized_force_workspace = (zeros(T, NDOF_robot), zeros(T, NDOF_vm))
819819
new{T}(t, q, q̇, q̈, u, gravity, inertance_matrix, inertance_matrix_workspace,
820820
generalized_force, generalized_force_workspace, robot_frame_cache,
821821
vm_frame_cache, robot_coord_cache, vm_coord_cache, coord_cache)

‎test/energy_test.jl‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -20,7 +20,7 @@ function test_energy(system)
2020
gravity = SVector(0.0, 0.0, -9.81)
2121
cache = new_dynamics_cache(system)
2222
f_dynamics! = get_ode_dynamics(cache, gravity)
23-
T, dt = 1.0, 1e-4
23+
T, dt = 0.5, 1e-4
2424

2525
q = zero_q(system)
2626
q̇ = zero_q̇(system)

‎test/runtests.jl‎

Lines changed: 13 additions & 33 deletions
Original file line numberDiff line numberDiff line change
@@ -1,16 +1,7 @@
1-
using Pkg
2-
3-
if (dir = splitpath(pwd())[end]) != "test" # If we are not in the test directory, cd to it
4-
if dir == "VMRobotControl.jl"
5-
Pkg.activate(".")
6-
@info "Changing directory to ./test, and activating test environment"
7-
cd("test")
8-
using TestEnv
9-
TestEnv.activate()
10-
else
11-
error("Not in correct directory for running tests. The current working directory is: '$(pwd())'. In this dir is: '$(readdir())'")
12-
end
13-
end
1+
using Pkg, TestEnv
2+
3+
4+
TestEnv.activate("VMRobotControl")
145

156
# TEST_ENZYME = "Enzyme" ∈ keys(Pkg.project().dependencies)
167
TEST_ENZYME = false
@@ -68,27 +59,10 @@ function test_on_mechanisms(test, mechanisms::Vector)
6859
end
6960
end
7061

71-
rsons = [
72-
"inertia.rson",
73-
"inerter.rson",
74-
"pendulum.rson",
75-
"robot.rson",
76-
"simple3link.rson",
77-
"planar_drill_guide.rson",
78-
"franka_panda/pandaDremelMount.rson",
79-
"franka_panda/pandaSurgical.rson",
80-
"franka_panda/pandaSurgicalSystem.rson"
81-
]
82-
83-
# urdfs = readdir("../URDFs/source")
84-
urdfs = [
85-
"../URDFs/sciurus17_description/urdf/sciurus17.urdf",
86-
"../URDFs/franka_description/urdfs/fr3.urdf"
87-
]
88-
89-
# Walk dir
62+
module_path = joinpath(splitpath(dirname(pathof(VMRobotControl)))[1:end-1])
63+
# Walk dir to find all rsons
9064
rsons = String[]
91-
for (root, dirs, files) in walkdir("../RSONs")
65+
for (root, dirs, files) in walkdir(joinpath(module_path, "RSONs"))
9266
for file in files
9367
if endswith(file, ".rson")
9468
path = joinpath(root, file)
@@ -97,6 +71,12 @@ for (root, dirs, files) in walkdir("../RSONs")
9771
end
9872
end
9973

74+
urdfs = joinpath.((module_path,), ("URDFs",), [
75+
"sciurus17_description/urdf/sciurus17.urdf",
76+
"franka_description/urdfs/fr3.urdf"
77+
])
78+
79+
10080
systems = let
10181
p = Progress(length(rsons) + length(urdfs); desc="Parsing robot files...", dt=0.1)
10282
systems = Any[]

0 commit comments

Comments
 (0)