From 6c22ee56bb0891ed1d654f42c6c1e193facc4931 Mon Sep 17 00:00:00 2001
From: Mitchell Fogelson <mfogelso@andrew.cmu.edu>
Date: Thu, 15 Jun 2023 10:12:29 -0400
Subject: [PATCH] added visualize without storage

---
 src/visuals/visualizer.jl | 45 +++++++++++++++++++++++++++++++++++----
 1 file changed, 41 insertions(+), 4 deletions(-)

diff --git a/src/visuals/visualizer.jl b/src/visuals/visualizer.jl
index 97977d05b..c4def02f5 100644
--- a/src/visuals/visualizer.jl
+++ b/src/visuals/visualizer.jl
@@ -31,7 +31,7 @@ function visualize(mechanism::Mechanism, storage::Storage{T,N};
 
     # Build robot in the visualizer
     build && build_robot(mechanism; 
-        vis, show_joint, show_contact, show_frame, color, name, visualize_floor)
+        vis, show_joint, joint_radius, show_contact, show_frame, color, name, visualize_floor)
 
     # Create animations
     timestep_max = 1/framerate
@@ -56,8 +56,7 @@ function visualize(mechanism::Mechanism, storage::Storage{T,N};
         if show_joint
             for (jd, joint) in enumerate(mechanism.joints)
                 if joint.child_id == body.id
-                    radius = 0.1
-                    joint_shape = Sphere(radius, 
+                    joint_shape = Sphere(joint_radius, 
                         position_offset=joint.translational.vertices[2],
                         color=RGBA(0.0, 0.0, 1.0, 0.5))
                     visshape = convert_shape(joint_shape)
@@ -116,6 +115,44 @@ function visualize(mechanism::Mechanism, storage::Storage{T,N};
     return_animation ? (return vis, animation) : (return vis) 
 end
 
+"""
+    visualize(mechanism; vis, build, show_contact, animation, color, name)
+
+    visualize mechanism pose from current state 
+
+    mechanism: Mechanism 
+    vis: Visualizer 
+    build: flag to construct mechanism visuals (only needs to be built once)
+    show_contact: flag to show contact locations on system 
+    color: RGBA 
+    name: unique identifier for mechanism
+"""
+function visualize(mechanism::Mechanism; 
+    vis::Visualizer=Visualizer(),
+    framerate=60, # Inf for 1/timestep
+    build::Bool=true, 
+    show_joint=false,
+    joint_radius=0.1,
+    show_contact=false,
+    show_frame=false, 
+    animation=nothing, 
+    color=nothing, 
+    name::Symbol=:robot,
+    return_animation=false,
+    visualize_floor=true) where {T,N}
+
+    # Create 1 step storage from current body state
+    storage = Storage(1, length(mechanism.bodies))
+    for (i, body) in enumerate(mechanism.bodies)
+        storage.x[i][1] = body.state.x2
+        storage.q[i][1] = body.state.q2
+    end
+
+    visualize(mechanism, storage; 
+        vis, framerate, build, show_joint, joint_radius, show_contact, show_frame, 
+        animation, color, name, return_animation, visualize_floor)
+end
+
 """
     build_robot(mechanism; vis, show_contact, name, color)
 
@@ -242,7 +279,7 @@ function animate_node!(storage::Storage{T,N}, id, shape, animation, shapevisuali
         x = storage.x[id][i]
         q = storage.q[id][i]
         atframe(animation, frame_id) do
-            set_node!(x, q, shape, shapevisualizer, showshape)
+             set_node!(x, q, shape, shapevisualizer, showshape)
         end
         frame_id += 1
     end