Merge pull request #232 from tkoolen/fix-211

Fix #211 (Erroneous contact resets in `contact_dynamics!`)

src/contact.jl

@@ -244,5 +244,6 @@ type ContactEnvironment{T}
 end
 
 Base.push!(environment::ContactEnvironment, halfspace::HalfSpace3D) = push!(environment.halfspaces, halfspace)
+Base.length(environment::ContactEnvironment) = length(environment.halfspaces)
 
 end

src/dynamics_result.jl

@@ -24,7 +24,7 @@ type DynamicsResult{M<:Number, T<:Number}
     totalwrenches::BodyDict{M, Wrench{T}}
     accelerations::BodyDict{M, SpatialAcceleration{T}}
     jointwrenches::BodyDict{M, Wrench{T}} # TODO: index by joint tree index?
-    contact_state_derivatives::BodyDict{M, Vector{DefaultSoftContactStateDeriv{T}}}
+    contact_state_derivatives::BodyDict{M, Vector{Vector{DefaultSoftContactStateDeriv{T}}}}
 
     # see solve_dynamics! for meaning of the following variables:
     L::Matrix{T} # lower triangular
@@ -52,17 +52,17 @@ type DynamicsResult{M<:Number, T<:Number}
         totalwrenches = BodyDict(b => zero(Wrench{T}, rootframe) for b in bodies(mechanism))
         accelerations = BodyDict(b => zero(SpatialAcceleration{T}, rootframe, rootframe, rootframe) for b in bodies(mechanism))
         jointwrenches = BodyDict(b => zero(Wrench{T}, rootframe) for b in bodies(mechanism))
-        contact_state_derivatives = BodyDict(b => Vector{DefaultSoftContactStateDeriv{T}}() for b in bodies(mechanism))
-        start = 1
-        for body in bodies(mechanism)
-            derivs = DefaultSoftContactStateDeriv{T}[]
-            for point in contact_points(body)
-                model = contact_model(point)
-                ṡ_part = view(ṡ, start : start + num_states(model) - 1)
-                push!(derivs, SoftContactStateDeriv(model, ṡ_part, root_frame(mechanism)))
-                start += num_states(model)
-            end
-            contact_state_derivatives[body] = derivs
+        contact_state_derivs = BodyDict(b => Vector{Vector{DefaultSoftContactStateDeriv{T}}}() for b in bodies(mechanism))
+        startind = 1
+        for body in bodies(mechanism), point in contact_points(body)
+            model = contact_model(point)
+            n = num_states(model)
+            push!(contact_state_derivs[body], collect(begin
+                ṡ_part = view(ṡ, startind : startind + n - 1)
+                contact_state_deriv = SoftContactStateDeriv(model, ṡ_part, root_frame(mechanism))
+                startind += n
+                contact_state_deriv
+            end for j = 1 : length(mechanism.environment)))
         end
 
         L = Matrix{T}(nv, nv)
@@ -71,7 +71,7 @@ type DynamicsResult{M<:Number, T<:Number}
         Y = Matrix{T}(nconstraints, nv)
 
         new{M, T}(mechanism, massmatrix, dynamicsbias, constraintjacobian, constraintbias,
-            v̇, λ, ṡ, contactwrenches, totalwrenches, accelerations, jointwrenches, contact_state_derivatives,
+            v̇, λ, ṡ, contactwrenches, totalwrenches, accelerations, jointwrenches, contact_state_derivs,
             L, A, z, Y)
     end
 end

src/mechanism.jl

@@ -124,11 +124,11 @@ $(SIGNATURES)
 Return the dimension of the vector of additional states ``s`` (used for stateful contact models).
 """
 function num_additional_states(mechanism::Mechanism)
-    ret = 0
+    num_states_per_environment_element = 0
     for body in bodies(mechanism), point in contact_points(body)
-        ret += num_states(contact_model(point))
+        num_states_per_environment_element += num_states(contact_model(point))
     end
-    ret
+    num_states_per_environment_element * length(mechanism.environment)
 end
 
 """

src/mechanism_algorithms.jl

@@ -586,17 +586,19 @@ function contact_dynamics!{X, M, C, T}(result::DynamicsResult{M, T}, state::Mech
             # TODO: AABB
             body_to_root = transform_to_root(state, body)
             twist = twist_wrt_world(state, body)
-            states = contact_states(state, body)
-            state_derivs = contact_state_derivatives(result, body)
+            states_for_body = contact_states(state, body)
+            state_derivs_for_body = contact_state_derivatives(result, body)
             for i = 1 : length(points)
                 @inbounds c = points[i]
-                # TODO: AABB check here
                 point = body_to_root * location(c)
                 velocity = point_velocity(twist, point)
-                for primitive in mechanism.environment.halfspaces
+                states_for_point = states_for_body[i]
+                state_derivs_for_point = state_derivs_for_body[i]
+                for j = 1 : length(mechanism.environment.halfspaces)
+                    primitive = mechanism.environment.halfspaces[j]
+                    contact_state = states_for_point[j]
+                    contact_state_deriv = state_derivs_for_point[j]
                     model = contact_model(c)
-                    contact_state = states[i]
-                    contact_state_deriv = state_derivs[i]
                     # TODO: would be good to move this to Contact module
                     # arguments: model, state, state_deriv, point, velocity, primitive
                     if point_inside(primitive, point)

src/mechanism_state.jl

@@ -43,7 +43,7 @@ immutable MechanismState{X<:Number, M<:Number, C<:Number}
     bias_accelerations_wrt_world::BodyDict{M, CacheElement{SpatialAcceleration{C}}}
     inertias::BodyDict{M, CacheElement{SpatialInertia{C}}}
     crb_inertias::BodyDict{M, CacheElement{SpatialInertia{C}}}
-    contact_states::BodyDict{M, Vector{DefaultSoftContactState{X}}} # TODO: consider moving to separate type
+    contact_states::BodyDict{M, Vector{Vector{DefaultSoftContactState{X}}}} # TODO: consider moving to separate type
 
     function (::Type{MechanismState{X, M, C}}){X<:Number, M<:Number, C<:Number}(mechanism::Mechanism{M})
         nb = num_bodies(mechanism)
@@ -80,14 +80,17 @@ immutable MechanismState{X<:Number, M<:Number, C<:Number}
         bias_accelerations_wrt_world = BodyDict(b => CacheElement{SpatialAcceleration{C}}() for b in bodies(mechanism))
         inertias = BodyDict(b => CacheElement{SpatialInertia{C}}() for b in bodies(mechanism))
         crb_inertias = BodyDict(b => CacheElement{SpatialInertia{C}}() for b in bodies(mechanism))
-        contact_states = BodyDict(b => Vector{DefaultSoftContactState{C}}() for b in bodies(mechanism))
+        contact_states = BodyDict{M, Vector{Vector{DefaultSoftContactState{C}}}}(b => Vector{Vector{DefaultSoftContactState{C}}}() for b in bodies(mechanism))
         startind = 1
         for body in bodies(mechanism), point in contact_points(body)
             model = contact_model(point)
             n = num_states(model)
-            s_part = view(s, startind : startind + n - 1)
-            push!(contact_states[body], SoftContactState(model, s_part, root_frame(mechanism)))
-            startind += n
+            push!(contact_states[body], collect(begin
+                s_part = view(s, startind : startind + n - 1)
+                contact_state = SoftContactState(model, s_part, root_frame(mechanism))
+                startind += n
+                contact_state
+            end for j = 1 : length(mechanism.environment)))
         end
 
         # Set root-body related cache elements once and for all.
@@ -191,9 +194,11 @@ $(SIGNATURES)
 Reset all contact state variables.
 """
 function reset_contact_state!(state::MechanismState)
-    for states in values(state.contact_states)
-        for state in states
-            reset!(state)
+    for states_for_body in values(state.contact_states)
+        for states_for_point in states_for_body
+            for contact_state in states_for_point
+                reset!(contact_state)
+            end
         end
     end
 end

test/test_simulate.jl

@@ -99,6 +99,8 @@
         worldframe = root_frame(mechanism)
         inclinedplane = HalfSpace3D(Point3D(worldframe, zeros(SVector{3})), FreeVector3D(worldframe, sin(θ), 0., cos(θ)))
         add_environment_primitive!(mechanism, inclinedplane)
+        irrelevantplane = HalfSpace3D(Point3D(worldframe, 0., 0., -100.), FreeVector3D(worldframe, 0., 0., 1.)) # #211
+        add_environment_primitive!(mechanism, irrelevantplane)
 
         # simulate inclined plane friction experiments
         normalmodel = hunt_crossley_hertz(k = 50e3; α = 1.)