Seg fault when running Standing Controller notebook example

[adubredu]

I get a segmentation fault error at the solve(.) line when running a script that contains the standing controller example in https://github.com/tkoolen/QPControl.jl/blob/master/notebooks/Standing%20controller.ipynb

I'm using Julia 1.3.1

Here's my script:

using LinearAlgebra
using QPControl
using RigidBodyDynamics
using RigidBodyDynamics.PDControl
using RigidBodyDynamics.Contact
using StaticArrays
using AtlasRobot
using BenchmarkTools
using Test
using RigidBodySim

mechanism = AtlasRobot.mechanism()
remove_fixed_tree_joints!(mechanism);


# add environment
rootframe = root_frame(mechanism)
ground = HalfSpace3D(Point3D(rootframe, 0., 0., 0.), FreeVector3D(rootframe, 0., 0., 1.))
add_environment_primitive!(mechanism, ground);

# create optimizer
using MathOptInterface
using OSQP
using OSQP.MathOptInterfaceOSQP: OSQPSettings
const MOI = MathOptInterface
optimizer = OSQP.Optimizer()
MOI.set(optimizer, OSQPSettings.Verbose(), false)
MOI.set(optimizer, OSQPSettings.EpsAbs(), 1e-5)
MOI.set(optimizer, OSQPSettings.EpsRel(), 1e-5)
MOI.set(optimizer, OSQPSettings.MaxIter(), 5000)
MOI.set(optimizer, OSQPSettings.AdaptiveRhoInterval(), 25) # required for deterministic behavior

# create low level controller
const num_basis_vectors = 4
lowlevel = MomentumBasedController{num_basis_vectors}(mechanism, optimizer,
    floatingjoint = findjoint(mechanism, "pelvis_to_world"));
for body in bodies(mechanism)
    for point in RigidBodyDynamics.contact_points(body)
        position = location(point)
        normal = FreeVector3D(default_frame(body), 0.0, 0.0, 1.0)
        μ = point.model.friction.μ
        contact = addcontact!(lowlevel, body, position, normal, μ)
        contact.maxnormalforce[] = 1e6 # TODO
        contact.weight[] = 1e-3
    end
end

function initialize!(state::MechanismState)
    mechanism = state.mechanism
    zero!(state)
    kneebend = 1.1
    hipbendextra = 0.1
    for sideprefix in ('l', 'r')
        knee = findjoint(mechanism, "$(sideprefix)_leg_kny")
        hippitch = findjoint(mechanism, "$(sideprefix)_leg_hpy")
        anklepitch = findjoint(mechanism, "$(sideprefix)_leg_aky")
        set_configuration!(state, knee, [kneebend])
        set_configuration!(state, hippitch, [-kneebend / 2 + hipbendextra])
        set_configuration!(state, anklepitch, [-kneebend / 2 - hipbendextra])
    end
    floatingjoint = first(out_joints(root_body(mechanism), mechanism))
    set_configuration!(state, floatingjoint, [1; 0; 0; 0; 0; 0; 0.85])
    state
end


# create standing controller
feet = findbody.(Ref(mechanism), ["l_foot", "r_foot"])
pelvis = findbody(mechanism, "pelvis")
nominalstate = MechanismState(mechanism)
initialize!(nominalstate)
controller = StandingController(lowlevel, feet, pelvis, nominalstate);




state = MechanismState(mechanism)
initialize!(state)
τ = similar(velocity(state));
# benchresult = @benchmark $controller($τ, 0.0, $state)
# @show benchresult.allocs
# @test benchresult.allocs <= 24
# benchresult


using MeshCat
using MeshCatMechanisms
if !@isdefined(vis) || !any(isopen, vis.core.scope.pool.connections)
    vis = Visualizer()[:atlas]
    visuals = URDFVisuals(AtlasRobot.urdfpath(); package_path = [AtlasRobot.packagepath()])
    mvis = MechanismVisualizer(mechanism, visuals, vis)
    set_configuration!(mvis, configuration(nominalstate))
    open(mvis)
    wait(mvis)
end

state = MechanismState(mechanism)
initialize!(state)
Δt = 1 / 500
pcontroller = PeriodicController(similar(velocity(state)), Δt, controller)
# TODO: add damping
dynamics = Dynamics(mechanism, pcontroller)
problem = ODEProblem(dynamics, state, (0., 10.))

sol = solve(problem, Tsit5(), abs_tol = 1e-8, dt = 1e-6)
@time sol = solve(problem, Tsit5(), abs_tol = 1e-8, dt = 1e-6)
@test sol.retcode == :Success
copyto!(state, last(sol.u))
@test norm(velocity(state)) ≈ 0 atol=1e-8
@test center_of_mass(state).v[3] > 1


setanimation!(mvis, sol)

And here's the full error message:

signal (11): Segmentation fault
in expression starting at /home/alphonsus/research/control_garage/walker/atlasstanding.jl:105
validate_data at /home/alphonsus/.julia/packages/OSQP/FaA6U/deps/usr/lib/libosqp.so (unknown line)
osqp_setup at /home/alphonsus/.julia/packages/OSQP/FaA6U/deps/usr/lib/libosqp.so (unknown line)
#setup!#3 at /home/alphonsus/.julia/packages/OSQP/FaA6U/src/interface.jl:150
#setup! at ./none:0
_jl_invoke at /buildworker/worker/package_linux64/build/src/gf.c:2141 [inlined]
jl_apply_generic at /buildworker/worker/package_linux64/build/src/gf.c:2305
#copy_to#3 at /home/alphonsus/.julia/packages/OSQP/FaA6U/src/MOI_wrapper.jl:129
copy_to at /home/alphonsus/.julia/packages/OSQP/FaA6U/src/MOI_wrapper.jl:123 [inlined]
initialize! at /home/alphonsus/.julia/packages/Parametron/5crnA/src/model.jl:118
solve! at /home/alphonsus/.julia/packages/Parametron/5crnA/src/model.jl:153 [inlined]
MomentumBasedController at /home/alphonsus/.julia/packages/QPControl/83DBP/src/lowlevel/momentum.jl:58
StandingController at /home/alphonsus/.julia/packages/QPControl/83DBP/src/highlevel/standing.jl:87
PeriodicController at /home/alphonsus/.julia/packages/RigidBodySim/Beung/src/control.jl:143 [inlined]
Dynamics at /home/alphonsus/.julia/packages/RigidBodySim/Beung/src/core.jl:83
ODEFunction at /home/alphonsus/.julia/packages/DiffEqBase/V7P18/src/diffeqfunction.jl:248 [inlined]
initialize! at /home/alphonsus/.julia/packages/OrdinaryDiffEq/VPJBD/src/perform_step/low_order_rk_perform_step.jl:623
#__init#418 at /home/alphonsus/.julia/packages/OrdinaryDiffEq/VPJBD/src/solve.jl:406
#__init at ./none:0
#__init at ./none:0 [inlined]
#__init at ./none:0 [inlined]
#__init at ./none:0 [inlined]
#__init at ./none:0 [inlined]
#__solve#417 at /home/alphonsus/.julia/packages/OrdinaryDiffEq/VPJBD/src/solve.jl:4 [inlined]
#__solve at ./none:0
_jl_invoke at /buildworker/worker/package_linux64/build/src/gf.c:2141 [inlined]
jl_apply_generic at /buildworker/worker/package_linux64/build/src/gf.c:2305
#solve_call#459 at /home/alphonsus/.julia/packages/DiffEqBase/V7P18/src/solve.jl:92
#solve_call at ./none:0 [inlined]
#solve_up#463 at /home/alphonsus/.julia/packages/DiffEqBase/V7P18/src/solve.jl:114 [inlined]
#solve_up at ./none:0 [inlined]
#solve#462 at /home/alphonsus/.julia/packages/DiffEqBase/V7P18/src/solve.jl:102 [inlined]
#solve at ./none:0
unknown function (ip: 0x7f5fd7b396a5)
_jl_invoke at /buildworker/worker/package_linux64/build/src/gf.c:2141 [inlined]
jl_apply_generic at /buildworker/worker/package_linux64/build/src/gf.c:2305
jl_apply at /buildworker/worker/package_linux64/build/src/julia.h:1631 [inlined]
do_call at /buildworker/worker/package_linux64/build/src/interpreter.c:328
eval_value at /buildworker/worker/package_linux64/build/src/interpreter.c:417
eval_stmt_value at /buildworker/worker/package_linux64/build/src/interpreter.c:368 [inlined]
eval_body at /buildworker/worker/package_linux64/build/src/interpreter.c:778
jl_interpret_toplevel_thunk_callback at /buildworker/worker/package_linux64/build/src/interpreter.c:888
unknown function (ip: 0xfffffffffffffffe)
unknown function (ip: 0x7f5feb40c80f)
unknown function (ip: 0x5)
jl_interpret_toplevel_thunk at /buildworker/worker/package_linux64/build/src/interpreter.c:897
jl_toplevel_eval_flex at /buildworker/worker/package_linux64/build/src/toplevel.c:814
jl_parse_eval_all at /buildworker/worker/package_linux64/build/src/ast.c:873
jl_load at /buildworker/worker/package_linux64/build/src/toplevel.c:878
include at ./boot.jl:328 [inlined]
include_relative at ./loading.jl:1105
include at ./Base.jl:31
_jl_invoke at /buildworker/worker/package_linux64/build/src/gf.c:2135 [inlined]
jl_apply_generic at /buildworker/worker/package_linux64/build/src/gf.c:2305
include at ./client.jl:424
_jl_invoke at /buildworker/worker/package_linux64/build/src/gf.c:2135 [inlined]
jl_apply_generic at /buildworker/worker/package_linux64/build/src/gf.c:2305
jl_apply at /buildworker/worker/package_linux64/build/src/julia.h:1631 [inlined]
do_call at /buildworker/worker/package_linux64/build/src/interpreter.c:328
eval_value at /buildworker/worker/package_linux64/build/src/interpreter.c:417
eval_stmt_value at /buildworker/worker/package_linux64/build/src/interpreter.c:368 [inlined]
eval_body at /buildworker/worker/package_linux64/build/src/interpreter.c:778
jl_interpret_toplevel_thunk_callback at /buildworker/worker/package_linux64/build/src/interpreter.c:888
unknown function (ip: 0xfffffffffffffffe)
unknown function (ip: 0x7f5fdf91120f)
unknown function (ip: 0xffffffffffffffff)
jl_interpret_toplevel_thunk at /buildworker/worker/package_linux64/build/src/interpreter.c:897
jl_toplevel_eval_flex at /buildworker/worker/package_linux64/build/src/toplevel.c:814
jl_toplevel_eval_flex at /buildworker/worker/package_linux64/build/src/toplevel.c:764
jl_toplevel_eval_in at /buildworker/worker/package_linux64/build/src/toplevel.c:843
eval at ./boot.jl:330
_jl_invoke at /buildworker/worker/package_linux64/build/src/gf.c:2135 [inlined]
jl_apply_generic at /buildworker/worker/package_linux64/build/src/gf.c:2305
eval_user_input at /buildworker/worker/package_linux64/build/usr/share/julia/stdlib/v1.3/REPL/src/REPL.jl:86
macro expansion at /buildworker/worker/package_linux64/build/usr/share/julia/stdlib/v1.3/REPL/src/REPL.jl:118 [inlined]
#26 at ./task.jl:333
_jl_invoke at /buildworker/worker/package_linux64/build/src/gf.c:2135 [inlined]
jl_apply_generic at /buildworker/worker/package_linux64/build/src/gf.c:2305
jl_apply at /buildworker/worker/package_linux64/build/src/julia.h:1631 [inlined]
start_task at /buildworker/worker/package_linux64/build/src/task.c:659
unknown function (ip: 0xffffffffffffffff)
Allocations: 283403752 (Pool: 283061191; Big: 342561); GC: 265
Segmentation fault (core dumped)