def find_closest(x0,
curve,
t_range=None,
max_time=INF,
max_iterations=INF,
distance_fn=None,
verbose=False):
assert (max_time < INF) or (max_iterations < INF)
if t_range is None:
t_range = Interval(curve.x[0], curve.x[-1])
t_range = Interval(max(t_range[0], curve.x[0]),
min(curve.x[-1], t_range[-1]))
if distance_fn is None:
distance_fn = get_distance
start_time = time.time()
closest_dist, closest_t = INF, None
for iteration in irange(max_iterations):
if elapsed_time(start_time) >= max_time:
break
t = random.uniform(*t_range) # TODO: halton
x = curve(t)
dist = distance_fn(x0, x)
if dist < closest_dist:
closest_dist, closest_t = dist, t
if verbose:
print(
'Iteration: {} | Dist: {:.3f} | T: {:.3f} | Time: {:.3f}'.
format(iteration, closest_dist, t,
elapsed_time(start_time)))
return closest_dist, closest_t
def rrt(start,
goal_sample,
distance,
sample,
extend,
collision,
goal_test=lambda q: False,
iterations=RRT_ITERATIONS,
goal_probability=.2):
if collision(start):
return None
if not callable(goal_sample):
g = goal_sample
goal_sample = lambda: g
nodes = [TreeNode(start)]
for i in irange(iterations):
goal = random() < goal_probability or i == 0
s = goal_sample() if goal else sample()
last = argmin(lambda n: distance(n.config, s), nodes)
for q in extend(last.config, s):
if collision(q):
break
last = TreeNode(q, parent=last)
nodes.append(last)
if goal_test(last.config):
return configs(last.retrace())
else:
if goal:
return configs(last.retrace())
return None
def birrt(q1,
q2,
distance,
sample,
extend,
collision,
restarts=RRT_RESTARTS,
iterations=RRT_ITERATIONS,
smooth=RRT_SMOOTHING):
path = direct_path(q1, q2, extend, collision)
if path is not None:
return path
for _ in irange(restarts + 1):
path = rrt_connect(q1,
q2,
distance,
sample,
extend,
collision,
iterations=iterations)
if path is not None:
if smooth is None:
return path
return smooth_path(path, extend, collision, iterations=smooth)
return None
def rrt_connect(q1,
q2,
distance,
sample,
extend,
collision,
iterations=RRT_ITERATIONS):
if collision(q1) or collision(q2):
return None
root1, root2 = TreeNode(q1), TreeNode(q2)
nodes1, nodes2 = [root1], [root2]
for _ in irange(iterations):
if len(nodes1) > len(nodes2):
nodes1, nodes2 = nodes2, nodes1
s = sample()
last1 = argmin(lambda n: distance(n.config, s), nodes1)
for q in extend(last1.config, s):
if collision(q):
break
last1 = TreeNode(q, parent=last1)
nodes1.append(last1)
last2 = argmin(lambda n: distance(n.config, last1.config), nodes2)
for q in extend(last2.config, last1.config):
if collision(q):
break
last2 = TreeNode(q, parent=last2)
nodes2.append(last2)
else:
path1, path2 = last1.retrace(), last2.retrace()
if path1[0] != root1:
path1, path2 = path2, path1
return configs(path1[:-1] + path2[::-1])
return None
def grow(self,
goal_sample,
iterations=50,
goal_probability=.2,
store=ts.PATH,
max_tree_size=500):
if not callable(goal_sample):
goal_sample = lambda: goal_sample
nodes, new_nodes = list(
take(randomize(self.nodes.values()), max_tree_size)), []
for i in irange(iterations):
goal = random() < goal_probability or i == 0
s = goal_sample() if goal else self.sample()
last = argmin(lambda n: self.distance(n.config, s),
nodes + new_nodes)
for q in self.extend(last.config, s):
if self.collision(q):
break
last = TreeNode(q, parent=last)
new_nodes.append(last)
else:
if goal:
path = last.retrace()
if store in [ts.ALL, ts.SUCCESS]:
self.add(*new_nodes)
elif store == ts.PATH:
new_nodes_set = set(new_nodes)
self.add(*[n for n in path if n in new_nodes_set])
return path
if store == ts.ALL:
self.add(*new_nodes)
return None
def grow(self, goal, iterations=50, store=ts.PATH, max_tree_size=500):
if goal in self:
return self[goal].retrace()
if self.collision(goal):
return None
nodes1, new_nodes1 = list(
take(randomize(self.nodes.values()), max_tree_size)), []
nodes2, new_nodes2 = [], [TreeNode(goal)]
for _ in irange(iterations):
if len(nodes1) + len(new_nodes1) > len(nodes2) + len(new_nodes2):
nodes1, nodes2 = nodes2, nodes1
new_nodes1, new_nodes2 = new_nodes2, new_nodes1
s = self.sample()
last1 = argmin(lambda n: self.distance(n.config, s),
nodes1 + new_nodes1)
for q in self.extend(last1.config, s):
if self.collision(q):
break
last1 = TreeNode(q, parent=last1)
new_nodes1.append(last1)
last2 = argmin(lambda n: self.distance(n.config, last1.config),
nodes2 + new_nodes2)
for q in self.extend(last2.config, last1.config):
if self.collision(q):
break
last2 = TreeNode(q, parent=last2)
new_nodes2.append(last2)
else:
if len(nodes1) == 0:
nodes1, nodes2 = nodes2, nodes1
new_nodes1, new_nodes2 = new_nodes2, new_nodes1
last1, last2 = last2, last1
path1, path2 = last1.retrace(), last2.retrace()[:-1][::-1]
for p, n in pairs(path2):
n.parent = p
if len(path2) == 0: # TODO - still some kind of circular error
for n in new_nodes2:
if n.parent == last2:
n.parent = path1[-1]
else:
path2[0].parent = path1[-1]
path = path1 + path2
if store in [ts.ALL, ts.SUCCESS]:
self.add(*(new_nodes1 + new_nodes2[:-1]))
elif store == ts.PATH:
new_nodes_set = set(new_nodes1 + new_nodes2[:-1])
self.add(*[n for n in path if n in new_nodes_set])
return path
if store == ts.ALL:
self.add(*new_nodes1)
return None
def mpc(x0,
v0,
curve,
dt_max=0.5,
max_time=INF,
max_iterations=INF,
v_max=None,
**kwargs):
assert (max_time < INF) or (max_iterations < INF)
from scipy.interpolate import CubicHermiteSpline
start_time = time.time()
best_cost, best_spline = INF, None
for iteration in irange(max_iterations):
if elapsed_time(start_time) >= max_time:
break
t1 = random.uniform(curve.x[0], curve.x[-1])
future = (curve.x[-1] - t1) # TODO: weighted
if future >= best_cost:
continue
x1 = curve(t1)
if (v_max is not None) and (max((x1 - x0) / v_max) > dt_max):
continue
# if quickest_inf_accel(x0, x1, v_max=v_max) > dt_max:
# continue
v1 = curve(t1, nu=1)
#dt = dt_max
dt = random.uniform(0, dt_max)
times = [0., dt]
positions = [x0, x1]
velocities = [v0, v1]
spline = CubicHermiteSpline(times, positions, dydx=velocities)
if not check_spline(spline, **kwargs):
continue
# TODO: optimize to find the closest on the path within a range
cost = future + (spline.x[-1] - spline.x[0])
if cost < best_cost:
best_cost, best_spline = cost, spline
print('Iteration: {} | Cost: {:.3f} | T: {:.3f} | Time: {:.3f}'.
format(iteration, cost, t1, elapsed_time(start_time)))
print(best_cost, t1, elapsed_time(start_time))
return best_cost, best_spline
def control_state(robot,
joints,
target_positions,
target_velocities=None,
position_tol=INF,
velocity_tol=INF,
max_velocities=None,
verbose=True): # TODO: max_accelerations
if target_velocities is None:
target_velocities = np.zeros(len(joints))
if max_velocities is None:
max_velocities = get_max_velocities(robot, joints)
assert (max_velocities > 0).all()
max_velocity_control_joints(robot,
joints,
positions=target_positions,
velocities=target_velocities,
max_velocities=max_velocities)
for i in irange(INF):
current_positions = np.array(get_joint_positions(robot, joints))
position_error = get_distance(current_positions,
target_positions,
norm=INF)
current_velocities = np.array(get_joint_velocities(robot, joints))
velocity_error = get_distance(current_velocities,
target_velocities,
norm=INF)
if verbose:
# print('Positions: {} | Target positions: {}'.format(current_positions.round(N_DIGITS), target_positions.round(N_DIGITS)))
# print('Velocities: {} | Target velocities: {}'.format(current_velocities.round(N_DIGITS), target_velocities.round(N_DIGITS)))
print(
'Step: {} | Position error: {:.3f}/{:.3f} | Velocity error: {:.3f}/{:.3f}'
.format(i, position_error, position_tol, velocity_error,
velocity_tol))
# TODO: draw the tolerance interval
if (position_error <= position_tol) and (velocity_error <=
velocity_tol):
return # TODO: declare success or failure by yielding or throwing an exception
yield i
def follow_curve_old(robot, joints, curve, goal_t=None):
# TODO: unify with /Users/caelan/Programs/open-world-tamp/open_world/simulation/control.py
if goal_t is None:
goal_t = curve.x[-1]
time_step = get_time_step()
target_step = 10 * time_step
#distance_fn = get_distance_fn(robot, joints, weights=None, norm=2)
distance_fn = get_duration_fn(robot,
joints,
velocities=MAX_VELOCITIES,
norm=INF)
for i in irange(INF):
# if (i % 10) != 0:
# continue
current_p = np.array(get_joint_positions(robot, joints))
current_v = np.array(get_joint_velocities(robot, joints))
goal_dist = distance_fn(current_p, curve(goal_t))
print('Positions: {} | Velocities: {} | Goal distance: {:.3f}'.format(
current_p.round(N_DIGITS), current_v.round(N_DIGITS), goal_dist))
if goal_dist < 1e-2:
return True
# _, connection = mpc(current_p, current_v, curve, v_max=MAX_VELOCITIES, a_max=MAX_ACCELERATIONS,
# dt_max=1e-1, max_time=1e-1)
# assert connection is not None
# target_t = 0.5*connection.x[-1]
# target_p = connection(target_t)
# target_v = connection(target_t, nu=1)
# #print(target_p)
closest_dist, closest_t = find_closest(current_p,
curve,
t_range=None,
max_time=1e-2,
max_iterations=INF,
distance_fn=distance_fn,
verbose=True)
target_t = min(closest_t + target_step, curve.x[-1])
target_p = curve(target_t)
#target_v = curve(target_t, nu=1)
target_v = curve(closest_t, nu=1)
#target_v = MAX_VELOCITIES
#target_v = INF*np.zeros(len(joints))
handles = draw_waypoint(target_p)
#times, confs = time_discretize_curve(curve, verbose=False, resolution=resolutions) # max_velocities=v_max,
# set_joint_positions(robot, joints, target_p)
max_velocity_control_joints(robot,
joints,
positions=target_p,
velocities=target_v,
max_velocities=MAX_VELOCITIES)
#next_t = closest_t + time_step
#next_p = current_p + current_v*time_step
yield target_t
actual_p = np.array(get_joint_positions(robot, joints))
actual_v = np.array(get_joint_velocities(robot, joints))
next_p = current_p + actual_v * time_step
print('Predicted: {} | Actual: {}'.format(next_p.round(N_DIGITS),
actual_p.round(N_DIGITS)))
remove_handles(handles)
