def min_time_traj_avoid_obs(self,
p0,
v0,
pf,
vf,
obstacles=None,
p_puck=None):
"""Minimum time trajectory while avoiding obstacles."""
x0 = np.array(np.concatenate((p0, v0), axis=0))
xf = np.concatenate((pf, vf), axis=0)
N = 20
prog = DirectCollocation(self.sys_c,
self.sys_c.CreateDefaultContext(),
N,
minimum_timestep=self.params.dt,
maximum_timestep=self.params.dt)
# Initial and final state
prog.AddBoundingBoxConstraint(x0, x0, prog.initial_state())
prog.AddQuadraticErrorCost(Q=np.eye(4),
x_desired=xf,
vars=prog.final_state())
u = prog.input()
prog.AddRunningCost(0.1 * u.dot(u))
prog.AddEqualTimeIntervalsConstraints()
## Input saturation
self.add_input_limits(prog)
# Arena constraints
self.add_arena_limits(prog)
prog.AddFinalCost(prog.time())
# Add non-linear constraints - will solve with SNOPT
# Avoid other players
if obstacles != None:
for p_obs in obstacles:
distance = prog.state()[0:2] - p_obs
prog.AddConstraintToAllKnotPoints(
distance.dot(distance) >= (2.0 *
self.params.player_radius)**2)
# avoid hitting the puck while generating a kicking trajectory
#if not p_puck.any(None):
# distance = prog.state()[0:2] - p_puck
# prog.AddConstraintToAllKnotPoints(distance.dot(distance) >= (self.params.player_radius + self.params.puck_radius)**2)
solver = SnoptSolver()
result = solver.Solve(prog)
solution_found = result.is_success()
if not solution_found:
print("Solution not found for intercepting_with_obs_avoidance")
u_traj = prog.ReconstructInputTrajectory(result)
u_values = u_traj.vector_values(u_traj.get_segment_times())
return solution_found, u_values
def runDircol(self,x0,xf,tf0):
N = 15 # constant
#N = np.int(tf0 * 10) # "10Hz" / samples per second
context = self.CreateDefaultContext()
dircol = DirectCollocation(self, context, num_time_samples=N,
minimum_timestep=0.05, maximum_timestep=1.0)
u = dircol.input()
# set some constraints on inputs
dircol.AddEqualTimeIntervalsConstraints()
dircol.AddConstraintToAllKnotPoints(u[1] <= self.slewmax)
dircol.AddConstraintToAllKnotPoints(u[1] >= -self.slewmax)
dircol.AddConstraintToAllKnotPoints(u[0] <= self.umax)
dircol.AddConstraintToAllKnotPoints(u[0] >= -self.umax)
# constrain the last input to be zero (at least for the u input)
#import pdb; pdb.set_trace()
dv = dircol.decision_variables()
for i in range(3, self.nX*N, 4):
alfa_state = dv[i] #u[t_end]
dircol.AddBoundingBoxConstraint(-self.alfamax, self.alfamax, alfa_state)
#final_u_decision_var = dv[self.nX*N + self.nU*N - 1] #u[t_end]
#dircol.AddLinearEqualityConstraint(final_u_decision_var, 0.0)
#first_u_decision_var = dv[self.nX*N + 1 ] #u[t_0]
#dircol.AddLinearEqualityConstraint(first_u_decision_var, 0.0)
# set some constraints on start and final pose
eps = 0.0 * np.ones(self.nX) # relaxing factor
dircol.AddBoundingBoxConstraint(x0, x0, dircol.initial_state())
dircol.AddBoundingBoxConstraint(xf-eps, \
xf+eps, dircol.final_state())
R = 1.0*np.eye(self.nU) # Cost on input "effort".
dircol.AddRunningCost( u.transpose().dot(R.dot(u)) )
# Add a final cost equal to the total duration.
dircol.AddFinalCost(dircol.time())
# guess initial trajectory
initial_x_trajectory = \
PiecewisePolynomial.FirstOrderHold([0., tf0], np.column_stack((x0, xf)))
dircol.SetInitialTrajectory(PiecewisePolynomial(), initial_x_trajectory)
# optimize
result = Solve(dircol)
print('******\nRunning trajectory optimization:')
print('w/ solver %s' %(result.get_solver_id().name()))
print(result.get_solution_result())
assert(result.is_success())
xtraj = dircol.ReconstructStateTrajectory(result)
utraj = dircol.ReconstructInputTrajectory(result)
# return nominal trajectory
return utraj,xtraj
def compute_control(self, x0_p1, x0_p2, xf_p1, xf_p2, obstacles):
"""This is basically the single-agent MPC algorithm"""
prog = DirectCollocation(self.mpc_params.sys_two_players_c, self.mpc_params.sys_two_players_c.CreateDefaultContext(), self.mpc_params.N+1,
minimum_timestep=self.mpc_params.minT, maximum_timestep=self.mpc_params.maxT)
x0 = np.concatenate((x0_p1, x0_p2), axis=0)
prog.AddBoundingBoxConstraint(x0, x0, prog.initial_state())
x_des = np.concatenate((xf_p1, xf_p2))
Q = np.zeros((8,8))
Q[0:4,0:4] = self.mpc_params.Omega_N_max
Q[4:8,4:8] = self.mpc_params.Omega_N_max
prog.AddQuadraticErrorCost(Q, x_desired=x_des, vars=prog.final_state())
prog.AddEqualTimeIntervalsConstraints()
for obs_pos in obstacles: # both players should avoid the other players
for n in range(self.mpc_params.N):
x = prog.state()
prog.AddConstraintToAllKnotPoints((x[0:2]-obs_pos).dot(x[0:2]-obs_pos) >= (2.0*self.sim_params.player_radius)**2)
prog.AddConstraintToAllKnotPoints((x[4:6]-obs_pos).dot(x[4:6]-obs_pos) >= (2.0*self.sim_params.player_radius)**2)
# players should avoid each other
prog.AddConstraintToAllKnotPoints((x[0:2]-x[4:6]).dot(x[0:2]-x[4:6]) >= (2.0*self.sim_params.player_radius)**2)
# input constraints
for i in range(4):
prog.AddConstraintToAllKnotPoints(prog.input()[i] <= self.sim_params.input_limit)
prog.AddConstraintToAllKnotPoints(prog.input()[i] >= -self.sim_params.input_limit)
r = self.sim_params.player_radius
prog.AddConstraintToAllKnotPoints(prog.state()[0] + r <= self.sim_params.arena_limits_x / 2.0)
prog.AddConstraintToAllKnotPoints(prog.state()[0] - r >= -self.sim_params.arena_limits_x / 2.0)
prog.AddConstraintToAllKnotPoints(prog.state()[1] + r <= self.sim_params.arena_limits_y / 2.0)
prog.AddConstraintToAllKnotPoints(prog.state()[1] - r >= -self.sim_params.arena_limits_y / 2.0)
prog.AddConstraintToAllKnotPoints(prog.state()[4] + r <= self.sim_params.arena_limits_x / 2.0)
prog.AddConstraintToAllKnotPoints(prog.state()[4] - r >= -self.sim_params.arena_limits_x / 2.0)
prog.AddConstraintToAllKnotPoints(prog.state()[5] + r <= self.sim_params.arena_limits_y / 2.0)
prog.AddConstraintToAllKnotPoints(prog.state()[5] - r >= -self.sim_params.arena_limits_y / 2.0)
prog.AddFinalCost(prog.time())
if not self.prev_u is None and not self.prev_x is None:
prog.SetInitialTrajectory(traj_init_u=self.prev_u, traj_init_x=self.prev_x)
solver = SnoptSolver()
result = solver.Solve(prog)
u_traj = prog.ReconstructInputTrajectory(result)
x_traj = prog.ReconstructStateTrajectory(result)
self.prev_u = u_traj
self.prev_x = x_traj
u_vals = u_traj.vector_values(u_traj.get_segment_times())
x_vals = x_traj.vector_values(x_traj.get_segment_times())
return True, u_vals[0:2,0], u_vals[2:4,0]
def compute_control(self, x_des, sim_state, team_name, player_id):
prog = DirectCollocation(self.mpc_params.sys,
self.mpc_params.sys.CreateDefaultContext(),
self.mpc_params.N,
minimum_timestep=self.mpc_params.minT,
maximum_timestep=self.mpc_params.maxT)
pos0 = sim_state.get_player_pos(team_name, player_id)
vel0 = sim_state.get_player_vel(team_name, player_id)
x0 = np.concatenate((pos0, vel0), axis=0)
prog.AddBoundingBoxConstraint(x0, x0, prog.initial_state())
prog.AddQuadraticErrorCost(Q=self.mpc_params.Omega_N_max,
x_desired=x_des,
vars=prog.final_state())
obstacle_positions = self.get_obstacle_positions(
sim_state, team_name, player_id)
for obs_pos in obstacle_positions:
for n in range(self.mpc_params.N):
x = prog.state()
prog.AddConstraintToAllKnotPoints(
(x[0:2] - obs_pos).dot(x[0:2] - obs_pos) >= (
2.0 * self.sim_params.player_radius)**2)
prog.AddEqualTimeIntervalsConstraints()
self.add_input_limits(prog)
self.add_arena_limits(prog)
prog.AddFinalCost(prog.time())
if not self.prev_u is None and not self.prev_x is None:
prog.SetInitialTrajectory(traj_init_u=self.prev_u,
traj_init_x=self.prev_x)
solver = SnoptSolver()
result = solver.Solve(prog)
u_traj = prog.ReconstructInputTrajectory(result)
x_traj = prog.ReconstructStateTrajectory(result)
u_vals = u_traj.vector_values(u_traj.get_segment_times())
self.prev_u = u_traj
self.prev_x = x_traj
return u_vals[:, 0]
def runDircol(self, x0, xf, tf0):
N = 21 #np.int(tf0 * 10) # "10Hz" samples per second
context = self.CreateDefaultContext()
dircol = DirectCollocation(self,
context,
num_time_samples=N,
minimum_timestep=0.05,
maximum_timestep=1.0)
u = dircol.input()
dircol.AddEqualTimeIntervalsConstraints()
dircol.AddConstraintToAllKnotPoints(u[0] <= 0.5 * self.omegamax)
dircol.AddConstraintToAllKnotPoints(u[0] >= -0.5 * self.omegamax)
dircol.AddConstraintToAllKnotPoints(u[1] <= 0.5 * self.umax)
dircol.AddConstraintToAllKnotPoints(u[1] >= -0.5 * self.umax)
eps = 0.0
dircol.AddBoundingBoxConstraint(x0, x0, dircol.initial_state())
dircol.AddBoundingBoxConstraint(xf - np.array([eps, eps, eps]),
xf + np.array([eps, eps, eps]),
dircol.final_state())
R = 1.0 * np.eye(2) # Cost on input "effort".
dircol.AddRunningCost(u.transpose().dot(R.dot(u)))
#dircol.AddRunningCost(R*u[0]**2)
# Add a final cost equal to the total duration.
dircol.AddFinalCost(dircol.time())
initial_x_trajectory = \
PiecewisePolynomial.FirstOrderHold([0., tf0], np.column_stack((x0, xf)))
dircol.SetInitialTrajectory(PiecewisePolynomial(),
initial_x_trajectory)
result = Solve(dircol)
print(result.get_solver_id().name())
print(result.get_solution_result())
assert (result.is_success())
#import pdb; pdb.set_trace()
xtraj = dircol.ReconstructStateTrajectory(result)
utraj = dircol.ReconstructInputTrajectory(result)
return utraj, xtraj
def min_time_traj_dir_col(self, p0, v0, pf, vf):
"""generate minimum time trajectory while avoiding obs"""
N = 15
minT = self.params.dt / N
maxT = 5.0 / N
x0 = np.concatenate((p0, v0), axis=0)
xf = np.concatenate((pf, vf), axis=0)
prog = DirectCollocation(self.sys_c, self.sys_c.CreateDefaultContext(), num_time_samples=N,
minimum_timestep=minT,
maximum_timestep=maxT)
prog.AddBoundingBoxConstraint(x0, x0, prog.initial_state())
prog.AddEqualTimeIntervalsConstraints()
self.add_input_limits(prog)
self.add_arena_limits(prog)
prog.AddQuadraticErrorCost(Q=10.0*np.eye(4), x_desired=xf, vars=prog.final_state())
prog.AddFinalCost(prog.time())
solver = SnoptSolver()
result = solver.Solve(prog)
if not result.is_success():
print("Minimum time trajectory: optimization failed")
return False, np.zeros((2, 1))
# subsample trajectory accordingly
u_trajectory = prog.ReconstructInputTrajectory(result)
duration = u_trajectory.end_time() - u_trajectory.start_time()
if duration > self.params.dt:
times = np.linspace(u_trajectory.start_time(), u_trajectory.end_time(), (u_trajectory.end_time() - u_trajectory.start_time()) / self.params.dt )
else:
times = np.array([0])
u_values = np.empty((2, len(times)))
for i, t in enumerate(times):
u_values[:, i] = u_trajectory.value(t).flatten()
return result.is_success(), u_values
def min_time_bounce_kick_traj_dir_col(self, p0, v0, p0_puck, v0_puck, v_puck_desired):
"""DO NOT USE. NOT WORKING.
Minimum time trajectory + bounce kick off the wall."""
N = 15
minT = self.params.dt / N
maxT = 5.0 / N
x0 = np.concatenate((p0, v0), axis=0)
prog = DirectCollocation(self.sys_c, self.sys_c.CreateDefaultContext(), num_time_samples=N,
minimum_timestep=minT,
maximum_timestep=maxT)
prog.AddBoundingBoxConstraint(x0, x0, prog.initial_state())
prog.AddEqualTimeIntervalsConstraints()
self.add_final_state_constraint_elastic_collision(prog, p0_puck, v0_puck, v_puck_desired)
self.add_input_limits(prog)
self.add_arena_limits(prog)
# prog.AddQuadraticErrorCost(Q=10.0*np.eye(4), x_desired=xf, vars=prog.final_state())
pf = p0_puck - self.get_normalized_vector(v_puck_desired)*(self.params.puck_radius + self.params.player_radius)
prog.AddQuadraticErrorCost(Q=10.0*np.eye(2), x_desired=pf, vars=prog.final_state()[:2])
prog.AddFinalCost(prog.time())
solver = SnoptSolver()
result = solver.Solve(prog)
if not result.is_success():
print("Minimum time trajectory: optimization failed")
return False, np.zeros((2, 1))
u_trajectory = prog.ReconstructInputTrajectory(result)
times = np.linspace(u_trajectory.start_time(), u_trajectory.end_time(), (u_trajectory.end_time() - u_trajectory.start_time()) / self.params.dt )
u_values = np.empty((2, len(times)))
for i, t in enumerate(times):
u_values[:, i] = u_trajectory.value(t).flatten()
return result.is_success(), u_values
def drake_trajectory_generation(file_name):
global x_cmd_drake
global u_cmd_drake
print(file_name)
Parser(plant).AddModelFromFile(file_name)
plant.Finalize()
context = plant.CreateDefaultContext()
global dircol
dircol= DirectCollocation(
plant,
context,
num_time_samples=11,
minimum_timestep=0.1,
maximum_timestep=0.4,
input_port_index=plant.get_actuation_input_port().get_index())
dircol.AddEqualTimeIntervalsConstraints()
initial_state = (0., 0., 0., 0.)
dircol.AddBoundingBoxConstraint(initial_state, initial_state,
dircol.initial_state())
final_state = (0., math.pi, 0., 0.)
dircol.AddBoundingBoxConstraint(final_state, final_state, dircol.final_state())
R = 10 # Cost on input "effort".weight
u = dircol.input()
dircol.AddRunningCost(R * u[0]**2)
# Add a final cost equal to the total duration.
dircol.AddFinalCost(dircol.time())
initial_x_trajectory = PiecewisePolynomial.FirstOrderHold(
[0., 4.], np.column_stack((initial_state, final_state))) # yapf: disable
dircol.SetInitialTrajectory(PiecewisePolynomial(), initial_x_trajectory)
dircol.AddConstraintToAllKnotPoints(dircol.input()[1] <= 0)
dircol.AddConstraintToAllKnotPoints(dircol.input()[1] >= 0)
global result
global u_values
result = Solve(dircol)
assert result.is_success()
#plotphase_portrait()
fig1, ax1 = plt.subplots()
u_trajectory = dircol.ReconstructInputTrajectory(result)
u_knots = np.hstack([
u_trajectory.value(t) for t in np.linspace(u_trajectory.start_time(),
u_trajectory.end_time(), 400)
])#here the u_knots now is 2x400
#u_trajectory = dircol.ReconstructInputTrajectory(result)
times = np.linspace(u_trajectory.start_time(), u_trajectory.end_time(), 400)
#u_lookup = np.vectorize(u_trajectory.value)
#now we have ndarray of u_values with 400 points for 4 seconds w/ 100hz pub frequency
#u_values = u_lookup(times)
#ax1.plot(times, u_values)
ax1.plot(times, u_knots[0])
ax1.plot(times, u_knots[1])
ax1.set_xlabel("time (seconds)")
ax1.set_ylabel("force (Newtons)")
ax1.set_title(' Direct collocation for Cartpole ')
print('here2')
plt.show()
print('here3')
#x_knots = np.hstack([
# x_trajectory.value(t) for t in np.linspace(x_trajectory.start_time(),
# x_trajectory.end_time(), 100)
#])
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(), 400)
])
print(x_trajectory.start_time())
print(x_trajectory.end_time())
fig, ax = plt.subplots(4,1,figsize=(8,8))
plt.subplots_adjust(wspace =0, hspace =0.4)
#plt.tight_layout(3)#adjust total space
ax[0].set_title('state of direct collocation for Cartpole')
ax[0].plot(x_knots[0, :], x_knots[2, :], linewidth=2, color='b', linestyle='-')
ax[0].set_xlabel("state_dot(theta1_dot and t|heta2_dot)")
ax[0].set_ylabel("state(theta1 and theta2)");
ax[0].plot(x_knots[1, :], x_knots[3, :],color='r',linewidth=2,linestyle='--')
ax[0].legend(('theta1&theta1dot','theta2&theta2dot'));
ax[1].set_title('input u(t) of direct collocation for Cartpole')
# ax[1].plot(times,u_values, 'g')
ax[1].plot(times, u_knots[0])
ax[1].plot(times, u_knots[1])
ax[1].legend(('input u(t)'))
ax[1].set_xlabel("time")
ax[1].set_ylabel("u(t)")
ax[1].legend(('x joint ','thetajoint'))
ax[1].set_title('input x(t) of direct collocation for Cartpole')
ax[2].plot(times, x_knots[0, :])
ax[2].set_xlabel("time")
ax[2].set_ylabel("x(t)")
ax[2].set_title('input theta(t) of direct collocation for Cartpole')
ax[3].set_title('input theta(t) of direct collocation for Cartpole')
ax[3].plot(times, x_knots[1, :])
ax[3].set_xlabel("time")
ax[3].set_ylabel("theta(t)")
print('here4')
plt.show()
print('here5')
x_cmd_drake=x_knots
#return x_knots[0, :]#u_values
# u_cmd_drake=u_values
u_cmd_drake=u_knots
initial_x_trajectory = \
PiecewisePolynomial.FirstOrderHold([0., 4.],
np.column_stack((initial_state,
final_state)))
dircol.SetInitialTrajectory(PiecewisePolynomial(), initial_x_trajectory)
result = Solve(dircol)
print(result.get_solver_id().name())
print(result.get_solution_result())
assert (result.is_success())
x_trajectory = dircol.ReconstructStateTrajectory(result)
tree = RigidBodyTree(FindResource("acrobot/acrobot.urdf"),
FloatingBaseType.kFixed)
vis = PlanarRigidBodyVisualizer(tree, xlim=[-4., 4.], ylim=[-4., 4.])
ani = vis.animate(x_trajectory, repeat=True)
u_trajectory = dircol.ReconstructInputTrajectory(result)
times = np.linspace(u_trajectory.start_time(), u_trajectory.end_time(), 100)
u_lookup = np.vectorize(u_trajectory.value)
u_values = u_lookup(times)
plt.figure()
plt.plot(times, u_values)
plt.xlabel('time (seconds)')
plt.ylabel('force (Newtons)')
plt.show()
print("Solving....")
result = Solve(prog)
print("Solve complete")
assert result.is_success()
print("Solver found solution!")
# Set up finite-horizon LQR
fh_lqr_context = fh_lqr_plant.CreateDefaultContext()
fh_lqr_plant.get_input_port(0).FixValue(fh_lqr_context, u_bar)
fh_lqr_context.SetContinuousState(x_bar)
Q = np.diag([100, 10, 10, 100, 1])
R = np.diag([0.5, 0.5, 0.1])
options = FiniteHorizonLinearQuadraticRegulatorOptions()
options.x0 = prog.ReconstructStateTrajectory(result)
options.u0 = prog.ReconstructInputTrajectory(result)
options.Qf = Q
traj_x_values = options.x0.vector_values(options.x0.get_segment_times())
traj_u_values = options.x0.vector_values(options.u0.get_segment_times())
plt.figure()
plt.subplot(321)
plt.plot(options.x0.get_segment_times(), traj_x_values[0, :])
plt.axhline(r_bar, color='gray', linestyle='--')
plt.xlabel("$t$")
plt.ylabel("$r$")
plt.subplot(322)
plt.plot(options.x0.get_segment_times(), traj_x_values[1, :])
plt.axhline(0, color='gray', linestyle='--')
def get_initial_guess(self, x_p1, p_goal, p_puck, obstacles):
"""This is basically the single-agent MPC algorithm"""
hit_dir = p_goal - p_puck
hit_dir = 6.0 * hit_dir / np.linalg.norm(hit_dir)
x_des = np.array([p_puck[0], p_puck[1], hit_dir[0], hit_dir[1]])
#x_des = np.array([1.0, 1.0, 0, 0])
print("x_des: {}, {}".format(x_des[0], x_des[1]))
print("x_des shape", x_des.shape)
print("zeros.shape", np.zeros(4).shape)
print("p_player", x_p1[0:2])
print("p_puck {}, {}".format(p_puck[0], p_puck[1]))
print("p_goal", p_goal)
prog = DirectCollocation(self.mpc_params.sys_c,
self.mpc_params.sys_c.CreateDefaultContext(),
self.mpc_params.N + 1,
minimum_timestep=self.mpc_params.minT,
maximum_timestep=self.mpc_params.maxT)
prog.AddBoundingBoxConstraint(x_p1, x_p1, prog.initial_state())
prog.AddQuadraticErrorCost(Q=self.mpc_params.Omega_N_max,
x_desired=x_des,
vars=prog.final_state())
prog.AddEqualTimeIntervalsConstraints()
# generate trajectory non in collision with puck
#for n in range(self.mpc_params.N):
# x = prog.state()
# eps = 0.1
# obs_pos = p_puck[0:2]
# prog.AddConstraintToAllKnotPoints((x[0:2]-obs_pos).dot(x[0:2]-obs_pos) >= (self.sim_params.player_radius + self.sim_params.puck_radius - eps)**2)
for obs_pos in obstacles:
for n in range(self.mpc_params.N):
x = prog.state()
prog.AddConstraintToAllKnotPoints(
(x[0:2] - obs_pos).dot(x[0:2] - obs_pos) >= (
2.0 * self.sim_params.player_radius)**2)
prog.AddConstraintToAllKnotPoints(
prog.input()[0] <= self.sim_params.input_limit)
prog.AddConstraintToAllKnotPoints(
prog.input()[0] >= -self.sim_params.input_limit)
prog.AddConstraintToAllKnotPoints(
prog.input()[1] <= self.sim_params.input_limit)
prog.AddConstraintToAllKnotPoints(
prog.input()[1] >= -self.sim_params.input_limit)
r = self.sim_params.player_radius
prog.AddConstraintToAllKnotPoints(
prog.state()[0] + r <= self.sim_params.arena_limits_x / 2.0)
prog.AddConstraintToAllKnotPoints(
prog.state()[0] - r >= -self.sim_params.arena_limits_x / 2.0)
prog.AddConstraintToAllKnotPoints(
prog.state()[1] + r <= self.sim_params.arena_limits_y / 2.0)
prog.AddConstraintToAllKnotPoints(
prog.state()[1] - r >= -self.sim_params.arena_limits_y / 2.0)
prog.AddFinalCost(prog.time())
if not self.prev_u is None and not self.prev_x is None:
prog.SetInitialTrajectory(traj_init_u=self.prev_u,
traj_init_x=self.prev_x)
solver = SnoptSolver()
result = solver.Solve(prog)
u_traj = prog.ReconstructInputTrajectory(result)
x_traj = prog.ReconstructStateTrajectory(result)
self.prev_u = u_traj
self.prev_x = x_traj
u_vals = u_traj.vector_values(u_traj.get_segment_times())
x_vals = x_traj.vector_values(x_traj.get_segment_times())
print(u_vals)
print(u_vals[:, 0])
return u_vals[:, 0]
# Solve the problem
print("Solving trajectory optimization")
start = timeit.default_timer()
result = solver.Solve(prog)
stop = timeit.default_timer()
print("Elapsed Time: ", stop - start)
# Get the details of the solution
print("Optimization successful? ", result.is_success())
print('solver is: ', result.get_solver_id().name())
print('optimal cost = ', result.get_optimal_cost())
# Get the exit code from SNOPT
details = result.get_solver_details()
print('SNOPT Exit Status: ', details.info)
# Unpack the trajectories
u_traj = prog.ReconstructInputTrajectory(result)
x_traj = prog.ReconstructStateTrajectory(result)
time = np.linspace(u_traj.start_time(), u_traj.end_time(), 101)
u_lookup = np.vectorize(u_traj.value)
u = u_lookup(time)
x = np.hstack([x_traj.value(t) for t in time])
# Plot the trajectory
plt.figure()
plt.subplot(3, 1, 1)
plt.plot(time, x[0, :], label="shoulder")
plt.plot(time, x[1, :], label="elbow")
plt.legend()
plt.ylabel('Positions (rad)')
plt.subplot(3, 1, 2)
plt.plot(time, x[2, :])
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
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