Merge pull request #16 from JuliaRobotics/manipulate

Add mechanism manipulation with interactBase.jl

.gitignore

@@ -1,3 +1,4 @@
 *.jl.cov
 *.jl.*.cov
 *.jl.mem
+.ipynb_checkpoints

README.md

@@ -10,6 +10,7 @@ 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
+* Interactive manipulation of the mechanism configuration using [InteractBase.jl](https://github.com/piever/InteractBase.jl)
 
 ## Related Projects
 
@@ -34,7 +35,7 @@ Pkg.checkout("MeshCat")
 
 # Usage
 
-See [mechanism-demo.ipynb](mechanism-demo.ipynb)
+See [examples/demo.ipynb](examples/demo.ipynb)
 
 # Examples
 

REQUIRE

@@ -2,6 +2,7 @@ julia 0.6
 ColorTypes 0.2.0
 CoordinateTransformations 0.4.1
 Interpolations 0.3.6
+InteractBase 0.3.0
 LoopThrottle 0.0.1
 MechanismGeometries 0.0.1
 MeshCat 0.2.1

examples/interactive_manipulation.ipynb

@@ -0,0 +1,146 @@
+{
+ "cells": [
+  {
+   "cell_type": "markdown",
+   "metadata": {},
+   "source": [
+    "# Mechanism Manipulation\n",
+    "\n",
+    "MeshCatMechanisms.jl can use [InteractBase.jl](https://github.com/piever/InteractBase.jl) to let you interactively modify the configuration of a mechanism state or a mechanism's visualizer. We'll show off a few use cases here. "
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": null,
+   "metadata": {},
+   "outputs": [],
+   "source": [
+    "# Import our packages\n",
+    "using MeshCatMechanisms\n",
+    "using RigidBodyDynamics"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": null,
+   "metadata": {},
+   "outputs": [],
+   "source": [
+    "# Create a random mechanism and its associated visualizer\n",
+    "srand(75)  # seed the random number generator so we get repeatable results\n",
+    "\n",
+    "mechanism = rand_chain_mechanism(Float64, \n",
+    "    [QuaternionFloating{Float64}; [Revolute{Float64} for i = 1:5]]...)\n",
+    "mvis = MechanismVisualizer(mechanism, Skeleton(randomize_colors=true))"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": null,
+   "metadata": {},
+   "outputs": [],
+   "source": [
+    "# Render the visualizer\n",
+    "IJuliaCell(mvis)"
+   ]
+  },
+  {
+   "cell_type": "markdown",
+   "metadata": {},
+   "source": [
+    "The simplest thing we might want to do is to manipulate a visualized mechanism. To do that, we simply need the `manipulate!()` function: "
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": null,
+   "metadata": {},
+   "outputs": [],
+   "source": [
+    "# Create sliders to manipulate the visualizer's configuration\n",
+    "widget = manipulate!(mvis)"
+   ]
+  },
+  {
+   "cell_type": "markdown",
+   "metadata": {},
+   "source": [
+    "The `manipulate!()` function can also do more than just adjust a visualizer. The low-level signature of `manipulate!()` is:\n",
+    "\n",
+    "```julia\n",
+    "manipulate!(callback::Function, state::RigidBodyDynamics.MechanismState)\n",
+    "```\n",
+    "\n",
+    "This method does the following things: \n",
+    "* Creates `InteractBase.slider`s for each joint in the mechanism\n",
+    "* Sets up observers (using Observables.jl) to watch for changes to those sliders\n",
+    "* On any slider change:\n",
+    "  * Update the configuration of the provided state\n",
+    "  * Call `callback(state)`\n",
+    "* Returns the collected sliders\n",
+    "\n",
+    "The jupyter notebook itself helps by automatically displaying the resulting sliders when they are the final item in a notebook cell. To force the sliders to display, just use the `display()` function: \n",
+    "\n",
+    "```julia\n",
+    "display(manipulate!(...))\n",
+    "```\n",
+    "\n",
+    "Let's try a simple example:"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": null,
+   "metadata": {},
+   "outputs": [],
+   "source": [
+    "state = MechanismState(mechanism)\n",
+    "manipulate!(state) do x\n",
+    "    @show configuration(x)\n",
+    "end"
+   ]
+  },
+  {
+   "cell_type": "markdown",
+   "metadata": {},
+   "source": [
+    "By the way, if you're not familiar with the way `do` blocks work in Julia, see <https://docs.julialang.org/en/stable/manual/functions/#Do-Block-Syntax-for-Function-Arguments-1>"
+   ]
+  },
+  {
+   "cell_type": "markdown",
+   "metadata": {},
+   "source": [
+    "With that in mind, we can easily reconstruct the behavior of `manipulate!()` when we pass it a `MechanismVisualizer` instead of a state and a callback: "
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": null,
+   "metadata": {},
+   "outputs": [],
+   "source": [
+    "# manipulate!(mvis) does essentially the following: \n",
+    "\n",
+    "manipulate!(state) do x\n",
+    "    set_configuration!(mvis, configuration(x))\n",
+    "end"
+   ]
+  }
+ ],
+ "metadata": {
+  "kernelspec": {
+   "display_name": "Julia 0.6.3",
+   "language": "julia",
+   "name": "julia-0.6"
+  },
+  "language_info": {
+   "file_extension": ".jl",
+   "mimetype": "application/julia",
+   "name": "julia",
+   "version": "0.6.3"
+  }
+ },
+ "nbformat": 4,
+ "nbformat_minor": 2
+}

examples/manipulation_with_blink.jl

@@ -0,0 +1,40 @@
+# This example demonstrates basic manipulation of a visualized
+# mechanism in a standalone window.
+
+#  For a similar demo rendered in Jupyter instead, see the
+#  `interactive_manipulation.ipynb` notebook.
+
+# Import our packages
+using MeshCatMechanisms
+using RigidBodyDynamics
+
+# Blink provides the standalone window we'll use to display
+# the visualizer and controls
+using Blink
+# Install the Blink.AtomShell if it isn't already installed
+AtomShell.isinstalled() || AtomShell.install()
+
+# Create a random mechanism and its associated visualizer
+srand(75)  # seed the random number generator so we get repeatable results
+mechanism = rand_chain_mechanism(Float64,
+    [QuaternionFloating{Float64}; [Revolute{Float64} for i = 1:5]]...)
+mvis = MechanismVisualizer(mechanism, Skeleton(randomize_colors=true))
+
+# Open the visualizer in a new window
+#
+# We don't open windows when we're running this test
+# on the Travis CI build servers
+if !haskey(ENV, "CI")
+    open(mvis, Window())
+end
+
+# Create sliders to manipulate the visualizer's configuration
+widget = manipulate!(mvis)
+
+# Render those sliders in a new window
+#
+# We don't open windows when we're running this test
+# on the Travis CI build servers
+if !haskey(ENV, "CI")
+    body!(Window(), widget)
+end

src/MeshCatMechanisms.jl

@@ -5,7 +5,8 @@ module MeshCatMechanisms
 export MechanismVisualizer,
        animate,
        MeshCatSink,
-       setelement!
+       setelement!,
+       manipulate!
 
 # Re-export from MeshCat.jl
 export IJuliaCell,
@@ -35,5 +36,6 @@ using GeometryTypes: HyperSphere, Point
 include("visualizer.jl")
 include("animate.jl")
 include("ode_callback.jl")
+include("manipulate.jl")
 
 end # module

src/animate.jl

@@ -31,7 +31,7 @@ function animate(vis::MechanismVisualizer,
 end
 
 function MeshCat.setanimation!(mvis::MechanismVisualizer,
-                      times::Vector{Float64},
+                      times::AbstractVector{<:Real},
                       configurations::AbstractVector{<:AbstractVector{<:Real}};
                       fps::Integer=30,
                       play::Bool=true,

src/manipulate.jl

@@ -0,0 +1,72 @@
+using RigidBodyDynamics: Bounds, position_bounds, lower, upper
+import InteractBase
+using InteractBase: slider, Widget, observe, vbox, widget
+using WebIO: Node
+
+function remove_infs(b::Bounds, default=Float64(π))
+    Bounds(isfinite(lower(b)) ? lower(b) : -default,
+           isfinite(upper(b)) ? upper(b) : default)
+end
+
+slider_range(joint::Joint) = remove_infs.(position_bounds(joint))
+function slider_range(joint::Joint{T, <: QuaternionFloating}) where {T}
+    defaults = [1., 1, 1, 1, 10, 10, 10]
+    remove_infs.(position_bounds(joint), defaults)
+end
+
+slider_labels(joint::Joint) = [string("q", i) for i in 1:num_positions(joint)]
+slider_labels(joint::Joint{T, <:QuaternionFloating}) where {T} = ["rw", "rx", "ry", "rz", "x", "y", "z"]
+
+function sliders(joint::Joint, values=zeros(num_positions(joint));
+                 bounds=slider_range(joint),
+                 labels=slider_labels(joint),
+                 resolution=0.01, prefix="")
+    map(bounds, labels, values) do b, label, value
+        slider(lower(b):resolution:upper(b),
+               value=value,
+               label=string(prefix, label))
+    end
+end
+
+function combined_observable(::Joint, sliders::AbstractVector{<:Widget})
+    map((args...) -> vcat(args...), observe.(sliders)...)
+end
+
+function combined_observable(::Joint{T, <:QuaternionFloating}, sliders::AbstractVector{<:Widget}) where T
+    map(observe.(sliders)...) do rw, rx, ry, rz, x, y, z
+        n = norm([rw, rx, ry, rz])
+        if n == 0
+            n = 1.0
+        end
+        [rw / n, rx / n, ry / n, rz / n, x, y, z]
+    end
+end
+
+function InteractBase.widget(joint::Joint, initial_value=zeros(num_positions(joint)); prefix=string(joint, '.'))
+    s = sliders(joint, initial_value, prefix=prefix)
+    observable = combined_observable(joint, s)
+    node = Node(:div, vbox(s...))
+    Widget{:rbd_joint}(node, observable)
+end
+
+function manipulate!(callback::Function, state::MechanismState)
+    joint_list = joints(state.mechanism)
+    widgets = widget.(joint_list, configuration.(state, joint_list))
+    obs = map(observe.(widgets)...) do signals...
+        for i in 1:length(joint_list)
+            set_configuration!(state, joint_list[i], signals[i])
+        end
+        callback(state)
+    end
+    node = Node(:div, vbox(widgets...))
+    Widget{:rbd_manipulator}(node, obs)
+end
+
+function manipulate!(callback::Function, vis::MechanismVisualizer)
+    manipulate!(vis.state) do x
+        set_configuration!(vis, configuration(x))
+        callback(x)
+    end
+end
+
+manipulate!(vis::MechanismVisualizer) = manipulate!(x -> nothing, vis)

src/visualizer.jl

@@ -118,7 +118,7 @@ function _render_state!(mvis::MechanismVisualizer, state::MechanismState=mvis.st
     tree = mechanism(mvis).tree # TODO: tree accessor?
     for body in vertices(tree)
         if body == root(tree)
-            settransform!(vis, to_affine_map(transform_to_root(state, body)))
+            continue
         else
             parent = source(edge_to_parent(body, tree), tree)
             tform = relative_transform(state, default_frame(body), default_frame(parent))

test/REQUIRE

@@ -1,2 +1,3 @@
 ValkyrieRobot 0.0.1
 NBInclude 1.1
+Blink

test/runtests.jl

@@ -87,7 +87,24 @@ vis = Visualizer()
         integrate(integrator, 10., 1e-3, max_realtime_rate = 1.)
     end
 
-    @testset "notebook" begin
-        nbinclude(joinpath(@__DIR__, "..", "mechanism-demo.ipynb"))
+    @testset "manipulation" begin
+        delete!(vis)
+        mechanism = rand_chain_mechanism(Float64, [QuaternionFloating{Float64}; [Revolute{Float64} for i = 1:5]]...)
+        mvis = MechanismVisualizer(mechanism)
+        widget = manipulate!(mvis)
+    end
+
+    @testset "examples" begin
+        dir = joinpath(@__DIR__, "..", "examples")
+        for file in readdir(dir)
+            base, ext = splitext(file)
+            if ext == ".jl"
+                println("Running $file")
+                include(joinpath(dir, file))
+            elseif ext == ".ipynb"
+                println("Running notebook $file")
+                nbinclude(joinpath(dir, file))
+            end
+        end
     end
 end