Merge pull request #7 from rdeits/geoms

Let MechanismGeometries handle all the geometry parsing and generation

.travis.yml

@@ -26,9 +26,10 @@ addons:
       - libzmq3
 
 ## uncomment the following lines to override the default test script
-# script:
-#  - julia -e 'Pkg.clone("https://github.com/rdeits/MeshCat.jl"); Pkg.build("MeshCat")'
-#  - julia -e 'Pkg.clone(pwd()); Pkg.checkout("RigidBodyDynamics"); Pkg.build("MeshCatMechanisms"); Pkg.test("MeshCatMechanisms"; coverage=true)';
+script:
+ - julia -e 'Pkg.clone("https://github.com/rdeits/MeshCat.jl"); Pkg.build("MeshCat")'
+ - julia -e 'Pkg.clone("https://github.com/rdeits/MechanismGeometries.jl"); Pkg.build("MechanismGeometries")'
+ - julia -e 'Pkg.clone(pwd()); Pkg.checkout("RigidBodyDynamics"); Pkg.build("MeshCatMechanisms"); Pkg.test("MeshCatMechanisms"; coverage=true)';
 after_success:
   # push coverage results to Codecov
   - julia -e 'cd(Pkg.dir("MeshCatMechanisms")); Pkg.add("Coverage"); using Coverage; Codecov.submit(Codecov.process_folder())'

README.md

@@ -3,18 +3,35 @@
 [![Build Status](https://travis-ci.org/rdeits/MeshCatMechanisms.jl.svg?branch=master)](https://travis-ci.org/rdeits/MeshCatMechanisms.jl)
 [![codecov.io](http://codecov.io/github/rdeits/MeshCatMechanisms.jl/coverage.svg?branch=master)](http://codecov.io/github/rdeits/MeshCatMechanisms.jl?branch=master)
 
-MeshCatMechanisms.jl adds support for visualizing mechanisms and robots from [RigidBodyDynamics.jl](https://github.com/tkoolen/RigidBodyDynamics.jl) with [MeshCat.jl](https://github.com/rdeits/MeshCat.jl).
+MeshCatMechanisms.jl adds support for visualizing mechanisms and robots from [RigidBodyDynamics.jl](https://github.com/tkoolen/RigidBodyDynamics.jl) with [MeshCat.jl](https://github.com/rdeits/MeshCat.jl). All geometries are constructed using [MechanismGeometries.jl](https://github.com/rdeits/MechanismGeometries.jl). 
 
 Features:
 
 * Parsing geometry directly from URDF files
 * Animation of robot trajectories from RigidBodyDynamics.jl simulations
 * Live rendering of simulation progress using the `OdeIntegrators.OdeResultsSink` interface
 
-# Related Projects
+## Related Projects
 
 MeshCatMechanisms.jl provides similar functionality to [RigidBodyTreeInspector.jl](https://github.com/rdeits/RigidBodyTreeInspector.jl), but is built on top of the lighter-weight MeshCat viewer instead of [DrakeVisualizer.jl](https://github.com/rdeits/DrakeVisualizer.jl).
 
+# Installation 
+
+Stable release:
+
+```julia
+Pkg.add("MeshCatMechanisms")
+```
+
+Latest and greatest:
+
+```julia
+Pkg.add("MeshCatMechanisms")
+Pkg.clone("https://github.com/rdeits/MechanismGeometries.jl")
+Pkg.checkout("MeshCatMechanisms")
+Pkg.checkout("MeshCat")
+```
+
 # Usage
 
 See [mechanism-demo.ipynb](mechanism-demo.ipynb)

REQUIRE

@@ -1,12 +1,9 @@
 julia 0.6
 MeshCat 0.0.1
-
 ColorTypes 0.2.0
 CoordinateTransformations 0.4.1
-FileIO 0.1.0
-GeometryTypes 0.4.0
 Interpolations 0.3.6
-LightXML 0.4.0
 LoopThrottle 0.0.1
-MeshIO 0.1.0
+MechanismGeometries
+MeshCat 0.0.1
 RigidBodyDynamics 0.4

mechanism-demo.ipynb

@@ -31,8 +31,8 @@
     "urdf = joinpath(Pkg.dir(\"MeshCatMechanisms\"), \"test\", \"urdf\", \"Acrobot.urdf\")\n",
     "robot = parse_urdf(Float64, urdf)\n",
     "delete!(vis)\n",
-    "mvis = MechanismVisualizer(robot, urdf, vis)\n",
-    "set_configuration!(mvis, [1.0, -0.5])"
+    "mvis = MechanismVisualizer(robot, URDFVisuals(urdf), vis)\n",
+    "set_configuration!(mvis, [0.0, 0.0])"
    ]
   },
   {
@@ -59,9 +59,8 @@
     "delete!(vis)\n",
     "mvis = MechanismVisualizer(\n",
     "    val.mechanism, \n",
-    "    ValkyrieRobot.urdfpath(),\n",
-    "    vis, \n",
-    "    package_path=[dirname(dirname(ValkyrieRobot.urdfpath()))]);"
+    "    URDFVisuals(ValkyrieRobot.urdfpath(), package_path=[dirname(dirname(ValkyrieRobot.urdfpath()))]),\n",
+    "    vis);"
    ]
   },
   {
@@ -92,6 +91,19 @@
     "set_configuration!(mvis, configuration(state))"
    ]
   },
+  {
+   "cell_type": "code",
+   "execution_count": null,
+   "metadata": {},
+   "outputs": [],
+   "source": [
+    "# Handling robots with fixed joints\n",
+    "urdf = joinpath(Pkg.dir(\"MeshCatMechanisms\"), \"test\", \"urdf\", \"Acrobot.urdf\")\n",
+    "robot = parse_urdf(Float64, urdf)\n",
+    "RigidBodyDynamics.remove_fixed_tree_joints!(robot)\n",
+    "IJuliaCell(MechanismVisualizer(robot, URDFVisuals(urdf)))"
+   ]
+  },
   {
    "cell_type": "code",
    "execution_count": null,

src/MeshCatMechanisms.jl

@@ -4,26 +4,22 @@ module MeshCatMechanisms
 
 export MechanismVisualizer,
        animate,
-       MeshCatSink
+       MeshCatSink,
+       Skeleton,
+       URDFVisuals,
+       visual_elements
 
-using LightXML
 using MeshCat
 using MeshCat: AbstractMaterial, AbstractObject, MeshMaterial
 using CoordinateTransformations
-using GeometryTypes
 using RigidBodyDynamics
-using RigidBodyDynamics.Graphs
-import RigidBodyDynamics: OdeIntegrators
 const rbd = RigidBodyDynamics
-import MeshIO
-using FileIO: load
-using ColorTypes: RGBA
 using Interpolations: interpolate, Gridded, Linear
 using LoopThrottle: @throttle
+using MechanismGeometries: visual_elements, VisualElement, Skeleton, URDFVisuals, AbstractGeometrySource
 
 include("visualizer.jl")
 include("animate.jl")
-include("parse_urdf.jl")
 include("ode_callback.jl")
 
 end # module

src/ode_callback.jl

@@ -1,4 +1,4 @@
-mutable struct MeshCatSink{M <: MechanismState} <: OdeIntegrators.OdeResultsSink
+mutable struct MeshCatSink{M <: MechanismState} <: rbd.OdeIntegrators.OdeResultsSink
     vis::MechanismVisualizer{M}
     min_wall_Δt::Float64
     last_update_wall_time::Float64
@@ -8,12 +8,12 @@ mutable struct MeshCatSink{M <: MechanismState} <: OdeIntegrators.OdeResultsSink
     end
 end
 
-function OdeIntegrators.initialize(sink::MeshCatSink, t, state)
+function rbd.OdeIntegrators.initialize(sink::MeshCatSink, t, state)
     sink.last_update_wall_time = -Inf
-    OdeIntegrators.process(sink, t, state)
+    rbd.OdeIntegrators.process(sink, t, state)
 end
 
-function OdeIntegrators.process(sink::MeshCatSink, t, state)
+function rbd.OdeIntegrators.process(sink::MeshCatSink, t, state)
     wall_Δt = time() - sink.last_update_wall_time
     if wall_Δt > sink.min_wall_Δt
         set_configuration!(sink.vis, configuration(state))

src/visualizer.jl

@@ -4,44 +4,40 @@ struct MechanismVisualizer{M <: MechanismState}
     modcount::Int
 
     function MechanismVisualizer(state::M,
-                                 frame_to_visuals::Associative{CartesianFrame3D}=create_visuals(state.mechanism),
+                                 source::AbstractGeometrySource=Skeleton(),
                                  vis::Visualizer=Visualizer()) where {M <: MechanismState}
         mvis = new{M}(state, vis, rbd.modcount(state.mechanism))
-        _set_mechanism!(mvis, frame_to_visuals)
+        elements = visual_elements(state.mechanism, source)
+        _set_mechanism!(mvis, elements)
         _render_state!(mvis)
         mvis
     end
 end
 
 MechanismVisualizer(m::Mechanism, args...) = MechanismVisualizer(MechanismState{Float64}(m), args...)
-MechanismVisualizer(m::Mechanism, fname::AbstractString, args...; kw...) =
-    MechanismVisualizer(m, parse_urdf_visuals(fname, m; kw...), args...)
-MechanismVisualizer(m::MechanismState, fname::AbstractString, args...; kw...) =
-    MechanismVisualizer(m, parse_urdf_visuals(fname, m.mechanism; kw...), args...)
 
 to_affine_map(tform::Transform3D) = AffineMap(rotation(tform), translation(tform))
 
-function _set_mechanism!(mvis::MechanismVisualizer, frame_to_visuals)
+function _set_mechanism!(mvis::MechanismVisualizer, elements::AbstractVector{<:VisualElement})
+    for (i, element) in enumerate(elements)
+        _set_element!(mvis, element, "geometry_$i")
+    end
+end
+
+function _set_element!(mvis::MechanismVisualizer, element::VisualElement, name::AbstractString)
     mechanism = mvis.state.mechanism
     vis = mvis.visualizer
-    tree = mechanism.tree # TODO: tree accessor?
-    for vertex in rbd.Graphs.vertices(tree)
-        body = vertex
-        body_ancestors = rbd.Graphs.ancestors(vertex, tree)
-        for definition in rbd.frame_definitions(body)
-            frame = definition.from
-            path = vcat(string.(reverse(body_ancestors)), string(frame))
-            frame_vis = vis[path...]
-            if frame in keys(frame_to_visuals)
-                settransform!(frame_vis, to_affine_map(definition))
-                for (i, (object, tform)) in enumerate(frame_to_visuals[frame])
-                    obj_vis = frame_vis["geometry_$i"]
-                    setobject!(obj_vis, object)
-                    settransform!(obj_vis, tform)
-                end
-            end
-        end
-    end
+    tree = mechanism.tree
+    # TODO: much of this information can be cached if this
+    # method becomes a performance bottleneck
+    body = rbd.body_fixed_frame_to_body(mechanism, element.frame)
+    body_ancestors = rbd.Graphs.ancestors(body, tree)
+    path = vcat(string.(reverse(body_ancestors)), string(element.frame))
+    frame_vis = vis[path...]
+    definition = rbd.frame_definition(body, element.frame)
+    settransform!(frame_vis, to_affine_map(definition))
+    setobject!(frame_vis[name], element.geometry, MeshLambertMaterial(color=element.color))
+    settransform!(frame_vis[name], element.transform)
 end
 
 function _render_state!(mvis::MechanismVisualizer, state::MechanismState=mvis.state)
@@ -65,7 +61,9 @@ function _render_state!(mvis::MechanismVisualizer, state::MechanismState=mvis.st
     end
 end
 
-function RigidBodyDynamics.set_configuration!(mvis::MechanismVisualizer, args...)
+function rbd.set_configuration!(mvis::MechanismVisualizer, args...)
     set_configuration!(mvis.state, args...)
     _render_state!(mvis)
 end
+
+MeshCat.IJuliaCell(mvis::MechanismVisualizer) = MeshCat.IJuliaCell(mvis.visualizer)