class Simulation(object): def __init__(self): self.show_impulse = False # self.name = 'GP_--N_handfree_naive_clean' # self.name = 'Atlas_##_--N_naive_clean' self.name = 'Atlas_##--N' # self.name = 'BioloidGP_--N' # self.name = 'BioloidGP_--N_from_one_foot' # Init api pydart.init() self.world = pydart.create_world(1.0 / 2000.0) self.world.add_skeleton(config.DATA_PATH + "sdf/ground.urdf", control=False) if 'GP' in self.name: self.world.add_skeleton(config.DATA_PATH + "urdf/BioloidGP/BioloidGP.URDF") self.world.skel.set_joint_damping(0.15) # self.world.add_skeleton(config.DATA_PATH + # "urdf/atlas/atlas_v3_no_head.urdf") else: self.world.add_skeleton(config.DATA_PATH + "urdf/atlas/atlas_v3_no_head.urdf") self.world.skel.set_joint_damping(0.15) self.skel = self.world.skel # shortcut for the control skeleton for i, dof in enumerate(self.skel.dofs): print i, dof, 'efforts:', self.skel.tau_lo[i], self.skel.tau_hi[i] # Configure the scene self.cfg = scene.configure.Configure(self) self.name = self.name.replace('--', '%.1f' % self.cfg.f_mag) if '##' in self.name and self.cfg.tag is not None: self.name = self.name.replace('##', self.cfg.tag) self.prob = problem.Problem(self, self.cfg.name) self.history = History(self) self.impulse_live_renderer = ImpulseLiveRenderer(self) # # ### Now, configure the controllers self.tip_controller = model.controller.Controller(self.skel, self.prob) self.tip_controller.pd.set_pd_params(self.name) self.event_handler = events.Handler() # Reset to the initial state self.reset() print 'skel.m = ', self.skel.m print 'skel.approx_inertia_x = ', self.skel.approx_inertia_x() print 'skel.q = ', self.skel.q self.rc = scene.range_checker.RangeChecker(self) self.rc.check_all() # Abstract model self.abstract_tip = abstract.dynamic.DynamicTIP(self.prob, self.rc) # For handle callbacks properly.. self.history.clear() self.history.push() def is_bioloid(self): return ("BioloidGP" in self.skel.filename) @property def tip(self): return self.tip_controller.tip() def do_plan(self): # return self.do_ik() # Just testing IK # Plan with Dynamic TIP self.abstract_tip.set_x0(self.tip_controller.tips) self.abstract_tip.plan_initial() # cProfile.runctx('self.abstract_tip.plan_initial()', # globals(), locals()) x0 = self.abstract_tip.x0 path = self.abstract_tip.path print 'self.x0 = ', str(x0) print 'self.path = ', str(path) # print 'sleep 5 seconds' # time.sleep(5) self.plan = abstract.plan.Plan(x0, path) self.plan.names = self.prob.contact_names() print 'new plan is generated' self.tip_controller = model.controller.Controller(self.skel, self.prob, self.plan) self.tip_controller.pd.set_pd_params(self.name) print 'new tip controller is generated' self.do_ik() self.name = self.name.replace('_naive', '') def do_ik(self): # self.plan.plot() # self.ik = IKMulti(self, self.plan) self.ik = ik.IKJac(self, self.plan) self.ik.optimize(restore=False) self.tip_controller.targets = self.ik.targets def do_ik_curr(self): curr = self.tip_controller.target_index print 'IK_curr:', curr # self.plan.plot() # # self.ik = IKMulti(self, self.plan) targets = self.tip_controller.targets self.ik = ik.IKJac(self, self.plan, curr, targets) self.ik.optimize(restore=False) self.tip_controller.targets = self.ik.targets def reset(self): self.cfg.reset_simulation(self) # Reset inner structures self.tip_controller.reset() self.event_handler.clear() self.history.clear() self.history.push() self.history.callbacks = [self.tip_controller] self.terminated = dict() def terminate_time(self): name = self.name if 'GP' in name: if '0.5' in name: return 1.25 elif '1.5' in name: return 1.3 else: if 'Lean_1000' in name: return 1.3 elif 'Step_2500' in name: return 1.3 elif 'Back' in name: return 1.35 return 1.0 def step(self): # if 'GP_0.5N' in self.name: # if not self.tip_controller.has_next(): # For GP 0.5N # self.tip_controller.update_target_with_balance() self.skel.tau = self.tip_controller.control() self.world.step() self.history.push() if self.tip_controller.check_next(): self.event_handler.push("proceed", 40) for e in self.event_handler.pop(): # print 'New Event: ', e.name, 'at', self.world.nframes if e.name == "proceed": self.tip_controller.proceed(self) # print 'proceed:', self.tip_controller.is_terminated() if self.tip_controller.is_terminated(): self.event_handler.push("terminate", 0) elif e.name == 'pause': return True elif e.name == 'terminate': self.event_handler.push("terminate", 9999) return True self.event_handler.step() # print self.world.nframes, ':', self.event_handler # return (self.terminate_time() <= self.world.t + 0.001) return False def render(self): glPushMatrix() # gltools.render_axis(10) # Draw chess board gltools.glMove([0.0, -0.01, 0.0]) # gltools.render_chessboard(10, 20.0) gltools.render_floor(20, 40.0) # Draw skeleton gltools.glMove([0, 0, 0]) glPushMatrix() M_s = [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, -1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, -0.001, 0.0, 1.0] glMultMatrixf(M_s) glColor(0.0, 0.0, 0.0, 1.0) gl.glEnable(gl.GL_LIGHTING) self.skel.render_with_color(0.0, 0.0, 0.0) gl.glEnable(gl.GL_LIGHTING) glPopMatrix() gltools.glMove([0, 0, 0]) self.skel.render() gltools.glMove([0, 0, 0]) glColor(1, 0, 0) # gltools.render_arrow(self.skel.C, # self.skel.C + 0.2 * self.tip.projected_Cdot()) # # Draw TIP # tip_index = self.history.get_frame()['tip_index'] # tips = self.tip_controller.tips # for i in range(tip_index, len(tips)): # tips[i].render() # for c in self.prob.contacts: # c.render() # Draw contacts gltools.glMove([0, 0, 0]) # glColor(0.7, 0.0, 0.3) # for c in self.history.get_frame()['contacts']: # gltools.render_arrow(c[0:3], c[0:3] - 0.001 * c[3:6]) if self.history.get_frame()['t'] < 10.0: gltools.glMove([0, 0, 0]) q = self.cfg.saved_target_point d = self.cfg.force() d[0] = 0.0 len_d = np.linalg.norm(d) d /= len_d l = len_d * 0.025 + 0.020 # d[0] = 0.0 # len_d = np.linalg.norm(d) # d /= len_d # len_d /= 60.0 # l = len_d * 0.025 p = q - l * d # p[0] = q[0] # gltools.render_arrow2([0, 0.4, 0], [0, 0.4, 0.4]) rb = 0.01 * (len_d * 0.025 + 1.0) hw = 0.015 * (len_d * 0.05 + 1.0) hl = 0.015 * (len_d * 0.05 + 1.0) rb *= 0.5 hw *= 0.5 # rb *= 2.0 # hw *= 2.0 gltools.render_arrow2(p, q, rb, hw, hl) glPopMatrix() def status_string(self): data = self.history.get_frame() status = "" status += "T = %.4f (%d) " % (data['t'], data['nframes']) status += "C = (%.4f %.4f) " % (data['C.x'], data['C.y']) status += "P = (%.4f %.4f) " % (data['P.x'], data['P.y']) status += "Impulse = %.4f (max %.4f) " % (data['impulse'], data['max_impulse']) status += "I = %.4f " % self.skel.approx_inertia_x() status += "TIP = " + str(data['tip']) + " " # tip = self.tip_controller.tip() # status += "TIP = " + str(tip) status += "Contacted = %s " % str(data['contactedBodies']) return status def __len__(self): return max(self.world.nframes, len(self.history)) def set_world_frame(self, i): if i < self.world.nframes: self.world.set_frame(i) elif i < len(self.history): data = self.history.get_frame_at(i) self.skel.x = data['x'] else: print 'set_world_frame: invalid index', i # pydart_api.setWorldSimFrame(i) self.history.pop(i) def save(self, filename): protocol = pickle.HIGHEST_PROTOCOL # protocol = 0 with open(filename, 'w+') as fp: pickle.dump(self.plan, fp, protocol) pickle.dump(self.tip_controller.targets, fp, protocol) def export_motion(self): # Load motor map data mm = gp.MotorMap() mm.load(config.DATA_PATH + "urdf/BioloidGP/BioloidGPMotorMap.xml") # Translate plan into .mtn m = gp.Motion(mm) transition_times = [e.nx_0.t for e in self.plan.path] durations = np.diff([0.0] + transition_times) poses = self.tip_controller.targets m.add_page('Init', [self.cfg.init_pose()], [1.0]) m.add_page('Falling', poses, durations) m.fill_with_empty_pages() # m.save('test.mtn') filename = '%s.mtn' % self.name m.save(filename) print 'export_motion OK:', filename def load(self, filename): self.name = self.name.replace('_naive', '') with open(filename, 'r') as fp: self.plan = pickle.load(fp) self.tip_controller = model.controller.Controller(self.skel, self.prob, self.plan) self.tip_controller.pd.set_pd_params(self.name) targets = pickle.load(fp) self.tip_controller.targets = targets def save_motion(self, filename): protocol = pickle.HIGHEST_PROTOCOL # protocol = 0 with open(filename, 'w+') as fp: pickle.dump(self.history, fp, protocol) print 'save #', len(self.history), 'frames' def load_motion(self, filename): with open(filename, 'r') as fp: self.history = pickle.load(fp) print 'load #', len(self.history), 'frames' def plot_com(self): # mycolors = ['r', 'b', 'g', 'k', 'c'] traces = [] colors = [] styles = [] traces += self.history.com_traces() n = len(traces) # colors += mycolors[:n] colors += ['r'] * n styles += ['-'] * n if hasattr(self, 'plan') and self.plan is not None: traces += self.plan.com_traces() m = len(traces) - n # colors += mycolors[:n] colors += ['b'] * m styles += ['-'] * m # Ploting with matplotlib plt.ioff() fig = plt.figure() fig.set_size_inches(18.5, 10.5) pp = [] for trace, c, ls in zip(traces, colors, styles): (x, y, text) = trace print 'plot' print 'x:', x print 'y:', y p = plt.plot(x, y, ls=ls, color=c, linewidth=2) pp.append(p[0]) # plt.title('Compare %d Trials on %s' % (num_trials, prob_name), name = self.name.replace('_', ' ') name = name.replace('Lean', '') t = plt.title('%s' % name, fontdict={'size': 40}) t.set_y(0.92) font = {'size': 36} plt.xlabel('X', fontdict=font) plt.ylabel('Y', fontdict=font) pp = [pp[0]] + [None] * (n - 1) + [pp[n]] + [None] * (m - 1) legends = [None] * len(pp) legends[0] = 'Robot' legends[n] = 'Abstract Model' print 'legends:', legends plt.legend(pp, legends, fontsize=36, # bbox_to_anchor=(0.15, 0.15)) loc='lower left') # loc='lower right') # (lo, hi) = plt.axes().get_xlim() # plt.axes().set_xlim(0.0, 0.70) # plt.axes().set_xlim(0.0, 1.50) (lo, hi) = plt.axes().get_ylim() # plt.axes().set_ylim(0.0, hi + 0.1) # plt.axes().set_ylim(0.0, 0.2) # For GP plt.axes().set_ylim(0.0, 1.2) # For GP plt.tick_params(axis='x', labelsize=28) plt.tick_params(axis='y', labelsize=28) outputfile = '%s_com.png' % self.name plt.savefig(outputfile, bbox_inches='tight') plt.close(fig) plt.close(fig) def plot_impulse(self): traces = [] traces += self.history.impulse_traces() colors = ['r'] styles = ['-'] max_t = max(traces[0][0]) print 'max_t:', max_t if hasattr(self, 'plan') and self.plan is not None: traces += self.plan.impulse_traces(max_t=max_t) colors += ['b'] styles += ['-'] # Ploting with matplotlib plt.ioff() fig = plt.figure() fig.set_size_inches(18.5, 10.5) pp = [] for trace, c, ls in zip(traces, colors, styles): (x, y) = trace print 'plot' print 'x:', x print 'y:', y p = plt.plot(x, y, ls=ls, color=c, linewidth=2) pp.append(p[0]) # plt.title('Compare %d Trials on %s' % (num_trials, prob_name), name = self.name.replace('_', ' ') t = plt.title('%s' % name, fontdict={'size': 32}) t.set_y(0.92) font = {'size': 28} plt.xlabel('X', fontdict=font) plt.ylabel('Y', fontdict=font) legends = [None] * len(pp) legends[0] = 'Robot' legends[1] = 'Abstract Model' plt.legend(pp, legends, fontsize=26, loc='upper left') # (lo, hi) = plt.axes().get_xlim() # plt.axes().set_xlim(0.0, 0.36) # Walking # (lo, hi) = plt.axes().get_ylim() # plt.axes().set_ylim(0.0, 0.24) # Walking outputfile = '%s_impulse.png' % self.name plt.savefig(outputfile, bbox_inches='tight') plt.close(fig) def plot_accel(self): t_data, y_data = [], [] # filename = 'OneFootStance.csv' # filename = 'TwoFeetStance.csv' # filename = 'Rolling.csv' # filename = 'GP00_new_2.csv' # filename = 'GP01.csv' # filename = 'test.csv' # filename = 'test2.csv' # filename = 'yogamatt00.csv' filename = 'yogamatt01.csv' # filename = ['yogamatt00.csv', 'yogamatt01.csv'] if isinstance(filename, str): with open(filename) as fin: for line in fin.readlines(): values = [float(x.strip()) for x in line.split(',')] t_data += [values[0]] accel = np.array([values[1], values[2], values[3]]) y_data += [np.linalg.norm(accel)] # print values, t_data[-1], y_data[-1] else: offset = 0.0 for f in filename: with open(f) as fin: for line in fin.readlines(): values = [float(x.strip()) for x in line.split(',')] t_data += [values[0] + offset] accel = np.array([values[1], values[2], values[3]]) y_data += [np.linalg.norm(accel)] offset += max(t_data) + 10.0 max_t = max(t_data) times = [(0, max_t)] colors = ['r'] legends = ['All'] interactive = True styles = None if 'TwoFeet' in filename: times = [(135.0, 139.0), (216.4, 220.4)] colors = ['r', 'b'] legends = ['Planned', 'Unplanned'] interactive = False elif 'OneFoot' in filename: times = [(81.0, 85.0), (122.0, 126.0)] # 4.5 6.9 colors = ['r', 'b'] legends = ['Planned', 'Unplanned'] interactive = False elif 'Rolling' in filename: times = [(154.5, 158.5), (197.5, 201.5)] # 5.23 13.36 colors = ['r', 'b'] legends = ['Planned', 'Unplanned'] interactive = False elif 'GP00_new.' in filename: times = [(119.5, 123.5), (235.0, 239.0)] # 8.8 12.1 colors = ['r', 'b'] legends = ['Planned', 'Unplanned'] interactive = False elif 'yogamatt00' in filename: times = [(152.9, 155.9), (223.7, 226.7)] # 8.8 12.1 colors = ['r', 'b'] legends = ['Planned', 'Unplanned'] interactive = False elif 'yogamatt01' in filename: x = 360.2396 + 10.0 times = [(466.1 - x, 469.1 - x), (557.51 - x, 560.51 - x)] colors = ['r', 'b'] legends = ['Planned', 'Unplanned'] interactive = False # elif 'yogamatt00.csv' in filename: # times = [(153.5, 156.5), (224.5, 227.5), # 5.49 7.98 # (467.0, 470.0), (558.5, 561.5)] # 5.50 8.04 # colors = ['r', 'r', 'b', 'b'] # styles = ['-', '--', '-', '--'] # legends = ['0.0N (Ours)', '0.0N (Baseline)', # '0.5 (Ours)', '0.5 (Baseline)'] # interactive = False if styles is None: styles = ['-'] * len(colors) traces = [] for lo, hi in times: trace_x = [] trace_y = [] for t, y in zip(t_data, y_data): if lo <= t and t <= hi: trace_x += [t - lo] trace_y += [y] traces += [(trace_x, trace_y)] if not interactive: plt.ioff() fig = plt.figure() fig.set_size_inches(18.5, 10.5) pp = [] for trace, c, ls in zip(traces, colors, styles): (x, y) = trace print 'plot', max(y) # print 'x:', x # print 'y:', y p = plt.plot(x, y, color=c, ls=ls, linewidth=2) pp.append(p[0]) t = plt.title('BioloidGP 0.5N', fontdict={'size': 36}) t.set_x(0.5) t.set_y(0.92) font = {'size': 28} plt.xlabel('Time', fontdict=font) plt.ylabel('Acceleration of Head', fontdict=font) plt.tick_params(axis='x', labelsize=28) plt.tick_params(axis='y', labelsize=28) (lo, hi) = plt.axes().get_ylim() plt.axes().set_ylim(0.0, 10.0) plt.legend(pp, legends, fontsize=26, loc='upper right') if not interactive: if isinstance(filename, str): outputfile = '%s_impulse.png' % (filename.replace('.csv', '')) else: outputfile = '%s_all_impulse.png' \ % (filename[0].replace('.csv', '')) plt.savefig(outputfile, bbox_inches='tight') plt.close(fig)