def piecewise_score(value, threshold):
if value <= threshold:
return rescale(value,
interval=(0, threshold),
new_interval=(FAILURE, THRESHOLD))
return rescale(value,
interval=(threshold, 1),
new_interval=(THRESHOLD, SUCCESS))
def scoop_bowl_difficulty(feature, threshold=0.5):
min_height = CM_FROM_M * BOWL_PROPERTIES['red'].height
max_height = CM_FROM_M * BOWL_PROPERTIES['large_red'].height
interval = (min_height, max_height)
percent = rescale(feature['bowl_height'],
interval=interval,
new_interval=(0, 1))
alpha = rescale(min(percent, threshold),
interval=(0, threshold),
new_interval=(0.5, 0.9))
# print(feature['bowl_type'], interval, feature['bowl_height'], alpha)
# print('{:.3f}, {:.3f}'.format(percent, alpha))
return alpha
def context_from_feature(self, feature):
context = np.array([
rescale(feature[name], self.feature_ranges[name])
for name in self.features
])
assert len(context) == len(self.context_idx)
return context
def sample_from_parameter(self, parameter):
sample = np.array([
rescale(parameter[name],
self.parameter_ranges[name],
new_interval=DEFAULT_INTERVAL) for name in self.parameters
])
assert len(sample) == len(self.param_idx)
return sample
def parameter_from_sample(self, sample):
assert len(sample) == len(self.param_idx)
parameter = sample_parameter(
self.parameter_ranges
) # In the event that self.parameters is a subset
parameter.update({
name: rescale(value,
DEFAULT_INTERVAL,
new_interval=self.parameter_ranges[name])
for name, value in safe_zip(self.parameters, sample)
})
return parameter
def predict(self, X):
if type(self.model) in [ExtraTreesRegressor, ExtraTreesClassifier]:
mu, _ = rf_predict(self.model, X)
elif self.model_type in CLASSIFIERS:
mu = self.model.predict_proba(X)[:, -1]
mu = np.array(
[rescale(v, interval=CLASSIFIER_INTERVAL) for v in mu])
mu = mu.reshape([-1, 1])
elif self.model_type in REGRESSORS:
mu = self.model.predict(X)
mu = mu.reshape([-1, 1])
else:
raise NotImplementedError(self.model_type)
var = None
return mu, var
def visualize_collected_pours(paths, num=6, save=True):
result_from_bowl = {}
for path in randomize(paths):
results = read_data(path)
print(path, len(results))
for result in results:
result_from_bowl.setdefault(result['feature']['bowl_type'], []).append(result)
world = create_world()
environment = get_bodies()
#collector = SKILL_COLLECTORS['pour']
print(get_camera())
for bowl_type in sorted(result_from_bowl):
# TODO: visualize same
for body in get_bodies():
if body not in environment:
remove_body(body)
print('Bowl type:', bowl_type)
bowl_body = load_cup_bowl(bowl_type)
bowl_pose = get_pose(bowl_body)
results = result_from_bowl[bowl_type]
results = randomize(results)
score_cup_pose = []
for i, result in enumerate(results):
if num <= len(score_cup_pose):
break
feature = result['feature']
parameter = result['parameter']
score = result['score']
if (score is None) or not result['execution'] or result['annotation']:
continue
cup_body = load_cup_bowl(feature['cup_type'])
world.bodies[feature['bowl_name']] = bowl_body
world.bodies[feature['cup_name']] = cup_body
fraction = compute_fraction_filled(score)
#value = collector.score_fn(feature, parameter, score)
value = pour_score(feature, parameter, score)
print(i, feature['cup_type'], fraction, value)
path, _ = pour_path_from_parameter(world, feature, parameter)
sort = fraction
#sort = parameter['pitch']
#sort = parameter['axis_in_bowl_z']
score_cup_pose.append((sort, fraction, value, cup_body, path[0]))
#order = score_cup_pose
order = randomize(score_cup_pose)
#order = sorted(score_cup_pose)
angles = np.linspace(0, 2*np.pi, num=len(score_cup_pose), endpoint=False) # Halton
for angle, (_, fraction, value, cup_body, pose) in zip(angles, order):
#fraction = clip(fraction, min_value=0, max_value=1)
value = clip(value, *DEFAULT_INTERVAL)
fraction = rescale(value, DEFAULT_INTERVAL, new_interval=(0, 1))
#color = (1 - fraction) * np.array(RED) + fraction * np.array(GREEN)
color = interpolate_hue(fraction)
set_color(cup_body, apply_alpha(color, alpha=0.25))
#angle = random.uniform(-np.pi, np.pi)
#angle = 0
rotate_bowl = Pose(euler=Euler(yaw=angle))
cup_pose = multiply(bowl_pose, invert(rotate_bowl), pose)
set_pose(cup_body, cup_pose)
#wait_for_user()
#remove_body(cup_body)
if save:
#filename = os.path.join('workspace', '{}.png'.format(bowl_type))
#save_image(filename, take_picture()) # [0, 255]
#wait_for_duration(duration=0.5)
# TODO: get window location
#os.system("screencapture -R {},{},{},{} {}".format(
# 275, 275, 500, 500, filename)) # -R<x,y,w,h> capture screen rect
wait_for_user()
remove_body(bowl_body)
def visualize_vector_field(learner, bowl_type='blue_bowl', cup_type='red_cup',
num=500, delta=False, alpha=0.5):
print(learner, len(learner.results))
xx, yy, ww = learner.xx, learner.yy, learner.weights
learner.hyperparameters = get_parameters(learner.model)
print(learner.hyperparameters)
learner.query_type = REJECTION # BEST | REJECTION | STRADDLE
world = create_world()
world.bodies[bowl_type] = load_cup_bowl(bowl_type)
world.bodies[cup_type] = load_cup_bowl(cup_type)
feature = get_pour_feature(world, bowl_type, cup_type)
set_point(world.bodies[cup_type], np.array([0.2, 0, 0]))
# TODO: visualize training data as well
# TODO: artificially limit training data to make a low-confidence region
# TODO: visualize mean and var at the same time using intensity and hue
print('Feature:', feature)
with LockRenderer():
#for parameter in islice(learner.parameter_generator(world, feature, valid=True), num):
parameters = []
while len(parameters) < num:
parameter = learner.sample_parameter()
# TODO: special color if invalid
if is_valid_pour(world, feature, parameter):
parameters.append(parameter)
center, _ = approximate_as_prism(world.get_body(cup_type))
bodies = []
with LockRenderer():
for parameter in parameters:
path, _ = pour_path_from_parameter(world, feature, parameter)
pose = path[0]
body = create_cylinder(radius=0.001, height=0.02, color=apply_alpha(GREY, alpha))
set_pose(body, multiply(pose, Pose(point=center)))
bodies.append(body)
#train_sizes = inclusive_range(10, 100, 1)
#train_sizes = inclusive_range(10, 100, 10)
#train_sizes = inclusive_range(100, 1000, 100)
#train_sizes = [1000]
train_sizes = [None]
# training_poses = []
# for result in learner.results[:train_sizes[-1]]:
# path, _ = pour_path_from_parameter(world, feature, result['parameter'])
# pose = path[0]
# training_poses.append(pose)
# TODO: draw the example as well
scores_per_size = []
for train_size in train_sizes:
if train_size is not None:
learner.xx, learner.yy, learner.weights = xx[:train_size], yy[:train_size], ww[:train_size]
learner.retrain()
X = np.array([learner.func.x_from_feature_parameter(feature, parameter) for parameter in parameters])
predictions = learner.predict_x(X, noise=False) # Noise is just a constant
scores_per_size.append([prediction['mean'] for prediction in predictions]) # mean | variance
#scores_per_size.append([1./np.sqrt(prediction['variance']) for prediction in predictions])
#scores_per_size.append([prediction['mean'] / np.sqrt(prediction['variance']) for prediction in predictions])
#plt.hist(scores_per_size[-1])
#plt.show()
#scores_per_size.append([scipy.stats.norm.interval(alpha=0.95, loc=prediction['mean'],
# scale=np.sqrt(prediction['variance']))[0]
# for prediction in predictions]) # mean | variance
# score = learner.score(feature, parameter)
if delta:
scores_per_size = [np.array(s2) - np.array(s1) for s1, s2 in zip(scores_per_size, scores_per_size[1:])]
train_sizes = train_sizes[1:]
all_scores = [score for scores in scores_per_size for score in scores]
#interval = (min(all_scores), max(all_scores))
interval = scipy.stats.norm.interval(alpha=0.95, loc=np.mean(all_scores), scale=np.std(all_scores))
#interval = DEFAULT_INTERVAL
print('Interval:', interval)
print('Mean: {:.3f} | Stdev: {:.3f}'.format(np.mean(all_scores), np.std(all_scores)))
#train_body = create_cylinder(radius=0.002, height=0.02, color=apply_alpha(BLACK, 1.0))
for i, (train_size, scores) in enumerate(zip(train_sizes, scores_per_size)):
print('Train size: {} | Average: {:.3f} | Min: {:.3f} | Max: {:.3f}'.format(
train_size, np.mean(scores), min(scores), max(scores)))
handles = []
# TODO: visualize delta
with LockRenderer():
#if train_size is None:
# train_size = len(learner.results)
#set_pose(train_body, multiply(training_poses[train_size-1], Pose(point=center)))
#set_pose(world.bodies[cup_type], training_poses[train_size-1])
for score, body in zip(scores, bodies):
score = clip(score, *interval)
fraction = rescale(score, interval, new_interval=(0, 1))
color = interpolate_hue(fraction)
#color = (1 - fraction) * np.array(RED) + fraction * np.array(GREEN) # TODO: convex combination
set_color(body, apply_alpha(color, alpha))
#set_pose(world.bodies[cup_type], pose)
#draw_pose(pose, length=0.05)
#handles.extend(draw_point(tform_point(pose, center), color=color))
#extent = np.array([0, 0, 0.01])
#start = tform_point(pose, center - extent/2)
#end = tform_point(pose, center + extent/2)
#handles.append(add_line(start, end, color=color, width=1))
wait_for_duration(0.5)
# TODO: test on boundaries
wait_for_user()