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 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, 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]
context = plant.CreateDefaultContext()
params = context.get_numeric_parameter(0)
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(),
alpha = np.pi / rw_params.number_of_spokes()
pendulum_params = context.get_numeric_parameter(0)
pendulum_params.set_mass(rw_params.mass())
pendulum_params.set_length(rw_params.length())
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())
num_time_samples=N,
minimum_timestep=prog_dt,
maximum_timestep=prog_dt * 2)
prog.AddEqualTimeIntervalsConstraints()
# prog = DirectTranscription(plant, prog_contexodt, N)
for idx, val in enumerate(x0[:-1]):
prog.SetInitialGuess(prog.initial_state()[idx], x0[idx])
# Set state to all epsilons -- otherwise, solver gets divide by zero error
prog.SetInitialGuessForAllVariables(np.full((prog.num_vars(), 1), 1e-5))
R_ = 0.01 # Cost on input "effort".
u = prog.input()
# prog.AddRunningCost(R_ * u[0]**2)
# prog.AddRunningCost(R_ * u[1]**2)
# prog.AddRunningCost(R_ * u[2]**2)
x = prog.state()
# prog.AddRunningCost(0.01*x[1]**2)
r = x[0]
r_dot = x[1]
theta_dot = x[2]
phi = x[4]
v_r_hat = r_dot
v_theta_hat = r * theta_dot
beta = sym.atan2(v_r_hat, v_theta_hat)
# Slip angles
alpha_F = phi - x[2] - beta
alpha_R = phi - beta
prog.AddConstraintToAllKnotPoints(alpha_F <= max_alpha)
prog.AddConstraintToAllKnotPoints(alpha_F >= -max_alpha)
def make_dircol_pendulum(ic=(-1., 0.),
num_samples=32,
min_timestep=0.002,
max_timestep=0.25,
warm_start="linear",
seed=1776,
should_vis=False,
target_traj=None,
**kwargs):
# if 'warm_start' in kwargs:
# print(kwargs['warm_start'])
# else:
# print("warm_start", warm_start)
global dircol
global plant
global context
plant = PendulumPlant()
context = plant.CreateDefaultContext()
dircol = DirectCollocation(plant,
context,
num_time_samples=num_samples,
minimum_timestep=min_timestep,
maximum_timestep=max_timestep)
dircol.AddEqualTimeIntervalsConstraints()
# torque_limit = input_limit # N*m.
torque_limit = 5.
u = dircol.input()
dircol.AddConstraintToAllKnotPoints(-torque_limit <= u[0])
dircol.AddConstraintToAllKnotPoints(u[0] <= torque_limit)
initial_state = ic
dircol.AddBoundingBoxConstraint(initial_state, initial_state,
dircol.initial_state())
final_state = (math.pi, 0.)
dircol.AddBoundingBoxConstraint(final_state, final_state,
dircol.final_state())
# R = 100 # Cost on input "effort".
u = dircol.input()
x = dircol.state()
# print(x)
dircol.AddRunningCost(2 * ((x[0] - math.pi) *
(x[0] - math.pi) + x[1] * x[1]) + 25 * u.dot(u))
# Add a final cost equal to the total duration.
# dircol.AddFinalCost(dircol.time())
if warm_start == "linear":
initial_u_trajectory = PiecewisePolynomial()
initial_x_trajectory = \
PiecewisePolynomial.FirstOrderHold([0., 4.],
np.column_stack((initial_state,
final_state)))
dircol.SetInitialTrajectory(initial_u_trajectory, initial_x_trajectory)
elif warm_start == "random":
assert isinstance(seed, int)
np.random.seed(seed)
breaks = np.linspace(0, 4, num_samples).reshape(
(-1, 1)) # using num_time_samples
u_knots = np.random.rand(
1, num_samples) - 0.5 # num_inputs vs num_samples?
x_knots = np.random.rand(
2, num_samples) - 0.5 # num_states vs num_samples?
initial_u_trajectory = PiecewisePolynomial.Cubic(
breaks, u_knots, False)
initial_x_trajectory = PiecewisePolynomial.Cubic(
breaks, x_knots, False)
dircol.SetInitialTrajectory(initial_u_trajectory, initial_x_trajectory)
elif warm_start == "target":
assert target_traj != [], "Need a valid target for warm starting"
(breaks, x_knots, u_knots) = target_traj
#(breaks, u_knots, x_knots) = target_traj
initial_u_trajectory = PiecewisePolynomial.Cubic(
breaks.T, u_knots.T, False)
initial_x_trajectory = PiecewisePolynomial.Cubic(
breaks.T, x_knots.T, False)
dircol.SetInitialTrajectory(initial_u_trajectory, initial_x_trajectory)
def cb(decision_vars):
global vis_cb_counter
vis_cb_counter += 1
if vis_cb_counter % 10 != 0:
return
# Get the total cost
all_costs = dircol.EvalBindings(dircol.GetAllCosts(), decision_vars)
# Get the total cost of the constraints.
# Additionally, the number and extent of any constraint violations.
violated_constraint_count = 0
violated_constraint_cost = 0
constraint_cost = 0
for constraint in dircol.GetAllConstraints():
val = dircol.EvalBinding(constraint, decision_vars)
# Consider switching to DoCheckSatisfied if you can find the binding...
nudge = 1e-1 # This much constraint violation is not considered bad...
lb = constraint.evaluator().lower_bound()
ub = constraint.evaluator().upper_bound()
good_lb = np.all(np.less_equal(lb, val + nudge))
good_ub = np.all(np.greater_equal(ub, val - nudge))
if not good_lb or not good_ub:
# print("{} <= {} <= {}".format(lb, val, ub))
violated_constraint_count += 1
# violated_constraint_cost += np.sum(np.abs(val))
if not good_lb:
violated_constraint_cost += np.sum(np.abs(lb - val))
if not good_ub:
violated_constraint_cost += np.sum(np.abs(val - ub))
constraint_cost += np.sum(np.abs(val))
print("total cost: {: .2f} | \tconstraint {: .2f} \tbad {}, {: .2f}".
format(sum(all_costs), constraint_cost,
violated_constraint_count, violated_constraint_cost))
#dircol.AddVisualizationCallback(cb, dircol.decision_variables())
def MyVisualization(sample_times, values):
global vis_cb_counter
vis_cb_counter += 1
#print("counter: ", vis_cb_counter)
if vis_cb_counter % 10 != 0:
return
x, x_dot = values[0], values[1]
plt.plot(x, x_dot, '-o', label=vis_cb_counter)
plt.show()
if should_vis:
plt.figure()
plt.title('Tip trajectories')
plt.xlabel('x')
plt.ylabel('x_dot')
dircol.AddStateTrajectoryCallback(MyVisualization)
from pydrake.all import (SolverType)
#dircol.SetSolverOption(SolverType.kSnopt, 'Major feasibility tolerance', 1.0e-6) # default="1.0e-6"
#dircol.SetSolverOption(SolverType.kSnopt, 'Major optimality tolerance', 1.0e-6) # default="1.0e-6"
#dircol.SetSolverOption(SolverType.kSnopt, 'Minor feasibility tolerance', 1.0e-6) # default="1.0e-6"
#dircol.SetSolverOption(SolverType.kSnopt, 'Minor optimality tolerance', 1.0e-6) # default="1.0e-6"
# dircol.SetSolverOption(SolverType.kSnopt, 'Major feasibility tolerance', 1.0e-6) # default="1.0e-6"
# dircol.SetSolverOption(SolverType.kSnopt, 'Major optimality tolerance', 5.0e-2) # default="1.0e-6" was 5.0e-1
# dircol.SetSolverOption(SolverType.kSnopt, 'Minor feasibility tolerance', 1.0e-6) # default="1.0e-6"
# dircol.SetSolverOption(SolverType.kSnopt, 'Minor optimality tolerance', 5.0e-2) # default="1.0e-6" was 5.0e-1
dircol.SetSolverOption(
SolverType.kSnopt, 'Time limit (secs)',
60.0) # default="9999999.0" # Very aggressive cutoff...
dircol.SetSolverOption(
SolverType.kSnopt, 'Major step limit',
0.1) # default="2.0e+0" # HUGE!!! default takes WAY too huge steps
# dircol.SetSolverOption(SolverType.kSnopt, 'Reduced Hessian dimension', 10000) # Default="min{2000, n1 + 1}"
# dircol.SetSolverOption(SolverType.kSnopt, 'Hessian updates', 30) # Default="10"
dircol.SetSolverOption(SolverType.kSnopt, 'Major iterations limit',
9300000) # Default="9300"
dircol.SetSolverOption(SolverType.kSnopt, 'Minor iterations limit',
50000) # Default="500"
dircol.SetSolverOption(SolverType.kSnopt, 'Iterations limit',
50 * 10000) # Default="10000"
# Factoriztion?
# dircol.SetSolverOption(SolverType.kSnopt, 'QPSolver Cholesky', True) # Default="*Cholesky/CG/QN"
#dircol.SetSolverOption(SolverType.kSnopt, 'Major iterations limit', 1) # Default="9300"
#dircol.SetSolverOption(SolverType.kSnopt, 'Minor iterations limit', 1) # Default="500"
return dircol
def make_dircol_cartpole(ic=(-1., 0., 0., 0.),
num_samples=21,
min_timestep=0.0001,
max_timestep=1.,
warm_start="linear",
seed=1776,
should_vis=False,
torque_limit=250.,
target_traj=None,
**kwargs):
global dircol
global plant
global context
tree = RigidBodyTree("/opt/underactuated/src/cartpole/cartpole.urdf",
FloatingBaseType.kFixed)
plant = RigidBodyPlant(tree)
context = plant.CreateDefaultContext()
dircol = DirectCollocation(
plant,
context,
num_time_samples=num_samples,
# minimum_timestep=0.01, maximum_timestep=0.01)
minimum_timestep=min_timestep,
maximum_timestep=max_timestep)
# dircol.AddEqualTimeIntervalsConstraints()
# torque_limit = input_limit # N*m.
# torque_limit = 64.
u = dircol.input()
dircol.AddConstraintToAllKnotPoints(-torque_limit <= u[0])
dircol.AddConstraintToAllKnotPoints(u[0] <= torque_limit)
initial_state = ic
dircol.AddBoundingBoxConstraint(initial_state, initial_state,
dircol.initial_state())
final_state = np.array([0., math.pi, 0., 0.]).astype(np.double)
dircol.AddBoundingBoxConstraint(final_state, final_state,
dircol.final_state())
# R = 100 # Cost on input "effort".
u = dircol.input()
x = dircol.state()
# t = dircol.time() # let's add 100*t (seconds) to get in that min-time component!
denom1 = float(10**2 + math.pi**2 + 10**2 + math.pi**2)
denom2 = float(180**2)
#denom1 = 10**2+math.pi**2+10**2+math.pi**2
#denom2 = 180**2
# dircol.AddRunningCost(u.dot(u)/denom2)
# dircol.AddRunningCost(2*(x-final_state).dot(x-final_state)/denom1)
dircol.AddRunningCost(1 + 2. *
(x - final_state).dot(x - final_state) / denom1 +
u.dot(u) / denom2)
# Add a final cost equal to the total duration.
#dircol.AddFinalCost(dircol.time()) # Enabled to sim min time cost?
if warm_start == "linear":
initial_u_trajectory = PiecewisePolynomial()
initial_x_trajectory = \
PiecewisePolynomial.FirstOrderHold([0., 4.],
np.column_stack((initial_state,
final_state)))
dircol.SetInitialTrajectory(initial_u_trajectory, initial_x_trajectory)
elif warm_start == "random":
assert isinstance(seed, int)
np.random.seed(seed)
breaks = np.linspace(0, 4, num_samples).reshape(
(-1, 1)) # using num_time_samples
u_knots = np.random.rand(
1, num_samples) - 0.5 # num_inputs vs num_samples?
x_knots = np.random.rand(
2, num_samples) - 0.5 # num_states vs num_samples?
initial_u_trajectory = PiecewisePolynomial.Cubic(
breaks, u_knots, False)
initial_x_trajectory = PiecewisePolynomial.Cubic(
breaks, x_knots, False)
dircol.SetInitialTrajectory(initial_u_trajectory, initial_x_trajectory)
elif warm_start == "target":
assert target_traj != [], "Need a valid target for warm starting"
(breaks, x_knots, u_knots) = target_traj
#(breaks, u_knots, x_knots) = target_traj
initial_u_trajectory = PiecewisePolynomial.Cubic(
breaks.T, u_knots.T, False)
initial_x_trajectory = PiecewisePolynomial.Cubic(
breaks.T, x_knots.T, False)
dircol.SetInitialTrajectory(initial_u_trajectory, initial_x_trajectory)
def cb(decision_vars):
global vis_cb_counter
vis_cb_counter += 1
if vis_cb_counter % 10 != 0:
return
# Get the total cost
all_costs = dircol.EvalBindings(dircol.GetAllCosts(), decision_vars)
# Get the total cost of the constraints.
# Additionally, the number and extent of any constraint violations.
violated_constraint_count = 0
violated_constraint_cost = 0
constraint_cost = 0
for constraint in dircol.GetAllConstraints():
val = dircol.EvalBinding(constraint, decision_vars)
# Consider switching to DoCheckSatisfied if you can find the binding...
nudge = 1e-1 # This much constraint violation is not considered bad...
lb = constraint.evaluator().lower_bound()
ub = constraint.evaluator().upper_bound()
good_lb = np.all(np.less_equal(lb, val + nudge))
good_ub = np.all(np.greater_equal(ub, val - nudge))
if not good_lb or not good_ub:
# print("{} <= {} <= {}".format(lb, val, ub))
violated_constraint_count += 1
# violated_constraint_cost += np.sum(np.abs(val))
if not good_lb:
violated_constraint_cost += np.sum(np.abs(lb - val))
if not good_ub:
violated_constraint_cost += np.sum(np.abs(val - ub))
constraint_cost += np.sum(np.abs(val))
print("total cost: {: .2f} | \tconstraint {: .2f} \tbad {}, {: .2f}".
format(sum(all_costs), constraint_cost,
violated_constraint_count, violated_constraint_cost))
#dircol.AddVisualizationCallback(cb, dircol.decision_variables())
def MyVisualization(sample_times, values):
def state_to_tip_coord(state_vec):
# State: (x, theta, x_dot, theta_dot)
x, theta, _, _ = state_vec
pole_length = 0.5 # manually looked this up
#return (x-pole_length*np.sin(theta), pole_length-np.cos(theta))
return (x - pole_length * np.sin(theta),
pole_length * (-np.cos(theta)))
global vis_cb_counter
vis_cb_counter += 1
if vis_cb_counter % 30 != 0:
return
coords = [state_to_tip_coord(state) for state in values.T]
x, y = zip(*coords)
plt.plot(x, y, '-o', label=vis_cb_counter)
#plt.show() # good?
#if should_vis:
if False:
plt.figure()
plt.title('Tip trajectories')
plt.xlabel('x')
plt.ylabel('x_dot')
dircol.AddStateTrajectoryCallback(MyVisualization)
from pydrake.all import (SolverType)
# dircol.SetSolverOption(SolverType.kSnopt, 'Major feasibility tolerance', 1.0e-6) # default="1.0e-6"
# dircol.SetSolverOption(SolverType.kSnopt, 'Major optimality tolerance', 5.0e-2) # default="1.0e-6" was 5.0e-1
# dircol.SetSolverOption(SolverType.kSnopt, 'Minor feasibility tolerance', 1.0e-6) # default="1.0e-6"
# dircol.SetSolverOption(SolverType.kSnopt, 'Minor optimality tolerance', 5.0e-2) # default="1.0e-6" was 5.0e-1
# dircol.SetSolverOption(SolverType.kSnopt, 'Time limit (secs)', 12.0) # default="9999999.0" # Very aggressive cutoff...
dircol.SetSolverOption(
SolverType.kSnopt, 'Major step limit',
0.1) # default="2.0e+0" # HUGE!!! default takes WAY too huge steps
dircol.SetSolverOption(SolverType.kSnopt, 'Time limit (secs)',
15.0) # default="9999999.0" # was 15
# dircol.SetSolverOption(SolverType.kSnopt, 'Reduced Hessian dimension', 10000) # Default="min{2000, n1 + 1}"
# dircol.SetSolverOption(SolverType.kSnopt, 'Hessian updates', 30) # Default="10"
dircol.SetSolverOption(SolverType.kSnopt, 'Major iterations limit',
9300000) # Default="9300"
dircol.SetSolverOption(SolverType.kSnopt, 'Minor iterations limit',
50000) # Default="500"
dircol.SetSolverOption(SolverType.kSnopt, 'Iterations limit',
50 * 10000) # Default="10000"
# Factoriztion?
# dircol.SetSolverOption(SolverType.kSnopt, 'QPSolver Cholesky', True) # Default="*Cholesky/CG/QN"
return dircol
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]
from pydrake.all import DirectCollocation, Solve
plant = RimlessWheel()
context = plant.CreateDefaultContext()
params = context.get_numeric_parameter(0)
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(),
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
