Merge pull request #478 from JuliaRobotics/tk/baumgarte-2

Add Baumgarte stabilization.

docs/make.jl

@@ -27,6 +27,6 @@ deploydocs(
     repo = "github.com/JuliaRobotics/RigidBodyDynamics.jl.git",
     target = "build",
     make = nothing,
-    julia = "0.6",
+    julia = "1.0",
     osname = "linux"
 )

docs/src/index.md

@@ -31,7 +31,7 @@ Current functionality of RigidBodyDynamics includes:
 * dynamics
 * simulation, either using an off-the-shelf ODE integrator or using an included custom Munthe-Kaas integrator that properly handles second-order ODEs defined on a manifold.
 
-There is currently partial support for closed-loop systems (parallel mechanisms). Support for contact is very limited (possibly subject to major changes in the future), implemented using penalty methods.
+Closed-loop systems (parallel mechanisms) are supported, with optional Baumgarte stabilization of the loop joint constraints. Support for contact is very limited (possibly subject to major changes in the future), implemented using penalty methods.
 
 ## Installation
 

src/RigidBodyDynamics.jl

@@ -167,6 +167,7 @@ using .Spatial # contains additional functions that are reexported
 using .CustomCollections
 using .Contact
 using .Graphs
+using .PDControl
 
 import .Spatial: rotation, translation, transform, center_of_mass, newton_euler, kinetic_energy
 

src/custom_collections.jl

@@ -2,6 +2,7 @@ module CustomCollections
 
 export
     ConstVector,
+    ConstDict,
     NullDict,
     CacheElement,
     AbstractIndexDict,
@@ -68,6 +69,17 @@ Base.@propagate_inbounds Base.getindex(A::ConstVector, i::Int) = (@boundscheck c
 Base.IndexStyle(::Type{<:ConstVector}) = IndexLinear()
 
 
+## ConstDict
+"""
+An immutable `AbstractDict` for which the value is the same, no matter what the key is.
+"""
+struct ConstDict{K, V} <: AbstractDict{K, V}
+    val::V
+    ConstDict{K}(val::V) where {K, V} = new{K, V}(val)
+end
+Base.getindex(d::ConstDict{K}, key) where K = d.val
+
+
 ## NullDict
 """
 $(TYPEDEF)

src/mechanism_algorithms.jl

@@ -621,16 +621,67 @@ function constraint_jacobian!(result::DynamicsResult, state::MechanismState)
     constraint_jacobian!(result.constraintjacobian, result.constraintrowranges, state)
 end
 
-function constraint_bias!(bias::SegmentedVector, state::MechanismState)
+"""
+$(SIGNATURES)
+
+Return the default Baumgarte constraint stabilization gains. These gains result in
+critical damping, and correspond to ``T_{stab} = 0.1`` in Featherstone (2008), section 8.3.
+"""
+function default_constraint_stabilization_gains(scalartype::Type{T}) where T
+    ConstDict{JointID}(SE3PDGains(PDGains(T(100), T(20)), PDGains(T(100), T(20))))
+end
+
+const stabilization_gains_doc = """
+The `stabilization_gains` keyword argument can be used to set PD gains for Baumgarte
+stabilization, which can be used to prevent separation of non-tree (loop) joints.
+See Featherstone (2008), section 8.3 for more information. There are several options
+for specifying gains:
+
+* `nothing` can be used to completely disable Baumgarte stabilization.
+* Gains can be specifed on a per-joint basis using any `AbstractDict{JointID, <:RigidBodyDynamics.PDControl.SE3PDGains}`,
+  which maps the `JointID` for the non-tree joints of the mechanism to the gains for that joint.
+* As a special case of the second option, the same gains can be used for all joints
+  by passing in a `RigidBodyDynamics.CustomCollections.ConstDict{JointID}`.
+
+The [`default_constraint_stabilization_gains`](@ref) function is called to produce the default gains,
+which use the last option.
+"""
+
+function constraint_bias!(bias::SegmentedVector, state::MechanismState{X};
+        stabilization_gains::Union{AbstractDict{JointID, <:SE3PDGains}, Nothing}=default_constraint_stabilization_gains(X)) where X
+    update_transforms!(state)
     update_twists_wrt_world!(state)
     update_bias_accelerations_wrt_world!(state)
     update_constraint_wrench_subspaces!(state)
     constraint_wrench_subspaces = state.constraint_wrench_subspaces.data
-    @inbounds for nontreejointid in state.nontreejointids
-        path = state.constraint_jacobian_structure[nontreejointid]
+    for nontreejointid in state.nontreejointids
         predid, succid = predsucc(nontreejointid, state)
-        crossterm = twist_wrt_world(state, succid, false) × twist_wrt_world(state, predid, false)
-        biasaccel = crossterm + (-bias_acceleration(state, predid, false) + bias_acceleration(state, succid, false)) # 8.47 in Featherstone
+
+        predtwist = twist_wrt_world(state, predid, false)
+        succtwist = twist_wrt_world(state, succid, false)
+        crossterm = succtwist × predtwist
+
+        succbias = bias_acceleration(state, succid, false)
+        predbias = bias_acceleration(state, predid, false)
+        jointbias = -predbias + succbias
+
+        biasaccel = crossterm + jointbias # what's inside parentheses in 8.47 in Featherstone
+
+        if stabilization_gains !== nothing
+            # TODO: make this nicer (less manual frame juggling and no manual transformations)
+            jointtransform = joint_transform(state, nontreejointid, false)
+            jointtwist = -predtwist + succtwist
+            jointtwist = Twist(jointtransform.from, jointtransform.to, jointtwist.frame, jointtwist.angular, jointtwist.linear) # make frames line up
+            successor_to_root = transform_to_root(state, jointtransform.from) # TODO: expensive
+            jointtwist = transform(jointtwist, inv(successor_to_root)) # twist needs to be in successor frame for pd method
+            stabaccel = pd(stabilization_gains[nontreejointid], jointtransform, jointtwist, SE3PDMethod{:Linearized}()) # in successor frame
+            stabaccel = SpatialAcceleration(stabaccel.body, stabaccel.base, biasaccel.frame,
+                Spatial.transform_spatial_motion(stabaccel.angular, stabaccel.linear,
+                rotation(successor_to_root), translation(successor_to_root))...) # back to world frame. TODO: ugly way to do this
+            stabaccel = SpatialAcceleration(biasaccel.body, biasaccel.body, stabaccel.frame, stabaccel.angular, stabaccel.linear) # make frames line up
+            @inbounds biasaccel = biasaccel + -stabaccel
+        end
+
         for cindex in constraint_range(state, nontreejointid)
             Tcol = constraint_wrench_subspaces[cindex]
             # TODO: make nicer:
@@ -640,7 +691,11 @@ function constraint_bias!(bias::SegmentedVector, state::MechanismState)
     end
     bias
 end
-constraint_bias!(result::DynamicsResult, state::MechanismState) = constraint_bias!(result.constraintbias, state)
+
+function constraint_bias!(result::DynamicsResult, state::MechanismState{X};
+        stabilization_gains::Union{AbstractDict{JointID, <:SE3PDGains}, Nothing}=default_constraint_stabilization_gains(X)) where X
+    constraint_bias!(result.constraintbias, state)
+end
 
 function contact_dynamics!(result::DynamicsResult{T, M}, state::MechanismState{X, M, C}) where {X, M, C, T}
     update_transforms!(state)
@@ -786,6 +841,7 @@ function dynamics_solve!(result::DynamicsResult{T, S}, τ::AbstractVector{T}) wh
     nothing
 end
 
+
 """
 $(SIGNATURES)
 
@@ -803,10 +859,13 @@ given joint configuration vector ``q``, joint velocity vector ``v``, and
 
 The `externalwrenches` argument can be used to specify additional
 wrenches that act on the `Mechanism`'s bodies.
+
+$stabilization_gains_doc
 """
 function dynamics!(result::DynamicsResult, state::MechanismState{X},
         torques::AbstractVector = ConstVector(zero(X), num_velocities(state)),
-        externalwrenches::AbstractDict{BodyID, <:Wrench} = NullDict{BodyID, Wrench{X}}()) where X
+        externalwrenches::AbstractDict{BodyID, <:Wrench} = NullDict{BodyID, Wrench{X}}();
+        stabilization_gains=default_constraint_stabilization_gains(X)) where X
     configuration_derivative!(result.q̇, state)
     contact_dynamics!(result, state)
     for jointid in state.treejointids
@@ -818,7 +877,7 @@ function dynamics!(result::DynamicsResult, state::MechanismState{X},
     mass_matrix!(result, state)
     if has_loops(state.mechanism)
         constraint_jacobian!(result, state)
-        constraint_bias!(result, state)
+        constraint_bias!(result, state; stabilization_gains=stabilization_gains)
     end
     dynamics_solve!(result, parent(torques))
     nothing
@@ -841,9 +900,10 @@ and returns a `Vector` ``\\dot{x}``.
 function dynamics!(ẋ::StridedVector{X},
         result::DynamicsResult, state::MechanismState{X}, x::AbstractVector{X},
         torques::AbstractVector = ConstVector(zero(X), num_velocities(state)),
-        externalwrenches::AbstractDict{BodyID, <:Wrench} = NullDict{BodyID, Wrench{X}}()) where X
+        externalwrenches::AbstractDict{BodyID, <:Wrench} = NullDict{BodyID, Wrench{X}}();
+        stabilization_gains=default_constraint_stabilization_gains(X)) where X
     copyto!(state, x)
-    dynamics!(result, state, torques, externalwrenches)
+    dynamics!(result, state, torques, externalwrenches; stabilization_gains=stabilization_gains)
     copyto!(ẋ, result)
     ẋ
 end

src/simulate.jl

@@ -28,19 +28,24 @@ end
 
 The integration time step can be specified using the `Δt` keyword argument (defaults to `1e-4`).
 
+$stabilization_gains_doc
+
 Uses `MuntheKaasIntegrator`. See [`RigidBodyDynamics.OdeIntegrators.MuntheKaasIntegrator`](@ref) for a lower
 level interface with more options.
 """
-function simulate(state0::MechanismState, final_time, control! = zero_torque!; Δt = 1e-4)
+function simulate(state0::MechanismState{X}, final_time, control! = zero_torque!;
+        Δt = 1e-4, stabilization_gains=default_constraint_stabilization_gains(X)) where X
     T = cache_eltype(state0)
     result = DynamicsResult{T}(state0.mechanism)
     control_torques = similar(velocity(state0))
-    closed_loop_dynamics! = (v̇::AbstractArray, ṡ::AbstractArray, t, state) -> begin
-        control!(control_torques, t, state)
-        dynamics!(result, state, control_torques)
-        copyto!(v̇, result.v̇)
-        copyto!(ṡ, result.ṡ)
-        nothing
+    closed_loop_dynamics! = let result=result, control_torques=control_torques, stabilization_gains=stabilization_gains # https://github.com/JuliaLang/julia/issues/15276
+        function (v̇::AbstractArray, ṡ::AbstractArray, t, state)
+            control!(control_torques, t, state)
+            dynamics!(result, state, control_torques; stabilization_gains=stabilization_gains)
+            copyto!(v̇, result.v̇)
+            copyto!(ṡ, result.ṡ)
+            nothing
+        end
     end
     tableau = runge_kutta_4(T)
     storage = ExpandingStorage{T}(state0, ceil(Int64, final_time / Δt * 1.001)) # very rough overestimate of number of time steps

test/test_simulate.jl

@@ -123,4 +123,106 @@
             end
         end
     end
+
+    @testset "four-bar linkage" begin
+        # gravitational acceleration
+        g = -9.81
+
+        # link lengths
+        l_0 = 1.10
+        l_1 = 0.5
+        l_2 = 1.20
+        l_3 = 0.75
+
+        # link masses
+        m_1 = 0.5
+        m_2 = 1.0
+        m_3 = 0.75
+
+        # link center of mass offsets from the preceding joint axes
+        c_1 = 0.25
+        c_2 = 0.60
+        c_3 = 0.375
+
+        # moments of inertia about the center of mass of each link
+        I_1 = 0.333
+        I_2 = 0.537
+        I_3 = 0.4
+
+        # scalar type
+        T = Float64
+
+        # Rotation axis: negative y-axis
+        axis = SVector(zero(T), -one(T), zero(T))
+
+        world = RigidBody{T}("world")
+        mechanism = Mechanism(world; gravity = SVector(0., 0., g))
+        rootframe = root_frame(mechanism)
+
+        # link1 and joint1
+        joint1 = Joint("joint1", Revolute(axis))
+        inertia1 = SpatialInertia(CartesianFrame3D("inertia1_centroidal"), I_1*axis*axis', zero(SVector{3, T}), m_1)
+        link1 = RigidBody(inertia1)
+        before_joint1_to_world = one(Transform3D, frame_before(joint1), default_frame(world))
+        c1_to_joint = Transform3D(inertia1.frame, frame_after(joint1), SVector(c_1, 0, 0))
+        attach!(mechanism, world, link1, joint1, joint_pose = before_joint1_to_world, successor_pose = c1_to_joint)
+
+        # link2 and joint2
+        joint2 = Joint("joint2", Revolute(axis))
+        inertia2 = SpatialInertia(CartesianFrame3D("inertia2_centroidal"), I_2*axis*axis', zero(SVector{3, T}), m_2)
+        link2 = RigidBody(inertia2)
+        before_joint2_to_after_joint1 = Transform3D(frame_before(joint2), frame_after(joint1), SVector(l_1, 0., 0.))
+        c2_to_joint = Transform3D(inertia2.frame, frame_after(joint2), SVector(c_2, 0, 0))
+        attach!(mechanism, link1, link2, joint2, joint_pose = before_joint2_to_after_joint1, successor_pose = c2_to_joint)
+
+        # link3 and joint3
+        joint3 = Joint("joint3", Revolute(axis))
+        inertia3 = SpatialInertia(CartesianFrame3D("inertia3_centroidal"), I_3*axis*axis', zero(SVector{3, T}), m_3)
+        link3 = RigidBody(inertia3)
+        before_joint3_to_world = Transform3D(frame_before(joint3), default_frame(world), SVector(l_0, 0., 0.))
+        c3_to_joint = Transform3D(inertia3.frame, frame_after(joint3), SVector(c_3, 0, 0))
+        attach!(mechanism, world, link3, joint3, joint_pose = before_joint3_to_world, successor_pose = c3_to_joint)
+
+        # loop joint between link2 and link3
+        joint4 = Joint("joint4", Revolute(axis))
+        before_joint4_to_joint2 = Transform3D(frame_before(joint4), frame_after(joint2), SVector(l_2, 0., 0.))
+        joint3_to_after_joint4 = Transform3D(frame_after(joint3), frame_after(joint4), SVector(-l_3, 0., 0.))
+        attach!(mechanism, link2, link3, joint4, joint_pose = before_joint4_to_joint2, successor_pose = joint3_to_after_joint4)
+
+        # initial state
+        set_initial_state! = function (state::MechanismState)
+            # found through nonlinear optimization. Slightly inaccurate.
+            set_configuration!(state, joint1, 1.6707963267948966) # θ
+            set_configuration!(state, joint2, -1.4591054166649482) # γ
+            set_configuration!(state, joint3, 1.5397303602625536) # ϕ
+            set_velocity!(state, joint1, 0.5)
+            set_velocity!(state, joint2, -0.47295)
+            set_velocity!(state, joint3, 0.341)
+        end
+
+        # no stabilization
+        state = MechanismState(mechanism)
+        set_initial_state!(state)
+        zero_velocity!(state)
+        energy0 = gravitational_potential_energy(state)
+        ts, qs, vs = simulate(state, 1., Δt = 1e-3, stabilization_gains=nothing)
+        @test kinetic_energy(state) ≉ 0 atol=1e-2
+        energy1 = gravitational_potential_energy(state) + kinetic_energy(state)
+        @test energy0 ≈ energy1 atol=1e-8
+        @test transform(state, Point3D(Float64, frame_before(joint4)), rootframe) ≈
+              transform(state, Point3D(Float64, frame_after(joint4)), rootframe) atol=1e-10 # no significant separation after a short simulation
+
+        # with default stabilization: start with some separation
+        set_initial_state!(state)
+        set_configuration!(state, joint1, 1.7)
+        @test transform(state, Point3D(Float64, frame_before(joint4)), rootframe) ≉
+              transform(state, Point3D(Float64, frame_after(joint4)), rootframe) atol=1e-2 # significant separation initially
+        simulate(state, 15., Δt = 1e-3)
+        @test transform(state, Point3D(Float64, frame_before(joint4)), rootframe) ≈
+              transform(state, Point3D(Float64, frame_after(joint4)), rootframe) atol=1e-5 # reduced separation after 15 seconds
+        energy15 = gravitational_potential_energy(state) + kinetic_energy(state)
+        simulate(state, 10, Δt = 1e-3)
+        energy20 = gravitational_potential_energy(state) + kinetic_energy(state)
+        @test energy20 ≈ energy15 atol=1e-5 # stabilization doesn't significantly affect energy after converging
+    end
 end