Merge pull request #15 from tkoolen/rbd-0.5.0

Adapt things to RBD 0.5.0.
JuliaRobotics · Mar 1, 2018 · bde1e3d · bde1e3d
2 parents 987b988 + c9c9e1c
commit bde1e3d
Show file tree

Hide file tree

Showing 11 changed files with 32 additions and 30 deletions.
diff --git a/REQUIRE b/REQUIRE
@@ -1,5 +1,5 @@
 julia 0.6
-RigidBodyDynamics 0.3
-JuMP
+RigidBodyDynamics 0.5
+JuMP 0.18 0.19-
 Parameters
 Requires # for optional visualization stuff
diff --git a/src/MotionCaptureJointCalibration.jl b/src/MotionCaptureJointCalibration.jl
@@ -21,7 +21,6 @@ using JuMP
 using MathProgBase: SolverInterface.AbstractMathProgSolver
 
 import RigidBodyDynamics.Graphs: TreePath
-import RigidBodyDynamics: GenericJoint
 
 include("util.jl")
 include("problem.jl")

diff --git a/src/deconstruct.jl b/src/deconstruct.jl
@@ -1,6 +1,6 @@
 function deconstruct(ordered_marker_bodies::AbstractVector{<:RigidBody}, q::AbstractVector,
         marker_positions_body::Associative{<:RigidBody, <:AbstractVector{<:Point3D}})
-    x = copy(q)
+    x = Vector(q)
     for body in ordered_marker_bodies
         positions = marker_positions_body[body]
         for j = 1 : length(positions)

diff --git a/src/problem.jl b/src/problem.jl
@@ -1,21 +1,21 @@
 struct PoseData{T}
-    configuration::Vector{T}
+    configuration::TreeJointSegmentedVector{T}
     marker_positions::Dict{RigidBody{T}, Vector{Point3DS{T}}}
 end
 
 mutable struct CalibrationProblem{T}
     mechanism::Mechanism{T}
-    calibration_param_bounds::Dict{GenericJoint{T}, Vector{Tuple{T, T}}}
-    free_joint_configuration_bounds::Dict{GenericJoint{T}, Vector{Tuple{T, T}}}
+    calibration_param_bounds::Dict{<:Joint{T}, Vector{Tuple{T, T}}}
+    free_joint_configuration_bounds::Dict{<:Joint{T}, Vector{Tuple{T, T}}}
     marker_location_bounds::Dict{RigidBody{T}, Vector{Tuple{Point3DS{T}, Point3DS{T}}}}
     pose_data::Vector{PoseData{T}}
     body_weights::Dict{RigidBody{T}, T}
     ordered_marker_bodies::Vector{RigidBody{T}}
 
     function CalibrationProblem(
             mechanism::Mechanism{T},
-            calibration_param_bounds::Dict{GenericJoint{T}, Vector{Tuple{T, T}}},
-            free_joint_configuration_bounds::Dict{GenericJoint{T}, Vector{Tuple{T, T}}},
+            calibration_param_bounds::Dict{<:Joint{T}, Vector{Tuple{T, T}}},
+            free_joint_configuration_bounds::Dict{<:Joint{T}, Vector{Tuple{T, T}}},
             marker_location_bounds::Dict{RigidBody{T}, Vector{Tuple{Point3DS{T}, Point3DS{T}}}},
             pose_data::Vector{PoseData{T}},
             body_weights::Dict{RigidBody{T}, T} = Dict(b => one(T) for b in keys(marker_location_bounds))) where {T}

diff --git a/src/residual.jl b/src/residual.jl
@@ -43,11 +43,11 @@ function _∇marker_residual!(g::AbstractVector{T},
         marker_positions_world::Associative{RigidBody{T}, <:AbstractVector{Point3DS{T}}},
         marker_positions_body::Associative{RigidBody{T}, <:AbstractVector{Point3DS{T}}},
         body_weights::Associative{RigidBody{T}, T},
-        jacobians::Dict{RigidBody{T}, Pair{TreePath{RigidBody{T}, GenericJoint{T}}, GeometricJacobian{Matrix{T}}}}) where {T}
+        jacobians::Dict{RigidBody{T}, Pair{TreePath{RigidBody{T}, Joint{T}}, GeometricJacobian{Matrix{T}}}}) where {T}
     nq = num_positions(state)
-    ∇residual_q = view(g, 1 : nq) # TODO: allocates
+    ∇residual_q = SegmentedVector(view(g, 1 : nq), tree_joints(state.mechanism), num_positions) # TODO: allocates
     ∇residual_ps = view(g, nq + 1 : length(g)) # TODO: allocates
-    ∇residual_v = zeros(T, num_velocities(state)) # TODO: allocates
+    ∇residual_v = zeros(velocity(state)) # TODO: allocates
     mechanism = state.mechanism
     nv = num_velocities(state)
     p_index = 0

diff --git a/src/result.jl b/src/result.jl
@@ -1,8 +1,8 @@
 struct CalibrationResult{T}
     status::Symbol
     residual::T
-    calibration_params::Dict{GenericJoint{T}, Vector{T}}
-    configurations::Vector{Vector{T}}
+    calibration_params::Dict{<:Joint{T}, Vector{T}}
+    configurations::Vector{TreeJointSegmentedVector{T}}
     marker_positions::Dict{RigidBody{T}, Vector{Point3DS{T}}}
 end
 

diff --git a/src/solve.jl b/src/solve.jl
@@ -15,8 +15,8 @@ function solve(problem::CalibrationProblem{T}, solver::AbstractMathProgSolver) w
     # variables
     calibration_params = Dict(j => @variable(m, [1 : num_calibration_params(problem, j)], basename="c_$(j.name)")
         for j in calibration_joints(problem))
-    configurations = [@variable(m, [1 : num_positions(mechanism)], basename="q$i") for i = 1 : num_poses]
-    marker_positions = Dict(b => [Point3D(default_frame(b), @variable(m, [1 : 3], basename="m_$(b.name)_$i")) 
+    configurations = [copy!(similar(configuration(state), JuMP.Variable), @variable(m, [1 : num_positions(mechanism)], basename="q$i")) for i = 1 : num_poses]
+    marker_positions = Dict(b => [Point3D(default_frame(b), @variable(m, [1 : 3], basename="m_$(b.name)_$i"))
         for i = 1 : num_markers(problem, b)] for b in marker_bodies)
     @variable(m, pose_residuals[1 : num_poses])
 
@@ -34,8 +34,7 @@ function solve(problem::CalibrationProblem{T}, solver::AbstractMathProgSolver) w
         qmeasured = pose_data[i].configuration
         setvalue.(q, qmeasured)
         for joint in tree_joints(mechanism) # to fix the order
-            range = configuration_range(state, joint)
-            qjoint = q[range]
+            qjoint = q[joint]
             if joint ∈ free_joints(problem)
                 # free joint configuration bounds
                 bounds = free_joint_configuration_bounds[joint]
@@ -54,13 +53,12 @@ function solve(problem::CalibrationProblem{T}, solver::AbstractMathProgSolver) w
                 # calibration joint model
                 # TODO: generalize to handle not just offsets but also other models
                 # TODO: add redundant bounds on qjoint?
-                qjoint_measured = qmeasured[range]
+                qjoint_measured = qmeasured[joint]
                 cjoint = calibration_params[joint]
                 @constraint(m, qjoint_measured .== qjoint .+ cjoint)
             else
                 # other joints: fix at measured position
-                qjoint_measured = qmeasured[range]
-                JuMP.fix.(qjoint, qjoint_measured)
+                JuMP.fix.(qjoint, qmeasured[joint])
             end
         end
     end
@@ -111,7 +109,7 @@ function solve(problem::CalibrationProblem{T}, solver::AbstractMathProgSolver) w
     status = JuMP.solve(m)
 
     calibration_params_sol = Dict(j => getvalue.(c) for (j, c) in calibration_params)
-    configurations_sol = getvalue.(configurations)
+    configurations_sol = [copy!(similar(configuration, T), getvalue.(configuration)) for configuration in configurations]
     marker_positions_sol = Dict(b => (p -> Point3D(p.frame, SVector{3}(getvalue.(p.v)))).(positions) for (b, positions) in marker_positions)
 
     CalibrationResult(status, getobjectivevalue(m), calibration_params_sol, configurations_sol, marker_positions_sol)

diff --git a/src/synthetic.jl b/src/synthetic.jl
@@ -81,14 +81,16 @@ function generate_pose_data(
         q_ground_truth = copy(configuration(state))
         q_measured = copy(q_ground_truth)
         for (joint, offset) in ground_truth_offsets
-            @views q_measured[configuration_range(state, joint)] .+= offset
+            qjoint = q_measured[joint]
+            qjoint .+= offset
         end
         for joint in free_joints
-            qjoint = view(q_measured, configuration_range(state, joint))
+            qjoint = q_measured[joint]
             zero_configuration!(qjoint, joint)
         end
         for joint in setdiff(tree_joints(mechanism), free_joints)
-            q_measured[configuration_range(state, joint)] .+= options.joint_configuration_noise_stddev * randn(num_positions(joint))
+            qjoint = q_measured[joint]
+            qjoint .+= options.joint_configuration_noise_stddev * randn(num_positions(joint))
         end
 
         # Markers
@@ -122,11 +124,11 @@ function generate_calibration_problem(state::MechanismState{T}, body_weights::Di
     bodies = collect(keys(body_weights))
     mechanism = state.mechanism
     correction_joints = unique(flatten([collect(path(mechanism, body1, body2)) for (body1, body2) in product(bodies, bodies)]))
-    calibration_param_bounds = Dict(j => fill((-0.05, 0.05), num_positions(j)) for j in correction_joints)
-    free_joint_configuration_bounds = Dict(j => fill((-1., 1.), num_positions(j)) for j in tree_joints(mechanism) if isfloating(j))
+    calibration_param_bounds = Dict{Joint{T}, Vector{Tuple{T, T}}}(j => fill((-0.05, 0.05), num_positions(j)) for j in correction_joints)
+    free_joint_configuration_bounds = Dict{Joint{T}, Vector{Tuple{T, T}}}(j => fill((-1., 1.), num_positions(j)) for j in tree_joints(mechanism) if isfloating(j))
     free_joints = collect(keys(free_joint_configuration_bounds))
     ground_truth_marker_positions, measured_marker_positions = generate_marker_positions(bodies, marker_options)
-    ground_truth_offsets = Dict(j => generate_joint_offset(j, 1e-2) for j in correction_joints)
+    ground_truth_offsets = Dict{Joint{T}, Vector{T}}(j => generate_joint_offset(j, 1e-2) for j in correction_joints)
     ground_truth_pose_data, measured_pose_data = generate_pose_data(state, ground_truth_marker_positions, ground_truth_offsets, free_joints, pose_options)
     problem = CalibrationProblem(mechanism, calibration_param_bounds, free_joint_configuration_bounds, measured_marker_positions, measured_pose_data, body_weights)
     ground_truth = CalibrationResult(:Optimal, 0., ground_truth_offsets, [data.configuration for data in ground_truth_pose_data], ground_truth_marker_positions)

diff --git a/src/util.jl b/src/util.jl
@@ -16,3 +16,6 @@ function canonicalize!(d::Dict{RigidBody{T}, Vector{Tuple{Point3DS{T}, Point3DS{
         end
     end
 end
+
+const TreeJointSegmentedVector{T} = SegmentedVector{
+        RigidBodyDynamics.JointID, T, Base.OneTo{RigidBodyDynamics.JointID}, Vector{T}}
diff --git a/src/visualization.jl b/src/visualization.jl
@@ -23,7 +23,7 @@
             if !cal
                 q_before_cal = problem.pose_data[i].configuration
                 for joint in keys(problem.calibration_param_bounds)
-                    set_configuration!(state, joint, q_before_cal[configuration_range(state, joint)])
+                    set_configuration!(state, joint, q_before_cal[joint])
                 end
             end
             settransform!(vis, state)

diff --git a/test/runtests.jl b/test/runtests.jl
@@ -53,7 +53,7 @@ end
     f(args) = [marker_residual_inefficient(args...)]
 
     for i = 1 : 100
-        q = rand(num_positions(mechanism)) # needs to work for non-normalized quaternions as well
+        q = rand!(similar(configuration(state))) # needs to work for non-normalized quaternions as well
         Jcheck = ForwardDiff.jacobian(f, deconstruct(markerbodies, q, groundtruth.marker_positions))
 
         set_configuration!(state, q)