# ---
# jupyter:
#   jupytext:
#     text_representation:
#       extension: .jl
#       format_name: light
#       format_version: '1.5'
#       jupytext_version: 1.14.4
#   kernelspec:
#     display_name: Julia 0.5.0
#     language: julia
#     name: julia-0.5
# ---

# # DrakeVisualizer.jl
# This notebook demonstrates interacting with the Drake Visualizer app from Julia. To run it, you'll need to:
#
# * install Drake: http://drake.mit.edu/
# * add the Drake lcmtypes to your python path: 
#     * `export PYTHONPATH="$PYTHONPATH:/path/to/drake-distro/build/lib/python2.7/dist-packages"`
# * install the DrakeVisualizer.jl package:
#     * `Pkg.clone("https://github.com/rdeits/DrakeVisualizer.jl.git")`
# * launch the `drake-visualizer` app:
#     * `cd /path/to/drake-distro/build/bin`
#     * `./drake-visualizer`
#         

# Activate the DrakeVisualizer package, and import some other 
# useful functions
using DrakeVisualizer
import GeometryTypes: HyperRectangle, Vec, HomogenousMesh, SignedDistanceField
using CoordinateTransformations
import Meshing
import ColorTypes: RGBA

using Interact

# First, we'll create a simple geometric object
box = HyperRectangle(Vec(0.,0,0), Vec(1.,1,1))

# Visualizer() causes the viewer to spawn a geometry or a set of geometries. 
# It returns a Visualizer, which includes all the information
# about that loaded geometry. 
# Note that the model is initially loaded in the zero configuration 
# (that is, its position and rotation are all zeros)
model = Visualizer(box);

# We can use draw() to tell the viewer to draw the box at a specific
# position. Translation() creates a CoordinateTransformations.Transformation
# corresponding to the given x; y; z translation.
draw(model, [Translation(1.,0,0)])

# We can also rotate the model by sending a different transformation
draw(model, [LinearMap(AngleAxis(pi/4, 0, 0, 1))])

# Let's look more at what's being done under the hood. The model
# owns a reference to a Robot object. A Robot contains a list of 
# Link objects. Each Link contains a list of GeometryData objects.
# And each GeometryData contains a single geometric primitive as
# well as information about its color and its position with
# respect to the link that contains it.
#
# Calling Visualizer() on a single geometry (the box above) results in 
# a new Robot being automatically created. That Robot has one link,
# and that Link has one GeometryData whose geometry is the box we
# provided. 
model.robot

model.robot.links

# Here's the box we created. It's now the first (and only) geometry
# of the first (and only) link of the robot:
model.robot.links[1].geometry_data[1].geometry

@assert model.robot.links[1].geometry_data[1].geometry === box

# Now let's make some more complicated robots. We'll create a 
# new GeometryData from the box, but color it green this time.
green_box_data = GeometryData(box)
green_box_data.color = RGBA(0., 1, 0, 0.5)
model = Visualizer(green_box_data);

# +
# Now let's create a blue box
blue_box_data = GeometryData(box, Translation(0,0,1.), RGBA(0,0,1,0.5))

# We can create a new Link that contains two geometries: the 
# green box and the blue box. 
link1 = Link([green_box_data; blue_box_data], "link1")
model = Visualizer(link1);

# +
# And we can draw that new link wherever we like.
draw(model, [Translation(0.75, 0, 0)])

# Note that the green and blue boxes move together. That's
# because they are part of the same rigid Link. 

# +
# If we want different parts of our robot to move separately,
# they need to be on separate links. Let's create a second
# Link and then a robot containing both of those links. 
red_box_data = GeometryData(box)
link2 = Link([red_box_data], "link2")
robot = Robot([link1; link2])

# When we load this new robot, the two links are both drawn 
# at position [0; 0; 0]; right on top of each other.
model = Visualizer(robot);

# -

# Now that we have two links, we need to specify two transforms
# for the draw() command. The order of the transforms matches
# the order of the links in "Robot([link1; link2])"
draw(model, [Translation(0.5, 0, 0), Translation(-0.5, 0, 0)])

# We can now move the two links independently
@manipulate for x1 in linspace(0, 2), x2 in linspace(0, 2)
    draw(model, [Translation(x1, 0, 0); Translation(x2, 0, 0)])
end

# +
# Of course, we can draw much more interesting geometries than 
# just simple boxes. Let's create a triangulated mesh by finding
# the 0-level set of some function. 
# 
# First, we'll define our function:
f = x -> norm(x)^2 - abs(sin(atan2(x[2], x[1])))

# Then we pick a region of interest in which to sample the function.
# This region starts at (-1, -1, -1) and extends to (1, 1, 1):
lower_bound = Vec(-1.,-1,-1)
upper_bound = Vec(1., 1, 1)

# Those two pieces of information are all we need to construct a robot
# geometry. For this, we'll need the contour_mesh function:
mesh = contour_mesh(f, lower_bound, upper_bound)
# Under the hood, this will sample f at regularly spaced points inside
# the bounding rectangle, then compute a surface that connects all the 
# points for which f(x) = 0.

# And now we can load that geometry into the visualizer
model = Visualizer(mesh)

# +
# We can even manipulate the geometry by changing the iso level. 
# By default, contour_mesh constructs a mesh connecting the points 
# in space for which f(x) = 0, where 0 is called the isosurface level
# or iso level. But we can change that iso level to any number we want:

f = x -> norm(x)^2 - abs(sin(atan2(x[2], x[1])))
lower_bound = Vec(-1.,-1,-1)
upper_bound = Vec(1., 1, 1)

@manipulate for iso_level in linspace(-1, 1)
    geometry = contour_mesh(f, lower_bound, upper_bound, iso_level)
    model = Visualizer(geometry);
end

# Note that for high iso_level values our geometry gets cut off at the
# edges. We could fix that by replacing the bounds with a bigger box. 

# +
# It is also possible to visualize multiple robots at the same time,
# while sending separate draw() commands for each robot. To do that,
# we construct two Robot() objects and pass them both in to the call
# to Visualizer. This will return *two* visualizers, which we can use
# to draw each robot separately:

vis1, vis2 = Visualizer([Robot([link1]), Robot([link2])])
# -

# To demonstrate this, let's move the first robot while leaving the
# second one where it is. We do that by calling draw() on vis1:
@manipulate for z in linspace(0, 2)
    draw(vis1, [Translation(0, 0, z)])
end

# Now let's move just the second robot by calling draw() on vis2:
@manipulate for x in linspace(0, 2)
    draw(vis2, [Translation(x, 0, 0)])
end



StarcoderdataJupyterScriptsDedupFiltered