- Updated compat to GeometryBasics 0.5

- Change new_XXXXXX_cache behaviour to run kinematics/dynamics etc. once to ensure the cache is populated with valid data
- Documentation fixes

Author: Larbino1

Project.toml

@@ -1,7 +1,7 @@
 name = "VMRobotControl"
 uuid = "7d923390-5bb2-4b12-9c06-5b46ef7ec375"
 authors = ["Daniel Larby <[email protected]>"]
-version = "0.1.0"
+version = "0.1.1"
 
 [deps]
 BenchmarkTools = "6e4b80f9-dd63-53aa-95a3-0cdb28fa8baf"
@@ -51,17 +51,17 @@ Colors = "0.12, 0.13"
 DataStructures = "0.18"
 DiffResults = "1"
 DifferentialEquations = "7"
-DigitalAssetExchangeFormatIO = "1.1.2"
+DigitalAssetExchangeFormatIO = "1.1.3"
 EzXML = "1"
 FileIO = "1"
 ForwardDiff = "0.10"
 FunctionWrappers = "1"
-GeometryBasics = "0.4"
+GeometryBasics = "0.5"
 Graphs = "1"
 JSON = "0.21"
 LoopVectorization = "0.12"
-Makie = "0.21"
-MeshIO = "0.4"
+Makie = "0.22"
+MeshIO = "0.5"
 Observables = "0.5"
 OrderedCollections = "1"
 Pkg = "1"

docs/src/api/api.md

@@ -135,12 +135,13 @@ VMRobotControl.CoordinateData
 configuration
 velocity
 jacobian
-vpa
+acceleration
 ```
 
 ### Coordinate types
 ```@docs
 CoordDifference
+CoordNorm
 CoordStack
 CoordSlice
 CoordSum

docs/src/developer/developer.md

@@ -78,6 +78,7 @@ VMRobotControl.Spherical
 VMRobotControl.Inertance
 VMRobotControl.rigid_joint_twist
 VMRobotControl._add_jacobian_transpose_times_force!
+VMRobotControl._add_opspace_force!
 VMRobotControl.jacobian_column
 VMRobotControl.state_idxs
 VMRobotControl.remake

docs/src/index.md

@@ -73,7 +73,7 @@ Please cite this library as
 and also consider citing the authors works on [optimal virtual model control](https://doi.org/10.48550/arXiv.2411.06627)
 and [robot control for robotic surgery](https://doi.org/10.1016/j.ifacol.2023.10.015).
 ```bibtex
-@misc{larby2024optimalvirtualmodelcontrol,
+@misc{Larby2024,
       title={Optimal Virtual Model Control for Robotics: Design and Tuning of Passivity-Based Controllers}, 
       author={Daniel Larby and Fulvio Forni},
       year={2024},
@@ -83,7 +83,7 @@ and [robot control for robotic surgery](https://doi.org/10.1016/j.ifacol.2023.10
       doi = {https://doi.org/10.48550/arXiv.2411.06627},
       url={https://arxiv.org/abs/2411.06627}, 
 }
-@article{LARBY20238548,
+@article{Larby2023,
     title = {A Generalized Approach to Impedance Control Design for Robotic Minimally Invasive Surgery},
     journal = {IFAC-PapersOnLine},
     year = {2023},
@@ -102,5 +102,5 @@ I was supported by my supervisor, [Fulvio Forni](https://sites.google.com/site/f
 the other students in the group.
 
 My PhD was graciously supported by the Engineering and Physical Sciences Research Council; 
-and by [CMR Surgical](https://us.cmrsurgical.com/).
+and by [CMR Surgical](https://cmrsurgical.com/).
 

docs/src/tutorials/simulating.md

@@ -1,3 +1,4 @@
+
 # Simulating with DifferentialEquations.jl
 
 ## Getting started
@@ -64,4 +65,6 @@ sol = solve(prob, Tsit5())
 sol.retcode
 ```
 
-## Using `f_setup` and `f_control`
+## Using `f_setup` and `f_control`
+
+Two callbacks, `f_setup` and `f_control` can be provided when simulating. The first runs once before the simulation starts. The second runs once per step. The examples section demonstrates their use and signatures.

src/components/components.jl

@@ -388,14 +388,13 @@ struct Visual{G, FID} <: ComponentData{Float64}
     shininess::Float32
     function Visual(frame, geometry; color=RGBAf(1.0f0, 0.0f0, 1.0f0, 1.0f0), specular=0.2f0, shininess=32.0f0)
         if isa(geometry, GeometryBasics.GeometryPrimitive)
-            _geom = hacky_normal_mesh(geometry)
+            _mesh = GeometryBasics.normal_mesh(geometry)
         elseif isa(geometry, GeometryBasics.Mesh)
-            _geom = geometry
+            _mesh = geometry
         else
             error("Invalid geometry type: $(typeof(geometry))")
         end
-        # _geom = standardize_geometrybasics_mesh(mesh)
-        new{typeof(_geom), typeof(frame)}(frame, _geom, color, specular, shininess)
+        new{typeof(_mesh), typeof(frame)}(frame, _mesh, color, specular, shininess)
     end
 
     function Visual(name, color, geometry)

src/interface.jl

@@ -706,40 +706,52 @@ See also [`control_step!`](@ref)
 """
 function new_control_cache end
 
-
+# Compiled mechanisms cache bundles
+new_kinematics_cache(m::CompiledMechanism; initialize::Bool=true) = new_kinematics_cache(m, eltype(m); initialize)
+new_jacobians_cache(m::CompiledMechanism; initialize::Bool=true) = new_jacobians_cache(m, eltype(m); initialize)
+new_rbstates_cache(m::CompiledMechanism; initialize::Bool=true) = new_rbstates_cache(m, eltype(m); initialize)
+new_dynamics_cache(m::CompiledMechanism; initialize::Bool=true) = new_dynamics_cache(m, eltype(m); initialize)
+new_inverse_dynamics_cache(m::CompiledMechanism; initialize::Bool=true) = new_inverse_dynamics_cache(m, eltype(m); initialize)
 
 function new_kinematics_cache(m::CompiledMechanism, ::Type{T}; initialize::Bool=true) where T 
     cache = MechKinematicsCache{T}(m)
     bundle = MechanismCacheBundle(m, cache)
-    if initialize
-        # As the kinematics cache is used for plotting, it would be confusing if when we plot with 
-        # a fresh cache, things look broken because the cache is full of invalid data. Therefore
-        # the default behaviour is to initialize the cache by calling `kinematics!` once.
-        kinematics!(bundle, zero(T), zeros(ndof(m))) 
-    end
+    # As the kinematics cache is used for plotting, it would be confusing if when we plot with 
+    # a fresh cache, things look broken because the cache is full of invalid data. Therefore
+    # the default behaviour is to initialize the cache by calling `kinematics!` once.
+    initialize && kinematics!(bundle, zero(T), zero_q(m)) 
     bundle
 end
-new_kinematics_cache(m::CompiledMechanism; initialize::Bool=true) = new_kinematics_cache(m, eltype(m); initialize)
 
-new_jacobians_cache(m::CompiledMechanism, ::Type{T}) where T = MechanismCacheBundle(m, MechJacobiansCache{T}(m))
-new_jacobians_cache(m::CompiledMechanism) = new_jacobians_cache(m, eltype(m))
-
-new_rbstates_cache(m::CompiledMechanism, ::Type{T}) where T = MechanismCacheBundle(m, MechRBStatesCache{T}(m))
-new_rbstates_cache(m::CompiledMechanism) = new_rbstates_cache(m, eltype(m))
-
-new_dynamics_cache(m::CompiledMechanism, ::Type{T}) where T = MechanismCacheBundle(m, MechDynamicsCache{T}(m))
-new_dynamics_cache(m::CompiledMechanism) = new_dynamics_cache(m, eltype(m))
-
-new_inverse_dynamics_cache(m::CompiledMechanism, ::Type{T}) where T = MechanismCacheBundle(m, MechRNECache{T}(m))
-new_inverse_dynamics_cache(m::CompiledMechanism) = new_inverse_dynamics_cache(m, eltype(m))
+function new_jacobians_cache(m::CompiledMechanism, ::Type{T}; initialize::Bool=true) where T
+    cache = MechJacobiansCache{T}(m)
+    bundle = MechanismCacheBundle(m, cache)
+    initialize && jacobians!(bundle, zero(T), zero_q(m))
+    bundle
+end
 
-precompute!(m::CompiledMechanism, t::Real, q::SVector, q̇::SVector, gravity::SVector{3}) = 
-    error("Deprecated. Please use the new interface with a mechanism cache constructed using `new_dynamics_cache(m)`")
+function new_rbstates_cache(m::CompiledMechanism, ::Type{T}; initialize::Bool=true) where T
+    cache = MechRBStatesCache{T}(m)
+    bundle = MechanismCacheBundle(m, cache)
+    initialize && velocity_kinematics!(bundle, zero(T), zero_q(m), zero_q̇(m))
+    bundle
+end
 
-precompute_transforms!(m::CompiledMechanism, t::Real, q::SVector) = 
-    error("Deprecated. Please use the new interface with a mechanism cache constructed using `new_kinematics_cache(m)`")
+function new_dynamics_cache(m::CompiledMechanism, ::Type{T}; initialize::Bool=true) where T
+    cache = MechDynamicsCache{T}(m)
+    bundle = MechanismCacheBundle(m, cache)
+    initialize && dynamics!(bundle, zero(T), zero_q(m), zero_q̇(m), DEFAULT_GRAVITY, zero_u(m))
+    bundle
+end
 
+function new_inverse_dynamics_cache(m::CompiledMechanism, ::Type{T}; initialize::Bool=true) where T
+    cache = MechRNECache{T}(m)
+    bundle = MechanismCacheBundle(m, cache)
+    initialize && inverse_dynamics!(bundle, zero(T), zero_q(m), zero_q̇(m), zero_q̈(m), DEFAULT_GRAVITY)
+    bundle
+end
 
+# VirtualMechanismSystem cache bundles
 function new_kinematics_cache(vms::CVMSystem, ::Type{T}; initialize::Bool=true) where T
     cache = VMSKinematicsCache{T}(vms)
     bundle = VirtualMechanismSystemCacheBundle(vms, cache)

src/mechanismcache.jl

@@ -490,6 +490,8 @@ signatures = [
     (:get_linear_acceleration,      :cache,     MechanismCacheBundle,   (CompiledFrameID,)),
     (:get_linear_jacobian,          :cache,     MechanismCacheBundle,   (CompiledFrameID,)),
     (:get_angular_jacobian,         :cache,     MechanismCacheBundle,   (CompiledFrameID,)),
+    (:get_frame_force,              :cache,     MechanismCacheBundle,   (CompiledFrameID,)),
+    (:get_frame_torque,             :cache,     MechanismCacheBundle,   (CompiledFrameID,)),  
     # Forwards to mechanism
     (:name,                         :mechanism, MechanismCacheBundle,   ()),
     (:frames,                       :mechanism, MechanismCacheBundle,   ()),
@@ -595,6 +597,10 @@ virtual_mechanism_ndof(bundle::VMSCacheBundle) = virtual_mechanism_ndof(bundle.v
 @inline _get_linear_acc(c::VirtualMechanismSystemCacheBundle, i::VMSFrameID) = _get_linear_acc(get_vms_subcache(c, i), i.idx)
 @inline get_linear_jacobian(c::VirtualMechanismSystemCacheBundle, i::VMSFrameID) = get_linear_jacobian(get_vms_subcache(c, i), i.idx)
 @inline get_angular_jacobian(c::VirtualMechanismSystemCacheBundle, i::VMSFrameID) = get_angular_jacobian(get_vms_subcache(c, i), i.idx)
+@inline get_frame_force(c::FrameRBStatesWrenchesCache, i::VMSFrameID) = error()
+@inline get_frame_torque(c::FrameRBStatesWrenchesCache, i::VMSFrameID) = error()
+
+
 @inline coordinate(c::VMSCacheBundle, id::VMSCoordID{C, S}) where {C, S} = coordinates(get_vms_subcache(c, id))[id.idx]
 
 #===================================================================================================
@@ -633,6 +639,8 @@ get_angular_acceleration(c::ComputeCache, i::CompiledFrameID) = error("angular_a
 get_linear_acceleration(c::ComputeCache, i::CompiledFrameID) = error("linear_acceleration not available from type $(typeof(c))")
 get_linear_jacobian(c::ComputeCache, i::CompiledFrameID) =  error("linear_jacobian not available from type $(typeof(c))")
 get_angular_jacobian(c::ComputeCache, i::CompiledFrameID) = error("angular_jacobian not available from type $(typeof(c))")
+get_frame_force(c::ComputeCache, i::CompiledFrameID) =      error("frame_force not available from type $(typeof(c))")
+get_frame_torque(c::ComputeCache, i::CompiledFrameID) =     error("frame_torque not available from type $(typeof(c))")
 get_frame_forces(c::ComputeCache) =                        error("frame_forces not available from type $(typeof(c))")
 get_frame_torques(c::ComputeCache) =                       error("frame_torques not available from type $(typeof(c))")
 get_inertance_matrix(c::ComputeCache) =                     error("inertance_matrix not available from type $(typeof(c))")
@@ -692,6 +700,8 @@ get_generalized_force_workspace(c::MechDynamicsCache) = c.generalized_force_work
 get_u(c::MechRNECache) = c.u
 get_frame_forces(c::MechRNECache) = c.frame_cache.forces
 get_frame_torques(c::MechRNECache) =c.frame_cache.torques
+get_frame_force(c::MechRNECache, i::CompiledFrameID) = c.frame_cache.forces[i]
+get_frame_torque(c::MechRNECache, i::CompiledFrameID) = c.frame_cache.torques[i]
 
 # Methods for num frames to avoid ambiguity
 num_frames(c::MechanismCache) = num_frames(c.frame_cache)

src/mesh_utils.jl

@@ -19,38 +19,5 @@ function rescale_mesh(mesh, scale)
 end
 
 function hacky_normal_mesh(primitive)
-    pointtype=GeometryBasics.Point3f
-    facetype=GeometryBasics.GLTriangleFace
-    normaltype=GeometryBasics.Vec3f
-    # This exists because GeometryBasics.normal_mesh does not result in correct normal vectors
-    # for visualization in GLMakie (see [this](https://discourse.julialang.org/t/mesh-plotting-in-glmakie-with-geometrybasics-are-normals-broken/118227))
-
-    # This function is a hacky workaround to fix the normal vectors of a mesh
-    # it works by duplicating vertices so that they we can associate different normal vectors
-    # to the same point, resulting in inefficient meshes.
-    
-    positions, faces = GeometryBasics.decompose_triangulate_fallback(primitive; pointtype=pointtype, facetype=facetype)
-
-    # We want to preserve any existing attributes!
-    attrs = GeometryBasics.attributes(primitive)
-    # Make sure this doesn't contain position, we'll add position explicitely via meta!
-    delete!(attrs, :position)
-
-
-    normals, normal_faces = let
-        normals = Vector{normaltype}(undef, length(faces))
-        normal_faces = Vector{facetype}(undef, length(faces))
-        for (i, face) in enumerate(faces)
-            @assert length(face) == 3
-            v1, v2, v3 = GeometryBasics.metafree.(getindex.((positions,), (face[1], face[2], face[3])))
-            # we can get away with two edges since faces are planar.
-            n = normalize(GeometryBasics.orthogonal_vector(v1, v2, v3))
-            normals[i] = n
-            normal_face = facetype(length(normals), length(normals), length(normals))
-            normal_faces[i] = normal_face
-        end
-        normals .= normalize.(normals)
-        normals, normal_faces
-    end    
-    DigitalAssetExchangeFormatIO.to_geometrybasics_mesh(positions, normals, faces, normal_faces)
+    GeometryBasics.normal_mesh(primitive)
 end

src/rson/parsing.jl

@@ -508,12 +508,12 @@ function parseVisual!(mechanism, name::JSON_STR, data, cfg::RSONParserConfig)
         expect = (field, TYPE) -> expectField(data, field, TYPE, cfg)
         if type == "box"
             widths = parseVec(expect("size", Vector{Any}), Float64, Val{3}(), cfg)
-            mesh = normal_mesh(Rect3{Float64}(-widths./2, widths))
+            mesh = normal_mesh(Rect{3, Float64}(-widths./2, widths))
         elseif type == "cylinder"
             L = expect("length", Float64)
             origin = L * SVector(0.0, 0.0, -0.5) 
             extremity = L * SVector(0.0, 0.0, 0.5) 
-            mesh = Cylinder3{Float64}(origin, extremity, expect("radius", Float64))
+            mesh = Cylinder{Float64}(origin, extremity, expect("radius", Float64))
         elseif type == "sphere"
             origin = SVector(0.0, 0.0, 0.0)
             mesh = normal_mesh(Sphere{Float64}(origin, expect("radius", Float64)))