handle massless components in URDF models (#145)

* handle massless components in URDF models

* add file

ext/Render.jl

@@ -949,4 +949,97 @@ function perp(r)
     end
 end
 
+
+# ==============================================================================
+## Visualizers
+# ==============================================================================
+
+function render!(scene, ::typeof(Multibody.BoxVisualizer), sys, sol, t)
+
+    # NOTE: This draws a solid box without the hole in the middle. Cannot figure out how to render a hollow box
+    color = get_color(sys, sol, [1, 0.2, 1, 0.9])
+    width = Float32(sol(sol.t[1], idxs=sys.width))
+    height = Float32(sol(sol.t[1], idxs=sys.height))
+    length = Float32(sol(sol.t[1], idxs=sys.length))
+
+    length_dir = sol(sol.t[1], idxs=collect(sys.render_length_dir))
+    width_dir = sol(sol.t[1], idxs=collect(sys.render_width_dir))
+    height_dir = normalize(cross(normalize(length_dir), normalize(width_dir)))
+    width_dir = normalize(cross(height_dir, length_dir))
+
+    Rfun = get_rot_fun(sol, sys.frame_a)
+    r_0a = get_fun(sol, collect(sys.frame_a.r_0)) # Origin is translated by r_shape
+    r_shape = sol(sol.t[1], idxs=collect(sys.r_shape))
+    # r = sol(sol.t[1], idxs=collect(sys.r))
+
+    R0 = [length_dir width_dir height_dir]
+    # R0 = Multibody.from_nxy(r, width_dir).R'
+    @assert isapprox(det(R0), 1.0, atol=1e-5) "Rotation matrix R0 is not a valid rotation matrix, got `R0 = $R0` with determinant `det(R0) = $(det(R0))`"
+    # NOTE: The rotation by this R and the translation with r_shape needs to be double checked
+
+    origin = Vec3f(-length/2, -width/2, -height/2) + r_shape
+    extent = Vec3f(length, width, height) 
+    thing = Makie.Rect3f(origin, extent)
+    m = mesh!(scene, thing; color, specular = Vec3f(1.5))
+    on(t) do t
+        r1 = Point3f(r_0a(t))
+        R = Rfun(t)
+        q = Rotations.QuatRotation(R*R0).q
+        Q = Makie.Quaternionf(q.v1, q.v2, q.v3, q.s)
+        Makie.transform!(m, translation=r1, rotation=Q)
+    end
+
+    true
+end
+
+function render!(scene, ::typeof(Multibody.SphereVisualizer), sys, sol, t)
+    sol(sol.t[1], idxs=sys.render)==true || return true # yes, == true
+    color = get_color(sys, sol, :purple)
+    framefun = get_frame_fun(sol, sys.frame_a)
+    radius = sol(sol.t[1], idxs=sys.radius) |> Float32
+    thing = @lift begin # Sphere
+        Ta = framefun($t)
+        coords = Ta[1:3, 4]
+        point = Point3f(coords)
+        Sphere(point, Float32(radius))
+    end
+    mesh!(scene, thing; color, specular = Vec3f(1.5), shininess=20f0, diffuse=Vec3f(1))
+end
+
+function render!(scene, ::typeof(Multibody.CylinderVisualizer), sys, sol, t)
+    color = get_color(sys, sol, :purple)
+    radius = Float32(sol(sol.t[1], idxs=sys.radius))
+    r_0a = get_fun(sol, collect(sys.frame_a.r_0))
+
+
+
+
+    length_dir = sol(sol.t[1], idxs=collect(sys.render_length_dir))
+    width_dir = randn(3,3)
+    height_dir = normalize(cross(normalize(length_dir), normalize(width_dir)))
+    width_dir = normalize(cross(height_dir, length_dir))
+
+    Rfun = get_rot_fun(sol, sys.frame_a)
+
+    R0 = [length_dir width_dir height_dir]
+
+    r1 = Point3f(0,0,0)
+    r2 = Point3f((length*length_direction)...)
+    origin = r1
+    extremity = r2
+    thing = Makie.GeometryBasics.Cylinder(origin, extremity, radius)
+    m = mesh!(scene, thing; color, specular = Vec3f(1.5), shininess=20f0, diffuse=Vec3f(1), transparency=true)
+
+
+    on(t) do t
+        r1 = Point3f(r_0a(t))
+        R = Rfun(t)
+        q = Rotations.QuatRotation(R*R0).q
+        Q = Makie.Quaternionf(q.v1, q.v2, q.v3, q.s)
+        Makie.transform!(m, translation=r1, rotation=Q)
+    end
+
+    true
+end
+
 end

ext/URDF.jl

@@ -55,7 +55,7 @@ function parse_pose(xml_pose::XMLElement)
     rot, trans
 end
 
-function parse_joint(xml_joint::XMLElement)
+function parse_joint(xml_joint::XMLElement, render_fixed, render_joints)
     urdf_joint_type = attribute(xml_joint, "type")
     name = getname(xml_joint)
 
@@ -69,7 +69,7 @@ function parse_joint(xml_joint::XMLElement)
             components = "$name = URDFRevolute(; r=$r, R=$R, n=$axis)"
         else
             components = """
-            $name = URDFRevolute(; r=$r, R=$R, n=$axis, axisflange=true)
+            $name = URDFRevolute(; r=$r, R=$R, n=$axis, axisflange=true, render=$render_joints)
             $(name)_damper = Rotational.Damper(; d=$damping)
             """
             connections = """
@@ -81,7 +81,7 @@ function parse_joint(xml_joint::XMLElement)
         axis = parse_vector(T, find_element(xml_joint, "axis"), "xyz", "1 0 0")
         damping = parse_scalar(Float64, find_element(xml_joint, "dynamics"), "damping", "0")
         if iszero(damping)
-            components = "$name = URDFPrismatic(; r=$r, R=$R, n=$axis)"
+            components = "$name = URDFPrismatic(; r=$r, R=$R, n=$axis, render=$render_joints)"
         else
             components = """
             $name = URDFPrismatic(; r=$r, R=$R, n=$axis, axisflange=true)
@@ -99,9 +99,9 @@ function parse_joint(xml_joint::XMLElement)
         if norm(r) == 0 && R == I
             components = "$name = NullJoint()" # Null joint
         elseif R == I(3)
-            components = "$name = FixedTranslation(; r=$r)"
+            components = "$name = FixedTranslation(; r=$r, render=$render_fixed)"
         else
-            components = "$name = FixedTranslation(; r=$r)"
+            components = "$name = FixedTranslation(; r=$r, render=$render_fixed)"
             @warn "Ignoring rotation of joint $name"
             # R = RotMatrix3(R)
             # components = "$name = FixedRotation(; r=$r, n = $(rotation_axis(R)), angle = $(rotation_angle(R)))"
@@ -130,6 +130,7 @@ function parse_inertia(xml_inertial::XMLElement)
         moment = R * moment * R'
         # TODO: Double-check and test inertia transform, rotation convention RotXYZ? Transformation RIR' or R'IR?
     end
+
     mass, moment, r_cm
 end
 
@@ -138,7 +139,8 @@ getname(xml_link::XMLElement) = attribute(xml_link, "name")
 function parse_geometry(xml_link::XMLElement)
     xml_geometry = find_element(xml_link, "visual", "geometry")
     if xml_geometry === nothing
-        return 0.1, 1, :sphere
+        @error "No geometry found for link $(getname(xml_link)), using sphere"
+        return (; radius=0.1), :sphere
     elseif (cylinder = find_element(xml_geometry, "cylinder")) !== nothing
         radius = parse_scalar(Float64, cylinder, "radius", "0.1")
         length = parse_scalar(Float64, cylinder, "length", "1")
@@ -166,22 +168,47 @@ function parse_geometry(xml_link::XMLElement)
     return geometry, type
 end
 
-function parse_body(graph, xml_link::XMLElement)
+function parse_body(graph, xml_link::XMLElement; min_mass)
     xml_inertial = find_element(xml_link, "inertial")
-    mass,inertia,r_cm = xml_inertial == nothing ? (0,0*I(3),zeros(3)) : parse_inertia(xml_inertial)
+    mass,inertia,r_cm = xml_inertial == nothing ? (0.0,0.0*I(3),zeros(3)) : parse_inertia(xml_inertial)
+    if min_mass > 0
+        mass = max(mass, min_mass)
+        for i = 1:3
+            inertia[i,i] = max(inertia[i,i], min_mass*0.01)
+        end
+    end
     linkname = getname(xml_link)
 
     R, r = parse_pose(find_element(xml_link, "visual", "origin"))
     color = parse_color(xml_link, "1 0 0 1")
     geometry, type = parse_geometry(xml_link)
+    if geometry === nothing
+        error("No geometry found for link $linkname")
+    end
     if R != I
         @warn "Ignoring rotation of link $linkname"
     end
-    if type === :sphere
+    if mass == 0
+        # We special case this since the dynamics becomes strange with zero mass
+        if type === :sphere
+            return "$(Symbol(linkname)) = SphereVisualizer(; radius=$(geometry.radius))" # color=$(color),
+        elseif type === :cylinder
+            if iszero(r)
+                r = [1, 0, 0]
+            end
+            return "$(Symbol(linkname)) = CylinderVisualizer(; length_direction=$(r), radius=$(geometry.radius), length=$(geometry.length))" # color=$(color),
+        elseif type === :box
+            return "$(Symbol(linkname)) = BoxVisualizer(; length_direction=[1,0,0], width_direction=[0,1,0], length=$(geometry.size[1]), width=$(geometry.size[2]), height=$(geometry.size[3]))" # color=$(color),
+        end
+
+    elseif type === :sphere
         radius = geometry.radius
         "$(Symbol(linkname)) = Body(; m=$(mass), r_cm=$(r_cm), I_11 = $(inertia[1,1]), I_22 = $(inertia[2,2]), I_33 = $(inertia[3,3]), I_21 = $(inertia[2,1]), I_31 = $(inertia[3,1]), I_32 = $(inertia[3,2]), color=$(color), radius=$(radius), sparse_I=true)"
     elseif type === :cylinder
         (; radius, length) = geometry
+        if iszero(r)
+            r = [1, 0, 0]
+        end
         r = normalize(r)*length
         "$(Symbol(linkname)) = BodyShape(; r=$(r), m=$(mass), r_cm=$(r_cm), I_11 = $(inertia[1,1]), I_22 = $(inertia[2,2]), I_33 = $(inertia[3,3]), I_21 = $(inertia[2,1]), I_31 = $(inertia[3,1]), I_32 = $(inertia[3,2]), color=$(color), radius=$(radius), sparse_I=true)"
     elseif type === :box
@@ -193,6 +220,8 @@ function parse_body(graph, xml_link::XMLElement)
             r = [1, 0, 0]
         end
         "$(Symbol(linkname)) = BodyBox(; r=$(r), body.m=$(mass), body.r_cm=$(r_cm), body.I_11 = $(inertia[1,1]), body.I_22 = $(inertia[2,2]), body.I_33 = $(inertia[3,3]), body.I_21 = $(inertia[2,1]), body.I_31 = $(inertia[3,1]), body.I_32 = $(inertia[3,2]), color=$(color), length=$(length), width=$(width), height=$(height))"
+    else
+        error("Geometry type $type not recognized")
     end
 end
 
@@ -209,11 +238,11 @@ urdf2multibody(joinpath(dirname(pathof(Multibody)), "..", "test", "urdf", "doubl
 
 ## Keyword arguments
 - `extras=false`: If `true`, the generated code will include package imports, a simulation of the model and a rendering of the model.
-- `out=nothing`: If provided, the generated code will be written to this file, otherwise the string will only be returned.
+- `out=nothing`: If provided, the generated code will be written to this file, otherwise the translated model is returned as a string.
 - `worldconnection=:rigid`: If `:rigid`, the world frame will be connected to the root link with a rigid connection. If a joint constructor is provided, this component will be instantiated and the root link is connected to the world through this, e.g., `worldconnection = FreeMotion`, `()->Prismatic(n=[0, 1, 0])` etc.
 - `modelname`: The name of the model, default is the name of the URDF file with the extension removed and the first letter upper case.
 """
-function Multibody.urdf2multibody(filename::AbstractString; extras=false, out=nothing, worldconnection = :rigid, modelname = default_modelname(filename))
+function Multibody.urdf2multibody(filename::AbstractString; extras=false, out=nothing, worldconnection = :rigid, modelname = default_modelname(filename), solver = "FBDF", render_fixed=false, render_joints = true, min_mass=0)
     gravity = 9.81
     floating::Bool = false
 
@@ -246,14 +275,14 @@ function Multibody.urdf2multibody(filename::AbstractString; extras=false, out=no
 
     # Parse all joints and possible extra connections due to axisflanges
     joints_extraconnections = map(xml_joints) do l
-        parse_joint(l)
+        parse_joint(l, render_fixed, render_joints)
     end
     joints = first.(joints_extraconnections)
     extra_connections = filter(!isempty, last.(joints_extraconnections))
 
     # Parse all bodies
     bodies = map(xml_links) do l
-        parse_body(graph, l)
+        parse_body(graph, l; min_mass)
     end
 
     roots = [v for v in vertices(graph) if indegree(graph, v) == 0]
@@ -305,7 +334,7 @@ function Multibody.urdf2multibody(filename::AbstractString; extras=false, out=no
         model = complete(model)
         ssys = structural_simplify(IRSystem(model))
         prob = ODEProblem(ssys, [], (0.0, 10.0))
-        sol = solve(prob, FBDF())
+        sol = solve(prob, $(solver)())
         plot(sol) |> display
 
         import GLMakie

src/Multibody.jl

@@ -251,4 +251,7 @@ include("robot/FullRobot.jl")
 export PlanarMechanics
 include("PlanarMechanics/PlanarMechanics.jl")
 
+export SphereVisualizer, CylinderVisualizer, BoxVisualizer
+include("visualizers.jl")
+
 end

src/visualizers.jl

@@ -0,0 +1,42 @@
+@mtkmodel Visaluzable begin
+    @components begin
+        frame_a = Frame()
+    end
+    @parameters begin
+        color[1:4] = [1.0, 0.0, 0.0, 1.0], [description = "Color of the body in animations"]
+        render = true, [description = "Render the joint in animations"]
+    end
+    @equations begin
+        frame_a.f ~ zeros(3)
+        frame_a.tau ~ zeros(3)
+    end
+end
+
+@mtkmodel SphereVisualizer begin
+    @extend () = v = Visaluzable()
+    @parameters begin
+        radius = 0.1, [description = "Radius of the sphere in animations"]
+    end
+end
+
+@mtkmodel CylinderVisualizer begin
+    @extend () = v = Visaluzable()
+    @parameters begin
+        radius = 0.1, [description = "Radius of the sphere in animations"]
+        length = 1.0, [description = "length of the cylinder"]
+        length_direction[1:3] = [1.0, 0.0, 0.0], [description = "length direction of the cylinder, resolved in frame_a"]
+    end
+end
+
+@mtkmodel BoxVisualizer begin
+    @extend () = v = Visaluzable()
+    @parameters begin
+        length_direction[1:3] = [1.0, 0.0, 0.0], [description = "length direction of the box, resolved in frame_a"]
+        width_direction[1:3] = [0.0, 1.0, 0.0], [description = "width direction of the box, resolved in frame_a"]
+        length = 1.0, [description = "length of the box"]
+        width = 1.0, [description = "width of the box"]
+        height = 1.0, [description = "height of the box"]
+        r_shape[1:3] = [0,0,0]
+    end
+end
+