diff --git a/Project.toml b/Project.toml index e6464953..f9ee464d 100644 --- a/Project.toml +++ b/Project.toml @@ -3,7 +3,10 @@ uuid = "366cf18f-59d5-5db9-a4de-86a9f6786172" version = "2.2.0" [deps] +CoordinateTransformations = "150eb455-5306-5404-9cee-2592286d6298" DocStringExtensions = "ffbed154-4ef7-542d-bbb7-c09d3a79fcae" +EnhancedGJK = "3d39a06a-b57e-5769-b499-4d62b23c243f" +GeometryTypes = "4d00f742-c7ba-57c2-abde-4428a4b178cb" LightXML = "9c8b4983-aa76-5018-a973-4c85ecc9e179" LinearAlgebra = "37e2e46d-f89d-539d-b4ee-838fcccc9c8e" LoopThrottle = "39f5be34-8529-5463-bac7-bf6867c840a3" diff --git a/src/RigidBodyDynamics.jl b/src/RigidBodyDynamics.jl index dc38a241..1561417a 100644 --- a/src/RigidBodyDynamics.jl +++ b/src/RigidBodyDynamics.jl @@ -164,14 +164,14 @@ export include(joinpath("custom_collections", "custom_collections.jl")) include(joinpath("graphs", "Graphs.jl")) include(joinpath("spatial", "Spatial.jl")) -include("contact.jl") +include("ode_integrators.jl") include("pdcontrol.jl") @reexport using .Spatial using .CustomCollections -using .Contact using .Graphs using .PDControl +using .OdeIntegrators import .Spatial: rotation, translation, transform, center_of_mass, newton_euler, kinetic_energy @@ -185,12 +185,13 @@ include("mechanism_state.jl") include("dynamics_result.jl") include("caches.jl") include("mechanism_algorithms.jl") -include("ode_integrators.jl") include("simulate.jl") include(joinpath("urdf", "URDF.jl")) @reexport using .URDF +include(joinpath("contact", "Contact.jl")) + # import these for MechanismGeometries compatibility. TODO: stop importing these after updating MechanismGeometries. import .URDF: parse_scalar, parse_vector, parse_pose diff --git a/src/contact.jl b/src/contact.jl deleted file mode 100644 index a1c514a0..00000000 --- a/src/contact.jl +++ /dev/null @@ -1,251 +0,0 @@ -module Contact - -using LinearAlgebra -using RigidBodyDynamics.Spatial -using StaticArrays - -using Base: promote_eltype - -# base types -export SoftContactModel, - SoftContactState, - SoftContactStateDeriv, - ContactPoint, - ContactEnvironment, - HalfSpace3D - -# interface functions -export num_states, - location, - contact_model, - contact_dynamics!, - reset!, - point_inside, - separation, - detect_contact - -# specific models -export HuntCrossleyModel, - hunt_crossley_hertz - -export ViscoelasticCoulombModel, - ViscoelasticCoulombState, - ViscoelasticCoulombStateDeriv - -# defaults -export DefaultContactPoint, - DefaultSoftContactState, - DefaultSoftContactStateDeriv - -# Normal force model/state/state derivative interface: -# * `normal_force(model, state, z::Number, ż::Number)`: return the normal force given penetration `z` and penetration velocity `ż` -# * `num_states(model)`: return the number of state variables associated with the model -# * `state(model, statevec, frame)`: create a new state associated with the model -# * `state_derivative(model, statederivvec, frame)`: create a new state derivative associated with the model -# * `reset!(state)`: resets the contact state -# * `dynamics!(state_deriv, model, state, fnormal)`: sets state_deriv given the current state and the normal force - -# Friction model/state/state derivative interface: -# * `friction_force(model, state, fnormal::Number, tangential_velocity::FreeVector3D)`: return the friction force given normal force `fnormal` and tangential velocity `tangential_velocity` -# * `num_states(model)`: return the number of state variables associated with the model -# * `state(model, statevec, frame)`: create a new state associated with the model -# * `state_derivative(model, statederivvec, frame)`: create a new state derivative associated with the model -# * `reset!(state)`: resets the contact state -# * `dynamics!(state_deriv, model, state, ftangential)`: sets state_deriv given the current state and the tangential force force - -## SoftContactModel and related types. -struct SoftContactModel{N, F} - normal::N - friction::F -end - -struct SoftContactState{N, F} - normal::N - friction::F -end - -struct SoftContactStateDeriv{N, F} - normal::N - friction::F -end - -normal_force_model(model::SoftContactModel) = model.normal -friction_model(model::SoftContactModel) = model.friction - -normal_force_state(state::SoftContactState) = state.normal -friction_state(state::SoftContactState) = state.friction - -normal_force_state_deriv(deriv::SoftContactStateDeriv) = deriv.normal -friction_state_deriv(deriv::SoftContactStateDeriv) = deriv.friction - -num_states(model::SoftContactModel) = num_states(normal_force_model(model)) + num_states(friction_model(model)) - -for (fun, returntype) in [(:state, :SoftContactState), (:state_derivative, :SoftContactStateDeriv)] - @eval function $returntype(model::SoftContactModel, vec::AbstractVector, frame::CartesianFrame3D) - nnormal = num_states(normal_force_model(model)) - nfriction = num_states(friction_model(model)) - vecnormal = view(vec, 1 : nnormal - 1) - vecfriction = view(vec, 1 + nnormal : nnormal + nfriction) - $returntype($fun(normal_force_model(model), vecnormal, frame), $fun(friction_model(model), vecfriction, frame)) - end -end - -reset!(state::SoftContactState) = (reset!(normal_force_state(state)); reset!(friction_state(state))) -zero!(deriv::SoftContactStateDeriv) = (zero!(friction_state_deriv(deriv)); zero!(friction_state_deriv(deriv))) - -## ContactPoint -mutable struct ContactPoint{T, M <: SoftContactModel} - location::Point3D{SVector{3, T}} - model::M -end -location(point::ContactPoint) = point.location -contact_model(point::ContactPoint) = point.model - -function contact_dynamics!(state_deriv::SoftContactStateDeriv, state::SoftContactState, - model::SoftContactModel, penetration::Number, velocity::FreeVector3D, normal::FreeVector3D) - @boundscheck penetration >= 0 || error("penetration must be nonnegative") - - z = penetration - ż = -dot(velocity, normal) # penetration velocity - fnormal = max(normal_force(normal_force_model(model), normal_force_state(state), z, ż), 0) - dynamics!(normal_force_state_deriv(state_deriv), normal_force_model(model), normal_force_state(state), fnormal) - - tangential_velocity = velocity + ż * normal - ftangential = friction_force(friction_model(model), friction_state(state), fnormal, tangential_velocity) - dynamics!(friction_state_deriv(state_deriv), friction_model(model), friction_state(state), ftangential) - - fnormal * normal + ftangential -end - - -## Models with no state -reset!(::Nothing) = nothing -zero!(::Nothing) = nothing - - -## Normal contact models -struct HuntCrossleyModel{T} - # (2) in Marhefka, Orin, "A Compliant Contact Model with Nonlinear Damping for Simulation of Robotic Systems" - k::T - λ::T - n::T -end - -function hunt_crossley_hertz(; k = 50e3, α = 0.2) - λ = 3/2 * α * k # (12) in Marhefka, Orin - HuntCrossleyModel(k, λ, 3/2) -end - -num_states(::HuntCrossleyModel) = 0 -state(::HuntCrossleyModel, ::AbstractVector, ::CartesianFrame3D) = nothing -state_derivative(::HuntCrossleyModel, ::AbstractVector, ::CartesianFrame3D) = nothing - -function normal_force(model::HuntCrossleyModel, ::Nothing, z, ż) - zn = z^model.n - f = model.λ * zn * ż + model.k * zn # (2) in Marhefka, Orin (note: z is penetration, returning repelling force) -end - -dynamics!(ẋ::Nothing, model::HuntCrossleyModel, state::Nothing, fnormal::Number) = nothing - - -# Friction models -struct ViscoelasticCoulombModel{T} - # See section 11.8 of Featherstone, "Rigid Body Dynamics Algorithms", 2008 - μ::T - k::T - b::T -end - -mutable struct ViscoelasticCoulombState{V} - tangential_displacement::FreeVector3D{V} # Use a 3-vector; technically only need one state for each tangential direction, but this is easier to work with. -end - -mutable struct ViscoelasticCoulombStateDeriv{V} - deriv::FreeVector3D{V} -end - -num_states(::ViscoelasticCoulombModel) = 3 -function state(::ViscoelasticCoulombModel, statevec::AbstractVector, frame::CartesianFrame3D) - ViscoelasticCoulombState(FreeVector3D(frame, statevec)) -end -function state_derivative(::ViscoelasticCoulombModel, statederivvec::AbstractVector, frame::CartesianFrame3D) - ViscoelasticCoulombStateDeriv(FreeVector3D(frame, statederivvec)) -end - -reset!(state::ViscoelasticCoulombState) = fill!(state.tangential_displacement.v, 0) -zero!(deriv::ViscoelasticCoulombStateDeriv) = fill!(deriv.deriv.v, 0) - -function friction_force(model::ViscoelasticCoulombModel, state::ViscoelasticCoulombState, - fnormal, tangential_velocity::FreeVector3D) - T = typeof(fnormal) - μ = model.μ - k = model.k - b = model.b - x = convert(FreeVector3D{SVector{3, T}}, state.tangential_displacement) - v = tangential_velocity - - # compute friction force that would be needed to avoid slip - fstick = -k * x - b * v - - # limit friction force to lie within friction cone - fstick_norm² = dot(fstick, fstick) - fstick_max_norm² = (μ * fnormal)^2 - ftangential = if fstick_norm² > fstick_max_norm² - fstick * sqrt(fstick_max_norm² / fstick_norm²) - else - fstick - end -end - -function dynamics!(ẋ::ViscoelasticCoulombStateDeriv, model::ViscoelasticCoulombModel, state::ViscoelasticCoulombState, ftangential::FreeVector3D) - k = model.k - b = model.b - x = state.tangential_displacement - @framecheck ẋ.deriv.frame x.frame - ẋ.deriv.v .= (-k .* x.v .- ftangential.v) ./ b -end - -## VectorSegment: type of a view of a vector -const VectorSegment{T} = SubArray{T,1,Array{T, 1},Tuple{UnitRange{Int64}},true} # TODO: a bit too specific - -const DefaultContactPoint{T} = ContactPoint{T,SoftContactModel{HuntCrossleyModel{T},ViscoelasticCoulombModel{T}}} -const DefaultSoftContactState{T} = SoftContactState{Nothing, ViscoelasticCoulombState{VectorSegment{T}}} -const DefaultSoftContactStateDeriv{T} = SoftContactStateDeriv{Nothing, ViscoelasticCoulombStateDeriv{VectorSegment{T}}} - - -# Contact detection -# TODO: should probably move this somewhere else - -mutable struct HalfSpace3D{T} - point::Point3D{SVector{3, T}} - outward_normal::FreeVector3D{SVector{3, T}} - - function HalfSpace3D(point::Point3D{SVector{3, T}}, outward_normal::FreeVector3D{SVector{3, T}}) where {T} - @framecheck point.frame outward_normal.frame - new{T}(point, normalize(outward_normal)) - end -end - -frame(halfspace::HalfSpace3D) = halfspace.point.frame - -function HalfSpace3D(point::Point3D, outward_normal::FreeVector3D) - T = promote_eltype(point, outward_normal) - HalfSpace3D(convert(Point3D{SVector{3, T}}, point), convert(FreeVector3D{SVector{3, T}}, outward_normal)) -end - -Base.eltype(::Type{HalfSpace3D{T}}) where {T} = T -separation(halfspace::HalfSpace3D, p::Point3D) = dot(p - halfspace.point, halfspace.outward_normal) -point_inside(halfspace::HalfSpace3D, p::Point3D) = separation(halfspace, p) <= 0 -detect_contact(halfspace::HalfSpace3D, p::Point3D) = separation(halfspace, p), halfspace.outward_normal - - -# ContactEnvironment -mutable struct ContactEnvironment{T} - halfspaces::Vector{HalfSpace3D{T}} - ContactEnvironment{T}() where {T} = new{T}(HalfSpace3D{T}[]) -end - -Base.push!(environment::ContactEnvironment, halfspace::HalfSpace3D) = push!(environment.halfspaces, halfspace) -Base.length(environment::ContactEnvironment) = length(environment.halfspaces) - -end diff --git a/src/contact/Contact.jl b/src/contact/Contact.jl new file mode 100644 index 00000000..f422eca0 --- /dev/null +++ b/src/contact/Contact.jl @@ -0,0 +1,74 @@ +module Contact + +using RigidBodyDynamics + +using LinearAlgebra +using RigidBodyDynamics.Spatial +using StaticArrays +using TypeSortedCollections +using EnhancedGJK +using DocStringExtensions + +import GeometryTypes +import CoordinateTransformations + +using RigidBodyDynamics: update_transforms!, update_twists_wrt_world! +using RigidBodyDynamics: frame_definition +using RigidBodyDynamics.CustomCollections: UnorderedPair +using CoordinateTransformations: Transformation, AffineMap, LinearMap, Translation, IdentityTransformation +using CoordinateTransformations: transform_deriv + +# base types +export + ContactForceModel, + CollisionElement, + CollisionGroup, + ContactModel, + SoftContactState, + SoftContactResult + +# geometry +export + HalfSpace, + HRep + +# interface functions +export + set_contact_force_model!, + contact_dynamics + +# specific models +export + SplitContactForceModel, + HuntCrossleyModel, + hunt_crossley_hertz, + ViscoelasticCoulombModel + +# TODO: move this somewhere else +CoordinateTransformations.AffineMap(tf::Transform3D) = AffineMap(rotation(tf), translation(tf)) + +function RigidBodyDynamics.Transform3D(from::CartesianFrame3D, to::CartesianFrame3D, map::AffineMap) + Transform3D(from, to, map.linear, map.translation) +end + +function RigidBodyDynamics.Transform3D(from::CartesianFrame3D, to::CartesianFrame3D, map::LinearMap) + Transform3D(from, to, map.linear) +end + +function RigidBodyDynamics.Transform3D(from::CartesianFrame3D, to::CartesianFrame3D, map::Translation) + Transform3D(from, to, map.translation) +end + +include("halfspace.jl") +include("hrep.jl") +include("collision_element.jl") +include("contact_force_model.jl") +include("collidable_pair.jl") +include("collision_detection.jl") +include("contact_model.jl") +include("soft_contact_state.jl") + +include("hunt_crossley.jl") +include("viscoelastic_coulomb.jl") + +end # module diff --git a/src/contact/collidable_pair.jl b/src/contact/collidable_pair.jl new file mode 100644 index 00000000..a5367260 --- /dev/null +++ b/src/contact/collidable_pair.jl @@ -0,0 +1,16 @@ +struct CollidablePair{A<:CollisionElement, B<:CollisionElement, M<:ContactForceModel} + a::A + b::B + model::M +end + +# struct GJKCollisionCache{G<:EnhancedGJK.CollisionCache} +# gjk_cache::G +# penetrated_face::TODO +# end +# PointGJKCollisionCache(gjk_cache::EnhancedGJK.CollisionCache) = PointGJKCollisionCache(gjk_cache, Ref(zero(UInt8))) +# function PointGJKCollisionCache(pair::CollidablePair) +# @assert pair.a.geometry isa GeometryTypes.Point || pair.b.geometry isa GeometryTypes.Point +# PointGJKCollisionCache(CollisionCache(pair.a.geometry, pair.b.geometry)) +# end + diff --git a/src/contact/collision_detection.jl b/src/contact/collision_detection.jl new file mode 100644 index 00000000..d9af4651 --- /dev/null +++ b/src/contact/collision_detection.jl @@ -0,0 +1,177 @@ + +""" +$(SIGNATURES) + +Return a suitable object to do collision checking for the given geometries. +""" +function collision_cache end + +mutable struct DefaultCollisionCache{T, C<:EnhancedGJK.CollisionCache} + gjkcache::C + halfspace_in_b::Union{HalfSpace{3, T}, Nothing} # fixed in frame of body B; normal points outward from B +end + +DefaultCollisionCache{T}(gjkcache::C) where {T, C<:EnhancedGJK.CollisionCache} = DefaultCollisionCache{T, C}(gjkcache, nothing) +DefaultCollisionCache{T}(a, b) where {T} = DefaultCollisionCache{T}(EnhancedGJK.CollisionCache(a, b)) + +collision_cache(::Type{T}, a, b) where {T} = DefaultCollisionCache{T}(a, b) + +function reset!(cache::DefaultCollisionCache) + EnhancedGJK.reset!(cache.gjkcache) + cache.halfspace_in_b = nothing + return cache +end + +function extract_halfspace(result::GJKResult, closest_in_b::SVector{3}) + # IMPORTANT: this only works when the closest face of the simplex corresponds + # to the closest face of the Minkowski sum. Should instead use EPA to get the + # halfspace and cache it for performance and to avoid tunelling. + simplex = result.simplex + best_dist_squared = nothing + best_face = nothing + best_point = nothing + for i in eachindex(simplex) + face = EnhancedGJK.simplex_face(simplex, i) + weights = EnhancedGJK.projection_weights(face) + point = EnhancedGJK.linear_combination(weights, face) + dist_squared = point ⋅ point + if best_dist_squared === nothing || dist_squared < best_dist_squared + best_dist_squared = dist_squared + best_face = face + best_point = point + end + end + # @show typeof(best_face) + # error() + normal = normalize(EnhancedGJK.normal(best_face)) + signed_distance = best_point ⋅ normal + if signed_distance > 0 + signed_distance = -signed_distance + normal = -normal + end + HalfSpace(normal, normal ⋅ closest_in_b - signed_distance) +end + +@inline function detect_contact(cache::DefaultCollisionCache, pose_a::Transformation, pose_b::Transformation) + result = gjk!(cache.gjkcache, pose_a, pose_b) + closest_in_a = pose_a(result.closest_point_in_body.a) + closest_in_b = pose_b(result.closest_point_in_body.b) + if result.in_collision + # NOTE: normal computation currently relies on the fact that the closest + # face of the simplex corresponds to the closest face of the Minkowski sum. + # This will not be the case with large penetrations. + # Caching the penetration halfspace from the previous result is meant to + # circumvent this. + if cache.halfspace_in_b === nothing + halfspace_in_world = extract_halfspace(result, closest_in_b) + cache.halfspace_in_b = inv(pose_b)(halfspace_in_world) + else + halfspace_in_world = pose_b(cache.halfspace_in_b) + end + separation = closest_in_b ⋅ halfspace_in_world.outward_normal - halfspace_in_world.offset + normal = halfspace_in_world.outward_normal + else + cache.halfspace_in_b = nothing + separation = separation_distance(result) + normal = nothing + end + separation, normal, closest_in_a, closest_in_b +end + + +collision_cache(::Type{T}, a::GeometryTypes.Point{N, T}, b::HalfSpace{N, T}) where {T, N} = a => b +collision_cache(::Type{T}, a::HalfSpace{N, T}, b::GeometryTypes.Point{N, T}) where {T, N} = a => b + +reset!(::Pair{<:GeometryTypes.Point, <:HalfSpace}) = nothing +reset!(::Pair{<:HalfSpace, <:GeometryTypes.Point}) = nothing + +@inline function detect_contact(geometries::Pair{<:GeometryTypes.Point, <:HalfSpace}, pose_a::Transformation, pose_b::Transformation) + point = pose_a(geometries[1]) + halfspace = pose_b(geometries[2]) + separation = point ⋅ halfspace.outward_normal - halfspace.offset + closest_in_halfspace = separation <= 0 ? point : point - halfspace.offset * halfspace.outward_normal + separation, halfspace.outward_normal, point, closest_in_halfspace +end + +@inline function detect_contact(geometries::Pair{<:HalfSpace, <:GeometryTypes.Point}, pose_a::Transformation, pose_b::Transformation) + separation, inward_normal, point_in_b, point_in_a = detect_contact(Base.reverse(geometries), pose_b, pose_a) + separation, -inward_normal, point_in_a, point_in_b +end + + +struct PointToHRepCollisionCache{F, M, T} # F: whether or not the order of point and hrep is flipped + point::GeometryTypes.Point{3, T} + hrep::HRep{M, 3, T} + penetrated_halfspace_index::Base.RefValue{Int} # 0: uninitialized +end + +PointToHRepCollisionCache(point::GeometryTypes.Point{3, T}, hrep::HRep{M, 3, T}) where {M, T} = PointToHRepCollisionCache{false, M, T}(point, hrep, Ref(0)) +PointToHRepCollisionCache(hrep::HRep{M, 3, T}, point::GeometryTypes.Point{3, T}) where {M, T} = PointToHRepCollisionCache{true, M, T}(point, hrep, Ref(0)) + +collision_cache(::Type{T}, point::GeometryTypes.Point{3, T}, hrep::HRep{M, 3, T}) where {M, T} = PointToHRepCollisionCache(point, hrep) +collision_cache(::Type{T}, hrep::HRep{M, 3, T}, point::GeometryTypes.Point{3, T}) where {M, T} = PointToHRepCollisionCache(hrep, point) + +reset!(cache::PointToHRepCollisionCache) = (cache.penetrated_halfspace_index[] = 0; return nothing) + +@inline function detect_contact(cache::PointToHRepCollisionCache{F, M}, pose_a::Transformation, pose_b::Transformation) where {F, M} + point = SVector(cache.point) + hrep = cache.hrep + flip = F + + # Handle flipping of a and b + point_to_world, hrep_to_world = flip ? (pose_b, pose_a) : (pose_a, pose_b) + point_world = point_to_world(point) + point_hrep = inv(hrep_to_world)(point_world) + + # Check all of the half-spaces (in hrep frame) + in_penetration = true + closest_index = 0 + T = eltype(point) # TODO + closest_distance = typemax(T) + halfspaces = hrep.halfspaces + point_hrep_homogeneous = SVector(tuple(point_hrep[1], point_hrep[2], point_hrep[3], -one(eltype(point_hrep)))) # TODO: make nicer + @inbounds for i in 1 : M + halfspace = halfspaces[i] + normal = halfspace.outward_normal + sep = SVector(tuple(normal[1], normal[2], normal[3], halfspace.offset)) ⋅ point_hrep_homogeneous # TODO: make nicer + in_penetration &= sep < 0 + distance = abs(sep) + if i === 1 || distance < closest_distance + closest_index = i + closest_distance = distance + end + end + + if in_penetration + # Determine which half-space to use + if cache.penetrated_halfspace_index[] != 0 + closest_index = cache.penetrated_halfspace_index[] + end + end + + # Compute normal based on closest plane + @inbounds halfspace = halfspaces[closest_index] + rotation_to_world = LinearMap(transform_deriv(hrep_to_world, 0)) + normal_world = rotation_to_world(halfspace.outward_normal) + + # compute separation, closest point, caching + if in_penetration + separation = let normal = halfspace.outward_normal + SVector(tuple(normal[1], normal[2], normal[3], halfspace.offset)) ⋅ point_hrep_homogeneous + end + @assert separation < 0 + closest_in_hrep = point_world + cache.penetrated_halfspace_index[] = closest_index + else + separation = closest_distance # kind of bogus... + closest_in_hrep = fill(NaN, typeof(point_world)) + cache.penetrated_halfspace_index[] = 0 + end + + # Handle flipping of a and b and return + if flip + return separation, -normal_world, closest_in_hrep, point_world + else + return separation, normal_world, point_world, closest_in_hrep + end +end diff --git a/src/contact/collision_element.jl b/src/contact/collision_element.jl new file mode 100644 index 00000000..5dbcd90b --- /dev/null +++ b/src/contact/collision_element.jl @@ -0,0 +1,13 @@ +struct CollisionElement{T, G} + bodyid::BodyID + transform::T # to body's default frame + geometry::G +end + +function CollisionElement(body::RigidBody, transform, geometry) + CollisionElement(BodyID(body), transform, geometry) +end + +function CollisionElement(body::RigidBody, frame::CartesianFrame3D, geometry) + CollisionElement(body, frame_definition(body, frame), geometry) +end diff --git a/src/contact/contact_force_model.jl b/src/contact/contact_force_model.jl new file mode 100644 index 00000000..db9640d7 --- /dev/null +++ b/src/contact/contact_force_model.jl @@ -0,0 +1,49 @@ +abstract type ContactForceModel end + +vectorize(x::Nothing) = SVector{0, Union{}}() +vectorize(x::AbstractVector) = x +vectorize(x::FreeVector3D) = x.v +vectorize(x::Tuple{}) = SVector{0, Union{}}() +vectorize(x::Tuple) = vcat(vectorize(x[1]), vectorize(Base.tail(x))) + +struct SplitContactForceModel{N, T} <: ContactForceModel + normal::N + tangential::T +end + +num_states(model::SplitContactForceModel) = num_states(model.normal) + num_states(model.tangential) +zero_state(model::SplitContactForceModel) = (zero_state(model.normal), zero_state(model.tangential)) + +function devectorize(model::SplitContactForceModel, x::AbstractVector) + nx_normal = num_states(model.normal) + nx_tangential = num_states(model.tangential) + # TODO: use uviews? + normal_state = let x_normal = view(x, 1 : nx_normal) + devectorize(model.normal, x_normal) + end + tangential_state = let x_tangential = view(x, nx_normal + 1 : nx_normal + nx_tangential) + devectorize(model.tangential, x_tangential) + end + normal_state, tangential_state +end + +@noinline throw_negative_penetration_error() = error("penetration must be nonnegative") + +function soft_contact_dynamics( + model::SplitContactForceModel, + (normal_state, tangential_state), + penetration::Number, # of A in B + relative_velocity::SVector{3}, # of A w.r.t. B + normal::SVector{3}) # outward normal from B + @boundscheck penetration >= 0 || throw_negative_penetration_error() + z = penetration + ż = -dot(relative_velocity, normal) # penetration velocity + fnormal, normal_state_deriv = normal_contact_dynamics( + model.normal, normal_state, z, ż) + tangential_velocity = relative_velocity + ż * normal + ftangential, tangential_state_deriv = tangential_contact_dynamics( + model.tangential, tangential_state, fnormal, tangential_velocity) + f = fnormal * normal + ftangential + state_deriv = (normal_state_deriv, tangential_state_deriv) + f, state_deriv +end diff --git a/src/contact/contact_model.jl b/src/contact/contact_model.jl new file mode 100644 index 00000000..de3543eb --- /dev/null +++ b/src/contact/contact_model.jl @@ -0,0 +1,60 @@ +const CollisionGroup = Vector{CollisionElement} + +struct ContactModel + collision_groups::Vector{CollisionGroup} + contact_force_models::Dict{UnorderedPair{CollisionGroup}, ContactForceModel} +end + +function ContactModel() + collision_groups = CollisionGroup[] + contact_force_models = Dict{UnorderedPair{CollisionGroup}, ContactForceModel}() + ContactModel(collision_groups, contact_force_models) +end + +function Base.show(io::IO, model::ContactModel) + print(io, "ContactModel with ", length(model.collision_groups), " collision group(s)") +end + +function Base.push!(model::ContactModel, group::CollisionGroup) + push!(model.collision_groups, group) + return model +end + +function set_contact_force_model!( + model::ContactModel, + group_i::CollisionGroup, + group_j::CollisionGroup, + force_model::ContactForceModel) + model.contact_force_models[UnorderedPair(group_i, group_j)] = force_model +end + +function collidable_pairs(model::ContactModel) + ret = CollidablePair[] + collision_groups = model.collision_groups + contact_force_models = model.contact_force_models + for i in 1 : length(collision_groups) + group_i = collision_groups[i] + for j in i : length(collision_groups) + group_j = collision_groups[j] + unordered_pair = UnorderedPair(group_i, group_j) + if haskey(contact_force_models, unordered_pair) + contact_force_model = model.contact_force_models[unordered_pair] + for k in 1 : length(group_i) + lstart = group_i == group_j ? k : 1 + a = group_i[k] + for l in lstart : length(group_j) + b = group_j[l] + push!(ret, CollidablePair(a, b, contact_force_model)) + end + end + end + end + end + return ret +end + +function num_continuous_states(model::ContactModel) + # TODO: make more efficient + pairs = collidable_pairs(model) + sum(pair -> num_states(pair.model), pairs) +end diff --git a/src/contact/halfspace.jl b/src/contact/halfspace.jl new file mode 100644 index 00000000..975855cd --- /dev/null +++ b/src/contact/halfspace.jl @@ -0,0 +1,30 @@ +struct HalfSpace{N, T} + outward_normal::SVector{N, T} + offset::T + + function HalfSpace(outward_normal::SVector{N, T}, offset::T) where {N, T} + new{N, T}(normalize(outward_normal), offset) + end +end + +function HalfSpace(outward_normal::StaticVector{N}, offset) where {N} + T = promote_type(typeof(offset), eltype(outward_normal)) + HalfSpace(SVector{N, T}(outward_normal), T(offset)) +end + +function HalfSpace(outward_normal::StaticVector{N}, point::StaticVector{N}) where N + HalfSpace(outward_normal, point ⋅ outward_normal) +end + +function (tf::AffineMap)(halfspace::HalfSpace) + linear = LinearMap(transform_deriv(tf, 0)) + normal = linear(halfspace.outward_normal) + offset = halfspace.offset + normal ⋅ tf.translation + HalfSpace(normal, offset) +end + +function (tf::Translation)(halfspace::HalfSpace) + normal = halfspace.outward_normal + offset = halfspace.offset + normal ⋅ tf.translation + HalfSpace(normal, offset) +end diff --git a/src/contact/hrep.jl b/src/contact/hrep.jl new file mode 100644 index 00000000..b6bb3ec0 --- /dev/null +++ b/src/contact/hrep.jl @@ -0,0 +1,3 @@ +struct HRep{M, N, T} + halfspaces::SVector{M, HalfSpace{N, T}} +end diff --git a/src/contact/hunt_crossley.jl b/src/contact/hunt_crossley.jl new file mode 100644 index 00000000..acf97b6b --- /dev/null +++ b/src/contact/hunt_crossley.jl @@ -0,0 +1,21 @@ +struct HuntCrossleyModel{T} + # (2) in Marhefka, Orin, "A Compliant Contact Model with Nonlinear Damping for Simulation of Robotic Systems" + k::T + λ::T + n::T +end + +function hunt_crossley_hertz(; k = 50e3, α = 0.2) + λ = 3/2 * α * k # (12) in Marhefka, Orin + HuntCrossleyModel(k, λ, 3/2) +end + +num_states(::HuntCrossleyModel) = 0 +zero_state(::HuntCrossleyModel) = nothing +devectorize(::HuntCrossleyModel, ::AbstractVector) = nothing + +function normal_contact_dynamics(model::HuntCrossleyModel, ::Nothing, z, ż) + zn = z^model.n + f = max(model.λ * zn * ż + model.k * zn, 0) # (2) in Marhefka, Orin (note: z is penetration, returning repelling force) + f, nothing +end diff --git a/src/contact/soft_contact_state.jl b/src/contact/soft_contact_state.jl new file mode 100644 index 00000000..72a37c48 --- /dev/null +++ b/src/contact/soft_contact_state.jl @@ -0,0 +1,118 @@ +struct SoftContactState{T, P, C} + x::Vector{T} # minimal description of continuous state + xsegments::Vector{SubArray{T, 1, Vector{T}, Tuple{UnitRange{Int}}, true}} + pairs::P + caches::C +end + +function SoftContactState{T}(model::ContactModel) where {T} + pairs = collidable_pairs(model) + num_x = sum(pair -> num_states(pair.model), pairs) + x = zeros(T, num_x) + xstart = 0 + xsegments = map(pairs) do pair + @assert pair.a.geometry isa GeometryTypes.Point || pair.b.geometry isa GeometryTypes.Point + xend = xstart + num_states(pair.model) + xsegment = view(x, xstart + 1 : xend) # TODO: try uview, see if it's faster + xstart = xend + xsegment + end + sorted_pairs = TypeSortedCollection(pairs) + caches = TypeSortedCollection( + map(pair -> collision_cache(T, pair.a.geometry, pair.b.geometry), pairs), + TypeSortedCollections.indices(sorted_pairs) + ) + ret = SoftContactState(x, xsegments, sorted_pairs, caches) + reset!(ret) + return ret +end + +SoftContactState(model::ContactModel) = SoftContactState{Float64}(model) + +function Base.show(io::IO, state::SoftContactState) + print(io, "SoftContactState for ", length(state.caches), " collidable pair(s)") +end + +function reset!(state::SoftContactState) + @inbounds foreach(state.pairs, state.xsegments) do pair, xsegment + @inbounds xsegment .= vectorize(zero_state(pair.model)) + end + @inbounds foreach(state.caches) do cache + reset!(cache) + end + return state +end + +struct SoftContactResult{T} + ẋ::Vector{T} + ẋsegments::Vector{SubArray{T, 1, Vector{T}, Tuple{UnitRange{Int}}, true}} + wrenches::BodyDict{Wrench{T}} +end + +function SoftContactResult{T}(mechanism::Mechanism, model::ContactModel) where {T} + pairs = collidable_pairs(model) + num_x = sum(pair -> num_states(pair.model), pairs) + ẋ = zeros(T, num_x) + ẋstart = 0 + ẋsegments = map(pairs) do pair + ẋend = ẋstart + num_states(pair.model) + ẋsegment = view(ẋ, ẋstart + 1 : ẋend) # TODO: try uview, see if it's faster + ẋstart = ẋend + ẋsegment + end + frame = root_frame(mechanism) + wrenches = BodyDict{Wrench{T}}(b => zero(Wrench{T}, frame) for b in bodies(mechanism)) + SoftContactResult(ẋ, ẋsegments, wrenches) +end + +function SoftContactResult(mechanism::Mechanism{M}, model::ContactModel) where {M} + SoftContactResult{M}(mechanism, model) +end + +function Base.show(io::IO, state::SoftContactResult) + print(io, typeof(state), "(…)") +end + +function contact_dynamics!(contact_result::SoftContactResult, contact_state::SoftContactState, mechanism_state::MechanismState) + wrenches = contact_result.wrenches + for bodyid in keys(wrenches) + wrenches[bodyid] = zero(wrenches[bodyid]) + end + # TODO: broad phase + update_transforms!(mechanism_state) + update_twists_wrt_world!(mechanism_state) + frame = root_frame(mechanism_state.mechanism) + pairs = contact_state.pairs + xsegments = contact_state.xsegments + ẋsegments = contact_result.ẋsegments + caches = contact_state.caches + handle_contact = let mechanism_state = mechanism_state, frame = frame + function(pair, cache, xsegment, ẋsegment) + # TODO: optimize case where a or b is root + # TODO: make it so that CollisionElements store transforms as Transformations. + a_to_root = transform_to_root(mechanism_state, pair.a.bodyid, false) * pair.a.transform + b_to_root = transform_to_root(mechanism_state, pair.b.bodyid, false) * pair.b.transform + separation, normal, closest_in_a, closest_in_b = detect_contact(cache, AffineMap(a_to_root), AffineMap(b_to_root)) + model = pair.model + if separation < 0 + @assert isapprox(closest_in_a, closest_in_b; atol=1e-5) + collision_location = Point3D(frame, closest_in_a) + twist = relative_twist(mechanism_state, pair.a.bodyid, pair.b.bodyid, false) + relative_velocity = point_velocity(twist, collision_location) + pair_state = devectorize(model, xsegment) + penetration = -separation + force, pair_state_deriv = soft_contact_dynamics(model, pair_state, penetration, relative_velocity.v, normal) + ẋsegment .= vectorize(pair_state_deriv) + wrench_a = Wrench(collision_location, FreeVector3D(frame, force)) + wrenches[pair.a.bodyid] += wrench_a + wrenches[pair.b.bodyid] -= wrench_a + else + xsegment .= vectorize(zero_state(model)) + ẋsegment .= 0 + end + return nothing + end + end + @inbounds foreach(handle_contact, pairs, caches, xsegments, ẋsegments) + return contact_result +end diff --git a/src/contact/viscoelastic_coulomb.jl b/src/contact/viscoelastic_coulomb.jl new file mode 100644 index 00000000..f35b2177 --- /dev/null +++ b/src/contact/viscoelastic_coulomb.jl @@ -0,0 +1,37 @@ +struct ViscoelasticCoulombModel{T} + # See section 11.8 of Featherstone, "Rigid Body Dynamics Algorithms", 2008 + μ::T + k::T + b::T +end + + # Use a 3-vector; technically only need one state for each tangential direction, but this is easier to work with. +num_states(::ViscoelasticCoulombModel) = 3 +zero_state(::ViscoelasticCoulombModel{T}) where {T} = zero(SVector{3, T}) +devectorize(::ViscoelasticCoulombModel, x::AbstractVector) = SVector{3}(x) + +function tangential_contact_dynamics(model::ViscoelasticCoulombModel, state::SVector{3}, fnormal::Number, tangential_velocity::SVector{3}) + T = typeof(fnormal) + μ = model.μ + k = model.k + b = model.b + x = state + v = tangential_velocity + + # compute friction force that would be needed to avoid slip + fstick = -k * x - b * v + + # limit friction force to lie within friction cone + fstick_norm² = dot(fstick, fstick) + fstick_max_norm² = (μ * fnormal)^2 + ftangential = if fstick_norm² > fstick_max_norm² + fstick * sqrt(fstick_max_norm² / fstick_norm²) + else + fstick + end + + # compute contact state derivative + ẋ = (-k * x - ftangential) ./ b + + ftangential, ẋ +end diff --git a/src/custom_collections/CatVector.jl b/src/custom_collections/CatVector.jl new file mode 100644 index 00000000..05a8dc0e --- /dev/null +++ b/src/custom_collections/CatVector.jl @@ -0,0 +1,117 @@ +""" +$(TYPEDEF) + +An `AbstractVector` subtype that acts as a lazy concatenation of a number +of subvectors. +""" +struct CatVector{T, N, V<:AbstractVector{T}} <: AbstractVector{T} + vecs::NTuple{N, V} +end + +@inline Base.size(vec::CatVector) = (mapreduce(length, +, vec.vecs; init=0),) + +# Note: getindex and setindex are pretty naive. Consider precomputing map from +# index to vector upon CatVector construction. +Base.@propagate_inbounds function Base.getindex(vec::CatVector, index::Int) + @boundscheck index >= 1 || throw(BoundsError(vec, index)) + i = 1 + j = index + @inbounds while true + subvec = vec.vecs[i] + l = length(subvec) + if j <= l + return subvec[eachindex(subvec)[j]] + else + j -= l + i += 1 + end + end + throw(BoundsError(vec, index)) +end + +Base.@propagate_inbounds function Base.setindex!(vec::CatVector, val, index::Int) + @boundscheck index >= 1 || throw(BoundsError(vec, index)) + i = 1 + j = index + while true + subvec = vec.vecs[i] + l = length(subvec) + if j <= l + subvec[eachindex(subvec)[j]] = val + return val + else + j -= l + i += 1 + end + end + throw(BoundsError(vec, index)) +end + +Base.@propagate_inbounds function Base.copyto!(dest::AbstractVector{T}, src::CatVector{T}) where {T} + @boundscheck length(dest) == length(src) || throw(DimensionMismatch()) + dest_indices = eachindex(dest) + k = 1 + @inbounds for i in eachindex(src.vecs) + vec = src.vecs[i] + for j in eachindex(vec) + dest[dest_indices[k]] = vec[j] + k += 1 + end + end + return dest +end + +Base.similar(vec::CatVector) = CatVector(map(similar, vec.vecs)) +Base.similar(vec::CatVector, ::Type{T}) where {T} = CatVector(map(x -> similar(x, T), vec.vecs)) + +@noinline cat_vectors_line_up_error() = throw(ArgumentError("Subvectors must line up")) + +@inline function check_cat_vectors_line_up(x::CatVector, y::CatVector) + length(x.vecs) == length(y.vecs) || cat_vectors_line_up_error() + for i in eachindex(x.vecs) + length(x.vecs[i]) == length(y.vecs[i]) || cat_vectors_line_up_error() + end + nothing +end + +@inline check_cat_vectors_line_up(x::CatVector, y) = nothing +@inline function check_cat_vectors_line_up(x::CatVector, y, tail...) + check_cat_vectors_line_up(x, y) + check_cat_vectors_line_up(x, tail...) +end + +@propagate_inbounds function Base.copyto!(dest::CatVector, src::CatVector) + for i in eachindex(dest.vecs) + copyto!(dest.vecs[i], src.vecs[i]) + end + return dest +end + +@inline function Base.map!(f::F, dest::CatVector, args::CatVector...) where F + @boundscheck check_cat_vectors_line_up(dest, args...) + @inbounds for i in eachindex(dest.vecs) + map!(f, dest.vecs[i], map(arg -> arg.vecs[i], args)...) + end + return dest +end + +Base.@propagate_inbounds catvec_broadcast_vec(arg::CatVector, range::UnitRange, k::Int) = arg.vecs[k] +Base.@propagate_inbounds catvec_broadcast_vec(arg::AbstractVector, range::UnitRange, k::Int) = view(arg, range) +Base.@propagate_inbounds catvec_broadcast_vec(arg::Number, range::UnitRange, k::Int) = arg + +@inline function Base.copyto!(dest::CatVector, bc::Broadcast.Broadcasted{Nothing}) + flat = Broadcast.flatten(bc) + @boundscheck check_cat_vectors_line_up(dest, flat.args...) + offset = 1 + @inbounds for i in eachindex(dest.vecs) + let i = i, f = flat.f, args = flat.args + dest′ = dest.vecs[i] + range = offset : offset + length(dest′) - 1 + args′ = map(arg -> catvec_broadcast_vec(arg, range, i), args) + axes′ = (eachindex(dest′),) + copyto!(dest′, Broadcast.Broadcasted{Nothing}(f, args′, axes′)) + offset = last(range) + 1 + end + end + return dest +end diff --git a/src/custom_collections/custom_collections.jl b/src/custom_collections/custom_collections.jl index 5a53041e..f3021d6d 100644 --- a/src/custom_collections/custom_collections.jl +++ b/src/custom_collections/custom_collections.jl @@ -10,7 +10,8 @@ export CacheIndexDict, SegmentedVector, SegmentedBlockDiagonalMatrix, - UnorderedPair + UnorderedPair, + CatVector export foreach_with_extra_args, @@ -34,5 +35,6 @@ include("IndexDict.jl") include("SegmentedVector.jl") include("SegmentedBlockDiagonalMatrix.jl") include("UnorderedPair.jl") +include("CatVector.jl") end # module diff --git a/src/dynamics_result.jl b/src/dynamics_result.jl index df052068..84bb5318 100644 --- a/src/dynamics_result.jl +++ b/src/dynamics_result.jl @@ -19,14 +19,10 @@ mutable struct DynamicsResult{T, M} q̇::SegmentedVector{JointID, T, Base.OneTo{JointID}, Vector{T}} v̇::SegmentedVector{JointID, T, Base.OneTo{JointID}, Vector{T}} - ṡ::Vector{T} λ::Vector{T} - contactwrenches::BodyDict{Wrench{T}} - totalwrenches::BodyDict{Wrench{T}} accelerations::BodyDict{SpatialAcceleration{T}} jointwrenches::BodyDict{Wrench{T}} # TODO: index by joint tree index? - contact_state_derivatives::BodyDict{Vector{Vector{DefaultSoftContactStateDeriv{T}}}} # see solve_dynamics! for meaning of the following variables: L::Matrix{T} # lower triangular @@ -48,30 +44,12 @@ mutable struct DynamicsResult{T, M} q̇ = SegmentedVector(Vector{T}(undef, nq), tree_joints(mechanism), num_positions) v̇ = SegmentedVector(Vector{T}(undef, nv), tree_joints(mechanism), num_velocities) - ṡ = Vector{T}(undef, num_additional_states(mechanism)) λ = Vector{T}(undef, nc) rootframe = root_frame(mechanism) - contactwrenches = BodyDict{Wrench{T}}(b => zero(Wrench{T}, rootframe) for b in bodies(mechanism)) - totalwrenches = BodyDict{Wrench{T}}(b => zero(Wrench{T}, rootframe) for b in bodies(mechanism)) accelerations = BodyDict{SpatialAcceleration{T}}( b => zero(SpatialAcceleration{T}, rootframe, rootframe, rootframe) for b in bodies(mechanism)) jointwrenches = BodyDict{Wrench{T}}(b => zero(Wrench{T}, rootframe) for b in bodies(mechanism)) - contact_state_derivs = BodyDict{Vector{Vector{DefaultSoftContactStateDeriv{T}}}}( - b => Vector{Vector{DefaultSoftContactStateDeriv{T}}}() for b in bodies(mechanism)) - startind = Ref(1) - for body in bodies(mechanism) - for point::DefaultContactPoint{M} 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 - end L = Matrix{T}(undef, nv, nv) A = Matrix{T}(undef, nc, nc) @@ -79,7 +57,7 @@ mutable struct DynamicsResult{T, M} Y = Matrix{T}(undef, nc, nv) new{T, M}(mechanism, massmatrix, dynamicsbias, constraintjacobian, constraintbias, constraintrowranges, - q̇, v̇, ṡ, λ, contactwrenches, totalwrenches, accelerations, jointwrenches, contact_state_derivs, + q̇, v̇, λ, accelerations, jointwrenches, L, A, z, Y) end end @@ -89,20 +67,12 @@ DynamicsResult(mechanism::Mechanism{M}) where {M} = DynamicsResult{M}(mechanism) function Base.copyto!(ẋ::AbstractVector, result::DynamicsResult) nq = length(result.q̇) nv = length(result.v̇) - ns = length(result.ṡ) - @boundscheck length(ẋ) == nq + nv + ns || throw(DimensionMismatch()) + @boundscheck length(ẋ) == nq + nv || throw(DimensionMismatch()) @inbounds copyto!(ẋ, 1, result.q̇, 1, nq) @inbounds copyto!(ẋ, nq + 1, result.v̇, 1, nv) - @inbounds copyto!(ẋ, nq + nv + 1, result.ṡ, 1, ns) ẋ end -contact_state_derivatives(result::DynamicsResult, body::Union{<:RigidBody, BodyID}) = - result.contact_state_derivatives[body] -contact_wrench(result::DynamicsResult, body::Union{<:RigidBody, BodyID}) = - result.contactwrenches[body] -set_contact_wrench!(result::DynamicsResult, body::Union{<:RigidBody, BodyID}, wrench::Wrench) = - (result.contactwrenches[body] = wrench) acceleration(result::DynamicsResult, body::Union{<:RigidBody, BodyID}) = result.accelerations[body] set_acceleration!(result::DynamicsResult, body::Union{<:RigidBody, BodyID}, accel::SpatialAcceleration) = diff --git a/src/mechanism.jl b/src/mechanism.jl index 0197b19b..95ef3c70 100644 --- a/src/mechanism.jl +++ b/src/mechanism.jl @@ -10,7 +10,6 @@ state-dependent information. mutable struct Mechanism{T} graph::DirectedGraph{RigidBody{T}, Joint{T}} tree::SpanningTree{RigidBody{T}, Joint{T}} - environment::ContactEnvironment{T} gravitational_acceleration::FreeVector3D{SVector{3, T}} # TODO: consider removing modcount::Int @@ -28,8 +27,7 @@ mutable struct Mechanism{T} add_vertex!(graph, root_body) tree = SpanningTree(graph, root_body) gravitational_acceleration = FreeVector3D(default_frame(root_body), convert(SVector{3, T}, gravity)) - environment = ContactEnvironment{T}() - new{T}(graph, tree, environment, gravitational_acceleration, 0) + new{T}(graph, tree, gravitational_acceleration, 0) end end @@ -135,17 +133,8 @@ Return the number of constraints imposed by the mechanism's non-tree joints (i.e """ num_constraints(mechanism::Mechanism)::Int = mapreduce(num_constraints, +, non_tree_joints(mechanism); init=0) -""" -$(SIGNATURES) - -Return the dimension of the vector of additional states ``s`` (used for stateful contact models). -""" function num_additional_states(mechanism::Mechanism) - num_states_per_environment_element = 0 - for body in bodies(mechanism), point in contact_points(body) - num_states_per_environment_element += num_states(contact_model(point)) - end - num_states_per_environment_element * length(mechanism.environment) + error("Mechanisms no longer store additional state information. Please refer to the new contact documentation") end """ diff --git a/src/mechanism_algorithms.jl b/src/mechanism_algorithms.jl index 04b9e735..08b6b98d 100644 --- a/src/mechanism_algorithms.jl +++ b/src/mechanism_algorithms.jl @@ -493,8 +493,11 @@ function dynamics_bias!( torques end -function dynamics_bias!(result::DynamicsResult, state::MechanismState) - dynamics_bias!(result.dynamicsbias, result.accelerations, result.jointwrenches, state, result.totalwrenches) +function dynamics_bias!( + result::DynamicsResult, + state::MechanismState{X}, + externalwrenches::AbstractDict{BodyID, <:Wrench} = NullDict{BodyID, Wrench{X}}()) where {X} + dynamics_bias!(result.dynamicsbias, result.accelerations, result.jointwrenches, state, externalwrenches) end """ @@ -677,50 +680,50 @@ function constraint_bias!(result::DynamicsResult, state::MechanismState{X}; constraint_bias!(result.constraintbias, state; stabilization_gains=stabilization_gains) end -function contact_dynamics!(result::DynamicsResult{T, M}, state::MechanismState{X, M, C}) where {X, M, C, T} - update_transforms!(state) - update_twists_wrt_world!(state) - mechanism = state.mechanism - root = root_body(mechanism) - frame = default_frame(root) - for body in bodies(mechanism) - bodyid = BodyID(body) - wrench = zero(Wrench{T}, frame) - points = contact_points(body) - if !isempty(points) - # TODO: AABB - body_to_root = transform_to_root(state, bodyid, false) - twist = twist_wrt_world(state, bodyid, false) - states_for_body = contact_states(state, bodyid) - state_derivs_for_body = contact_state_derivatives(result, bodyid) - for i = 1 : length(points) - @inbounds c = points[i] - point = body_to_root * location(c) - velocity = point_velocity(twist, point) - 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) - # TODO: would be good to move this to Contact module - # arguments: model, state, state_deriv, point, velocity, primitive - if point_inside(primitive, point) - separation, normal = Contact.detect_contact(primitive, point) - force = Contact.contact_dynamics!(contact_state_deriv, contact_state, - model, -separation, velocity, normal) - wrench += Wrench(point, force) - else - Contact.reset!(contact_state) - Contact.zero!(contact_state_deriv) - end - end - end - end - set_contact_wrench!(result, body, wrench) - end -end +# function contact_dynamics!(result::DynamicsResult{T, M}, state::MechanismState{X, M, C}) where {X, M, C, T} +# update_transforms!(state) +# update_twists_wrt_world!(state) +# mechanism = state.mechanism +# root = root_body(mechanism) +# frame = default_frame(root) +# for body in bodies(mechanism) +# bodyid = BodyID(body) +# wrench = zero(Wrench{T}, frame) +# points = contact_points(body) +# if !isempty(points) +# # TODO: AABB +# body_to_root = transform_to_root(state, bodyid, false) +# twist = twist_wrt_world(state, bodyid, false) +# states_for_body = contact_states(state, bodyid) +# state_derivs_for_body = contact_state_derivatives(result, bodyid) +# for i = 1 : length(points) +# @inbounds c = points[i] +# point = body_to_root * location(c) +# velocity = point_velocity(twist, point) +# 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) +# # TODO: would be good to move this to Contact module +# # arguments: model, state, state_deriv, point, velocity, primitive +# if point_inside(primitive, point) +# separation, normal = Contact.detect_contact(primitive, point) +# force = Contact.contact_dynamics!(contact_state_deriv, contact_state, +# model, -separation, velocity, normal) +# wrench += Wrench(point, force) +# else +# Contact.reset!(contact_state) +# Contact.zero!(contact_state_deriv) +# end +# end +# end +# end +# set_contact_wrench!(result, body, wrench) +# end +# end function dynamics_solve!(result::DynamicsResult, τ::AbstractVector) # version for general scalar types @@ -847,13 +850,7 @@ function dynamics!(result::DynamicsResult, state::MechanismState{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 - bodyid = successorid(jointid, state) - contactwrench = result.contactwrenches[bodyid] - result.totalwrenches[bodyid] = haskey(externalwrenches, bodyid) ? externalwrenches[bodyid] + contactwrench : contactwrench - end - dynamics_bias!(result, state) + dynamics_bias!(result, state, externalwrenches) mass_matrix!(result, state) if has_loops(state.mechanism) constraint_jacobian!(result, state) diff --git a/src/mechanism_modification.jl b/src/mechanism_modification.jl index 2ac1767a..f2581093 100644 --- a/src/mechanism_modification.jl +++ b/src/mechanism_modification.jl @@ -337,7 +337,10 @@ function canonicalize_graph!(mechanism::Mechanism) mechanism end -add_environment_primitive!(mechanism::Mechanism, halfspace::HalfSpace3D) = push!(mechanism.environment, halfspace) +function add_environment_primitive!(mechanism::Mechanism, ::Any) + error("Mechanisms no longer store environment primitives. Please refer to the new contact documentation.") +end + """ $(SIGNATURES) diff --git a/src/mechanism_state.jl b/src/mechanism_state.jl index 279aa404..9b9e9abd 100644 --- a/src/mechanism_state.jl +++ b/src/mechanism_state.jl @@ -22,12 +22,12 @@ end $(TYPEDEF) A `MechanismState` stores state information for an entire `Mechanism`. It -contains the joint configuration and velocity vectors ``q`` and ``v``, and -a vector of additional states ``s``. In addition, it stores cache -variables that depend on ``q`` and ``v`` and are aimed at preventing double work. +contains the joint configuration and velocity vectors ``q`` and ``v``. +In addition, it stores cache variables that depend on ``q`` and ``v`` and +are aimed at preventing double work. Type parameters: -* `X`: the scalar type of the ``q``, ``v``, and ``s`` vectors. +* `X`: the scalar type of the ``q`` and ``v`` vectors. * `M`: the scalar type of the `Mechanism` * `C`: the scalar type of the cache variables (`== promote_type(X, M)`) * `JointCollection`: the type of the `treejoints` and `nontreejoints` members (a `TypeSortedCollection` subtype) @@ -55,7 +55,6 @@ struct MechanismState{X, M, C, JointCollection} # minimal representation of state q::SegmentedVector{JointID, X, Base.OneTo{JointID}, Vector{X}} # configurations v::SegmentedVector{JointID, X, Base.OneTo{JointID}, Vector{X}} # velocities - s::Vector{X} # additional state # joint-related cache joint_transforms::JointCacheDict{Transform3D{C}} @@ -74,12 +73,10 @@ struct MechanismState{X, M, C, JointCollection} bias_accelerations_wrt_world::BodyCacheDict{SpatialAcceleration{C}} inertias::BodyCacheDict{SpatialInertia{C}} crb_inertias::BodyCacheDict{SpatialInertia{C}} - contact_states::BodyCacheDict{Vector{Vector{DefaultSoftContactState{X}}}} # TODO: consider moving to separate type - function MechanismState{X, M, C, JointCollection}(m::Mechanism{M}, q::Vector{X}, v::Vector{X}, s::Vector{X}) where {X, M, C, JointCollection} + function MechanismState{X, M, C, JointCollection}(m::Mechanism{M}, q::Vector{X}, v::Vector{X}) where {X, M, C, JointCollection} @assert length(q) == num_positions(m) @assert length(v) == num_velocities(m) - @assert length(s) == num_additional_states(m) # mechanism layout nonrootbodies = collect(non_root_bodies(m)) @@ -136,21 +133,6 @@ struct MechanismState{X, M, C, JointCollection} inertias = BodyCacheDict{SpatialInertia{C}}(bodyids) crb_inertias = BodyCacheDict{SpatialInertia{C}}(bodyids) - # contact. TODO: move out of MechanismState: - contact_states = BodyCacheDict{Vector{Vector{DefaultSoftContactState{X}}}}( - BodyID(b) => Vector{Vector{DefaultSoftContactState{X}}}() for b in bodies(m)) - startind = 1 - for body in bodies(m), point in contact_points(body) - model = contact_model(point) - n = num_states(model) - push!(contact_states[body], collect(begin - s_part = view(s, startind : startind + n - 1) - contact_state = SoftContactState(model, s_part, root_frame(m)) - startind += n - contact_state - end for j = 1 : length(m.environment))) - end - # initialize state-independent cache data for root bodies: root = root_body(m) rootframe = default_frame(root) @@ -164,35 +146,33 @@ struct MechanismState{X, M, C, JointCollection} jointids, treejointids, nontreejointids, predecessor_and_successor_ids, qranges, vranges, constraintranges, support_set_masks, constraint_jacobian_structure, q_index_to_joint_id, v_index_to_joint_id, - qsegmented, vsegmented, s, + qsegmented, vsegmented, joint_transforms, joint_twists, joint_bias_accelerations, tree_joint_transforms, non_tree_joint_transforms, motion_subspaces, constraint_wrench_subspaces, - transforms_to_root, twists_wrt_world, bias_accelerations_wrt_world, inertias, crb_inertias, - contact_states) + transforms_to_root, twists_wrt_world, bias_accelerations_wrt_world, inertias, crb_inertias) end - function MechanismState{X, M, C}(mechanism::Mechanism{M}, q::Vector{X}, v::Vector{X}, s::Vector{X}) where {X, M, C} + function MechanismState{X, M, C}(mechanism::Mechanism{M}, q::Vector{X}, v::Vector{X}) where {X, M, C} JointCollection = typeof(TypeSortedCollection(joints(mechanism))) - MechanismState{X, M, C, JointCollection}(mechanism, q, v, s) + MechanismState{X, M, C, JointCollection}(mechanism, q, v) end - function MechanismState{X, M}(mechanism::Mechanism{M}, q::Vector{X}, v::Vector{X}, s::Vector{X}) where {X, M} + function MechanismState{X, M}(mechanism::Mechanism{M}, q::Vector{X}, v::Vector{X}) where {X, M} C = promote_type(X, M) - MechanismState{X, M, C}(mechanism, q, v, s) + MechanismState{X, M, C}(mechanism, q, v) end function MechanismState{X}(mechanism::Mechanism{M}) where {X, M} q = Vector{X}(undef, num_positions(mechanism)) v = Vector{X}(undef, num_velocities(mechanism)) - s = Vector{X}(undef, num_additional_states(mechanism)) - state = MechanismState{X, M}(mechanism, q, v, s) + state = MechanismState{X, M}(mechanism, q, v) zero!(state) state end end -function MechanismState(mechanism::Mechanism{M}, q::Vector{X}, v::Vector{X}, s=zeros(X, num_additional_states(mechanism))) where {X, M} - MechanismState{X, M}(mechanism, q, v, s) +function MechanismState(mechanism::Mechanism{M}, q::Vector{X}, v::Vector{X}) where {X, M} + MechanismState{X, M}(mechanism, q, v) end MechanismState(mechanism::Mechanism{M}) where {M} = MechanismState{M}(mechanism) @@ -219,13 +199,7 @@ Return the length of the joint velocity vector ``v``. """ num_velocities(state::MechanismState) = length(state.v) -""" -$(SIGNATURES) - -Return the length of the vector of additional states ``s`` (currently used -for stateful contact models). -""" -num_additional_states(state::MechanismState) = length(state.s) +num_additional_states(state::MechanismState) = error("MechanismState no longer has additional state. Please refer to new contact documentation.") state_vector_eltype(state::MechanismState{X, M, C}) where {X, M, C} = X mechanism_eltype(state::MechanismState{X, M, C}) where {X, M, C} = M @@ -264,19 +238,8 @@ function setdirty!(state::MechanismState) nothing end -""" -$(SIGNATURES) - -Reset all contact state variables. -""" function reset_contact_state!(state::MechanismState) - 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 + error("MechanismState no longer has contact state. Please refer to new contact documentation.") end """ @@ -293,7 +256,6 @@ function zero_configuration!(state::MechanismState) foreach(state.treejoints, values(segments(state.q))) do joint, qjoint zero_configuration!(qjoint, joint) end - reset_contact_state!(state) setdirty!(state) end @@ -304,7 +266,6 @@ Zero the velocity vector ``v``. Invalidates cache variables. """ function zero_velocity!(state::MechanismState) state.v .= 0 - reset_contact_state!(state) setdirty!(state) end @@ -329,7 +290,6 @@ function rand_configuration!(state::MechanismState) foreach(state.treejoints, values(segments(state.q))) do joint, qjoint rand_configuration!(qjoint, joint) end - reset_contact_state!(state) setdirty!(state) end @@ -341,7 +301,6 @@ Invalidates cache variables. """ function rand_velocity!(state::MechanismState) rand!(state.v) - reset_contact_state!(state) setdirty!(state) end @@ -375,12 +334,7 @@ vector to ensure that dependent cache variables are invalidated. """ velocity(state::MechanismState) = state.v -""" -$(SIGNATURES) - -Return the vector of additional states ``s``. -""" -additional_state(state::MechanismState) = state.s +additional_state(state::MechanismState) = error("MechanismState no longer has additional state. Please refer to new contact documentation.") for fun in (:num_velocities, :num_positions) @eval function $fun(p::TreePath{RigidBody{T}, <:Joint{T}} where {T}) @@ -396,7 +350,6 @@ Invalidates cache variables. """ function set_configuration!(state::MechanismState, joint::Joint, config) set_configuration!(configuration(state, joint), joint, config) - reset_contact_state!(state) setdirty!(state) end @@ -408,7 +361,6 @@ Invalidates cache variables. """ function set_velocity!(state::MechanismState, joint::Joint, vel) set_velocity!(velocity(state, joint), joint, vel) - reset_contact_state!(state) setdirty!(state) end @@ -432,14 +384,8 @@ function set_velocity!(state::MechanismState, v::AbstractVector) setdirty!(state) end -""" -$(SIGNATURES) - -Set the vector of additional states ``s``. -""" function set_additional_state!(state::MechanismState, s::AbstractVector) - copyto!(state.s, s) - # note: setdirty! is currently not needed because no cache variables depend on s + error("MechanismState no longer has additional state. Please refer to new contact documentation.") end """ @@ -452,7 +398,6 @@ function Base.copyto!(dest::MechanismState, src::MechanismState) @modcountcheck dest src copyto!(dest.q, src.q) copyto!(dest.v, src.v) - copyto!(dest.s, src.s) setdirty!(dest) dest end @@ -460,16 +405,14 @@ end """ $(SIGNATURES) -Copy state information in vector `src` (ordered `[q; v; s]`) to state `dest`. +Copy state information in vector `src` (ordered `[q; v]`) to state `dest`. """ function Base.copyto!(dest::MechanismState, src::AbstractVector) nq = num_positions(dest) nv = num_velocities(dest) - ns = num_additional_states(dest) - @boundscheck length(src) == nq + nv + ns || throw(DimensionMismatch()) + @boundscheck length(src) == nq + nv || throw(DimensionMismatch()) @inbounds copyto!(parent(dest.q), 1, src, 1, nq) @inbounds copyto!(parent(dest.v), 1, src, nq + 1, nv) - @inbounds copyto!(dest.s, 1, src, nq + nv + 1, ns) setdirty!(dest) dest end @@ -477,26 +420,24 @@ end """ $(SIGNATURES) -Copy state information in state `dest` to vector `src` (ordered `[q; v; s]`). +Copy state information in state `dest` to vector `src` (ordered `[q; v]`). """ function Base.copyto!(dest::AbstractVector, src::MechanismState) nq = num_positions(src) nv = num_velocities(src) - ns = num_additional_states(src) - length(dest) == nq + nv + ns || throw(DimensionMismatch()) + length(dest) == nq + nv || throw(DimensionMismatch()) @inbounds copyto!(dest, 1, src.q, 1, nq) @inbounds copyto!(dest, nq + 1, src.v, 1, nv) - @inbounds copyto!(dest, nq + nv + 1, src.s, 1, ns) dest end """ $(SIGNATURES) -Create a `Vector` that represents the same state as `state` (ordered `[q; v; s]`). +Create a `Vector` that represents the same state as `state` (ordered `[q; v]`). """ function Base.Vector{T}(state::MechanismState) where T - dest = Vector{T}(undef, num_positions(state) + num_velocities(state) + num_additional_states(state)) + dest = Vector{T}(undef, num_positions(state) + num_velocities(state)) copyto!(dest, state) end @@ -867,7 +808,9 @@ end nothing end -contact_states(state::MechanismState, body::Union{<:RigidBody, BodyID}) = state.contact_states[body] +function contact_states(state::MechanismState, body::Union{<:RigidBody, BodyID}) + error("MechanismState no longer has contact states. Please refer to new contact documentation.") +end @propagate_inbounds function newton_euler(state::MechanismState, body::Union{<:RigidBody, BodyID}, accel::SpatialAcceleration, safe=true) inertia = spatial_inertia(state, body, safe) @@ -1008,8 +951,8 @@ $(SIGNATURES) Return the homogeneous transform from `from` to `to`. """ -function relative_transform(state::MechanismState, from::CartesianFrame3D, to::CartesianFrame3D) - update_transforms!(state) +function relative_transform(state::MechanismState, from::CartesianFrame3D, to::CartesianFrame3D, safe=true) + safe && update_transforms!(state) inv(transform_to_root(state, to, false)) * transform_to_root(state, from, false) end @@ -1019,8 +962,8 @@ $(SIGNATURES) Return the twist of `body` with respect to `base`, expressed in the `Mechanism`'s root frame. """ -function relative_twist(state::MechanismState, body::Union{<:RigidBody, BodyID}, base::Union{<:RigidBody, BodyID}) - update_twists_wrt_world!(state) +function relative_twist(state::MechanismState, body::Union{<:RigidBody, BodyID}, base::Union{<:RigidBody, BodyID}, safe=true) + safe && update_twists_wrt_world!(state) -twist_wrt_world(state, base, false) + twist_wrt_world(state, body, false) end diff --git a/src/ode_integrators.jl b/src/ode_integrators.jl index 4e32bb31..2bfed1fa 100644 --- a/src/ode_integrators.jl +++ b/src/ode_integrators.jl @@ -1,12 +1,12 @@ module OdeIntegrators using LinearAlgebra -using RigidBodyDynamics using StaticArrays using DocStringExtensions using LoopThrottle -export runge_kutta_4, +export + runge_kutta_4, MuntheKaasIntegrator, ButcherTableau, OdeResultsSink, @@ -139,6 +139,17 @@ function process(storage::ExpandingStorage, t, state) nothing end +function configuration end +function velocity end +function additional_state end + +function set_configuration! end +function set_velocity! end +function set_additional_state! end + +function global_coordinates! end +function local_coordinates! end + mutable struct MuntheKaasStageCache{N, T, Q<:AbstractVector, V<:AbstractVector, S<:AbstractVector} q0::Q # global coordinates vs::SVector{N, V} # velocity vector for each stage diff --git a/src/rigid_body.jl b/src/rigid_body.jl index c8f83ead..9243c984 100644 --- a/src/rigid_body.jl +++ b/src/rigid_body.jl @@ -13,18 +13,17 @@ mutable struct RigidBody{T} name::String inertia::Union{SpatialInertia{T}, Nothing} frame_definitions::Vector{Transform3D{T}} - contact_points::Vector{DefaultContactPoint{T}} # TODO: allow different contact models id::BodyID # inertia undefined; can be used for the root of a kinematic tree function RigidBody{T}(name::String) where {T} frame = CartesianFrame3D(name) - new{T}(name, nothing, [one(Transform3D{T}, frame)], DefaultContactPoint{T}[], BodyID(-1)) + new{T}(name, nothing, [one(Transform3D{T}, frame)], BodyID(-1)) end # other bodies function RigidBody(name::String, inertia::SpatialInertia{T}) where {T} - new{T}(name, inertia, [one(Transform3D{T}, inertia.frame)], DefaultContactPoint{T}[], BodyID(-1)) + new{T}(name, inertia, [one(Transform3D{T}, inertia.frame)], BodyID(-1)) end end @@ -159,28 +158,13 @@ function change_default_frame!(body::RigidBody, new_default_frame::CartesianFram if has_defined_inertia(body) body.inertia = transform(spatial_inertia(body), old_to_new) end - for point in contact_points(body) - point.location = old_to_new * point.location - end end end -""" -$(SIGNATURES) - -Add a new contact point to the rigid body -""" -function add_contact_point!(body::RigidBody{T}, point::DefaultContactPoint{T}) where {T} - loc = location(point) - tf = fixed_transform(body, loc.frame, default_frame(body)) - point.location = tf * loc - push!(body.contact_points, point) - nothing +function add_contact_point!(body::RigidBody{T}, point::Any) where {T} + error("RigidBody objects no longer store contact points. Please refer to the new contact documentation.") end -""" -$(SIGNATURES) - -Return the contact points attached to the body as an ordered collection. -""" -contact_points(body::RigidBody) = body.contact_points +function contact_points(body::RigidBody) + error("RigidBody objects no longer store contact points. Please refer to the new contact documentation.") +end diff --git a/src/simulate.jl b/src/simulate.jl index 67ee625c..767867ce 100644 --- a/src/simulate.jl +++ b/src/simulate.jl @@ -1,4 +1,14 @@ -using RigidBodyDynamics.OdeIntegrators +OdeIntegrators.configuration(state::MechanismState) = RigidBodyDynamics.configuration(state) +OdeIntegrators.velocity(state::MechanismState) = RigidBodyDynamics.velocity(state) +OdeIntegrators.set_configuration!(state::MechanismState, q) = RigidBodyDynamics.set_configuration!(state, q) +OdeIntegrators.set_velocity!(state::MechanismState, v) = RigidBodyDynamics.set_velocity!(state, v) +OdeIntegrators.global_coordinates!(state::MechanismState, q0, ϕ) = RigidBodyDynamics.global_coordinates!(state, q0, ϕ) +OdeIntegrators.local_coordinates!(ϕ, ϕd, state::MechanismState, q0) = RigidBodyDynamics.local_coordinates!(ϕ, ϕd, state, q0) + +# TODO: remove: +OdeIntegrators.additional_state(state::MechanismState{X}) where {X} = zero(SVector{0, X}) +OdeIntegrators.set_additional_state!(state::MechanismState, s) = nothing + """ $(SIGNATURES) @@ -39,11 +49,11 @@ function simulate(state0::MechanismState{X}, final_time, control! = zero_torque! result = DynamicsResult{T}(state0.mechanism) control_torques = similar(velocity(state0)) 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) + function (v̇::AbstractArray, ṡ, t, state) + @assert length(ṡ) == 0 control!(control_torques, t, state) dynamics!(result, state, control_torques; stabilization_gains=stabilization_gains) copyto!(v̇, result.v̇) - copyto!(ṡ, result.ṡ) nothing end end diff --git a/test/runtests.jl b/test/runtests.jl index a090d8f5..741e6596 100644 --- a/test/runtests.jl +++ b/test/runtests.jl @@ -13,21 +13,21 @@ import Base.Iterators: filter import ForwardDiff import LightXML -include("test_exports.jl") -include("test_graph.jl") -include("test_custom_collections.jl") -include("test_frames.jl") -include("test_spatial.jl") +# include("test_exports.jl") +# include("test_graph.jl") +# include("test_custom_collections.jl") +# include("test_frames.jl") +# include("test_spatial.jl") include("test_contact.jl") -include("test_urdf.jl") -include("test_double_pendulum.jl") -include("test_caches.jl") -include("test_mechanism_algorithms.jl") -include("test_simulate.jl") -include("test_mechanism_modification.jl") -include("test_pd_control.jl") -include("test_notebooks.jl") +# include("test_urdf.jl") +# include("test_double_pendulum.jl") +# include("test_caches.jl") +# include("test_mechanism_algorithms.jl") +# include("test_simulate.jl") +# include("test_mechanism_modification.jl") +# include("test_pd_control.jl") +# include("test_notebooks.jl") -@testset "benchmarks" begin - @test begin include("../perf/runbenchmarks.jl"); true end -end +# @testset "benchmarks" begin +# @test begin include("../perf/runbenchmarks.jl"); true end +# end diff --git a/test/test_caches.jl b/test/test_caches.jl index 966738c4..591abbee 100644 --- a/test/test_caches.jl +++ b/test/test_caches.jl @@ -47,18 +47,6 @@ end cache = DynamicsResultCache(mechanism) cachetest(cache, result -> eltype(result.v̇)) end - - @testset "Mechanism with contact points (Issue #483)" begin - Random.seed!(3) - mechanism = randmech() - contactmodel = SoftContactModel(hunt_crossley_hertz(k = 500e3), - ViscoelasticCoulombModel(0.8, 20e3, 100.)) - body = rand(bodies(mechanism)) - add_contact_point!(body, - ContactPoint(Point3D(default_frame(body), 0.0, 0.0, 0.0), contactmodel)) - cache = DynamicsResultCache(mechanism) - cachetest(cache, result -> eltype(result.v̇)) - end end @testset "SegmentedVectorCache" begin diff --git a/test/test_custom_collections.jl b/test/test_custom_collections.jl index cc2be3ba..d45859e3 100644 --- a/test/test_custom_collections.jl +++ b/test/test_custom_collections.jl @@ -153,4 +153,73 @@ Base.axes(m::NonOneBasedMatrix) = ((1:m.m) .- 2, (1:m.n) .+ 1) dict = Dict(p1 => 3) @test dict[p2] == 3 end + + @testset "CatVector" begin + CatVector = RigidBodyDynamics.CatVector + Random.seed!(52) + vecs = ntuple(i -> rand(rand(0 : 5)), Val(10)) + l = sum(length, vecs) + x = zeros(l) + y = CatVector(vecs) + + @test length(y) == l + + x .= y + for i in eachindex(x) + @test x[i] == y[i] + end + + x .= 0 + @test x != y + copyto!(x, y) + for i in eachindex(x) + @test x[i] == y[i] + end + @test x == y + + y .= 0 + rand!(x) + y .= x .+ y .+ 1 + @test x .+ 1 == y + + allocs = let x=x, vecs=vecs + @allocated copyto!(x, RigidBodyDynamics.CatVector(vecs)) + end + @test allocs == 0 + + y2 = similar(y) + @test eltype(y2) == eltype(y) + for i in eachindex(y.vecs) + @test length(y2.vecs[i]) == length(y.vecs[i]) + @test y2.vecs[i] !== y.vecs[i] + end + + y3 = similar(y, Int) + @test eltype(y3) == Int + for i in eachindex(y.vecs) + @test length(y3.vecs[i]) == length(y.vecs[i]) + @test y3.vecs[i] !== y.vecs[i] + end + + y4 = similar(y) + copyto!(y4, y) + @test y4 == y + + y5 = similar(y) + map!(+, y5, y, y) + @test Vector(y5) == Vector(y) + Vector(y) + + z = similar(y) + rand!(z) + yvec = Vector(y) + zvec = Vector(z) + + z .= muladd.(1e-3, y, z) + zvec .= muladd.(1e-3, yvec, zvec) + @test zvec == z + allocs = let y=y, z=z + @allocated z .= muladd.(1e-3, y, z) + end + @test allocs == 0 + end end diff --git a/test/test_mechanism_algorithms.jl b/test/test_mechanism_algorithms.jl index ccda4050..1d306f0e 100644 --- a/test/test_mechanism_algorithms.jl +++ b/test/test_mechanism_algorithms.jl @@ -133,7 +133,7 @@ end mechanism = randmech() x1 = MechanismState(mechanism) rand!(x1) - @test Vector(x1) == [configuration(x1); velocity(x1); additional_state(x1)] + @test Vector(x1) == [configuration(x1); velocity(x1)] x2 = MechanismState(mechanism) rand!(x2) diff --git a/test/test_simulate.jl b/test/test_simulate.jl index 292a6320..1927b871 100644 --- a/test/test_simulate.jl +++ b/test/test_simulate.jl @@ -18,9 +18,9 @@ # use RingBufferStorage using RigidBodyDynamics.OdeIntegrators passive_dynamics! = (vd::AbstractArray, sd::AbstractArray, t, state) -> begin + @assert length(sd) == 0 dynamics!(result, state) copyto!(vd, result.v̇) - copyto!(sd, result.ṡ) nothing end storage = RingBufferStorage{Float64}(x, 3) @@ -31,98 +31,98 @@ @test isapprox(gravitational_potential_energy(x) + kinetic_energy(x), total_energy_before, atol = 1e-3) end - @testset "elastic ball drop" begin - # Drop a single rigid body with a contact point at the center of mass - # onto the floor with a conservative contact model. Check energy - # balances and bounce heights. - Random.seed!(61) - world = RigidBody{Float64}("world") - mechanism = Mechanism(world) - bodyframe = CartesianFrame3D() - body = RigidBody("body", rand(SpatialInertia{Float64}, bodyframe)) - floatingjoint = Joint("floating", QuaternionFloating{Float64}()) - attach!(mechanism, world, body, floatingjoint) - - com = center_of_mass(spatial_inertia(body)) - model = SoftContactModel(hunt_crossley_hertz(; α = 0.), ViscoelasticCoulombModel(0.5, 1e3, 1e3)) - contactpoint = ContactPoint(com, model) - add_contact_point!(body, contactpoint) - - point = Point3D(root_frame(mechanism), zero(SVector{3})) - normal = FreeVector3D(root_frame(mechanism), 0., 0., 1.) - halfspace = HalfSpace3D(point, normal) - add_environment_primitive!(mechanism, halfspace) - - state = MechanismState(mechanism) - z0 = 0.05 - zero!(state) - tf = transform_to_root(state, body) - tf = Transform3D(frame_after(floatingjoint), frame_before(floatingjoint), rotation(tf), SVector(1., 2., z0 - com.v[3])) - set_configuration!(state, floatingjoint, tf) - - energy0 = gravitational_potential_energy(state) - com_world = transform_to_root(state, body) * com - - ts, qs, vs = simulate(state, 0.5; Δt = 1e-3) - - currentsign = 0. - switches = 0 - for (t, q, v) in zip(ts, qs, vs) - set_configuration!(state, q) - set_velocity!(state, v) - twist = twist_wrt_world(state, body) - com_world = transform_to_root(state, body) * com - velocity = point_velocity(twist, com_world) - - z = com_world.v[3] - penetration = max(-z, zero(z)) - n = model.normal.n - elastic_potential_energy = model.normal.k * penetration^(n + 1) / (n + 1) - energy = elastic_potential_energy + kinetic_energy(state) + gravitational_potential_energy(state) - @test isapprox(energy0, energy; atol = 1e-2) - - newsign = sign(velocity.v[3]) - (newsign != currentsign) && (switches += 1) - currentsign = newsign - end - @test switches > 3 - end - - @testset "inclined plane" begin - θ = 0.5 # plane angle - μcrit = tan(θ) # μ > μcrit should show sticking behavior, μ < μcrit should show sliding behavior - - # set up point mass + inclined plane - world = RigidBody{Float64}("world") - mechanism = Mechanism(world) - floatingjoint = Joint("floating", QuaternionFloating{Float64}()) - body = RigidBody("body", SpatialInertia(CartesianFrame3D("inertia"), SMatrix{3, 3}(1.0I), zero(SVector{3}), 2.)) - attach!(mechanism, world, body, floatingjoint) - worldframe = root_frame(mechanism) - inclinedplane = HalfSpace3D(Point3D(worldframe, zero(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.) - contactlocation = Point3D(default_frame(body), 0., 0., 0.) - for μ in (μcrit + 1e-2, μcrit - 1e-2) - frictionmodel = ViscoelasticCoulombModel(μ, 50e3, 1e4) - m, b = deepcopy((mechanism, body)) - add_contact_point!(b, ContactPoint(contactlocation, SoftContactModel(normalmodel, frictionmodel))) - state = MechanismState(m) - simulate(state, 1., Δt = 1e-3) # settle into steady state - x1 = transform(state, contactlocation, worldframe) - simulate(state, 0.5, Δt = 1e-3) - x2 = transform(state, contactlocation, worldframe) - if μ > μcrit # should stick - @test isapprox(x1, x2, atol = 1e-4) - else # should slip - @test !isapprox(x1, x2, atol = 5e-2) - end - end - end + # @testset "elastic ball drop" begin + # # Drop a single rigid body with a contact point at the center of mass + # # onto the floor with a conservative contact model. Check energy + # # balances and bounce heights. + # Random.seed!(61) + # world = RigidBody{Float64}("world") + # mechanism = Mechanism(world) + # bodyframe = CartesianFrame3D() + # body = RigidBody("body", rand(SpatialInertia{Float64}, bodyframe)) + # floatingjoint = Joint("floating", QuaternionFloating{Float64}()) + # attach!(mechanism, world, body, floatingjoint) + + # com = center_of_mass(spatial_inertia(body)) + # model = ContactForceModel(hunt_crossley_hertz(; α = 0.), ViscoelasticCoulombModel(0.5, 1e3, 1e3)) + # contactpoint = ContactPoint(com, model) + # add_contact_point!(body, contactpoint) + + # point = Point3D(root_frame(mechanism), zero(SVector{3})) + # normal = FreeVector3D(root_frame(mechanism), 0., 0., 1.) + # halfspace = HalfSpace3D(point, normal) + # add_environment_primitive!(mechanism, halfspace) + + # state = MechanismState(mechanism) + # z0 = 0.05 + # zero!(state) + # tf = transform_to_root(state, body) + # tf = Transform3D(frame_after(floatingjoint), frame_before(floatingjoint), rotation(tf), SVector(1., 2., z0 - com.v[3])) + # set_configuration!(state, floatingjoint, tf) + + # energy0 = gravitational_potential_energy(state) + # com_world = transform_to_root(state, body) * com + + # ts, qs, vs = simulate(state, 0.5; Δt = 1e-3) + + # currentsign = 0. + # switches = 0 + # for (t, q, v) in zip(ts, qs, vs) + # set_configuration!(state, q) + # set_velocity!(state, v) + # twist = twist_wrt_world(state, body) + # com_world = transform_to_root(state, body) * com + # velocity = point_velocity(twist, com_world) + + # z = com_world.v[3] + # penetration = max(-z, zero(z)) + # n = model.normal.n + # elastic_potential_energy = model.normal.k * penetration^(n + 1) / (n + 1) + # energy = elastic_potential_energy + kinetic_energy(state) + gravitational_potential_energy(state) + # @test isapprox(energy0, energy; atol = 1e-2) + + # newsign = sign(velocity.v[3]) + # (newsign != currentsign) && (switches += 1) + # currentsign = newsign + # end + # @test switches > 3 + # end + + # @testset "inclined plane" begin + # θ = 0.5 # plane angle + # μcrit = tan(θ) # μ > μcrit should show sticking behavior, μ < μcrit should show sliding behavior + + # # set up point mass + inclined plane + # world = RigidBody{Float64}("world") + # mechanism = Mechanism(world) + # floatingjoint = Joint("floating", QuaternionFloating{Float64}()) + # body = RigidBody("body", SpatialInertia(CartesianFrame3D("inertia"), SMatrix{3, 3}(1.0I), zero(SVector{3}), 2.)) + # attach!(mechanism, world, body, floatingjoint) + # worldframe = root_frame(mechanism) + # inclinedplane = HalfSpace3D(Point3D(worldframe, zero(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.) + # contactlocation = Point3D(default_frame(body), 0., 0., 0.) + # for μ in (μcrit + 1e-2, μcrit - 1e-2) + # frictionmodel = ViscoelasticCoulombModel(μ, 50e3, 1e4) + # m, b = deepcopy((mechanism, body)) + # add_contact_point!(b, ContactPoint(contactlocation, ContactForceModel(normalmodel, frictionmodel))) + # state = MechanismState(m) + # simulate(state, 1., Δt = 1e-3) # settle into steady state + # x1 = transform(state, contactlocation, worldframe) + # simulate(state, 0.5, Δt = 1e-3) + # x2 = transform(state, contactlocation, worldframe) + # if μ > μcrit # should stick + # @test isapprox(x1, x2, atol = 1e-4) + # else # should slip + # @test !isapprox(x1, x2, atol = 5e-2) + # end + # end + # end @testset "four-bar linkage" begin # gravitational acceleration