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 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 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 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]
