def compute_position(ramp_time, max_duration, acceleration, t):
velocity = acceleration * ramp_time
max_time = max_duration - 2 * ramp_time
t1 = clip(t, 0, ramp_time)
t2 = clip(t - ramp_time, 0, max_time)
t3 = clip(t - ramp_time - max_time, 0, ramp_time)
#assert t1 + t2 + t3 == t
return 0.5 * acceleration * math.pow(t1, 2) + velocity * t2 + \
(velocity * t3 - 0.5 * acceleration * math.pow(t3, 2))
def slow_trajectory(robot,
joints,
path,
min_fraction=0.1,
ramp_duration=1.0,
**kwargs):
"""
:param robot:
:param joints:
:param path:
:param min_fraction: percentage
:param ramp_duration: seconds
:param kwargs:
:return:
"""
time_from_starts = instantaneous_retime_path(robot, joints, path, **kwargs)
mid_times = [np.average(pair) for pair in get_pairs(time_from_starts)]
mid_durations = [t2 - t1 for t1, t2 in get_pairs(time_from_starts)]
new_time_from_starts = [0.]
for mid_time, mid_duration in zip(mid_times, mid_durations):
time_from_start = mid_time - time_from_starts[0]
up_fraction = clip(time_from_start / ramp_duration,
min_value=min_fraction,
max_value=1.)
time_from_end = time_from_starts[-1] - mid_time
down_fraction = clip(time_from_end / ramp_duration,
min_value=min_fraction,
max_value=1.)
new_fraction = min(up_fraction, down_fraction)
new_duration = mid_duration / new_fraction
#print(new_time_from_starts[-1], up_fraction, down_fraction, new_duration)
new_time_from_starts.append(new_time_from_starts[-1] + new_duration)
# print(time_from_starts)
# print(new_time_from_starts)
# raw_input('Continue?)
# time_from_starts = new_time_from_starts
return new_time_from_starts
def score_masses(args, task, ros_world, init_total_mass, bowl_masses,
final_raw_masses):
# TODO: assert that mass is not zero if object known to be above
# TODO: unify this with the simulator scoring
cup_name, bowl_name = REQUIREMENT_FNS[args.problem](ros_world)
name_from_type = {
'cup': cup_name,
}
if POUR:
name_from_type.update({
'bowl': bowl_name,
})
final_masses = estimate_masses(final_raw_masses, bowl_masses,
args.material)
if POUR:
final_total_mass = sum(final_masses.values())
print('Total mass:', final_total_mass)
print(
'Spilled (%):',
clip(init_total_mass - final_total_mass, min_value=0, max_value=1))
score = {'total_mass': init_total_mass}
score.update({
'mass_in_{}'.format(ty): final_masses[name]
for ty, name in name_from_type.items()
})
if POUR:
final_percentages = {
name: mass / init_total_mass
for name, mass in final_masses.items()
}
print('Percentages (%):', str_from_object(final_percentages))
#score.update({'fraction_in_{}'.format(ty): final_percentages[name]
# for ty, name in name_from_type.items()})
if task.holding:
[spoon_name] = task.holding
spoon_capacity = SPOON_CAPACITIES[get_type(spoon_name), args.material]
fraction_filled = final_masses[cup_name] / spoon_capacity
print('Spoon percentage filled (%):', fraction_filled)
score.update({
'mass_in_spoon': final_masses[cup_name],
#'fraction_in_spoon': final_masses[cup_name] / init_total_mass,
#'spoon_capacity': spoon_capacity,
#'fraction_filled': fraction_filled,
})
return score
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()