diff --git a/python/ur_simple_control/visualize/__pycache__/visualize.cpython-312.pyc b/python/ur_simple_control/visualize/__pycache__/visualize.cpython-312.pyc
index 8fcb405e56bc736d66c541ba4caf5b5ff6f02c36..74ae94bdf5b0861593f0609b21e017bba3c7be97 100644
Binary files a/python/ur_simple_control/visualize/__pycache__/visualize.cpython-312.pyc and b/python/ur_simple_control/visualize/__pycache__/visualize.cpython-312.pyc differ
diff --git a/python/ur_simple_control/visualize/manipulator_comparison_visualizer.py b/python/ur_simple_control/visualize/manipulator_comparison_visualizer.py
index 3dd8012350d72e63bb632eeebc78f9c33e8aecea..6682326d87da63d4aff4d17667a11075916bab25 100644
--- a/python/ur_simple_control/visualize/manipulator_comparison_visualizer.py
+++ b/python/ur_simple_control/visualize/manipulator_comparison_visualizer.py
@@ -6,7 +6,7 @@ from itertools import zip_longest
from multiprocessing import Process, Queue
from ur_simple_control.managers import getMinimalArgParser, RobotManager
from ur_simple_control.util.logging_utils import LogManager
-from ur_simple_control.visualize.visualize import manipulatorComparisonVisualizer
+from ur_simple_control.visualize.visualize import manipulatorComparisonVisualizer, logPlotter
def getLogComparisonArgs():
parser = getMinimalArgParser()
@@ -53,40 +53,81 @@ class ManipulatorComparisonManager:
print("you did not give me a valid path, exiting")
exit()
- self.manipulator_visualizer_queue = Queue()
+ self.manipulator_visualizer_cmd_queue = Queue()
+ self.manipulator_visualizer_ack_queue = Queue()
# we are assuming both robots are the same,
# but this does not necessarily have to be true
self.manipulator_visualizer_process = Process(target=manipulatorComparisonVisualizer,
args=(args, self.robot1.model, self.robot1.collision_model,
- self.robot2.visual_model, self.manipulator_visualizer_queue, ))
+ self.robot2.visual_model, self.manipulator_visualizer_cmd_queue,
+ self.manipulator_visualizer_ack_queue))
if self.args.debug_prints:
print("MANIPULATOR_COMPARISON_VISUALIZER: manipulator_comparison_visualizer_process started")
self.manipulator_visualizer_process.start()
# wait for meshcat to start
- self.manipulator_visualizer_queue.put((np.zeros(self.robot1.model.nq), np.ones(self.robot2.model.nq)))
+ self.manipulator_visualizer_cmd_queue.put((np.zeros(self.robot1.model.nq), np.ones(self.robot2.model.nq)))
if self.args.debug_prints:
print("COMPARE_LOGS_MAIN: i managed to put initializing (q1, q2) to manipulator_comparison_visualizer_queue")
# wait until it's ready (otherwise you miss half the sim potentially)
# 5 seconds should be more than enough,
# and i don't want to complicate this code by complicating IPC
time.sleep(5)
+ self.manipulator_visualizer_ack_queue.get()
+
+ ###########################################
+ # in case you will want log plotter too #
+ ###########################################
+ self.log_plotter = False
+ self.log_plotter_cmd_queue = None
+ self.log_plotter_ack_queue = None
+ self.log_plotter_process = None
+
+
+ # NOTE i assume what you want to plot is a time-indexed with
+ # the same frequency that the control loops run in.
+ # if it's not then it's pointless to have a running plot anyway.
+ def createRunningPlot(self, log, log_plotter_time_start, log_plotter_time_stop):
+ self.log_plotter = True
+ self.log_plotter_time_start = log_plotter_time_start
+ self.log_plotter_time_stop = log_plotter_time_stop
+ self.log_plotter_cmd_queue = Queue()
+ self.log_plotter_ack_queue = Queue()
+ self.log_plotter_process = Process(target=logPlotter,
+ args=(args, log, self.log_plotter_cmd_queue,
+ self.log_plotter_ack_queue))
+ self.log_plotter_process.start()
+ self.log_plotter_ack_queue.get()
+
+ def updateViz(self, q1, q2, time_index):
+ self.manipulator_visualizer_cmd_queue.put((q1, q2))
+ if self.log_plotter and (time_index >= self.log_plotter_time_start) and (time_index < self.log_plotter_time_stop):
+ self.log_plotter_cmd_queue.put(time_index - self.log_plotter_time_start)
+ self.log_plotter_ack_queue.get()
+ self.manipulator_visualizer_ack_queue.get()
# NOTE: this uses slightly fancy python to make it bareable to code
# NOTE: dict keys are GUARANTEED to be in insert order from python 3.7 onward
- def visualizeManipulatorRuns(self):
+ def visualizeWholeRuns(self):
+ time_index = 0
for control_loop1, control_loop2 in zip_longest(self.logm1.loop_logs, self.logm2.loop_logs):
print(f'run {self.logm1.args.run_name}, controller: {control_loop1}')
print(f'run {self.logm2.args.run_name}, controller: {control_loop2}')
# relying on python's default thing.toBool()
if not control_loop1:
+ print(f"run {self.logm1.args.run_name} is finished")
q1 = self.lastq1
for q2 in self.logm2.loop_logs[control_loop2]['qs']:
- self.manipulator_visualizer_queue.put_nowait((q1, q2))
+ self.updateViz(q1, q2, time_index)
+ time_index += 1
+ print(f"run {self.logm2.args.run_name} is finished")
if not control_loop2:
+ print(f"run {self.logm2.args.run_name} is finished")
q2 = self.lastq2
for q1 in self.logm1.loop_logs[control_loop1]['qs']:
- self.manipulator_visualizer_queue.put_nowait((q1, q2))
+ self.updateViz(q1, q2, time_index)
+ time_index += 1
+ print(f"run {self.logm1.args.run_name} is finished")
if control_loop1 and control_loop2:
for q1, q2 in zip_longest(self.logm1.loop_logs[control_loop1]['qs'], \
self.logm2.loop_logs[control_loop2]['qs']):
@@ -94,20 +135,21 @@ class ManipulatorComparisonManager:
self.lastq1 = q1
if not (q2 is None):
self.lastq2 = q2
- print(self.lastq1)
- print(self.lastq2)
- self.manipulator_visualizer_queue.put_nowait((self.lastq1, self.lastq2))
+ self.updateViz(self.lastq1, self.lastq2, time_index)
+ time_index += 1
if __name__ == "__main__":
args = getLogComparisonArgs()
- visualizer = ManipulatorComparisonManager(args)
- visualizer.visualizeManipulatorRuns()
+ cmp_manager = ManipulatorComparisonManager(args)
+ log_plot = {'random_noise' : np.random.normal(size=(1000, 2))}
+ cmp_manager.createRunningPlot(log_plot, 200, 1200)
+ cmp_manager.visualizeWholeRuns()
time.sleep(100)
- visualizer.manipulator_visualizer_queue.put("befree")
+ cmp_manager.manipulator_visualizer_cmd_queue.put("befree")
print("main done")
time.sleep(0.1)
- visualizer.manipulator_visualizer_process.terminate()
+ cmp_manager.manipulator_visualizer_process.terminate()
if args.debug_prints:
print("terminated manipulator_visualizer_process")
diff --git a/python/ur_simple_control/visualize/visualize.py b/python/ur_simple_control/visualize/visualize.py
index 93f4a1347b63b71d99f7f3fc85156c4ff7dd01ea..9890be02c686b6795c4ef457de2f777797ec3314 100644
--- a/python/ur_simple_control/visualize/visualize.py
+++ b/python/ur_simple_control/visualize/visualize.py
@@ -58,16 +58,16 @@ def plotFromDict(plot_data, final_iteration, args):
plt.show()
-"""
-realTimePlotter
----------------
-- true to its name
-"""
# STUPID MATPLOTLIB CAN'T HANDLE MULTIPLE FIGURES FROM DIFFERENT PROCESS
# LITERALLY REFUSES TO GET ME A FIGURE
def realTimePlotter(args, queue):
+ """
+ realTimePlotter
+ ---------------
+ - true to its name
+ - plots whatever you are logging if you use the --real-time-plotting flag
+ """
if args.debug_prints:
-# print("REAL_TIME_PLOTTER: real time visualizer has been summoned")
print("REAL_TIME_PLOTTER: i got this queue:", queue)
plt.ion()
fig = plt.figure()
@@ -203,7 +203,7 @@ def manipulatorVisualizer(args, model, collision_model, visual_model, queue):
# could be merged with the above function.
# but they're different enough in usage to warrent a different function,
# instead of polluting the above one with ifs
-def manipulatorComparisonVisualizer(args, model, collision_model, visual_model, queue):
+def manipulatorComparisonVisualizer(args, model, collision_model, visual_model, cmd_queue, ack_queue):
# for whatever reason the hand-e files don't have/
# meshcat can't read scaling information.
# so we scale manually
@@ -213,6 +213,9 @@ def manipulatorComparisonVisualizer(args, model, collision_model, visual_model,
# this looks exactly correct lmao
s *= 0.001
geom.meshScale = s
+ # there has to be a way
+ # if not here than elsewhere
+ geom.meshColor = np.array([0.2,0.2,0.2,0.2])
for geom in collision_model.geometryObjects:
if "hand" in geom.name:
s = geom.meshScale
@@ -222,24 +225,59 @@ def manipulatorComparisonVisualizer(args, model, collision_model, visual_model,
viz = MeshcatVisualizer(model, collision_model, visual_model)
viz.initViewer(open=True)
# load the first one
- viz.loadViewerModel()
+ viz.loadViewerModel(collision_color=[0.2,0.2,0.2,0.6])
+ #viz.viewer["pinocchio/visuals"].set_property("visible", False)
+ #viz.viewer["pinocchio/collisions"].set_property("visible", True)
+ viz.displayVisuals(False)
+ viz.displayCollisions(True)
# maybe needed
#viz.displayVisuals(True)
+# meshpath = viz.viewerVisualGroupName
+# for geom in visual_model(6):
+# meshpath_i = meshpath + "/" + geometry_object.name
+ #viz.viewer["pinocchio/visuals"].set_property("opacity", 0.2)
+ #viz.viewer["pinocchio"].set_property("opacity", 0.2)
+ #viz.viewer["pinocchio/visuals/forearm_link_0"].set_property("opacity", 0.2)
+
+ #viz.viewer["pinocchio"].set_property("color", (0.2,0.2,0.2,0.2))
+ #viz.viewer["pinocchio/visuals"].set_property("color",(0.2,0.2,0.2,0.2))
+ #viz.viewer["pinocchio/visuals/forearm_link_0"].set_property("color", (0.2,0.2,0.2,0.2))
+
+ ## this is the path we want, with the /<object> at the end
+ #node = viz.viewer["pinocchio/visuals/forearm_link_0/<object>"]
+ #print(node)
+ #node.set_property("opacity", 0.2)
+ #node.set_property("modulated_opacity", 0.2)
+ #node.set_property("color", [0.2] * 4)
+ #node.set_property("scale", 100.2)
+ # this one actually works
+ #node.set_property("visible", False)
+
+ node = viz.viewer["pinocchio/visuals"]
+ #node.set_property("visible", False)
+ node.set_property("modulated_opacity", 0.4)
+ node.set_property("opacity", 0.2)
+ node.set_property("color", [0.2] * 4)
+
+ #meshcat->SetProperty("path/to/my/thing/<object>", "opacity", alpha);
+
# other robot display
viz2 = MeshcatVisualizer(model, collision_model, visual_model)
viz2.initViewer(viz.viewer)
# i don't know if rootNodeName does anything apart from being different
+ #viz2.loadViewerModel(rootNodeName="pinocchio2", visual_color=(1.0,1.0,1.0,0.1))
viz2.loadViewerModel(rootNodeName="pinocchio2")
# initialize
- q1, q2 = queue.get()
+ q1, q2 = cmd_queue.get()
viz.display(q1)
viz2.display(q2)
+ ack_queue.put("ready")
print("MANIPULATOR_COMPARISON_VISUALIZER: FULLY ONLINE")
try:
while True:
- q = queue.get()
+ q = cmd_queue.get()
if type(q) == str:
print("got str q")
if q == "befree":
@@ -251,9 +289,93 @@ def manipulatorComparisonVisualizer(args, model, collision_model, visual_model,
q1, q2 = q
viz.display(q1)
viz2.display(q2)
+ # this doesn't really work because meshcat is it's own server
+ # and display commands just relay their command and return immediatelly.
+ # but it's better than nothing.
+ # NOTE: if there's lag in meshcat, just add a small sleep here before the
+ # ack signal - that will ensure synchronization because meshat will actually be ready
+ ack_queue.put("ready")
except KeyboardInterrupt:
if args.debug_prints:
print("MANIPULATOR_COMPARISON_VISUALIZER: caught KeyboardInterrupt, i'm out")
viz.viewer.window.server_proc.kill()
viz.viewer.window.server_proc.wait()
+
+
+def logPlotter(args, log, cmd_queue, ack_queue):
+ """
+ logPlotter
+ ---------------
+ - plots whatever you want as long as you pass the data
+ as a dictionary where the key will be the name on the plot,
+ and the value have to be the dependent variables -
+ the independent variable is time and has to be the same for all items.
+ if you want to plot something else, you need to write your own function.
+ use this as a skeleton if you want that new plot to be updating too
+ as then you don't need to think about IPC - just use what's here.
+ - this might be shoved into a tkinter gui if i decide i really need buttons
+ """
+ if len(log) == 0:
+ print("you've send me nothing, so no real-time plotting for you")
+ return
+
+ plt.ion()
+ fig = plt.figure()
+ canvas = fig.canvas
+ AxisAndArtists = namedtuple("AxAndArtists", "ax artists")
+ axes_and_updating_artists = {}
+
+ n_cols, n_rows = getNRowsMColumnsFromTotalNumber(len(log))
+ # this is what subplot wants
+ subplot_col_row = str(n_cols) + str(n_rows)
+ # preload some zeros and initialize plots
+ for i, data_key in enumerate(log):
+ # you give single-vector numpy arrays, i instantiate and plot lists of these
+ # so for your (6,) vector, i plot (N, 6) ndarrays resulting in 6 lines of length N.
+ # i manage N because plot data =/= all data for efficiency reasons.
+ assert type(log[data_key]) == np.ndarray
+
+ colors = plt.cm.jet(np.linspace(0, 1, log[data_key].shape[1]))
+ ax = fig.add_subplot(int(subplot_col_row + str(i + 1)))
+ ax.set_title(data_key)
+ # we plot each line separately so that they have different colors
+ # we assume (N_timesteps, your_vector) shapes.
+ # values do not update
+ for j in range(log[data_key].shape[1]):
+ # NOTE the same length assumption plays a part for correctness,
+ # but i don't want that to be an error in case you know what you're doing
+ ax.plot(np.arange(len(log[data_key])), log[data_key][:,j],
+ color=colors[j], label=data_key + "_" + str(j))
+
+ # vertical bar does update
+ point_in_time_line = ax.axvline(x=0, color='red', animated=True)
+ axes_and_updating_artists[data_key] = AxisAndArtists(ax, point_in_time_line)
+ axes_and_updating_artists[data_key].ax.legend(loc='upper left')
+
+ # need to call it once
+ canvas.draw()
+ canvas.flush_events()
+ background = canvas.copy_from_bbox(fig.bbox)
+
+ ack_queue.put("ready")
+ if args.debug_prints:
+ print("LOG_PLOTTER: FULLY ONLINE")
+ try:
+ while True:
+ time_index = cmd_queue.get()
+ if time_index == "befree":
+ if args.debug_prints:
+ print("LOG_PLOTTER: got befree, logPlotter out")
+ break
+ canvas.restore_region(background)
+ for data_key in log:
+ axes_and_updating_artists[data_key].artists.set_xdata([time_index])
+ axes_and_updating_artists[data_key].ax.draw_artist(axes_and_updating_artists[data_key].artists)
+ canvas.blit(fig.bbox)
+ canvas.flush_events()
+ ack_queue.put("ready")
+ except KeyboardInterrupt:
+ if args.debug_prints:
+ print("LOG_PLOTTER: caught KeyboardInterrupt, i'm out")
+ plt.close(fig)