def decompose_into_paths(joints, path):
current_path = []
joint_sequence = []
path_sequence = []
for q1, q2 in get_pairs(path):
# Zero velocities aren't enforced otherwise
indices, = np.nonzero(get_difference(q1, q2))
current_joints = tuple(joints[j] for j in indices)
if not joint_sequence or (current_joints != joint_sequence[-1]):
if current_path:
path_sequence.append(current_path)
joint_sequence.append(current_joints)
current_path = [tuple(q1[j] for j in indices)]
current_path.append(tuple(q2[j] for j in indices))
if current_path:
path_sequence.append(current_path)
return zip(joint_sequence, path_sequence)
def ramp_retime_path(path,
max_velocities,
acceleration_fraction=1.5,
sample_step=None):
assert np.all(max_velocities)
accelerations = max_velocities * acceleration_fraction
dim = len(max_velocities)
#difference_fn = get_difference_fn(robot, joints)
# TODO: more fine grain when moving longer distances
# Assuming instant changes in accelerations
waypoints = [path[0]]
time_from_starts = [0.]
for q1, q2 in get_pairs(path):
differences = get_difference(q1, q2) # assumes not circular anymore
#differences = difference_fn(q1, q2)
distances = np.abs(differences)
duration = 0
for idx in range(dim):
total_time = compute_min_duration(distances[idx],
max_velocities[idx],
accelerations[idx])
duration = max(duration, total_time)
time_from_start = time_from_starts[-1]
if sample_step is not None:
ramp_durations = [
compute_ramp_duration(distances[idx], max_velocities[idx],
accelerations[idx], duration)
for idx in range(dim)
]
directions = np.sign(differences)
for t in np.arange(sample_step, duration, sample_step):
positions = []
for idx in range(dim):
distance = compute_position(ramp_durations[idx], duration,
accelerations[idx], t)
positions.append(q1[idx] + directions[idx] * distance)
waypoints.append(positions)
time_from_starts.append(time_from_start + t)
waypoints.append(q2)
time_from_starts.append(time_from_start + duration)
return waypoints, time_from_starts
def ramp_retime_path(path,
max_velocities,
acceleration_fraction=INF,
sample_step=None):
"""
:param path:
:param max_velocities:
:param acceleration_fraction: fraction of velocity_fraction*max_velocity per second
:param sample_step:
:return:
"""
assert np.all(max_velocities)
accelerations = max_velocities * acceleration_fraction
dim = len(max_velocities)
#difference_fn = get_difference_fn(robot, joints)
# TODO: more fine grain when moving longer distances
# Assuming instant changes in accelerations
waypoints = [path[0]]
time_from_starts = [0.]
for q1, q2 in get_pairs(path):
differences = get_difference(q1, q2) # assumes not circular anymore
#differences = difference_fn(q1, q2)
distances = np.abs(differences)
duration = max([
compute_min_duration(distances[idx], max_velocities[idx],
accelerations[idx]) for idx in range(dim)
] + [0.])
time_from_start = time_from_starts[-1]
if sample_step is not None:
waypoints, time_from_starts = add_ramp_waypoints(
differences, accelerations, q1, duration, sample_step,
waypoints, time_from_starts)
waypoints.append(q2)
time_from_starts.append(time_from_start + duration)
return waypoints, time_from_starts