Code Monkey home page Code Monkey logo

qpcontrol.jl's Introduction

QPControl

Build Status codecov.io

qpcontrol.jl's People

Contributors

juliatagbot avatar rdeits avatar tkoolen avatar

Stargazers

avatar avatar avatar avatar avatar avatar avatar avatar avatar avatar avatar avatar avatar avatar avatar avatar

Watchers

avatar avatar avatar avatar avatar

Forkers

rdeits

qpcontrol.jl's Issues

Seg fault when running Standing Controller notebook example

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)

Performance improvement opportunities

  • #21 made the standing controller a little bit slower. Could special-case QuaternionFloating (skip premultiplying the constraint by the full-rank matrix S^T).
  • improve handling of sum of many terms in SimpleQP for wrench balance constraint
  • don't call modifyconstraint! for Constraints that have a fixed LazyExpression in SimpleQP.

Recommend Projects

  • React photo React

    A declarative, efficient, and flexible JavaScript library for building user interfaces.

  • Vue.js photo Vue.js

    🖖 Vue.js is a progressive, incrementally-adoptable JavaScript framework for building UI on the web.

  • Typescript photo Typescript

    TypeScript is a superset of JavaScript that compiles to clean JavaScript output.

  • TensorFlow photo TensorFlow

    An Open Source Machine Learning Framework for Everyone

  • Django photo Django

    The Web framework for perfectionists with deadlines.

  • D3 photo D3

    Bring data to life with SVG, Canvas and HTML. 📊📈🎉

Recommend Topics

  • javascript

    JavaScript (JS) is a lightweight interpreted programming language with first-class functions.

  • web

    Some thing interesting about web. New door for the world.

  • server

    A server is a program made to process requests and deliver data to clients.

  • Machine learning

    Machine learning is a way of modeling and interpreting data that allows a piece of software to respond intelligently.

  • Game

    Some thing interesting about game, make everyone happy.

Recommend Org

  • Facebook photo Facebook

    We are working to build community through open source technology. NB: members must have two-factor auth.

  • Microsoft photo Microsoft

    Open source projects and samples from Microsoft.

  • Google photo Google

    Google ❤️ Open Source for everyone.

  • D3 photo D3

    Data-Driven Documents codes.