add URDF importer (#143)

* add URDF importer

WIP

* simplify

* importer working reasonably well

* add docs

* move to extension

* add test

* add test targets

* try again

* add forgotten file

* add test urdf files

* tweak rendering

* handle inertia transform

Author: baggepinnen

Project.toml

@@ -18,9 +18,15 @@ StaticArrays = "90137ffa-7385-5640-81b9-e52037218182"
 
 [weakdeps]
 Makie = "ee78f7c6-11fb-53f2-987a-cfe4a2b5a57a"
+JuliaFormatter = "98e50ef6-434e-11e9-1051-2b60c6c9e899"
+LightXML = "9c8b4983-aa76-5018-a973-4c85ecc9e179"
+Graphs = "86223c79-3864-5bf0-83f7-82e725a168b6"
+MetaGraphsNext = "fa8bd995-216d-47f1-8a91-f3b68fbeb377"
+
 
 [extensions]
 Render = ["Makie"]
+URDF = ["LightXML", "Graphs", "MetaGraphsNext", "JuliaFormatter"]
 
 [compat]
 CoordinateTransformations = "0.6"
@@ -39,6 +45,10 @@ julia = "1"
 [extras]
 OrdinaryDiffEq = "1dea7af3-3e70-54e6-95c3-0bf5283fa5ed"
 Test = "8dfed614-e22c-5e08-85e1-65c5234f0b40"
+JuliaFormatter = "98e50ef6-434e-11e9-1051-2b60c6c9e899"
+LightXML = "9c8b4983-aa76-5018-a973-4c85ecc9e179"
+Graphs = "86223c79-3864-5bf0-83f7-82e725a168b6"
+MetaGraphsNext = "fa8bd995-216d-47f1-8a91-f3b68fbeb377"
 
 [targets]
-test = ["OrdinaryDiffEq", "Test"]
+test = ["OrdinaryDiffEq", "Test", "JuliaFormatter", "LightXML", "Graphs", "MetaGraphsNext"]

docs/make.jl

@@ -43,6 +43,7 @@ makedocs(;
              ],
              "Rotations and orientation" => "rotations.md",
              "3D rendering" => "rendering.md",
+             "URDF import" => "urdf.md",
          ])
 
 deploydocs(;

docs/src/urdf.md

@@ -0,0 +1,29 @@
+# URDF import
+
+Multibody.jl supports import of [URDF files](https://wiki.ros.org/urdf) by means of the function [`urdf2multibody`](@ref).
+The functionality requires the user to install and load the packages LightXML.jl, Graphs.jl, MetaGraphsNext.jl and JuliaFormatter.jl, e.g.,
+```julia
+using Pkg
+Pkg.add([
+    "LightXML",
+    "Graphs",
+    "MetaGraphsNext",
+    "JuliaFormatter"
+])
+using Multibody, LightXML, Graphs, MetaGraphsNext, JuliaFormatter
+```
+
+
+## Usage
+The following example demonstrates how to import a URDF file, the translated model is saved in file `multibody_urdf.jl`. `extras = true` makes the file self contained by including package imports, simulation and plotting.
+```julia
+filename = joinpath(dirname(pathof(Multibody)), "..", "test/doublependulum.urdf")
+out = "multibody_urdf.jl"
+urdf2multibody(filename; extras=true, out)
+
+include(out) # Include model, perform simulation and plotting
+```
+
+```@docs
+Multibody.urdf2multibody
+```

ext/Project.toml

@@ -1,7 +1,11 @@
 [deps]
 CoordinateTransformations = "150eb455-5306-5404-9cee-2592286d6298"
+Graphs = "86223c79-3864-5bf0-83f7-82e725a168b6"
+JuliaFormatter = "98e50ef6-434e-11e9-1051-2b60c6c9e899"
 JuliaSimCompiler = "8391cb6b-4921-5777-4e45-fd9aab8cb88d"
+LightXML = "9c8b4983-aa76-5018-a973-4c85ecc9e179"
 Makie = "ee78f7c6-11fb-53f2-987a-cfe4a2b5a57a"
+MetaGraphsNext = "fa8bd995-216d-47f1-8a91-f3b68fbeb377"
 ModelingToolkit = "961ee093-0014-501f-94e3-6117800e7a78"
 Multibody = "e1cad5d1-98ef-44f9-a79a-9ca4547f95b9"
 OrdinaryDiffEq = "1dea7af3-3e70-54e6-95c3-0bf5283fa5ed"

ext/Render.jl

@@ -405,6 +405,7 @@ render!(scene, ::typeof(FreeMotion), sys, sol, t) = true
 
 
 function render!(scene, ::typeof(FixedTranslation), sys, sol, t)
+    sol(sol.t[1], idxs=sys.render)==true || return true # yes, == true
     r_0a = get_fun(sol, collect(sys.frame_a.r_0))
     r_0b = get_fun(sol, collect(sys.frame_b.r_0))
     color = get_color(sys, sol, :purple)
@@ -508,7 +509,7 @@ function render!(scene, ::typeof(BodyBox), sys, sol, t)
     @assert isapprox(det(R0), 1.0, atol=1e-6)
     # NOTE: The rotation by this R and the translation with r_shape needs to be double checked
 
-    origin = Vec3f(0, -width/2, -height/2) + r_shape
+    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))
@@ -695,6 +696,9 @@ function rot_from_line(d)
     RotMatrix{3}([x y d])
 end
 
+Multibody.render!(scene, ::typeof(Multibody.URDFRevolute), sys, sol, t) = false
+Multibody.render!(scene, ::typeof(Multibody.URDFPrismatic), sys, sol, t) = false
+
 # ==============================================================================
 ## PlanarMechanics
 # ==============================================================================