def linear_intermediate_states(start_theta_discrete, endcell,
number_of_intermediate_states, resolution,
number_of_angles):
assert number_of_intermediate_states >= 2, "There has to be at least start and final state"
angle_bin_size = 2 * np.pi / number_of_angles
interpolation = np.arange(number_of_intermediate_states) / (
number_of_intermediate_states - 1)
start_angle = normalize_angle(start_theta_discrete * angle_bin_size)
end_angle = normalize_angle(endcell[2] * angle_bin_size)
angle_diff = normalize_angle(end_angle - start_angle)
states = np.array([
interpolation * endcell[0] * resolution,
interpolation * endcell[1] * resolution,
normalize_angle(start_angle + interpolation * angle_diff)
]).T
return np.ascontiguousarray(states)
def angle_discrete_to_cont(angle_cell, num_angles):
'''
Python port of DiscTheta2Cont from sbpl utils (from discrete angle to continuous)
:param angle_cell int: discrete angle
:param num_angles int: number of angles in 2*pi range
:return: discrete angle
'''
bin_size = 2 * np.pi / num_angles
return normalize_angle(angle_cell * bin_size)
def angle_cont_to_discrete(angle, num_angles):
'''
Python port of ContTheta2Disc from sbpl utils (from continuous angle to one of uniform ones)
:param angle float: float angle
:param num_angles int: number of angles in 2*pi range
:return: discrete angle
'''
theta_bin_size = 2.0 * np.pi / num_angles
return (int)((normalize_angle(angle + theta_bin_size / 2.0) + 2 * np.pi) /
(2.0 * np.pi) * num_angles)
def kinematic_body_pose_motion_step_with_noise(pose, linear_velocity, angular_velocity, dt, noise_parameters):
""" Execute noisy motion step for an array of poses.
:param pose: A (n_poses, 3) array of poses.
The second dimension corresponds to (x_position, y_position, angle)
:param linear_velocity float: the linear velocity
:param angular_velocity flaot: the angular velocity
:param dt float: time interval passing between steps
:param noise_parameters Dict: dictionary of noise parameters
:return float np.ndarray(n_poses, 3): array of output posese
"""
linear_velocity = linear_velocity + _gaussian_noise(
noise_parameters['alpha1']*linear_velocity**2 + noise_parameters['alpha2']*angular_velocity**2)
angular_velocity = angular_velocity + _gaussian_noise(
noise_parameters['alpha3']*linear_velocity**2 + noise_parameters['alpha4']*angular_velocity**2)
final_rotation_noise = _gaussian_noise(
noise_parameters['alpha5']*linear_velocity**2 + noise_parameters['alpha6']*angular_velocity**2)
new_pose = kinematic_body_pose_motion_step(pose, linear_velocity, angular_velocity, dt)
new_pose[:, 2] = normalize_angle(new_pose[:, 2] + final_rotation_noise * dt)
return new_pose
def kinematic_body_pose_motion_step(pose, linear_velocity, angular_velocity, dt):
"""
Compute a new pose of the robot based on the previous pose and velocity.
:param pose: A (n_poses, 3) array of poses.
The second dimension corresponds to (x_position, y_position, angle)
:param linear_velocity: An (n_poses, ) Array indicating forward velocity
:param angular_velocity: An (n_poses, ) Array indicating angular velocity
:param dt: A float time-step (e.g. 0.1)
:return: A (n_poses, 3) array of poses after 1 step.
"""
pose_result = np.array(pose, dtype=float)
angle = pose[..., 2]
half_wdt = 0.5*angular_velocity*dt
v_factor = linear_velocity * dt * np.sinc(half_wdt / np.pi)
pose_result[..., 0] += v_factor * np.cos(angle + half_wdt)
pose_result[..., 1] += v_factor * np.sin(angle + half_wdt)
pose_result[..., 2] = normalize_angle(angle + angular_velocity * dt)
return pose_result
def refine_path(data, delta, angle_delta=None):
'''
Insert points in the path if distance between existing points along a path is larger than delta.
There are two ways to insert points given a constraint on a distance delta:
Imagine path is from 0 to 1 and constraint is 0.49.
- option 1:
step with delta from the begging till the end:
(0, 0.49, 0.98, 1)
most of the time distance is equal to delta, but there might be really short steps like the last one
- option 2:
determine how many points to insert and make equal steps (1/0.49 + 1 = 3 intervals
(0, 0.333, 0.666, 1)
distance between points is smaller than delta, but there are no really small steps
This function uses option 2.
Angle is interpolated if angle_delta is not None. Interpolation happens
by copying the second point in a consecutive pair several times
with different angles.
:param data: a np array of n x (x, y) or (x, y, angle) elements
:param delta: maximum distance between points
:param angle_delta: if not None, angle differences bigger than this will be
interpolated; interpolation happens by copying the second point in
a consecutive pair several times with different angles. Angle must be
provided if not None.
:returns regularized_path: a np array of m x (x, y) or (x, y, angle) elements
'''
if isinstance(data, (list, tuple)):
data = np.array(data, dtype=float)
else:
assert (data.dtype in [np.float, np.float32])
if data.shape[1] not in (2, 3):
raise Exception(
"This function takes n x (x, y) or n x (x, y, angle) arrays")
if angle_delta is not None and data.shape[1] != 3:
raise Exception(
"Path does not include angles but angle interpolation was requested"
)
xy = data[:, :2]
segment_lengths = np.linalg.norm(np.diff(xy, axis=0), axis=1)
if angle_delta is not None:
angles = diff_angles(data[1:, 2], data[:-1, 2])
pieces = []
for i, d in enumerate(segment_lengths):
if d > delta:
# interpolate all coordinates one by one (e.g. x, y)
npoints = int(d / delta) + 2
interpolated = [
np.linspace(data[i, j], data[i + 1, j], num=npoints)
for j in range(2)
]
# copy angle between interpolations
if data.shape[1] == 3:
interpolated.append(
np.ones((npoints, ), dtype=float) * data[i, 2])
piece = np.vstack(interpolated).T
# cut down the end in order to join with the next piece
pieces.append(piece[:-1])
else:
pieces.append(data[i])
if angle_delta is not None and np.abs(angles[i]) > angle_delta:
n_points = int(np.abs(angles[i]) / angle_delta) + 1
interpolated_angles = normalize_angle(data[i, 2] +
angles[i] / n_points *
np.arange(n_points))
pieces.append([[data[i + 1, 0], data[i + 1, 1], a]
for a in interpolated_angles])
# add path endpoint
pieces.append(data[-1])
return np.vstack(pieces)
def recovery_choices_tricycle(forward_model, wheel_angle, max_v, max_w,
max_front_wheel_angle, max_front_wheel_speed, front_wheel_from_axis):
"""
forward_model: a function getting robot pose and state, dt and controls and returning
the new pose and state
Generate multiple turns in place
"""
refine_dt = 0.5
rotation_angular_velocity = max_w*0.33
turn_the_wheel_steps = int(1.5*(2*max_front_wheel_angle/max_front_wheel_speed)/refine_dt + 0.5)
turn_around_steps = int((2*np.pi/rotation_angular_velocity)/refine_dt + 0.5) + turn_the_wheel_steps
def _turn_in_place_policy(pose, state, direction, angle_to_turn_to):
last_wheel_angle = state[-1, 0]
last_robot_angle = pose[-1, 2]
# In order to make trajectories the same length, we need to fill the last poses
# with 'stay in place' for those trajectories that finished early. So here we
# return stationary position if the robot pose and wheel are good enough.
# The actual turn in place is happening below.
do_nothing_command = np.array([[0., last_wheel_angle]])
if direction > 0:
if last_robot_angle < 0:
last_robot_angle += 2*np.pi
if angle_to_turn_to < 0:
angle_to_turn_to += 2*np.pi
if last_robot_angle >= angle_to_turn_to:
return do_nothing_command
elif direction < 0:
if last_robot_angle > 0:
last_robot_angle -= 2*np.pi
if angle_to_turn_to > 0:
angle_to_turn_to -= 2*np.pi
if last_robot_angle <= angle_to_turn_to:
return do_nothing_command
else:
assert False
# Here is the actual turn in place
v, desired_angle = diff_drive_control_to_tricycle(0.0, direction*rotation_angular_velocity,
front_wheel_angle=last_wheel_angle,
max_front_wheel_angle=max_front_wheel_angle,
front_wheel_from_axis_distance=front_wheel_from_axis)
return np.array([[v, desired_angle]])
def _prepare_rotation(angle_to_turn_to, direction):
return statefull_branching(
[0., 0., 0.], [wheel_angle, max_v, 0.], [0., 0],
[(lambda pose, state, _: _turn_in_place_policy(
pose, state, direction=direction, angle_to_turn_to=angle_to_turn_to), turn_around_steps)],
forward_model, refine_dt)
pose_evolution = np.empty((0, turn_around_steps, 3), dtype=float)
state_evolution = np.empty((0, turn_around_steps, 3), dtype=float)
control_evolution = np.empty((0, turn_around_steps, 2), dtype=float)
angle_resolution = np.pi/5
discretization = int(np.pi*2/angle_resolution)
control_costs = []
# clockwise and anticlockwise
for direction in [1, -1]:
for angle_i in range(1, discretization):
angle_to_turn_to = direction*normalize_angle((float(angle_i)/discretization)*2*np.pi)
pose_rotation, state_rotation, control_rotation = _prepare_rotation(angle_to_turn_to=angle_to_turn_to,
direction=direction)
pose_evolution = np.vstack((pose_evolution, pose_rotation))
state_evolution = np.vstack((state_evolution, state_rotation))
control_evolution = np.vstack((control_evolution, control_rotation))
# penalize turning the wheel if its against the rotation
if wheel_angle*direction < 0:
costs_to_turn_the_wheel = 1.
else:
costs_to_turn_the_wheel = 0.
# how much costs are given to turning the wheel vs turning the robot.
# 0.5 - means equal costs
# smaller number means that robot will prefer to turn the wheel vs turning the whole body.
turn_wheel_weight = 0.3
part_of_angle = (float(angle_i)/discretization)
current_control_costs = (1-turn_wheel_weight)*part_of_angle + \
turn_wheel_weight*costs_to_turn_the_wheel
control_costs.append(current_control_costs)
control_costs = np.array(control_costs)
return pose_evolution, state_evolution, control_evolution, refine_dt, control_costs
def recovery_choices_diff_drive(forward_model, start_state, max_v, max_w):
"""
This function use the robot's forward model and a few constraints to generate multiple turn in place policies
:param forward_model: a function getting robot pose and state, dt and controls and returning the new pose and state
:param start_state: the initial state of the robot
:param max_v: linear velocity limit
:param max_w: angular velocity limit
:return: Generate multiple turns in place policies
"""
refine_dt = 0.5
rotation_angular_velocity = max_w
# compute how many iterations is needed to make a full circle
turn_full_circle_steps = int((2 * np.pi /
(rotation_angular_velocity)) / refine_dt +
0.5)
def _turn_in_place_policy(pose, state, last_controls, v, direction,
angle_to_turn_to):
last_robot_angle = pose[-1, 2]
# In order to make trajectories the same length, we need to fill the last poses
# with 'stay in place' for those trajectories that finished early. So here we
# return stationary position if the robot pose and wheel are good enough.
# The actual turn in place is happening below.
do_nothing_command = np.array([[0., 0.]])
if reach_end_of_turn(direction, last_robot_angle, angle_to_turn_to):
return do_nothing_command
# Here is the actual turn in place
return np.array([[v, direction * rotation_angular_velocity]])
# TODO: this is confusing, need refactoring
def _generate_turn_in_place_trajectory(v, angle_to_turn_to, direction):
"""
This function do planning trajectory rollout given a set of parameters (v, angle_to_turn_to, direction)
:param v: commanding linear velocity for the in place turn
:param angle_to_turn_to: end of turn angle
:param direction: turning direction, i.e. clockwise or counterclockwise
:return: robot's pose, state, control input in time series with turn_full_circle_steps steps
"""
initial_pose = [0., 0., 0.] # (x, y, theta)
initial_state = [None, max_v,
0.] # (wheel_angle, linear_velocity, ang_velocity)
initial_control = [0., 0] # (linear_velocity, angular_velocity)
# control policy function handler: (control_generator, n_steps_to_apply_this_generator)
# in this case, list below contains only 1 control policy with turn_full_circle_steps steps
list_of_policies = [
(lambda pose, state, controls: _turn_in_place_policy(
pose,
state,
controls,
v=v,
direction=direction,
angle_to_turn_to=angle_to_turn_to), turn_full_circle_steps)
]
return statefull_branching(initial_pose, initial_state,
initial_control, list_of_policies,
forward_model, refine_dt)
pose_evolution = np.empty((0, turn_full_circle_steps, 3), dtype=float)
state_evolution = np.empty((0, turn_full_circle_steps, 3), dtype=float)
control_evolution = np.empty((0, turn_full_circle_steps, 2), dtype=float)
num_of_end_of_turn_positions = 5 # 5 used here is an arbitrary parameter
control_costs = []
# clockwise and anticlockwise
for direction in [1, -1]:
for angle_i in range(1, num_of_end_of_turn_positions):
angle_to_turn_to = direction * normalize_angle(
(float(angle_i) / num_of_end_of_turn_positions) * 2 * np.pi)
pose_rotation, state_rotation, control_rotation = _generate_turn_in_place_trajectory(
v=0.0, angle_to_turn_to=angle_to_turn_to, direction=direction)
pose_evolution = np.vstack((pose_evolution, pose_rotation))
state_evolution = np.vstack((state_evolution, state_rotation))
control_evolution = np.vstack(
(control_evolution, control_rotation))
# compute the cost of this turn in place policy
part_of_angle = (float(angle_i) / num_of_end_of_turn_positions)
current_control_costs = 1.0 * part_of_angle
control_costs.append(current_control_costs)
control_costs = np.array(control_costs)
return pose_evolution, state_evolution, control_evolution, refine_dt, control_costs
def debug_motion_primitives(motion_primitives, only_zero_angle=False):
angle_to_primitive = check_motion_primitives(motion_primitives)
all_angles = normalize_angle(
np.arange(motion_primitives.get_number_of_angles()) * np.pi * 2 /
motion_primitives.get_number_of_angles())
print('All angles: %s' % all_angles)
print('(in degrees: %s)' % np.degrees(all_angles))
for angle_c, primitives in angle_to_primitive.items():
print('------', angle_c)
for p in primitives:
if np.all(p.get_intermediate_states()[0, :2] ==
p.get_intermediate_states()[:, :2]):
turn_in_place = 'turn in place'
else:
turn_in_place = ''
print(turn_in_place, p.endcell,
p.get_intermediate_states()[0],
p.get_intermediate_states()[-1])
final_float_state = pixel_to_world(
p.endcell[:2], np.zeros((2, )),
motion_primitives.get_resolution())
final_float_angle = angle_discrete_to_cont(
p.endcell[2], motion_primitives.get_number_of_angles())
try:
np.testing.assert_array_almost_equal(
final_float_state,
p.get_intermediate_states()[-1][:2])
except AssertionError:
print("POSE DOESN'T LAND TO A CELL", final_float_state,
p.get_intermediate_states()[-1])
try:
np.testing.assert_array_almost_equal(
final_float_angle,
p.get_intermediate_states()[-1][2],
decimal=3)
except AssertionError:
print("ANGLE DOESN'T LAND TO AN ANGLE CELL",
np.degrees(final_float_angle),
np.degrees(p.get_intermediate_states()[-1][2]))
endcells = np.array([p.endcell for p in primitives])
image_half_width = np.amax(np.amax(np.abs(endcells), 0)[:2]) + 1
zoom = 10
img = np.full(
(zoom * image_half_width * 2, zoom * image_half_width * 2, 3),
255,
dtype=np.uint8)
resolution = motion_primitives.get_resolution() / zoom
origin = np.array(
(-image_half_width * motion_primitives.get_resolution(),
-image_half_width * motion_primitives.get_resolution()))
for p in primitives:
draw_arrow(img,
p.get_intermediate_states()[0],
0.7 * motion_primitives.get_resolution(),
origin,
resolution,
color=(0, 0, 0))
draw_trajectory(img,
resolution,
origin,
p.get_intermediate_states()[:, :2],
color=(0, 200, 0))
draw_arrow(img,
p.get_intermediate_states()[-1],
0.5 * motion_primitives.get_resolution(),
origin,
resolution,
color=(0, 0, 200))
cv2.imshow('a', img)
cv2.waitKey(-1)
if only_zero_angle:
return
