Unverified Commit d08f8dc5 authored by Justin Carpentier's avatar Justin Carpentier Committed by GitHub
Browse files

Merge pull request #9 from tkoolen/tk/julia-updates

RigidBodyDynamics.jl benchmark updates / fixes.
parents bb3f84ef 8b40373f
......@@ -16,4 +16,5 @@ $PREFIX/bin/benchmarks-pinocchio
$PREFIX/bin/benchmarks-rbdl
#$PREFIX/bin/benchmarks-kdl
#$PREFIX/bin/benchmarks-metapod
#$PREFIX/bin/benchmarks-julia.jl
#julia -O3 --check-bounds=no $PREFIX/bin/benchmarks-julia.jl
#!/usr/bin/env julia
options = Base.JLOptions()
@assert VERSION >= v"1.0.2"
@assert options.can_inline == 1
@assert options.check_bounds == 2
@assert options.opt_level == 3
using RigidBodyDynamics
using StaticArrays
using Random
using LinearAlgebra
include("models.jl")
function benchmark_julia_rnea(model, log_filename)
robot = parse_urdf(Float64, PATH * "$model.urdf")
if model != "lwr"
for joint in out_joints(root_body(robot), robot)
floatingjoint = Joint(string(joint), frame_before(joint), frame_after(joint), QuaternionFloating{Float64}())
replace_joint!(robot, joint, floatingjoint)
end
function write_times(log_filename, times)
file = open(log_filename, "w") do io
println.(Ref(io), times[2 : end]) # skip the first sample, which includes compilation time
end
return nothing
end
states = Array{MechanismState}(undef, NBT)
for i = 1:NBT
states[i] = MechanismState(robot)
rand!(states[i])
function benchmark_julia_rnea(model, log_filename)
robot = parse_urdf(PATH * "$model.urdf", floating=model != "lwr")
state = MechanismState(robot)
result = DynamicsResult(robot)
qs = [similar(configuration(state)) for _ = 1 : NBT]
vs = [similar(velocity(state)) for _ = 1 : NBT]
v̇s = [similar(velocity(state)) for _ = 1 : NBT]
τ = similar(velocity(state))
times = Vector{Int}(undef, NBT)
Random.seed!(1)
for i = 1 : NBT
rand!(state)
qs[i] .= configuration(state)
vs[i] .= velocity(state)
rand!(v̇s[i])
end
run_rnea_benchmark(state, qs, vs, v̇s, τ, result.jointwrenches, result.accelerations, times)
write_times(log_filename, times)
return nothing
end
file = open(log_filename, "w")
for i = 1:NBT
function run_rnea_benchmark(state, qs, vs, v̇s, τ, jointwrenches, accelerations, times)
# Do garbage collection of previously allocated objects now, rather than randomly
# in the middle of the benchmark.
# If garbage is generated during the benchmark run, so be it (it shouldn't in this case),
# but time spent doing garbage collection of objects allocated outside this loop shouldn't be included.
GC.gc()
for i in eachindex(qs)
debut = time_ns()
inverse_dynamics(states[i], states[i].v)
copyto!(configuration(state), qs[i])
copyto!(velocity(state), vs[i])
setdirty!(state)
= v̇s[i]
inverse_dynamics!(τ, jointwrenches, accelerations, state, )
fin = time_ns()
write(file, Int(fin - debut))
times[i] = Int(fin - debut)
end
close(file)
return nothing
end
function benchmark_julia_crba(model, log_filename)
robot = parse_urdf(Float64, PATH * "$model.urdf")
if model != "lwr"
for joint in out_joints(root_body(robot), robot)
floatingjoint = Joint(string(joint), frame_before(joint), frame_after(joint), QuaternionFloating{Float64}())
replace_joint!(robot, joint, floatingjoint)
end
robot = parse_urdf(PATH * "$model.urdf", floating=model != "lwr")
state = MechanismState(robot)
qs = [similar(configuration(state)) for _ = 1 : NBT]
times = Vector{Int}(undef, NBT)
nv = num_velocities(robot)
M = Symmetric(Matrix{Float64}(undef, nv, nv), :L)
Random.seed!(1)
for i = 1 : NBT
rand!(state)
qs[i] .= configuration(state)
end
run_crba_benchmark(state, qs, M, times)
write_times(log_filename, times)
return nothing
end
states = Array{MechanismState}(undef, NBT)
for i = 1:NBT
states[i] = MechanismState(robot)
rand!(states[i])
end
file = open(log_filename, "w")
for i = 1:NBT
function run_crba_benchmark(state, qs, M, times)
# Do garbage collection of previously allocated objects now, rather than randomly
# in the middle of the benchmark.
# If garbage is generated during the benchmark run, so be it (it shouldn't in this case),
# but time spent doing garbage collection of objects allocated outside this loop shouldn't be included.
for i in eachindex(qs)
debut = time_ns()
mass_matrix(states[i])
copyto!(configuration(state), qs[i])
setdirty!(state)
mass_matrix!(M, state)
fin = time_ns()
write(file, Int(fin - debut))
times[i] = Int(fin - debut)
end
close(file)
end
for model in MODELS
benchmark_julia_rnea(model, get_log_filename("Julia", "ID", model))
benchmark_julia_crba(model, get_log_filename("Julia", "FD", model))
benchmark_julia_crba(model, get_log_filename("Julia", "CRBA", model))
end
......@@ -2,6 +2,7 @@
using RigidBodyDynamics
using StaticArrays
using Random
include("models.jl")
......
PATH = "@CMAKE_INSTALL_PREFIX@/share/pinocchio-benchmarks/models/"
MODELS = ["@MODELS_CXX@"]
NBT = 100 * 1000
const PATH = "@CMAKE_INSTALL_PREFIX@/share/pinocchio-benchmarks/models/"
const MODELS = ["@MODELS_CXX@"]
const NBT = 100_000
function get_log_filename(lib, algo, model)
hostname = gethostname()
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment