diff --git a/.travis.yml b/.travis.yml index 4da2444..6726ae9 100644 --- a/.travis.yml +++ b/.travis.yml @@ -28,7 +28,7 @@ addons: ## 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.build("MeshCatMechanisms"); Pkg.test("MeshCatMechanisms"; coverage=true)' + - 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())' diff --git a/REQUIRE b/REQUIRE index dd0ae3f..f1a2c2d 100644 --- a/REQUIRE +++ b/REQUIRE @@ -1,10 +1,12 @@ julia 0.6 MeshCat -LightXML -CoordinateTransformations -GeometryTypes -RigidBodyDynamics -MeshIO -FileIO -ColorTypes +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 +RigidBodyDynamics 0.4 diff --git a/mechanism-demo.ipynb b/mechanism-demo.ipynb index 937e6f5..d0297cf 100644 --- a/mechanism-demo.ipynb +++ b/mechanism-demo.ipynb @@ -1,14 +1,5 @@ { "cells": [ - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "using Revise" - ] - }, { "cell_type": "code", "execution_count": null, @@ -27,7 +18,7 @@ "outputs": [], "source": [ "vis = Visualizer()\n", - "open(vis) # open the visualizer in a separate tab/window\n", + "# open(vis) # open the visualizer in a separate tab/window\n", "IJuliaCell(vis)" ] }, @@ -37,12 +28,11 @@ "metadata": {}, "outputs": [], "source": [ - "robot = parse_urdf(Float64, \"test/urdf/Acrobot.urdf\")\n", + "urdf = joinpath(Pkg.dir(\"MeshCatMechanisms\"), \"test\", \"urdf\", \"Acrobot.urdf\")\n", + "robot = parse_urdf(Float64, urdf)\n", "delete!(vis)\n", - "mvis = MechanismVisualizer(robot, \"test/urdf/Acrobot.urdf\", vis)\n", - "state = MechanismState(robot)\n", - "set_configuration!(state, [1.0, -0.5])\n", - "setstate!(mvis, state)" + "mvis = MechanismVisualizer(robot, urdf, vis)\n", + "set_configuration!(mvis, [1.0, -0.5])" ] }, { @@ -51,22 +41,10 @@ "metadata": {}, "outputs": [], "source": [ - "t, q, v = simulate(state, 5.0);" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "for i in 1:10:length(t)\n", - " set_configuration!(state, q[i])\n", - " setstate!(mvis, state)\n", - " if i < length(t)\n", - " sleep(t[i + 1] - t[i])\n", - " end\n", - "end" + "state = MechanismState(robot, randn(2), randn(2))\n", + "t, q, v = simulate(state, 5.0);\n", + "\n", + "animate(mvis, t, q)" ] }, { @@ -78,12 +56,12 @@ "using ValkyrieRobot\n", "\n", "val = Valkyrie();\n", - "\n", "delete!(vis)\n", - "mvis = MechanismVisualizer(val.mechanism, ValkyrieRobot.urdfpath(), vis, package_path=[dirname(dirname(ValkyrieRobot.urdfpath()))])\n", - "\n", - "state = MechanismState(val.mechanism)\n", - "setstate!(mvis, state)" + "mvis = MechanismVisualizer(\n", + " val.mechanism, \n", + " ValkyrieRobot.urdfpath(),\n", + " vis, \n", + " package_path=[dirname(dirname(ValkyrieRobot.urdfpath()))]);" ] }, { @@ -94,7 +72,7 @@ "source": [ "using ValkyrieRobot.BipedControlUtil: Side, flipsign_if_right\n", "\n", - "function initialize(state::MechanismState, val::Valkyrie, vis::MechanismVisualizer)\n", + "function initialize!(state::MechanismState, val::Valkyrie)\n", " zero!(state)\n", " mechanism = val.mechanism\n", " for side in instances(Side)\n", @@ -107,40 +85,11 @@ " set_configuration!(state, findjoint(mechanism, \"$(side)ForearmYaw\"), [1.571])\n", " end\n", " set_configuration!(state, val.basejoint, [1; 0; 0; 0; 0; 0; 1.025])\n", - " setstate!(vis, state)\n", " nothing\n", "end\n", - "\n", - "initialize(state, val, mvis)" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "robot = parse_urdf(Float64, \"test/urdf/Acrobot_fixed.urdf\")\n", - "RigidBodyDynamics.remove_fixed_tree_joints!(robot)\n", - "delete!(vis)\n", - "mvis = MechanismVisualizer(robot, \"test/urdf/Acrobot_fixed.urdf\", vis)\n", - "\n", - "state = MechanismState(robot)\n", - "set_configuration!(state, [0.5])\n", - "setstate!(mvis, state)" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [ - "for theta in linspace(-pi, pi, 1000)\n", - " set_configuration!(state, [theta])\n", - " setstate!(mvis, state)\n", - " sleep(0.001)\n", - "end" + "state = MechanismState(val.mechanism)\n", + "initialize!(state, val)\n", + "set_configuration!(mvis, configuration(state))" ] }, { diff --git a/src/MeshCatMechanisms.jl b/src/MeshCatMechanisms.jl index 298b691..d4ef225 100644 --- a/src/MeshCatMechanisms.jl +++ b/src/MeshCatMechanisms.jl @@ -3,7 +3,8 @@ __precompile__() module MeshCatMechanisms export MechanismVisualizer, - setstate! + animate, + MeshCatSink using LightXML using MeshCat @@ -12,227 +13,17 @@ 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 -function parse_geometries(xml_geometry::XMLElement, package_path, file_path="") - geometries = Union{AbstractGeometry, AbstractMesh}[] - for xml_cylinder in get_elements_by_tagname(xml_geometry, "cylinder") - length = rbd.parse_scalar(Float32, xml_cylinder, "length") - radius = rbd.parse_scalar(Float32, xml_cylinder, "radius") - push!(geometries, HyperCylinder{3, Float32}(length, radius)) - end - for xml_box in get_elements_by_tagname(xml_geometry, "box") - size = Vec{3, Float32}(rbd.parse_vector(Float32, xml_box, "size", "0 0 0")) - push!(geometries, HyperRectangle(-size / 2, size)) - end - for xml_sphere in get_elements_by_tagname(xml_geometry, "sphere") - radius = rbd.parse_scalar(Float32, xml_sphere, "radius") - push!(geometries, HyperSphere(zero(Point{3, Float32}), radius)) - end - for xml_mesh in get_elements_by_tagname(xml_geometry, "mesh") - filename = attribute(xml_mesh, "filename") - dae_pattern = r".dae$" - replaced_extension_with_obj = false - if ismatch(dae_pattern, filename) - filename = replace(filename, dae_pattern, ".obj") - replaced_extension_with_obj = true - end - package_pattern = r"^package://" - - if ismatch(package_pattern, filename) - found_mesh = false - for package_directory in package_path - filename_in_package = joinpath(package_directory, replace(filename, package_pattern, "")) - if ispath(filename_in_package) - mesh = load(filename_in_package, GLUVMesh) - push!(geometries, mesh) - found_mesh = true - break - end - end - if !found_mesh - warning_message = "Could not find the mesh file: $(filename). I tried substituting the following folders for the 'package://' prefix: $(package_path)." - if replaced_extension_with_obj - warning_message *= " Note that I replaced the file's original extension with .obj to try to find a mesh in a format I can actually load." - end - warn(warning_message) - end - else - filename = joinpath(file_path, filename) - if ispath(filename) - mesh = load(filename) - push!(geometries, mesh) - else - warning_message = "Could not find the mesh file: $(filename)." - if replaced_extension_with_obj - warning_message *= " Note that I replaced the file's original extension with .obj to try to find a mesh in a format I can actually load." - end - warn(warning_message) - end - end - end - geometries -end - -function parse_material!(materials::Dict{String, <:AbstractMaterial}, xml_material) - if xml_material === nothing - return MeshLambertMaterial() - end - name = attribute(xml_material, "name") - material = get!(materials, name) do - MeshLambertMaterial() - end - xml_color = find_element(xml_material, "color") - if xml_color !== nothing - default = "0.7 0.7 0.7 1." - material.color = RGBA{Float32}(rbd.parse_vector(Float32, xml_color, "rgba", default)...) - end - material -end - -function parse_link!(materials::Dict, xml_link, - package_path=ros_package_path(), file_path="", tag="visual") - xml_visuals = get_elements_by_tagname(xml_link, tag) - visuals = Vector{Pair{Mesh, Transformation}} - visual_groups = map(xml_visuals) do xml_visual - xml_material = find_element(xml_visual, tag) - material = parse_material!(materials, find_element(xml_visual, "material")) - rot, trans = rbd.parse_pose(Float64, find_element(xml_visual, "origin")) - tform = AffineMap(rot, trans) - map(parse_geometries(find_element(xml_visual, "geometry"), package_path, file_path)) do geometry - Mesh(geometry, material) => tform - end - end - reduce(vcat, [], visual_groups) -end - -function create_graph(xml_links, xml_joints) - # create graph structure of XML elements - graph = DirectedGraph{Vertex{XMLElement}, Edge{XMLElement}}() - vertices = Vertex.(xml_links) - for vertex in vertices - add_vertex!(graph, vertex) - end - name_to_vertex = Dict(attribute(data(v), "name") => v for v in vertices) - for xml_joint in xml_joints - parent = name_to_vertex[attribute(find_element(xml_joint, "parent"), "link")] - child = name_to_vertex[attribute(find_element(xml_joint, "child"), "link")] - add_edge!(graph, parent, child, Edge(xml_joint)) - end - graph -end - -ros_package_path() = split(get(ENV, "ROS_PACKAGE_PATH", ""), ':') - -function parse_urdf_visuals(filename, mechanism; - package_path=ros_package_path(), file_path="", tag="visual") - xdoc = parse_file(filename) - xroot = LightXML.root(xdoc) - @assert LightXML.name(xroot) == "robot" - - xml_links = get_elements_by_tagname(xroot, "link") - xml_joints = get_elements_by_tagname(xroot, "joint") - xml_materials = get_elements_by_tagname(xroot, "material") - - graph = create_graph(xml_links, xml_joints) - roots = collect(filter(v -> isempty(in_edges(v, graph)), rbd.Graphs.vertices(graph))) - length(roots) != 1 && error("Can only handle a single root") - tree = SpanningTree(graph, first(roots)) - - materials = Dict{String, MeshMaterial}() - for xml_material in xml_materials - parse_material!(materials, xml_material) - end - -# name_to_body = Dict(string(body) => body for body in bodies(mechanism)) - name_to_frame_and_body = Dict(string(tf.from) => (tf.from, body) for body in bodies(mechanism) for tf in rbd.frame_definitions(body)) - - visuals = Dict( - map(rbd.Graphs.vertices(tree)) do vertex - xml_link = data(vertex) - - linkname = attribute(xml_link, "name") - framename = if vertex == rbd.Graphs.root(tree) - linkname - else - xml_joint = data(edge_to_parent(vertex, tree)) - jointname = attribute(xml_joint, "name") - string("after_", jointname) # TODO: create function in RBD, call it here - end - body_frame, body = name_to_frame_and_body[framename] - - vis = parse_link!(materials, xml_link, package_path, file_path, tag) - body_frame => vis - end - ) -end - - -struct MechanismVisualizer{M <: Mechanism} - mechanism::M - visualizer::Visualizer - modcount::Int - - function MechanismVisualizer(mechanism::M, - frame_to_visuals::Associative{CartesianFrame3D}=create_visuals(mechanism), - vis::Visualizer=Visualizer()) where {M <: Mechanism} - mvis = new{M}(mechanism, vis, rbd.modcount(mechanism)) - _setmechanism!(mvis, frame_to_visuals) - mvis - end -end - -MechanismVisualizer(state::MechanismState, args...) = MechanismVisualizer(state.mechanism, args...) -MechanismVisualizer(m::Mechanism, fname::String, args...; kw...) = - MechanismVisualizer(m, parse_urdf_visuals(fname, m; kw...), args...) - -to_affine_map(tform::Transform3D) = AffineMap(rotation(tform), translation(tform)) - -function _setmechanism!(mvis::MechanismVisualizer, frame_to_visuals) - mechanism = mvis.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 = Symbol.(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[Symbol("geometry_", i)] - setobject!(obj_vis, object) - settransform!(obj_vis, tform) - end - end - end - end -end - -function setstate!(mvis::MechanismVisualizer, state::MechanismState) - @assert mvis.mechanism === state.mechanism - if rbd.modcount(state.mechanism) != mvis.modcount - error("Mechanism has been modified after creating the visualizer. Please create a new MechanismVisualizer") - end - mechanism = state.mechanism - vis = mvis.visualizer - tree = mechanism.tree # TODO: tree accessor? - for vertex in rbd.Graphs.vertices(tree) - body = vertex - if body == rbd.Graphs.root(tree) - settransform!(vis, to_affine_map(transform_to_root(state, body))) - else - body_ancestors = rbd.Graphs.ancestors(vertex, tree) - path = Symbol.(string.(reverse(body_ancestors))) - tform = relative_transform(state, default_frame(body), default_frame(body_ancestors[2])) - settransform!(vis[path...], to_affine_map(tform)) - end - end -end +include("visualizer.jl") +include("animate.jl") +include("parse_urdf.jl") +include("ode_callback.jl") end # module diff --git a/src/animate.jl b/src/animate.jl new file mode 100644 index 0000000..89c6882 --- /dev/null +++ b/src/animate.jl @@ -0,0 +1,57 @@ +""" +Interpolations.jl requires that one(::Type{T}) be defined for any data +type we want to interpolate. Rather than defining one(::Type{Vector}) here, +which might have unforeseen consequences in other packages, we'll create +a very simple wrapper type that just knows one() and * +""" +struct InterpolatableArray{A <: AbstractArray} + data::A +end + +Base.one(::Type{InterpolatableArray{A}}) where {A} = 1 +Base.:*(n::Number, a::InterpolatableArray) = n * a.data + +rbd.normalize_configuration!(joint_type::JointType, q) = nothing +function rbd.normalize_configuration!(joint_type::QuaternionFloating, q) + n = norm(q[1:4]) + for i = 1:4 + q[i] /= n + end +end + +""" + animate(vis::MechanismVisualizer, + times::Vector{Float64}, + configurations::Vector{Vector{Float64}}; + fps::Float64=60, realtimerate::Float64=1.) + +Animate the given mechanism passing through a time-coded series of +configurations by linearly interpolating the configuration vectors. +""" +function animate(vis::MechanismVisualizer, + times::Vector{Float64}, + configurations::AbstractVector{<:AbstractVector{Float64}}; + fps::Float64 = 60., realtimerate::Float64 = 1.) + @assert fps > 0 + @assert 0 < realtimerate < Inf + + state = vis.state + interp_values = [InterpolatableArray(c) for c in configurations] + interpolated_configurations = interpolate((times,), interp_values, Gridded(Linear())) + t0, tf = first(times), last(times) + framenum = 0 + walltime0 = time() + @throttle framenum while true + t = min(tf, t0 + (time() - walltime0) * realtimerate) + q = interpolated_configurations[t] + set_configuration!(state, q) + rbd.normalize_configuration!(state) + _render_state!(vis) + framenum += 1 + t == tf && break + end max_rate = fps +end + +animate(mechanism::Mechanism, times::Vector{Float64}, + configurations::Vector{Vector{Float64}}) = + animate(Visualizer(mechanism), mechanism, times, configurations) diff --git a/src/ode_callback.jl b/src/ode_callback.jl new file mode 100644 index 0000000..5a40e0f --- /dev/null +++ b/src/ode_callback.jl @@ -0,0 +1,23 @@ +mutable struct MeshCatSink{M <: MechanismState} <: OdeIntegrators.OdeResultsSink + vis::MechanismVisualizer{M} + min_wall_Δt::Float64 + last_update_wall_time::Float64 + + function MeshCatSink(vis::MechanismVisualizer{M}; max_fps::Float64 = 60.) where {M <: MechanismState} + new{M}(vis, 1 / max_fps, -Inf) + end +end + +function OdeIntegrators.initialize(sink::MeshCatSink, t, state) + sink.last_update_wall_time = -Inf + OdeIntegrators.process(sink, t, state) +end + +function 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)) + sink.last_update_wall_time = time() + end + nothing +end diff --git a/src/parse_urdf.jl b/src/parse_urdf.jl new file mode 100644 index 0000000..83a3590 --- /dev/null +++ b/src/parse_urdf.jl @@ -0,0 +1,152 @@ +function parse_geometries(xml_geometry::XMLElement, package_path, file_path="") + geometries = Union{AbstractGeometry, AbstractMesh}[] + for xml_cylinder in get_elements_by_tagname(xml_geometry, "cylinder") + length = rbd.parse_scalar(Float32, xml_cylinder, "length") + radius = rbd.parse_scalar(Float32, xml_cylinder, "radius") + push!(geometries, HyperCylinder{3, Float32}(length, radius)) + end + for xml_box in get_elements_by_tagname(xml_geometry, "box") + size = Vec{3, Float32}(rbd.parse_vector(Float32, xml_box, "size", "0 0 0")) + push!(geometries, HyperRectangle(-size / 2, size)) + end + for xml_sphere in get_elements_by_tagname(xml_geometry, "sphere") + radius = rbd.parse_scalar(Float32, xml_sphere, "radius") + push!(geometries, HyperSphere(zero(Point{3, Float32}), radius)) + end + for xml_mesh in get_elements_by_tagname(xml_geometry, "mesh") + filename = attribute(xml_mesh, "filename") + dae_pattern = r".dae$" + replaced_extension_with_obj = false + if ismatch(dae_pattern, filename) + filename = replace(filename, dae_pattern, ".obj") + replaced_extension_with_obj = true + end + package_pattern = r"^package://" + + if ismatch(package_pattern, filename) + found_mesh = false + for package_directory in package_path + filename_in_package = joinpath(package_directory, replace(filename, package_pattern, "")) + if ispath(filename_in_package) + mesh = load(filename_in_package, GLUVMesh) + push!(geometries, mesh) + found_mesh = true + break + end + end + if !found_mesh + warning_message = "Could not find the mesh file: $(filename). I tried substituting the following folders for the 'package://' prefix: $(package_path)." + if replaced_extension_with_obj + warning_message *= " Note that I replaced the file's original extension with .obj to try to find a mesh in a format I can actually load." + end + warn(warning_message) + end + else + filename = joinpath(file_path, filename) + if ispath(filename) + mesh = load(filename) + push!(geometries, mesh) + else + warning_message = "Could not find the mesh file: $(filename)." + if replaced_extension_with_obj + warning_message *= " Note that I replaced the file's original extension with .obj to try to find a mesh in a format I can actually load." + end + warn(warning_message) + end + end + end + geometries +end + +function parse_material!(materials::Dict{String, <:AbstractMaterial}, xml_material) + if xml_material === nothing + return MeshLambertMaterial() + end + name = attribute(xml_material, "name") + material = get!(materials, name) do + MeshLambertMaterial() + end + xml_color = find_element(xml_material, "color") + if xml_color !== nothing + default = "0.7 0.7 0.7 1." + material.color = RGBA{Float32}(rbd.parse_vector(Float32, xml_color, "rgba", default)...) + end + material +end + +function parse_link!(materials::Dict, xml_link, + package_path=ros_package_path(), file_path="", tag="visual") + xml_visuals = get_elements_by_tagname(xml_link, tag) + visuals = Vector{Pair{Mesh, Transformation}} + visual_groups = map(xml_visuals) do xml_visual + xml_material = find_element(xml_visual, tag) + material = parse_material!(materials, find_element(xml_visual, "material")) + rot, trans = rbd.parse_pose(Float64, find_element(xml_visual, "origin")) + tform = AffineMap(rot, trans) + map(parse_geometries(find_element(xml_visual, "geometry"), package_path, file_path)) do geometry + Mesh(geometry, material) => tform + end + end + reduce(vcat, [], visual_groups) +end + +function create_graph(xml_links, xml_joints) + # create graph structure of XML elements + graph = DirectedGraph{Vertex{XMLElement}, Edge{XMLElement}}() + vertices = Vertex.(xml_links) + for vertex in vertices + add_vertex!(graph, vertex) + end + name_to_vertex = Dict(attribute(data(v), "name") => v for v in vertices) + for xml_joint in xml_joints + parent = name_to_vertex[attribute(find_element(xml_joint, "parent"), "link")] + child = name_to_vertex[attribute(find_element(xml_joint, "child"), "link")] + add_edge!(graph, parent, child, Edge(xml_joint)) + end + graph +end + +ros_package_path() = split(get(ENV, "ROS_PACKAGE_PATH", ""), ':') + +function parse_urdf_visuals(filename, mechanism; + package_path=ros_package_path(), file_path="", tag="visual") + xdoc = parse_file(filename) + xroot = LightXML.root(xdoc) + @assert LightXML.name(xroot) == "robot" + + xml_links = get_elements_by_tagname(xroot, "link") + xml_joints = get_elements_by_tagname(xroot, "joint") + xml_materials = get_elements_by_tagname(xroot, "material") + + graph = create_graph(xml_links, xml_joints) + roots = collect(filter(v -> isempty(in_edges(v, graph)), rbd.Graphs.vertices(graph))) + length(roots) != 1 && error("Can only handle a single root") + tree = SpanningTree(graph, first(roots)) + + materials = Dict{String, MeshMaterial}() + for xml_material in xml_materials + parse_material!(materials, xml_material) + end + +# name_to_body = Dict(string(body) => body for body in bodies(mechanism)) + name_to_frame_and_body = Dict(string(tf.from) => (tf.from, body) for body in bodies(mechanism) for tf in rbd.frame_definitions(body)) + + visuals = Dict( + map(rbd.Graphs.vertices(tree)) do vertex + xml_link = data(vertex) + + linkname = attribute(xml_link, "name") + framename = if vertex == rbd.Graphs.root(tree) + linkname + else + xml_joint = data(edge_to_parent(vertex, tree)) + jointname = attribute(xml_joint, "name") + string("after_", jointname) # TODO: create function in RBD, call it here + end + body_frame, body = name_to_frame_and_body[framename] + + vis = parse_link!(materials, xml_link, package_path, file_path, tag) + body_frame => vis + end + ) +end diff --git a/src/visualizer.jl b/src/visualizer.jl new file mode 100644 index 0000000..af4f1b2 --- /dev/null +++ b/src/visualizer.jl @@ -0,0 +1,71 @@ +struct MechanismVisualizer{M <: MechanismState} + state::M + visualizer::Visualizer + modcount::Int + + function MechanismVisualizer(state::M, + frame_to_visuals::Associative{CartesianFrame3D}=create_visuals(state.mechanism), + vis::Visualizer=Visualizer()) where {M <: MechanismState} + mvis = new{M}(state, vis, rbd.modcount(state.mechanism)) + _set_mechanism!(mvis, frame_to_visuals) + _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) + 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 = Symbol.(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[Symbol("geometry_", i)] + setobject!(obj_vis, object) + settransform!(obj_vis, tform) + end + end + end + end +end + +function _render_state!(mvis::MechanismVisualizer, state::MechanismState=mvis.state) + @assert mvis.state.mechanism === state.mechanism + if rbd.modcount(state.mechanism) != mvis.modcount + error("Mechanism has been modified after creating the visualizer. Please create a new MechanismVisualizer") + end + mechanism = mvis.state.mechanism + vis = mvis.visualizer + tree = mechanism.tree # TODO: tree accessor? + for vertex in rbd.Graphs.vertices(tree) + body = vertex + if body == rbd.Graphs.root(tree) + settransform!(vis, to_affine_map(transform_to_root(state, body))) + else + body_ancestors = rbd.Graphs.ancestors(vertex, tree) + path = Symbol.(string.(reverse(body_ancestors))) + tform = relative_transform(state, default_frame(body), default_frame(body_ancestors[2])) + settransform!(vis[path...], to_affine_map(tform)) + end + end +end + +function RigidBodyDynamics.set_configuration!(mvis::MechanismVisualizer, args...) + set_configuration!(mvis.state, args...) + _render_state!(mvis) +end diff --git a/test/REQUIRE b/test/REQUIRE index 4f08330..147837a 100644 --- a/test/REQUIRE +++ b/test/REQUIRE @@ -1 +1,2 @@ -ValkyrieRobot +ValkyrieRobot 0.0.1 +NBInclude 1.1 diff --git a/test/runtests.jl b/test/runtests.jl index f3618a0..b64212d 100644 --- a/test/runtests.jl +++ b/test/runtests.jl @@ -1,42 +1,76 @@ -using MeshCatMechanisms using Base.Test -using MeshCat: Visualizer, setobject!, settransform! -using RigidBodyDynamics: Mechanism, MechanismState, parse_urdf, set_configuration! -using CoordinateTransformations: Translation -using ValkyrieRobot: Valkyrie +using MeshCat +using MeshCatMechanisms +using RigidBodyDynamics +using RigidBodyDynamics.OdeIntegrators +using ValkyrieRobot +using NBInclude vis = Visualizer() - if !haskey(ENV, "CI") - open(vis) - wait(vis) + open(vis) + wait(vis) end -@testset "acrobot" begin - urdf = "urdf/Acrobot.urdf" - robot = parse_urdf(Float64, urdf) - mvis = MechanismVisualizer(robot, urdf, vis[:acrobot, :robot]) - state = MechanismState(robot) - set_configuration!(state, [1.0, -0.5]) - setstate!(mvis, state) -end +@testset "MeshCatMechanisms" begin + @testset "URDF mechanism" begin + urdf = joinpath(@__DIR__, "urdf", "Acrobot.urdf") + robot = parse_urdf(Float64, urdf) + mvis = MechanismVisualizer(robot, urdf, vis) + set_configuration!(mvis, [1.0, -0.5]) -@testset "acrobot with fixed elbow" begin - urdf = "urdf/Acrobot_fixed.urdf" - robot = parse_urdf(Float64, urdf) - mvis = MechanismVisualizer(robot, urdf, vis[:acrobot_fixed, :robot]) - settransform!(vis[:acrobot_fixed], Translation(0, 1, 0)) - state = MechanismState(robot) - set_configuration!(state, [1.0]) - setstate!(mvis, state) -end + @testset "simulation and animation" begin + state = MechanismState(robot, randn(2), randn(2)) + t, q, v = simulate(state, 5.0); + animate(mvis, t, q) + end + end -@testset "valkyrie" begin - val = Valkyrie(); - mvis = MechanismVisualizer(val.mechanism, ValkyrieRobot.urdfpath(), vis[:valkyrie, :robot], - package_path=[dirname(dirname(ValkyrieRobot.urdfpath()))]) - settransform!(vis[:valkyrie], Translation(0, 2, 1)) - state = MechanismState(val.mechanism) - setstate!(mvis, state) -end + @testset "URDF with fixed joints" begin + urdf = joinpath(@__DIR__, "urdf", "Acrobot_fixed.urdf") + robot = parse_urdf(Float64, urdf) + RigidBodyDynamics.remove_fixed_tree_joints!(robot) + delete!(vis) + mvis = MechanismVisualizer(robot, urdf, vis) + set_configuration!(mvis, [0.5]) + + @testset "simulation and animation" begin + state = MechanismState(robot, randn(1), randn(1)) + t, q, v = simulate(state, 5.0); + animate(mvis, t, q) + end + end + + @testset "Valkyrie" begin + val = Valkyrie(); + delete!(vis) + mvis = MechanismVisualizer( + val.mechanism, + ValkyrieRobot.urdfpath(), + vis, + package_path=[dirname(dirname(ValkyrieRobot.urdfpath()))]) + end + @testset "visualization during simulation" begin + urdf = joinpath(@__DIR__, "urdf", "Acrobot.urdf") + robot = parse_urdf(Float64, urdf) + delete!(vis) + mvis = MechanismVisualizer(robot, urdf, vis) + result = DynamicsResult{Float64}(robot) + function damped_dynamics!(vd::AbstractArray, sd::AbstractArray, t, state) + damping = 2. + τ = -damping * velocity(state) + dynamics!(result, state, τ) + copy!(vd, result.v̇) + copy!(sd, result.ṡ) + nothing + end + state = MechanismState(robot, randn(2), randn(2)) + integrator = MuntheKaasIntegrator(state, damped_dynamics!, runge_kutta_4(Float64), MeshCatSink(mvis)) + integrate(integrator, 10., 1e-3, max_realtime_rate = 1.) + end + + @testset "notebook" begin + nbinclude(joinpath(@__DIR__, "..", "mechanism-demo.ipynb")) + end +end