New contact

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"

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
 

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

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
+