Adds CompiledVisual type, and changes Visual, for better parsing/saving of rsons/urdfs. Retains filename if mesh for a visual came from a file. Some minor updates to RNE related code.

Author: Larbino1

RSONs/rsons/franka_panda/calibrated_frodo.rson

@@ -19,6 +19,7 @@ using VMRobotControl:
     CompiledFrameID,
     CompiledJointID,
     CompiledCoordID,
+    CompiledVisual,
     VMSFrameID,
     VMSCoordID
 using VMRobotControl: 
@@ -503,21 +504,16 @@ function Makie.plot!(plot::RobotVisualize{Tuple{C}}) where C<:MechanismCacheBund
     frames = CompiledFrameID[]
 
     foreach(vis) do v
-        # geom = normal_mesh(v.geometry)
-        geom = v.geometry
-        if isa(geom, Mesh)
-            msh = Makie.mesh!(plot, geom; 
+        v::CompiledVisual
+        msh = Makie.mesh!(plot, v.mesh; 
                 shading=plot.shading,
                 color=v.color, 
                 specular=v.specular,
                 shininess=v.shininess,
                 transparency=plot.transparency
-            )
-            push!(meshes, msh)
-            push!(frames, v.frame)
-        else
-            error("Unsupported geometry type: $(typeof(geom))")
-        end       
+        )
+        push!(meshes, msh)
+        push!(frames, v.frame)
     end
 
     function update_from_cache(cache)

ext/VMRobotControlMakieExt.jl

@@ -224,6 +224,9 @@ import DiffResults:
 import
     GeometryBasics # Use import to avoid name conflicts
 
+using GeometryBasics:
+    normal_mesh
+
 using 
     DigitalAssetExchangeFormatIO,
     FileIO,

src/VMRobotControl.jl

@@ -454,7 +454,14 @@ function _compile_component!(
     cd1 = reassign_frames(component_data,   frame_id_map)
     cd2 = reassign_joints(cd1,              joint_id_map)
     cd3 = reassign_coords(cd2,              coord_id_map)
-    cc, idx = extend(cc, cd3)
+    cd4 = isa(cd3, Visual) ? compile_visual(cd3) : cd3 # this may return a vector of compiled visuals
+    if isa(cd4, AbstractVector)
+        for cd in cd4
+            cc, idx = extend(cc, cd)
+        end
+    else
+        cc, idx = extend(cc, cd4)
+    end 
     ret_component_id_map[id] = CompiledComponentID(idx)
     cc
 end
@@ -513,7 +520,7 @@ inertances(m::CompiledMechanism) = filtertype(m.components, Inertance)
 storages(m::CompiledMechanism) = filtertype(m.components, Storage)
 dissipations(m::CompiledMechanism) = filtertype(m.components, Dissipation)
 generic_components(m::CompiledMechanism) = filtertype(m.components, GenericComponent)
-visuals(m::CompiledMechanism) = filtertype(m.components, Visual)
+visuals(m::CompiledMechanism) = filtertype(m.components, CompiledVisual)
 
 
 compile(m::Mechanism) = CompiledMechanism(m)
@@ -538,15 +545,15 @@ get_compiled_coordID(m::CompiledMechanism, c::String) = get_compiled_coordID(m.r
 get_compiled_componentID(m::CompiledMechanism, c::String) = m.component_id_map[c]
 
 get_compiled_joint(m::CompiledMechanism, jointID::CompiledJointID) = joints(m)[jointID.idx]
-get_compiled_coord(m::CompiledMechanism, coordID::CompiledCoordID) = coordinates(m)[coordID]
-get_compiled_component(m::CompiledMechanism, componentID::CompiledComponentID) = components(m)[componentID]
+get_compiled_coord(m::CompiledMechanism, coordID::CompiledCoordID) = coordinates(m)[coordID.depth][coordID.idx]
+get_compiled_component(m::CompiledMechanism, componentID::CompiledComponentID) = components(m)[componentID.idx]
 
 Base.getindex(m::CompiledMechanism, id::CompiledJointID) = get_compiled_joint(m, id)
 Base.getindex(m::CompiledMechanism, id::CompiledCoordID) = get_compiled_coord(m, id)
 Base.getindex(m::CompiledMechanism, id::CompiledComponentID) = get_compiled_component(m, id)
 
 Base.setindex!(m::CompiledMechanism, val::J, id::CompiledJointID{J}) where J = joints(m)[id.idx] = val
-Base.setindex!(m::CompiledMechanism, val::C, id::CompiledCoordID{C}) where C = coordinates(m)[id.idx] = val
+Base.setindex!(m::CompiledMechanism, val::C, id::CompiledCoordID{C}) where C = coordinates(m)[id.depth][id.idx] = val
 Base.setindex!(m::CompiledMechanism, val::C, id::CompiledComponentID{C}) where C = components(m)[id.idx] = val
 
 get_force_components(m::CompiledMechanism) = filtertype(components(m), Union{Storage, Dissipation, Inertance, GenericComponent})

src/compiled_mechanisms.jl

@@ -378,41 +378,96 @@ end
 
 #####################################################
 
+struct MeshFromFile{M} # used for urdf/rson parsing/serializing
+    mesh::M
+    path::String
+end
+
 struct Visual{G, FID} <: ComponentData{Float64}
     frame::FID
+    transform::Union{Nothing, Transform{Float64}}
     geometry::G
     color::RGBAf
     specular::Float32
     shininess::Float32
-    function Visual(frame, geometry; color=RGBAf(1.0f0, 0.0f0, 1.0f0, 1.0f0), specular=0.2f0, shininess=32.0f0)
+    function Visual(frame, transform, geometry; color=RGBAf(1.0f0, 0.0f0, 1.0f0, 1.0f0), specular=0.2f0, shininess=32.0f0)
         if isa(geometry, GeometryBasics.GeometryPrimitive)
-            _mesh = GeometryBasics.normal_mesh(geometry)
+            # _mesh = GeometryBasics.normal_mesh(geometry)
+            new{typeof(geometry), typeof(frame)}(frame, transform, geometry, color, specular, shininess)
         elseif isa(geometry, GeometryBasics.Mesh)
+            _mesh = GeometryBasics.normal_mesh(geometry)
+            new{typeof(_mesh), typeof(frame)}(frame, transform, _mesh, color, specular, shininess)
+        elseif isa(geometry, MeshFromFile{<:GeometryBasics.Mesh})
             _mesh = geometry
+            new{typeof(_mesh), typeof(frame)}(frame, transform, _mesh, color, specular, shininess)
+        elseif isa(geometry, MeshFromFile{DAEScene})
+            new{typeof(geometry), typeof(frame)}(frame, transform, geometry, color, specular, shininess)
         else
-            error("Invalid geometry type: $(typeof(geometry))")
+            error("Unrecognized geometry type: $(typeof(geometry))")
         end
-        new{typeof(_mesh), typeof(frame)}(frame, _mesh, color, specular, shininess)
     end
 
-    function Visual(name, color, geometry)
-        # legacy constructor
-        Visual(name, geometry; color=color)
-    end
+    # function Visual(name, color, geometry)
+    #     # legacy constructor
+    #     Visual(name, geometry; color=color)
+    # end
 
-    function Visual(name, frame, color, geometry)
-        @warn "Visual(name, frame, color, geometry) is deprecated. Use Visual(frame, color, geometry) instead."
-        Visual(frame, color, geometry)
-    end
-    # Keyword constructor
-    Visual(; frame, geometry, color, specular, shininess) = Visual(frame, geometry; color=color, specular=specular, shininess=shininess)
+    # function Visual(name, frame, color, geometry)
+    #     @warn "Visual(name, frame, color, geometry) is deprecated. Use Visual(frame, color, geometry) instead."
+    #     Visual(frame, color, geometry)
+    # end
+    # # Keyword constructor
+    Visual(; frame, transform, geometry, color, specular, shininess) = Visual(frame, transform, geometry; color, specular, shininess)
 end
 
 function Base.:*(tf::Transform, v::Visual)
-    Visual(v.frame, v.color, VMRobotControl.transform_mesh(v.geometry, tf))
+    if isnothing(v.transform)
+        remake(v; transform=tf)
+    else
+        remake(v; transform=tf*v.transform)
+    end
 end
 
-
 _reassign_frames(c::Visual, frd) = remake(c; frame=get_compiled_frameID(frd, c.frame))
 _reassign_joints(c::Visual, jrd) = c
 _reassign_coords(c::Visual, crd) = c
+
+struct CompiledVisual{M<:GeometryBasics.Mesh} <:ComponentData{Float64}
+    frame::FrameID
+    mesh::M
+    color::RGBAf
+    specular::Float32
+    shininess::Float32
+end
+
+function compile_visual(v::Visual)
+    if isa(v.geometry, MeshFromFile{DAEScene})
+        # If the geometry is a DAE scene, load it
+        meshes_kwargs = convert_for_glmakie(v.geometry.mesh)
+        return [
+            begin
+                color = haskey(kwargs, :color) ? kwargs[:color] : v.color
+                specular = haskey(kwargs, :specular) ? kwargs[:specular] : v.specular
+                shininess = haskey(kwargs, :shininess) ? kwargs[:shininess] : v.shininess
+                _mesh = convert(GeometryBasics.GLNormalMesh{3}, mesh)
+                transformed_mesh = VMRobotControl.transform_mesh(_mesh, v.transform)
+                M = typeof(transformed_mesh)
+                CompiledVisual{M}(v.frame, transformed_mesh, color, specular, shininess)
+            end
+            for (mesh, kwargs) in meshes_kwargs
+        ]
+    end
+
+    if isa(v.geometry, MeshFromFile{<:GeometryBasics.Mesh})
+        # If the geometry is a mesh from file, load it
+        mesh = normal_mesh(v.geometry.mesh)
+    elseif isa(v.geometry, GeometryBasics.GeometryPrimitive)
+        # Otherwise, use the geometry as is
+        mesh = normal_mesh(v.geometry)
+    elseif isa(v.geometry, GeometryBasics.Mesh)
+        mesh = normal_mesh(v.geometry)
+    else
+        error("Unrecognized geometry type: $(typeof(v.geometry))")
+    end
+    CompiledVisual(v.frame, mesh, v.color, v.specular, v.shininess)
+end

src/components/components.jl

@@ -37,6 +37,7 @@ function compute_in_coordinate_order(f, coords::Vector{<:TypeStableCollection})
     for tsc in coords
         foreach(tsc) do coord
             f(coord)
+            nothing
         end
     end
     nothing
@@ -751,7 +752,18 @@ See also [`dynamics!`](@ref)
 function new_dynamics_cache end
 
 
-# TODO
+"""
+    new_inverse_dynamics_cache(m[, T])
+
+Creates a cache with element type `::T` (defaulting to the eltype of the mechanism) for performing
+inverse dynamics computations with the Recursive Newton Euler (RNE) algorithm on  a compiled mechanism.
+
+This cache supports computation of frame transforms, velocities, velocity-product-accelerations.
+It also supports [`configuration`](@ref), [`velocity`](@ref), [`acceleration`](@ref), but 
+not [`jacobian`](@ref) for coordinates.
+
+See also [`dynamics!`](@ref)
+"""
 function new_inverse_dynamics_cache end
 
 """

src/interface.jl

@@ -623,12 +623,23 @@ function _project_forces_to_jointspace(j::RevoluteData, tf_p::Transform{T}, tf_c
 
     # The minus sign is due to the fact that the torque is expressed on the right side of the 
     # equation 
-    u = -dot(rotor(tf_p) * rotor(j.transform) * j.axis, τ_c)
+    axis = rotor(tf_p) * rotor(j.transform) * j.axis
+    u = -dot(axis, τ_c)
     return SVector(u)
 end
 
 function _project_forces_to_jointspace(j::PrismaticData, tf_p::Transform{T}, tf_c::Transform{T}, f_p::S, f_c::S, τ_p::S, τ_c::S) where {T, S<: SVector{3, T}} 
     # Rotate axis to world frame, then dot with force
-    u = -dot(rotor(tf_c) * j.axis, f_c)
+    axis = rotor(tf_c) * j.axis
+    u = -dot(axis, f_c)
+    return SVector(u)
+end
+
+function _project_forces_to_jointspace(j::RailData, tf_p::Transform{T}, tf_c::Transform{T}, f_p::S, f_c::S, τ_p::S, τ_c::S) where {T, S<: SVector{3, T}} 
+    # Rotate axis to world frame, then dot with force
+    # TODO test
+    dspline = spline_derivative(q[1]/joint.scaling, joint.spline)/joint.scaling
+    axis = rotor(tfₚ) * rotor(joint.transform) * dspline
+    u = -dot(axis, f_c)
     return SVector(u)
 end

src/joints/joint_definitions.jl

@@ -508,15 +508,15 @@ 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(Rect{3, Float64}(-widths./2, widths))
+            geom = 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 = Cylinder{Float64}(origin, extremity, expect("radius", Float64))
+            geom = 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)))
+            geom = Sphere{Float64}(origin, expect("radius", Float64))
         elseif type == "mesh"
             if isnothing(cfg.rson_path)
                 rson_path = "."
@@ -527,27 +527,23 @@ function parseVisual!(mechanism, name::JSON_STR, data, cfg::RSONParserConfig)
             mesh_path = expect("filename", String)
             path = joinpath(rson_path, mesh_path)
             mesh = load_mesh(path, cfg) # This can return multiple meshes
+            geom = MeshFromFile(mesh, mesh_path) # MeshFromFile is a wrapper for GeometryBasics.Mesh
             # visual.scale = expect("scale")
         elseif type == "meshdata"
             verts = expect("vertices", Vector{Any})
             faces = expect("faces", Vector{Any})
             # TODO verify that verts and faces are correct types/lengths
             verts = [parseVertex(vert, cfg) for vert in verts]
             faces = [parseFace(face, cfg) for face in faces]
-            mesh = GeometryBasics.Mesh(verts, faces)
+            geom = GeometryBasics.Mesh(verts, faces)
         else
             rson_throw_exception("Unrecognised visual type '$(type)'", cfg)
         end
         frame = parseFrameOutsideFramesList(mechanism, expect("frame", Union{String, OrderedDict{String, Any}}), cfg)
         color = parseMaterial(expect("material", Union{String, Vector{Any}}), cfg)
 
-        if isa(mesh, Vector{<:Tuple{<:Mesh, <:NamedTuple}})
-            for (i, (mesh, kwargs)) in enumerate(mesh)
-                add_component!(mechanism, Visual(frame, mesh; kwargs...); id=name*"_$i")
-            end
-        else
-            add_component!(mechanism, Visual(frame, color, mesh); id=name)
-        end
+        tf = zero(Transform{Float64})
+        add_component!(mechanism, Visual(frame, tf, geom; color); id=name)
         check_for_remaining_fields(data, cfg)
         nothing
     else
@@ -561,7 +557,7 @@ function load_mesh(path, cfg)
         if isa(ret, GeometryBasics.Mesh)
             return ret
         elseif isa(ret, DAEScene)
-            return convert_for_glmakie(ret)
+            return ret
         else
             throw("Unhandled mesh type $(typeof(ret))")
         end

src/rson/parsing.jl

@@ -29,7 +29,8 @@ using ..VMRobotControl:
     RevoluteData,
     PrismaticData,
     Rigid,
-    MechanismJoint
+    MechanismJoint,
+    MeshFromFile
 
 import JSON
 

src/rson/rson.jl

@@ -283,14 +283,47 @@ end
 # Visual
 
 function serialize_component(v::Visual)
-    @assert v.geometry isa Mesh
-    verts = GeometryBasics.coordinates(v.geometry)
-    faces = [Int[face...] for face in GeometryBasics.faces(v.geometry)]
-    OrderedDict(
-        "type" => "meshdata",
-        "frame" => v.frame,
-        "material" => jsonify(v.color),
-        "vertices" => verts,
-        "faces" => faces
-    )
+    if v.geometry isa Mesh
+        @warn "Serializing a mesh directly will create a large rson file."
+        verts = GeometryBasics.coordinates(v.geometry)
+        faces = [Int[face...] for face in GeometryBasics.faces(v.geometry)]
+        OrderedDict(
+            "type" => "meshdata",
+            "frame" => v.frame,
+            "material" => jsonify(v.color),
+            "vertices" => verts,
+            "faces" => faces
+        )
+    elseif v.geometry isa MeshFromFile
+        OrderedDict(
+            "type" => "mesh",
+            "frame" => v.frame,
+            "material" => jsonify(v.color),
+            "filename" => v.geometry.path 
+        )
+    elseif v.geometry isa Rect
+        OrderedDict(
+            "type" => "box",
+            "frame" => v.frame,
+            "material" => jsonify(v.color),
+            "size" => v.geometry.widths 
+        )
+    elseif v.geometry isa Cylinder
+        OrderedDict(
+            "type" => "cylinder",
+            "frame" => v.frame,
+            "material" => jsonify(v.color),
+            "radius" => v.geometry.radius,
+            "length" => norm(v.geometry.origin - v.origin.extremity)
+        )
+    elseif v.geometry isa Sphere 
+        OrderedDict(
+            "type" => "sphere",
+            "frame" => v.frame,
+            "material" => jsonify(v.color),
+            "radius" => v.geometry.r 
+        )
+    else
+        rson_throw_exception("Unrecognised visual type '$(type)'", cfg)
+    end
 end