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 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):
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 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 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 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()')
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()')
