From 489604c40d5d8be72cfbeeece0f551433aa84416 Mon Sep 17 00:00:00 2001 From: Twan Koolen Date: Thu, 2 Aug 2018 14:01:07 -0400 Subject: [PATCH 1/3] Fix more deprecations on 0.7. --- notebooks/Derivatives and gradients using ForwardDiff.ipynb | 2 +- notebooks/Jacobian IK and Control.ipynb | 4 ++-- notebooks/README.md | 2 +- .../Rigorous error bounds using IntervalArithmetic.ipynb | 2 +- ...tion and visualization with a controller in the loop.ipynb | 2 +- src/RigidBodyDynamics.jl | 2 +- test/test_spatial.jl | 2 +- 7 files changed, 8 insertions(+), 8 deletions(-) diff --git a/notebooks/Derivatives and gradients using ForwardDiff.ipynb b/notebooks/Derivatives and gradients using ForwardDiff.ipynb index 9a964ff7..9ed13f3b 100644 --- a/notebooks/Derivatives and gradients using ForwardDiff.ipynb +++ b/notebooks/Derivatives and gradients using ForwardDiff.ipynb @@ -17,7 +17,7 @@ "using ForwardDiff\n", "using Test\n", "using Random\n", - "srand(1);" + "Random.seed!(1);" ] }, { diff --git a/notebooks/Jacobian IK and Control.ipynb b/notebooks/Jacobian IK and Control.ipynb index 24863d68..2dad739e 100644 --- a/notebooks/Jacobian IK and Control.ipynb +++ b/notebooks/Jacobian IK and Control.ipynb @@ -33,7 +33,7 @@ "\n", "# Fix the random seed, so we get repeatable results\n", "using Random\n", - "srand(42)" + "Random.seed!(42)" ] }, { @@ -65,7 +65,7 @@ } ], "source": [ - "urdf = Pkg.dir(\"RigidBodyDynamics\", \"test\", \"urdf\", \"Acrobot.urdf\")\n", + "urdf = joinpath(dirname(pathof(RigidBodyDynamics)), \"..\", \"test\", \"urdf\", \"Acrobot.urdf\")\n", "\n", "mechanism = parse_urdf(Float64, urdf)\n", "state = MechanismState(mechanism)\n", diff --git a/notebooks/README.md b/notebooks/README.md index f0e314f4..6fdeb407 100644 --- a/notebooks/README.md +++ b/notebooks/README.md @@ -11,5 +11,5 @@ You can also run the notebooks locally by performing the following steps: 3. [install IJulia](https://github.com/JuliaLang/IJulia.jl) 4. in the Julia REPL, run ``` - using IJulia; notebook(dir=Pkg.dir("RigidBodyDynamics", "notebooks")) + using IJulia, RigidBodyDynamics; notebook(dir=joinpath(dirname(pathof(RigidBodyDynamics)), "..", "notebooks")) ``` \ No newline at end of file diff --git a/notebooks/Rigorous error bounds using IntervalArithmetic.ipynb b/notebooks/Rigorous error bounds using IntervalArithmetic.ipynb index 7d929ded..30a34e03 100644 --- a/notebooks/Rigorous error bounds using IntervalArithmetic.ipynb +++ b/notebooks/Rigorous error bounds using IntervalArithmetic.ipynb @@ -304,7 +304,7 @@ ], "source": [ "const T = Interval{Float64}\n", - "const mechanism = parse_urdf(T, Pkg.dir(\"RigidBodyDynamics\", \"test\", \"urdf\", \"Acrobot.urdf\"))\n", + "const mechanism = parse_urdf(T, joinpath(dirname(pathof(RigidBodyDynamics)), \"..\", \"test\", \"urdf\", \"Acrobot.urdf\"))\n", "remove_fixed_tree_joints!(mechanism)\n", "state = MechanismState(mechanism)" ] diff --git a/notebooks/Simulation and visualization with a controller in the loop.ipynb b/notebooks/Simulation and visualization with a controller in the loop.ipynb index 707cae90..e50c4d90 100644 --- a/notebooks/Simulation and visualization with a controller in the loop.ipynb +++ b/notebooks/Simulation and visualization with a controller in the loop.ipynb @@ -58,7 +58,7 @@ } ], "source": [ - "urdf = Pkg.dir(\"RigidBodyDynamics\", \"test\", \"urdf\", \"Acrobot.urdf\")\n", + "urdf = joinpath(dirname(pathof(RigidBodyDynamics)), \"..\", \"test\", \"urdf\", \"Acrobot.urdf\")\n", "mechanism = parse_urdf(Float64, urdf)\n", "remove_fixed_tree_joints!(mechanism)\n", "mechanism" diff --git a/src/RigidBodyDynamics.jl b/src/RigidBodyDynamics.jl index 154efa4a..f94b1857 100644 --- a/src/RigidBodyDynamics.jl +++ b/src/RigidBodyDynamics.jl @@ -1,4 +1,4 @@ -__precompile__() +VERSION < v"0.7.0-beta2.199" && __precompile__() module RigidBodyDynamics diff --git a/test/test_spatial.jl b/test/test_spatial.jl index 0c25bd09..63bde3b3 100644 --- a/test/test_spatial.jl +++ b/test/test_spatial.jl @@ -205,7 +205,7 @@ end @testset "log / exp" begin hat = RigidBodyDynamics.Spatial.hat - srand(1) # TODO: https://github.com/JuliaRobotics/RigidBodyDynamics.jl/issues/135 + Random.seed!(1) # TODO: https://github.com/JuliaRobotics/RigidBodyDynamics.jl/issues/135 for θ in [LinRange(0., 10 * eps(), 100); LinRange(0., π - eps(), 100)] # have magnitude of parts of twist be bounded by θ to check for numerical issues From 9cd427a416003b3e252e8d8952e0f8a2a00fbffe Mon Sep 17 00:00:00 2001 From: Twan Koolen Date: Thu, 2 Aug 2018 14:44:09 -0400 Subject: [PATCH 2/3] Use let block to measure allocations accurately. --- test/test_mechanism_algorithms.jl | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/test/test_mechanism_algorithms.jl b/test/test_mechanism_algorithms.jl index d84cb29c..0192fc2c 100644 --- a/test/test_mechanism_algorithms.jl +++ b/test/test_mechanism_algorithms.jl @@ -301,7 +301,9 @@ end @test_throws ArgumentError point_jacobian!(J_point, x, p, transform(x, point, default_frame(base))) end point_jacobian!(J_point, x, p, point) - @test @allocated(point_jacobian!(J_point, x, p, point)) == 0 + let x = x + @test @allocated(point_jacobian!(J_point, x, p, point)) == 0 + end J = geometric_jacobian(x, p) T = Twist(J, velocity(x)) point_in_world = transform(x, point, root_frame(mechanism)) From 4fa9e51d01de49917e1010a70188e6cf29d94645 Mon Sep 17 00:00:00 2001 From: Twan Koolen Date: Thu, 2 Aug 2018 17:24:22 -0400 Subject: [PATCH 3/3] Fix center_of_mass allocations. --- src/mechanism_algorithms.jl | 14 ++++++++------ 1 file changed, 8 insertions(+), 6 deletions(-) diff --git a/src/mechanism_algorithms.jl b/src/mechanism_algorithms.jl index 437a80d4..511228de 100644 --- a/src/mechanism_algorithms.jl +++ b/src/mechanism_algorithms.jl @@ -35,11 +35,13 @@ function center_of_mass(state::MechanismState, itr) com = Point3D(frame, zero(SVector{3, T})) mass = zero(T) for body in itr - inertia = spatial_inertia(body) - if inertia.mass > 0 - bodycom = transform_to_root(state, body, false) * center_of_mass(inertia) - com += inertia.mass * FreeVector3D(bodycom) - mass += inertia.mass + if body.inertia !== nothing + inertia = spatial_inertia(body) + if inertia.mass > 0 + bodycom = transform_to_root(state, body, false) * center_of_mass(inertia) + com += inertia.mass * FreeVector3D(bodycom) + mass += inertia.mass + end end end com /= mass @@ -51,7 +53,7 @@ $(SIGNATURES) Compute the center of mass of the whole `Mechanism` in the given state. """ -center_of_mass(state::MechanismState) = center_of_mass(state, non_root_bodies(state.mechanism)) +center_of_mass(state::MechanismState) = center_of_mass(state, bodies(state.mechanism)) const geometric_jacobian_doc = """Compute a geometric Jacobian (also known as a basic, or spatial Jacobian) associated with a directed path in the `Mechanism`'s