def evaluate(sample_file, **kwargs): use_duration = True if kwargs['D'] else False window = int(kwargs['w']) pv('window', 'use_duration', stdout=True) samples = None models = None if kwargs['M'] or kwargs['B']: samples = {classification.normalize_name(t): [t] for t in sample_file} models = extract_models(ZIPPED_MODELS) if samples and models: if kwargs['M']: classification.evaluate(samples, models, use_duration=use_duration, window=window) if kwargs['B']: binary_samples = classification.binarize(samples) binary_models = classification.binarize(models) if binary_samples and binary_models: classification.evaluate(binary_samples, binary_models, use_duration=use_duration, window=window)
def plan(self, start, goal): traj, cost = self.plan_with_inittraj(start, goal) if traj is not None: return traj, cost bounds = bound.get_bounds(self.robot, self.params.dof) solutions = [] for i, waypoint in enumerate(self.gen_waypoint(bounds)): print '{} - int_planners try waypoint {} at depth {}...'.format(self.name(), i, self.params.depth) if self.verbose: utils.pv('waypoint') inittraj = [] traj1, _ = self.int_planner1.plan(start, waypoint) if traj1 is not None: traj2, _ = self.int_planner2.plan(waypoint, goal) if traj2 is not None: print '{} - int_planners find plans at depth {}...'.format(self.name(), self.params.depth) if self.params.dof == 4: traj1 = Planner.six_to_four([t for t in traj1]) traj2 = Planner.six_to_four([t for t in traj2]) inittraj.extend(traj1[0:self.int_planner1.params.n_steps] + traj2[-1-self.int_planner2.params.n_steps:]) if inittraj: traj, cost = self.plan_with_inittraj(start, goal, inittraj=inittraj) if traj is not None: solutions.append((traj, cost)) if self.params.first_return: break if solutions: return sorted(solutions, key=lambda x: x[1])[0] return None, None
def simulate(self, command, max_steps): if 'trajectory' in command: draw.draw_trajectory(self.state.env, command.trajectory, reset=True) for step in xrange(max_steps): start = time.time() if self.verbose: print '\nstep: {}, time: {}'.format(step, self.get_sim_time()) self.state.update(step) pipeline = [command] for c in self.controllers: pipeline.append(c.update(pipeline[-1], self.params.timestep)) self.state.apply( self.aerodynamics.apply( pipeline[-1].wrench, self.params.timestep), self.params.timestep) finished = self.controllers[0].finished() if finished or not self.state.valid(): if not self.state.valid(): utils.pv('self.state.load_factor') utils.pv('self.state.position') return False, step break if self.params.sleep: time.sleep(max(self.params.timestep - time.time() + start, 0.0)) return True, step
def simulate(self, command, max_steps): if 'trajectory' in command: draw.draw_trajectory(self.state.env, command.trajectory, reset=True) for step in xrange(max_steps): start = time.time() if self.verbose: print '\nstep: {}, time: {}'.format(step, self.get_sim_time()) self.state.update(step) pipeline = [command] for c in self.controllers: pipeline.append(c.update(pipeline[-1], self.params.timestep)) self.state.apply( self.aerodynamics.apply(pipeline[-1].wrench, self.params.timestep), self.params.timestep) finished = self.controllers[0].finished() if finished or not self.state.valid(): if not self.state.valid(): utils.pv('self.state.load_factor') utils.pv('self.state.position') return False, step break if self.params.sleep: time.sleep(max(self.params.timestep - time.time() + start, 0.0)) return True, step
def test(self, command_func, test_count=1, max_steps=10000): utils.pv('command_func', 'test_count', 'max_steps') experiments, success, progress = 0, 0, 0.0 for _ in range(test_count): experiments += 1 command = command_func() with self.robot: ret = self.simulator.run(command, max_steps=max_steps) success += ret[0] progress = (progress * (experiments - 1) + ret[1] / ret[2]) / experiments utils.pv('experiments', 'success', 'success/experiments', 'progress') time.sleep(1.0)
def process(self): twist_body = addict.Dict() twist_body.linear = self.state.to_body(self.state.twist.linear) twist_body.angular = self.state.to_body(self.state.twist.angular) load_factor = utils.bound(self.state.load_factor, self.load_factor_limit) acceleration_command = np.array([0.0, 0.0, 0.0]) acceleration_command[0] = self.pid.linear.x.update( self.input_.twist.linear.x, self.state.twist.linear[0], self.state.acceleration[0], self.dt ) acceleration_command[1] = self.pid.linear.y.update( self.input_.twist.linear.y, self.state.twist.linear[1], self.state.acceleration[1], self.dt ) acceleration_command[2] = ( self.pid.linear.z.update( self.input_.twist.linear.z, self.state.twist.linear[2], self.state.acceleration[2], self.dt ) + self.state.gravity ) acceleration_command_body = self.state.to_body(acceleration_command) if self.verbose: utils.pv( "twist_body", "self.state.load_factor", "load_factor", "acceleration_command", "acceleration_command_body", ) self.output.wrench.torque.x = self.state.inertia[0] * self.pid.angular.x.update( -acceleration_command_body[1] / self.state.gravity, 0.0, twist_body.angular[0], self.dt ) self.output.wrench.torque.y = self.state.inertia[1] * self.pid.angular.y.update( acceleration_command_body[0] / self.state.gravity, 0.0, twist_body.angular[1], self.dt ) self.output.wrench.torque.z = self.state.inertia[2] * self.pid.angular.z.update( self.input_.twist.angular.z, self.state.twist.angular[2], 0.0, self.dt ) self.output.wrench.force.x = 0.0 self.output.wrench.force.y = 0.0 self.output.wrench.force.z = self.state.mass * ( (acceleration_command[2] - self.state.gravity) * load_factor + self.state.gravity ) self.output.wrench.force.z = utils.bound(self.output.wrench.force.z, self.force_z_limit) self.output.wrench.force.z = max(self.output.wrench.force.z, 0.0) self.output.wrench.torque.x = utils.bound(self.output.wrench.torque.x, self.torque_xy_limit) self.output.wrench.torque.y = utils.bound(self.output.wrench.torque.y, self.torque_xy_limit) self.output.wrench.torque.z = utils.bound(self.output.wrench.torque.z, self.torque_z_limit)
def process(self): twist_body = addict.Dict() twist_body.linear = self.state.to_body(self.state.twist.linear) twist_body.angular = self.state.to_body(self.state.twist.angular) load_factor = utils.bound(self.state.load_factor, self.load_factor_limit) acceleration_command = np.array([0.0, 0.0, 0.0]) acceleration_command[0] = self.pid.linear.x.update( self.input_.twist.linear.x, self.state.twist.linear[0], self.state.acceleration[0], self.dt) acceleration_command[1] = self.pid.linear.y.update( self.input_.twist.linear.y, self.state.twist.linear[1], self.state.acceleration[1], self.dt) acceleration_command[2] = self.pid.linear.z.update( self.input_.twist.linear.z, self.state.twist.linear[2], self.state.acceleration[2], self.dt) + self.state.gravity acceleration_command_body = self.state.to_body(acceleration_command) if self.verbose: utils.pv('twist_body', 'self.state.load_factor', 'load_factor', 'acceleration_command', 'acceleration_command_body') self.output.wrench.torque.x = self.state.inertia[ 0] * self.pid.angular.x.update( -acceleration_command_body[1] / self.state.gravity, 0.0, twist_body.angular[0], self.dt) self.output.wrench.torque.y = self.state.inertia[ 1] * self.pid.angular.y.update( acceleration_command_body[0] / self.state.gravity, 0.0, twist_body.angular[1], self.dt) self.output.wrench.torque.z = self.state.inertia[ 2] * self.pid.angular.z.update(self.input_.twist.angular.z, self.state.twist.angular[2], 0.0, self.dt) self.output.wrench.force.x = 0.0 self.output.wrench.force.y = 0.0 self.output.wrench.force.z = self.state.mass * \ ((acceleration_command[2] - self.state.gravity) * load_factor + self.state.gravity) self.output.wrench.force.z = utils.bound(self.output.wrench.force.z, self.force_z_limit) self.output.wrench.force.z = max(self.output.wrench.force.z, 0.0) self.output.wrench.torque.x = utils.bound(self.output.wrench.torque.x, self.torque_xy_limit) self.output.wrench.torque.y = utils.bound(self.output.wrench.torque.y, self.torque_xy_limit) self.output.wrench.torque.z = utils.bound(self.output.wrench.torque.z, self.torque_z_limit)
def process(self): self.output.twist.linear.x = self.pid.x.update( self.input_.pose.x, self.state.position[0], self.state.twist.linear[0], self.dt) self.output.twist.linear.y = self.pid.y.update( self.input_.pose.y, self.state.position[1], self.state.twist.linear[1], self.dt) self.output.twist.linear.z = self.pid.z.update( self.input_.pose.z, self.state.position[2], self.state.twist.linear[2], self.dt) yaw_command = angles.normalize(self.input_.pose.yaw, self.state.euler[2] - math.pi, self.state.euler[2] + math.pi) self.output.twist.angular.z = self.pid.yaw.update( yaw_command, self.state.euler[2], self.state.twist.angular[2], self.dt) if self.verbose: utils.pv('self.state.euler[2]', 'yaw_command')
def process(self): self.output.twist.linear.x = self.pid.x.update( self.input_.pose.x, self.state.position[0], self.state.twist.linear[0], self.dt ) self.output.twist.linear.y = self.pid.y.update( self.input_.pose.y, self.state.position[1], self.state.twist.linear[1], self.dt ) self.output.twist.linear.z = self.pid.z.update( self.input_.pose.z, self.state.position[2], self.state.twist.linear[2], self.dt ) yaw_command = angles.normalize( self.input_.pose.yaw, self.state.euler[2] - math.pi, self.state.euler[2] + math.pi ) self.output.twist.angular.z = self.pid.yaw.update( yaw_command, self.state.euler[2], self.state.twist.angular[2], self.dt ) if self.verbose: utils.pv("self.state.euler[2]", "yaw_command")
def run(self): while True: start = self.robot.GetActiveDOFValues() goal = self.collision_free(self.random_goal) draw.draw_pose(self.env, goal, reset=True) draw.draw_trajectory(self.env, None, reset=True) with self.robot: if self.verbose: print 'planning to: {}'.format(goal) traj, cost = self.planner.plan(start, goal) if traj is not None: time.sleep(1) draw.draw_trajectory(self.env, traj, reset=True) if self.verbose: utils.pv('traj', 'cost') self.execute_trajectory(traj) time.sleep(1)
def evaluate(samples, models, use_duration=True, window=semi_markov.WINDOW): confusion_matrix = makehash() for klass, file_names in samples.items(): counters = statistics( file_names, models, use_duration=use_duration, window=window) for threshold in counters: for term, counter in counters[threshold].items(): key = '_'.join([klass, term]) confusion_matrix[threshold][key] = counter for threshold, matrix in sorted(confusion_matrix.items()): pv('threshold', stdout=True) print_statistics(matrix, threshold) print
def apply(self, wrench, dt): if self.drag_model.enabled: self.drag_model.u[0] = (self.state.twist.linear[0] - self.wind[0]) self.drag_model.u[1] = -(self.state.twist.linear[1] - self.wind[1]) self.drag_model.u[2] = -(self.state.twist.linear[2] - self.wind[2]) self.drag_model.u[3] = self.state.twist.angular[0] self.drag_model.u[4] = -self.state.twist.angular[1] self.drag_model.u[5] = -self.state.twist.angular[2] self.drag_model.u[0:3] = utils.rotate(self.drag_model.u[0:3], self.state.quaternion) self.drag_model.u[3:6] = utils.rotate(self.drag_model.u[3:6], self.state.quaternion) self.drag_model.limit(-100.0, 100.0) if self.verbose: print utils.pv('self.__class__.__name__') self.f(self.drag_model.u, dt, self.drag_model.y) if self.verbose: utils.pv('self.drag_model') if self.verbose: utils.pv('wrench') if len(wrench): wrench.force.x += -self.drag_model.y[0] wrench.force.y += self.drag_model.y[1] wrench.force.z += self.drag_model.y[2] wrench.torque.x += -self.drag_model.y[3] wrench.torque.y += self.drag_model.y[4] wrench.torque.z += self.drag_model.y[5] else: wrench.force.x = -self.drag_model.y[0] wrench.force.y = self.drag_model.y[1] wrench.force.z = self.drag_model.y[2] wrench.torque.x = -self.drag_model.y[3] wrench.torque.y = self.drag_model.y[4] wrench.torque.z = self.drag_model.y[5] if self.verbose: utils.pv('wrench') return wrench
def done(self, game): if game.verbose: utils.pv('self.full_name(game)') utils.pv('self.Q') utils.pv('self.pi') numplots = game.numplots if game.numplots >= 0 else len(self.record) for s, record in sorted( self.record.items(), key=lambda x: -len(x[1]))[:numplots]: self.plot_record(s, record, game) self.record.clear()
def apply(self, wrench, dt): if 'force' in wrench and 'torque' in wrench: g_com = self.center_of_mass l_com = self.to_body(g_com - self.position) force = np.r_[wrench.force.x, wrench.force.y, wrench.force.z] torque = np.r_[wrench.torque.x, wrench.torque.y, wrench.torque.z] torque = torque - np.cross(l_com, force) if self.verbose: print utils.pv('self.__class__.__name__') utils.pv('g_com', 'l_com') utils.pv('force', 'torque') utils.pv('self.from_body(force)', 'self.from_body(torque)') with self.env: self.base_link.SetForce(self.from_body(force), g_com, True) self.base_link.SetTorque(self.from_body(torque), True) with self.env: self.env.StepSimulation(dt) self.env.UpdatePublishedBodies()
def update(self, input_, dt): self.input_ = input_ self.output = addict.Dict() self.dt = dt if self.verbose: print utils.pv("self.__class__.__name__", "self.dt") utils.pv("self.input_") if self.command in self.input_ and self.input_[self.command]: self.process() else: self.output = self.input_ if self.verbose: utils.pv("self.output") return self.output
def update(self, input_, dt): self.input_ = input_ self.output = addict.Dict() self.dt = dt if self.verbose: print utils.pv('self.__class__.__name__', 'self.dt') utils.pv('self.input_') if self.command in self.input_ and self.input_[self.command]: self.process() else: self.output = self.input_ if self.verbose: utils.pv('self.output') return self.output
def print_statistics(matrix, threshold): tabs = lambda x: ''.join(['\t'] * x) + 'threshold: %s, ' % threshold print_table(matrix) for klass, counter in sorted(matrix.items()): recall = counter[klass] / sum(counter.values()) total = sum(c[klass] for c in matrix.values()) precision = counter[klass] / total if total > 0.0 else 0.0 pv('klass', prefix=tabs(1), stdout=True) pv('recall', prefix=tabs(2), stdout=True) pv('precision', prefix=tabs(2), stdout=True) pv('g_measure(precision, recall)', prefix=tabs(2), stdout=True) pv('f_measure(precision, recall, 0.5)', prefix=tabs(2), stdout=True) pv('f_measure(precision, recall, 1.0)', prefix=tabs(2), stdout=True) pv('f_measure(precision, recall, 2.0)', prefix=tabs(2), stdout=True)
def update(self, step): self.step = step if self.verbose: print utils.pv('self.__class__.__name__') utils.pv('self.pose', 'self.position') utils.pv('self.euler', 'self.quaternion', 'self.inverse_quaternion') utils.pv('self.twist', 'self.acceleration') utils.pv('self.mass', 'self.inertia', 'self.gravity') utils.pv('self.center_of_mass', 'self.robot.GetCenterOfMass()')