slope = params.slope()
alpha = np.pi / params.number_of_spokes()
dircol = DirectCollocation(plant,
context,
num_time_samples=15,
minimum_timestep=0.01,
maximum_timestep=0.1,
assume_non_continuous_states_are_fixed=True)
dircol.AddEqualTimeIntervalsConstraints()
dircol.AddConstraintToAllKnotPoints(dircol.state()[0] >= slope - alpha)
dircol.AddConstraintToAllKnotPoints(dircol.state()[0] <= slope + alpha)
dircol.AddConstraint(dircol.initial_state()[0] == slope - alpha)
dircol.AddConstraint(dircol.final_state()[0] == slope + alpha)
dircol.AddConstraint(dircol.initial_state()[1] == dircol.final_state()[1] *
np.cos(2 * alpha))
result = Solve(dircol)
assert result.is_success()
x_trajectory = dircol.ReconstructStateTrajectory(result)
x_knots = np.hstack([
x_trajectory.value(t) for t in np.linspace(x_trajectory.start_time(),
x_trajectory.end_time(), 100)
])
def direct_collocation_zhao_glider():
print("Running direct collocation")
plant = SlotineGlider()
context = plant.CreateDefaultContext()
N = 21
initial_guess = True
max_dt = 0.5
max_tf = N * max_dt
dircol = DirectCollocation(
plant,
context,
num_time_samples=N,
minimum_timestep=0.05,
maximum_timestep=max_dt,
)
# Constrain all timesteps, $h[k]$, to be equal, so the trajectory breaks are evenly distributed.
dircol.AddEqualTimeIntervalsConstraints()
# Add input constraints
u = dircol.input()
dircol.AddConstraintToAllKnotPoints(0 <= u[0])
dircol.AddConstraintToAllKnotPoints(u[0] <= 3)
dircol.AddConstraintToAllKnotPoints(-np.pi / 2 <= u[1])
dircol.AddConstraintToAllKnotPoints(u[1] <= np.pi / 2)
# Add state constraints
x = dircol.state()
min_speed = 5
dircol.AddConstraintToAllKnotPoints(x[0] >= min_speed)
min_height = 0.5
dircol.AddConstraintToAllKnotPoints(x[3] >= min_height)
# Add initial state
travel_angle = (3 / 2) * np.pi
h0 = 10
dir_vector = np.array([np.cos(travel_angle), np.sin(travel_angle)])
# Start at initial position
x0_pos = np.array([h0, 0, 0])
dircol.AddBoundingBoxConstraint(x0_pos, x0_pos,
dircol.initial_state()[3:6])
# Periodicity constraints
dircol.AddLinearConstraint(
dircol.final_state()[0] == dircol.initial_state()[0])
dircol.AddLinearConstraint(
dircol.final_state()[1] == dircol.initial_state()[1])
dircol.AddLinearConstraint(
dircol.final_state()[2] == dircol.initial_state()[2])
dircol.AddLinearConstraint(
dircol.final_state()[3] == dircol.initial_state()[3])
# Always end in right direction
# NOTE this assumes that we always are starting in origin
if travel_angle % np.pi == 0: # Travel along x-axis
dircol.AddConstraint(
dircol.final_state()[5] == dircol.initial_state()[5])
elif travel_angle % ((1 / 2) * np.pi) == 0: # Travel along y-axis
dircol.AddConstraint(
dircol.final_state()[4] == dircol.initial_state()[4])
else:
dircol.AddConstraint(
dircol.final_state()[5] == dircol.final_state()[4] *
np.tan(travel_angle))
# Maximize distance travelled in desired direction
p0 = dircol.initial_state()
p1 = dircol.final_state()
Q = 1
dist_travelled = np.array([p1[4], p1[5]]) # NOTE assume starting in origin
dircol.AddFinalCost(-(dir_vector.T.dot(dist_travelled)) * Q)
if True:
# Cost on input effort
R = 0.1
dircol.AddRunningCost(R * (u[0])**2 + R * u[1]**2)
# Initial guess is a straight line from x0 in direction
if initial_guess:
avg_vel_guess = 10 # Guess for initial velocity
x0_guess = np.array([avg_vel_guess, travel_angle, 0, h0, 0, 0])
guessed_total_dist_travelled = 200
xf_guess = np.array([
avg_vel_guess,
travel_angle,
0,
h0,
dir_vector[0] * guessed_total_dist_travelled,
dir_vector[1] * guessed_total_dist_travelled,
])
initial_x_trajectory = PiecewisePolynomial.FirstOrderHold(
[0.0, 4.0], np.column_stack((x0_guess, xf_guess)))
dircol.SetInitialTrajectory(PiecewisePolynomial(),
initial_x_trajectory)
# Solve direct collocation
result = Solve(dircol)
assert result.is_success()
print("Found a solution!")
# PLOTTING
N_plot = 200
# Plot trajectory
x_trajectory = dircol.ReconstructStateTrajectory(result)
times = np.linspace(x_trajectory.start_time(), x_trajectory.end_time(),
N_plot)
x_knots = np.hstack([x_trajectory.value(t) for t in times])
z = x_knots[3, :]
x = x_knots[4, :]
y = x_knots[5, :]
plot_trj_3_wind(np.vstack((x, y, z)).T, dir_vector)
# Plot input
u_trajectory = dircol.ReconstructInputTrajectory(result)
u_knots = np.hstack([u_trajectory.value(t) for t in times])
plot_input_zhao_glider(times, u_knots.T)
plt.show()
return 0
pendulum_params.set_damping(0.)
dircol = DirectCollocation(plant,
context,
num_time_samples=11,
minimum_timestep=0.01,
maximum_timestep=0.1)
dircol.AddEqualTimeIntervalsConstraints()
dircol.AddConstraintToAllKnotPoints(
dircol.state()[0] >= np.pi + rw_params.slope() - alpha)
dircol.AddConstraintToAllKnotPoints(
dircol.state()[0] <= np.pi + rw_params.slope() + alpha)
dircol.AddConstraint(dircol.initial_state()[0] == np.pi + rw_params.slope() -
alpha)
dircol.AddConstraint(dircol.final_state()[0] == np.pi + rw_params.slope() +
alpha)
dircol.AddConstraint(dircol.initial_state()[1] == dircol.final_state()[1] *
np.cos(2 * alpha))
result = Solve(dircol)
assert (result.is_success())
x_trajectory = dircol.ReconstructStateTrajectory(result)
x_knots = np.hstack([
x_trajectory.value(t) for t in np.linspace(x_trajectory.start_time(),
x_trajectory.end_time(), 100)
def _solve_traj_opt(self, initial_state, constrain_final_state=True, duration_bounds=None, d=0.0, verbose=False):
'''Finds a trajectory from an initial state, optionally to a final state.
Args:
initial_state (tuple): the initial state
final_state (tuple): the final state (default to None, final state unconstrained)
duration (tuple): the min and max duration of the trajectory (default to None,
no duration constraints)
d (float): constant disturbance force
verbose (bool): enables/disables verbose output
Returns:
pydrake.trajectories.PiecewisePolynomial: the planned trajectory
float: the cost of the planned trajectory
Raises:
RuntimeError: raised if the optimization fails
'''
print("Initial state: {}\nFinal state: {}\nMin duration: {} s\nMax duration: {} s".format(
initial_state, constrain_final_state, duration_bounds[0], duration_bounds[1]))
traj_opt = DirectCollocation(self.plant, self.context,
self.opt_params['num_time_samples'],
self.opt_params['minimum_timestep'],
self.opt_params['maximum_timestep'])
traj_opt.AddEqualTimeIntervalsConstraints()
# Add bounds on the total duration of the trajectory
if duration_bounds:
traj_opt.AddDurationBounds(duration_bounds[0], duration_bounds[1])
# TODO make input limits a paramter
limits_low = [-15., -15.]
limits_upp = [15., 15.]
x = traj_opt.state()
u = traj_opt.input()
t = traj_opt.time()
# TODO assuming disturbance is at the last index
for i in range(len(u) - 1):
traj_opt.AddConstraintToAllKnotPoints(limits_low[i] <= u[i])
traj_opt.AddConstraintToAllKnotPoints(u[i] <= limits_upp[i])
traj_opt.AddConstraintToAllKnotPoints(u[len(u) - 1] == d)
for signed_dist_func in self.signed_dist_funcs:
traj_opt.AddConstraintToAllKnotPoints(signed_dist_func(x) >= 0)
traj_opt.AddRunningCost(traj_opt.timestep(0) * self.running_cost(x, u, t))
traj_opt.AddFinalCost(self.final_cost(x, u, t))
# Add initial and final state constraints
traj_opt.AddBoundingBoxConstraint(initial_state, initial_state,
traj_opt.initial_state())
if self.final_state_constraint and constrain_final_state:
traj_opt.AddConstraint(self.final_state_constraint(traj_opt.final_state()) == 0)
# # TODO this is redundant with the final state equality constraint above
# if final_state:
# traj_opt.AddBoundingBoxConstraint(final_state, final_state,
# traj_opt.final_state())
# initial_x_trajectory = PiecewisePolynomial.FirstOrderHold([0., 0.4 * 21],
# np.column_stack((initial_state,
# final_state)))
# traj_opt.SetInitialTrajectory(PiecewisePolynomial(), initial_x_trajectory)
# else:
# initial_x_trajectory = PiecewisePolynomial.FirstOrderHold([0., 0.4 * 21],
# np.column_stack((initial_state,
# initial_state)))
# traj_opt.SetInitialTrajectory(PiecewisePolynomial(), initial_x_trajectory)
initial_x_trajectory = PiecewisePolynomial.FirstOrderHold([0., 0.4 * 21],
np.column_stack((initial_state,
initial_state)))
traj_opt.SetInitialTrajectory(PiecewisePolynomial(), initial_x_trajectory)
result = traj_opt.Solve()
if result != SolutionResult.kSolutionFound:
raise RuntimeError('Direct collocation failed from initial state {}!'.format(initial_state))
state_samples = traj_opt.GetStateSamples()
input_samples = traj_opt.GetInputSamples()
time_samples = traj_opt.GetSampleTimes()
# for debugging
hs = [time_samples[i+1] - time_samples[i] for i in range(len(time_samples)) if i < len(time_samples) - 1]
#print(hs)
total_cost = 0.
for k in range(state_samples.shape[1]):
total_cost += (hs[0] *
self.running_cost(state_samples[:, k],
input_samples[:, k],
time_samples[k]))
if verbose:
for i, phi in enumerate(self.signed_dist_funcs):
print("\tsigned dist {}: {}".format(i, signed_dist_func(state_samples[:, k])))
if verbose:
print("Total cost is {}".format(total_cost))
u_traj = traj_opt.ReconstructInputTrajectory()
times = np.linspace(u_traj.start_time(), u_traj.end_time(), 100)
u_lookup = np.vectorize(lambda t: u_traj.value(t)[0])
u_values = u_lookup(times)
plt.figure()
plt.plot(times, u_values)
plt.xlabel('time (seconds)')
plt.ylabel('force (Newtons)')
plt.show()
return traj_opt.ReconstructStateTrajectory(), total_cost
