Commit 8e854ccf authored by Hakan GIRGIN's avatar Hakan GIRGIN
Browse files

adding to gui

parent 790e3725
......@@ -2,17 +2,12 @@ import numpy as np
import os
import matplotlib.pyplot as plt
from .interactive import Interactive
from colorama import init # need this to print colored on Windows and init() after
from termcolor import colored
import sys
if sys.version_info[0] == 3:
import tkinter as tk
from tkinter.filedialog import asksaveasfilename
else:
import Tkinter as tk
from tkFileDialog import asksaveasfilename
import tkinter as tk
from tkinter.filedialog import asksaveasfilename
from matplotlib import gridspec
from ... import pbdlib as pbd
class Robot(object):
def __init__(self, T):
......@@ -20,15 +15,19 @@ class Robot(object):
self.ddx, self.fsx, self.fx = np.array([0., 0.]), np.array([0.]), np.array([0., 0.])
self.sensor_mode = 0
class InteractiveDemos(Interactive, Robot):
"""
GUI for recording demonstrations in 2D
"""
def __init__(self, filename='test', path='', **kwargs):
def __init__(self, filename='test', path='', plot_function=None, **kwargs):
Interactive.__init__(self)
Robot.__init__(self, self.simulation_T)
init() # need to import colorama to use this, see imports
self.path = os.path.dirname(pbd.__file__) + '/data/gui/' if path == '' else path
self.path = os.path.dirname(__file__) if path == '' else path
self.fig = plt.figure(figsize=(15, 8), facecolor='white')
self.bindings.update({
'q': (self.save_demos, [], "save demos"),
......@@ -38,8 +37,6 @@ class InteractiveDemos(Interactive, Robot):
'd': (self.clear_demos, [False, True], "clear selected demos"),
})
Robot.__init__(self, self.simulation_T)
gs = gridspec.GridSpec(1, 2)
self.filename = filename
......@@ -47,6 +44,13 @@ class InteractiveDemos(Interactive, Robot):
self.ax_x = plt.subplot(gs[0])
self.ax_dx = plt.subplot(gs[1])
if plot_function:
if type(plot_function) is list:
for fnc in plot_function:
fnc(self.ax_x)
else:
plot_function(self.ax_x)
self.set_events()
self.set_plots()
......@@ -74,10 +78,10 @@ class InteractiveDemos(Interactive, Robot):
# win.attributes("-topmost", True)
# win.attributes("-alpha", 0.4)
try:
self.demos = np.load(self.path + filename + '.npy')[()]
self.nb_demos = self.demos['x'].__len__(); self.params['current_demo'][2] = self.nb_demos - 1
self.nb_demos = self.demos['x'].__len__();
self.params['current_demo'][2] = self.nb_demos - 1
print(colored('Existing skill, demos loaded', 'green'))
self.replot_demos()
self.fig.canvas.draw()
......@@ -88,7 +92,7 @@ class InteractiveDemos(Interactive, Robot):
def highlight_demos(self):
data = self.demos['x'][self.params['current_demo'][0]]
self.plots['current_demo'].set_data(data[:, 0], data[:, 1] )
self.plots['current_demo'].set_data(data[:, 0], data[:, 1])
self.fig.canvas.draw()
def plot_sensor_value(self, s, scale=1.):
......@@ -99,15 +103,15 @@ class InteractiveDemos(Interactive, Robot):
def set_plots(self):
self.plots.update({
'robot_plot':self.ax_x.plot([], [], 'o-', mew=4, mec='orangered', ms=10, mfc='w')[0],
'sensor_value':self.ax_x.plot([], [],ls=(0,(1,1)), lw=10)[0],
'attractor_plot':self.ax_x.plot([], [], 'o-', mew=4, mec='teal', ms=10, mfc='w')[0],
'obj_plot':self.ax_x.plot([], [], 'o-', mew=4, mec='steelblue', ms=10, mfc='w')[0],
'robot_plot': self.ax_x.plot([], [], 'o-', mew=4, mec='orangered', ms=10, mfc='w')[0],
'sensor_value': self.ax_x.plot([], [], ls=(0, (1, 1)), lw=10)[0],
'attractor_plot': self.ax_x.plot([], [], 'o-', mew=4, mec='teal', ms=10, mfc='w')[0],
'obj_plot': self.ax_x.plot([], [], 'o-', mew=4, mec='steelblue', ms=10, mfc='w')[0],
'current_demo': self.ax_x.plot([], [], lw=3, ls='--', color='orangered')[0],
'current_demo_dx': self.ax_dx.plot([], [], lw=3, ls='--', color='orangered')[0]
})
for ax, lim in zip([self.ax_x, self.ax_dx], [100, 25]):
for ax, lim in zip([self.ax_x, self.ax_dx], [10, 2.5]): # [100, 25]
ax.set_xlim([-lim, lim])
ax.set_ylim([-lim, lim])
......@@ -115,26 +119,25 @@ class InteractiveDemos(Interactive, Robot):
if not self.velocity_mode:
m = 1.0
ddx = ffx/m
ddx = ffx / m
self.x += self.dt / n_steps * self.dx + 0.5 * self.ddx * (
self.dt / n_steps) ** 2
self.dt / n_steps) ** 2
self.dx += self.dt / n_steps * 0.5 * (self.ddx + ddx)
self.dxx = np.copy(ddx)
else:
kp = 0.;
kp = 0
kv = kp ** 0.5 * 2
for i in range(50):
ddx = kp * (self.curr_mouse_pos - self.dx)
self.dx += self.dt * ddx
self.x += self.dt * self.dx + (self.dt ** 2) / 2. * ddx
def timer_event(self, event):
if self.is_demonstrating:
if self.curr_mouse_pos is None: self.pretty_print('Outside'); return
# print self.x, self.dx
kp = 400.;
kp = 400.
kv = kp ** 0.5 * 2
n_steps = 10
......@@ -142,7 +145,6 @@ class InteractiveDemos(Interactive, Robot):
ffx = kp * (self.curr_mouse_pos - self.x) - kv * self.dx
self.sim_dynamics(ffx)
# self.curr_demo += [np.copy(self.x)]; self.curr_demo_dx += [np.copy(self.dx)]
self._current_demo['x'] += [np.copy(self.x)]
......@@ -171,7 +173,7 @@ class InteractiveDemos(Interactive, Robot):
curr_demo_arr[:, 1])
self.plots['current_demo_dx'].set_data(curr_demo_dx_arr[:, 0],
curr_demo_dx_arr[:, 1])
curr_demo_dx_arr[:, 1])
self.fig.canvas.draw()
def click_event(self, event):
......@@ -186,7 +188,6 @@ class InteractiveDemos(Interactive, Robot):
self.x = self.demos['x'][-1][0] if self.nb_demos > 0 else np.array([0., 0.])
self.dx = self.curr_mouse_pos
[t.start() for t in [self.timer, self.plot_timer]]
def release_event(self, event):
......@@ -201,10 +202,10 @@ class InteractiveDemos(Interactive, Robot):
for i in range(self.nb_demos):
data = self.demos['x'][i]
self.plots['demo_%d' % i] = \
self.ax_x.plot(data[:, 0], data[:, 1], lw=2, ls='--')[0]
self.ax_x.plot(data[:, 0], data[:, 1], lw=2, ls='--')[0]
data = self.demos['dx'][i]
self.plots['demo_dx_%d' % i] = \
self.ax_dx.plot(data[:, 0], data[:, 1], lw=2, ls='--')[0]
self.ax_dx.plot(data[:, 0], data[:, 1], lw=2, ls='--')[0]
def clear_demos(self, last=False, selected=False):
"""
......@@ -221,7 +222,8 @@ class InteractiveDemos(Interactive, Robot):
self.plots['demo_%d' % (i)].remove()
self.plots['demo_dx_%d' % (i)].remove()
self.nb_demos = len(self.demos['x']); self.params['current_demo'][2] = self.nb_demos - 1
self.nb_demos = len(self.demos['x']);
self.params['current_demo'][2] = self.nb_demos - 1
self.replot_demos()
......@@ -238,8 +240,8 @@ class InteractiveDemos(Interactive, Robot):
for s in self.demos:
self.demos[s] = []
self.nb_demos = 0; self.params['current_demo'][2] = self.nb_demos - 1
self.nb_demos = 0;
self.params['current_demo'][2] = self.nb_demos - 1
def finish_demo(self):
"""
......@@ -256,26 +258,30 @@ class InteractiveDemos(Interactive, Robot):
self.demos[s] += [np.array(self._current_demo[s])]
self._current_demo[s] = []
self.plots['current_demo'].set_data([], []); self.plots['current_demo_dx'].set_data([], [])
self.plots['demo_%d' % self.nb_demos] = self.ax_x.plot(curr_demo_arr[:, 0], curr_demo_arr[:, 1], lw=2, ls='--')[0]
self.plots['demo_dx_%d' % self.nb_demos] = self.ax_dx.plot(curr_demo_dx_arr[:, 0], curr_demo_dx_arr[:, 1], lw=2, ls='--')[0]
self.plots['current_demo'].set_data([], []);
self.plots['current_demo_dx'].set_data([], [])
self.plots['demo_%d' % self.nb_demos] = self.ax_x.plot(curr_demo_arr[:, 0], curr_demo_arr[:, 1], lw=2, ls='--')[
0]
self.plots['demo_dx_%d' % self.nb_demos] = \
self.ax_dx.plot(curr_demo_dx_arr[:, 0], curr_demo_dx_arr[:, 1], lw=2, ls='--')[0]
self.nb_demos += 1; self.params['current_demo'][2] = self.nb_demos - 1
self.nb_demos += 1;
self.params['current_demo'][2] = self.nb_demos - 1
self.fig.canvas.draw()
def save_demos(self):
"""
Saving demonstrations with filename prompt
:return:
"""
root = tk.Tk(); root.withdraw()
root = tk.Tk();
root.withdraw()
file_path = asksaveasfilename(initialdir=self.path, initialfile=self.filename + '.npy')
self.pretty_print("Demonstrations saved as\n "+ file_path)
self.pretty_print("Demonstrations saved as\n " + file_path)
np.save(file_path, self.demos)
pass
\ No newline at end of file
pass
from .demos import *
from ... import pbdlib as pbd
from ..utils import angle_to_rotation
class CoordinateSys2D(object):
x, alpha, d = np.array([0, 0]), 0, 0
_angle_based = True
_A = None
_size = 10
@property
def size(self):
return self._size
class CoordinateSys2D(object):
x, alpha, d = np.array([0, 0]), 0, 0
@size.setter
def size(self, value):
self._size = value
_angle_based = True
_A = None
_size = 10
@property
def A(self):
if self._A is None:
self._A = pbd.utils.angle_to_rotation(self.alpha).T
@property
def size(self):
return self._size
return self._A
@size.setter
def size(self, value):
self._size = value
@A.setter
def A(self, A):
self._angle_based = False
self._A = A
@property
def A(self):
if self._A is None:
self._A = angle_to_rotation(self.alpha).T
def get_points(self, size=None, shape='L'):
return self._A
if size is not None: self.size = size
mat = self.size * self.A.T
@A.setter
def A(self, A):
self._angle_based = False
self._A = A
if shape == 'L':
return np.vstack([self.x + mat[0], self.x, self.x + mat[1]])
def get_points(self, size=None, shape='L'):
elif shape == 'T':
return np.vstack([self.x - mat[0], self.x, self.x + mat[0], self.x, self.x + mat[1]/4.])
if size is not None: self.size = size
mat = self.size * self.A.T
def wall_reaction_force(self, x, dx, size=None, k=5e4, thickness=3., sensor_mode=0):
n = self.A[:, 1] # normal vector of the wall
p = self.A[:, 0] # perpendicular
kv = 2. * k**0.5
kv_tang = kv / 2.
if size is not None: self.size = size
if shape == 'L':
return np.vstack([self.x + mat[0], self.x, self.x + mat[1]])
sf = np.zeros(1)
elif shape == 'T':
return np.vstack([self.x - mat[0], self.x, self.x + mat[0], self.x, self.x + mat[1] / 4.])
if sensor_mode == 1:
def wall_reaction_force(self, x, dx, size=None, k=5e4, thickness=3., sensor_mode=0):
n = self.A[:, 1] # normal vector of the wall
p = self.A[:, 0] # perpendicular
kv = 2. * k ** 0.5
kv_tang = kv / 2.
if size is not None: self.size = size
sf = np.minimum(np.abs(n.dot(x - self.x)), 40.)
sf = np.zeros(1)
if (np.abs(p.dot(x-self.x)) < self.size) and (np.abs(n.dot(x-self.x)) < thickness):
f = -k * n * np.maximum(n.dot(x-self.x), 0)
if sensor_mode == 1:
sf = np.minimum(np.abs(n.dot(x - self.x)), 40.)
if sensor_mode == 0:
sf += -k * np.maximum(n.dot(x-self.x), 0)
if (np.abs(p.dot(x - self.x)) < self.size) and (np.abs(n.dot(x - self.x)) < thickness):
f = -k * n * np.maximum(n.dot(x - self.x), 0)
if n.dot(x-self.x) > 0:
f += -kv * dx #* np.maximum(n.dot(dx)/dx, 0.) #- kv_tang * p * p.dot(dx)
# sf += np.norm(-kv * np.maximum(n.dot(dx) / dx, 0.))
if sensor_mode == 0:
sf += -k * np.maximum(n.dot(x - self.x), 0)
else:
f = np.zeros(2)
if n.dot(x - self.x) > 0:
f += -kv * dx # * np.maximum(n.dot(dx)/dx, 0.) #- kv_tang * p * p.dot(dx)
# sf += np.norm(-kv * np.maximum(n.dot(dx) / dx, 0.))
else:
f = np.zeros(2)
return f, sf
return f, sf
class MultiCsInteractive(object):
def __init__(self, nb_experts=2, **kwargs):
self.nb_experts = nb_experts
self.expert_cs = [CoordinateSys2D() for i in range(self.nb_experts)]
self.object_experts_cs = CoordinateSys2D()
self.curr_cs = {'A': [], 'b': []}
def __init__(self, nb_experts=2, **kwargs):
self.nb_experts = nb_experts
self.expert_cs = [CoordinateSys2D() for i in range(self.nb_experts)]
self.object_experts_cs = CoordinateSys2D()
self.curr_cs = {'A': [], 'b': []}
def add_bindings(self):
for i in range(1, self.nb_experts + 1):
self.bindings['%d' % (i)] = (self.select_cs, [i], "select frame %d" % i)
def add_bindings(self):
for i in range(1, self.nb_experts + 1):
self.bindings['%d' % (i)] = (self.select_cs, [i], "select frame %d" % i)
def set_plots(self):
for i in range(self.nb_experts):
self.plots['cs_%d' % i] = self.ax_x.plot([], [], lw=2)[0]
def set_plots(self):
for i in range(self.nb_experts):
self.plots['cs_%d' % i] = self.ax_x.plot([], [], lw=2)[0]
self.plots['cs_obj'] = self.ax_x.plot([], [], lw=2)[0]
self.plots['cs_obj'] = self.ax_x.plot([], [], lw=2)[0]
def scroll_event(self, event):
if event.key in [str(i + 1) for i in range(self.nb_experts)]:
exp_idx = int(event.key) - 1
def scroll_event(self, event):
if event.key in [str(i + 1) for i in range(self.nb_experts)]:
exp_idx = int(event.key) - 1
incr = np.pi / 16
if event.button == 'up': self.expert_cs[exp_idx].alpha += incr
elif event.button == 'down': self.expert_cs[exp_idx].alpha -= incr
self.expert_cs[exp_idx].A = None
self.update_cs(exp_idx)
incr = np.pi / 16
if event.button == 'up':
self.expert_cs[exp_idx].alpha += incr
elif event.button == 'down':
self.expert_cs[exp_idx].alpha -= incr
self.expert_cs[exp_idx].A = None
self.update_cs(exp_idx)
self.fig.canvas.draw()
self.fig.canvas.draw()
def select_cs(self, idx):
self.pretty_print('Coordinate system %d selected' % idx)
def select_cs(self, idx):
self.pretty_print('Coordinate system %d selected' % idx)
def update_cs(self, exp_idx, obj_exp=False):
if obj_exp:
pts = self.object_experts_cs.get_points()
self.plots['cs_obj'].set_data(pts[:, 0], pts[:, 1])
return
pts = self.expert_cs[exp_idx].get_points()
self.plots['cs_%d' % exp_idx].set_data(pts[:, 0], pts[:, 1])
def update_cs(self, exp_idx, obj_exp=False):
if obj_exp:
pts = self.object_experts_cs.get_points()
self.plots['cs_obj'].set_data(pts[:, 0], pts[:, 1])
return
pts = self.expert_cs[exp_idx].get_points()
self.plots['cs_%d' % exp_idx].set_data(pts[:, 0], pts[:, 1])
def move_event(self, event):
cs_updated = False
def move_event(self, event):
cs_updated = False
if event.key in [str(i + 1) for i in range(self.nb_experts)]:
cs_updated = True
exp_idx = int(event.key) - 1
if event.key in [str(i + 1) for i in range(self.nb_experts)]:
cs_updated = True
exp_idx = int(event.key) - 1
self.expert_cs[exp_idx].x = np.copy(self.curr_mouse_pos)
self.update_cs(exp_idx)
self.expert_cs[exp_idx].x = np.copy(self.curr_mouse_pos)
self.update_cs(exp_idx)
return cs_updated
return cs_updated
class MutliCsInteractiveDemos(InteractiveDemos, MultiCsInteractive):
"""
"""
GUI for recording demonstrations in 2D with moveable frame of reference
"""
def __init__(self, nb_experts=2, erase=False, **kwargs):
MultiCsInteractive.__init__(self, nb_experts=nb_experts, **kwargs)
InteractiveDemos.__init__(self, **kwargs)
if erase:
self.clear_demos()
def __init__(self, nb_experts=2, erase=False, **kwargs):
MultiCsInteractive.__init__(self, nb_experts=nb_experts, **kwargs)
InteractiveDemos.__init__(self, **kwargs)
if not 'A' in self.demos:
for s in ['A', 'b', 'obj_x']: # adding list for coordinate systems
self.demos[s] = []
if erase:
self.clear_demos()
MultiCsInteractive.add_bindings(self)
if 'A' not in self.demos:
for s in ['A', 'b', 'obj_x']: # adding list for coordinate systems
self.demos[s] = []
def scroll_event(self, event):
MultiCsInteractive.scroll_event(self, event)
MultiCsInteractive.add_bindings(self)
def timer_event(self, event):
super(MutliCsInteractiveDemos, self).timer_event(event)
def scroll_event(self, event):
MultiCsInteractive.scroll_event(self, event)
self.curr_cs['A'] += [np.copy(np.array([exp.A for exp in self.expert_cs]))]
self.curr_cs['b'] += [np.copy(np.array([exp.x for exp in self.expert_cs]))]
self.curr_demo_obj += [np.copy(self.object_experts_cs.x)]
def timer_event(self, event):
super(MutliCsInteractiveDemos, self).timer_event(event)
self.curr_cs['A'] += [np.copy(np.array([exp.A for exp in self.expert_cs]))]
self.curr_cs['b'] += [np.copy(np.array([exp.x for exp in self.expert_cs]))]
self.curr_demo_obj += [np.copy(self.object_experts_cs.x)]
def set_plots(self):
def set_plots(self):
InteractiveDemos.set_plots(self)
MultiCsInteractive.set_plots(self)
InteractiveDemos.set_plots(self)
MultiCsInteractive.set_plots(self)
def finish_demo(self):
for s in ['A', 'b']:
self.demos[s] += [np.copy(np.array(self.curr_cs[s]))]
self.curr_cs[s] = []
def finish_demo(self):
for s in ['A', 'b']:
self.demos[s] += [np.copy(np.array(self.curr_cs[s]))]
self.curr_cs[s] = []
self.demos['obj_x'] += [np.copy(np.array(self.curr_demo_obj))]
self.curr_demo_obj = []
self.demos['obj_x'] += [np.copy(np.array(self.curr_demo_obj))]
self.curr_demo_obj = []
super(MutliCsInteractiveDemos, self).finish_demo()
super(MutliCsInteractiveDemos, self).finish_demo()
def move_event(self, event):
super(MutliCsInteractiveDemos, self).move_event(event)
MultiCsInteractive.move_event(self, event)
#
if event.key in [str(i + 1) for i in range(self.nb_experts)]:
exp_idx = int(event.key) - 1
self.expert_cs[exp_idx].x = np.copy(self.curr_mouse_pos)
self.update_cs(exp_idx)
self.fig.canvas.draw()
def move_event(self, event):
super(MutliCsInteractiveDemos, self).move_event(event)
MultiCsInteractive.move_event(self, event)
#
if event.key in [str(i + 1) for i in range(self.nb_experts)]:
exp_idx = int(event.key) - 1
self.expert_cs[exp_idx].x = np.copy(self.curr_mouse_pos)
self.update_cs(exp_idx)
self.fig.canvas.draw()
# if event.key == 'w':
# self.object_experts_cs.x = self.curr_mouse_pos
# self.object_experts_cs.d = self.object_experts_cs.x - self.robot_pos
# self.update_cs(0, obj_exp=True)
# self.fig.canvas.draw()
# else:
# self.object_experts_cs.x = self.object_experts_cs.d + self.robot_pos
# self.update_cs(0, obj_exp=True)
# self.fig.canvas.draw()
# if event.key == 'w':
# self.object_experts_cs.x = self.curr_mouse_pos
# self.object_experts_cs.d = self.object_experts_cs.x - self.robot_pos
# self.update_cs(0, obj_exp=True)
# self.fig.canvas.draw()
# else:
# self.object_experts_cs.x = self.object_experts_cs.d + self.robot_pos
# self.update_cs(0, obj_exp=True)
# self.fig.canvas.draw()
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment