From 2b689274fce06c492d3c4fb1d44300da2fc06029 Mon Sep 17 00:00:00 2001 From: Twan Koolen Date: Thu, 16 Aug 2018 15:30:40 -0400 Subject: [PATCH] Add Baumgarte stabilization. --- docs/make.jl | 2 +- docs/src/index.md | 2 +- src/RigidBodyDynamics.jl | 1 + src/custom_collections.jl | 12 +++++ src/mechanism_algorithms.jl | 80 ++++++++++++++++++++++++---- src/simulate.jl | 19 ++++--- test/test_simulate.jl | 102 ++++++++++++++++++++++++++++++++++++ 7 files changed, 199 insertions(+), 19 deletions(-) diff --git a/docs/make.jl b/docs/make.jl index dbefeb94..ef1fa4b9 100644 --- a/docs/make.jl +++ b/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" ) diff --git a/docs/src/index.md b/docs/src/index.md index 6ada75ee..21c29815 100644 --- a/docs/src/index.md +++ b/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 diff --git a/src/RigidBodyDynamics.jl b/src/RigidBodyDynamics.jl index 07014de9..ebd3c4d1 100644 --- a/src/RigidBodyDynamics.jl +++ b/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 diff --git a/src/custom_collections.jl b/src/custom_collections.jl index f3409518..d258fb69 100644 --- a/src/custom_collections.jl +++ b/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) diff --git a/src/mechanism_algorithms.jl b/src/mechanism_algorithms.jl index 511228de..706ea5dc 100644 --- a/src/mechanism_algorithms.jl +++ b/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 diff --git a/src/simulate.jl b/src/simulate.jl index bbc93245..67ee625c 100644 --- a/src/simulate.jl +++ b/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 diff --git a/test/test_simulate.jl b/test/test_simulate.jl index 997d48d6..8574484c 100644 --- a/test/test_simulate.jl +++ b/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