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 make_real_dircol_mp():
global tree
global plant
global context
global dircol
# expmt = "acrobot" # State: (theta1, theta2, theta1_dot, theta2_dot) Input: Elbow torque
tree = RigidBodyTree("/opt/underactuated/src/acrobot/acrobot.urdf",
FloatingBaseType.kFixed)
plant = AcrobotPlant()
context = plant.CreateDefaultContext()
dircol = DirectCollocation(plant, context, num_time_samples=21,
minimum_timestep=0.05, maximum_timestep=0.2)
dircol.AddEqualTimeIntervalsConstraints()
# Add input limits.
torque_limit = 8.0 # N*m.
u = dircol.input()
dircol.AddConstraintToAllKnotPoints(-torque_limit <= u[0])
dircol.AddConstraintToAllKnotPoints(u[0] <= torque_limit)
initial_state = (0., 0., 0., 0.)
dircol.AddBoundingBoxConstraint(initial_state, initial_state,
dircol.initial_state())
final_state = (math.pi, 0., 0., 0.)
dircol.AddBoundingBoxConstraint(final_state, final_state,
dircol.final_state())
R = 10 # Cost on input "effort".
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)))
dircol.SetInitialTrajectory(PiecewisePolynomial(), initial_x_trajectory)
print(id(dircol))
return dircol
# Initial state on the surface of section (and velocity > .1).
dircol.AddBoundingBoxConstraint([0., 0.1], [0., 10.], dircol.initial_state())
# Periodicity constraint.
# TODO(russt): Replace this with the vectorized version pending drake #8315.
dircol.AddLinearConstraint(
dircol.final_state()[0] == dircol.initial_state()[0])
dircol.AddLinearConstraint(
dircol.final_state()[1] == dircol.initial_state()[1])
samples = np.linspace(0, 2*math.pi, 10)
x_guess = np.vstack(([2*math.sin(t) for t in samples],
[2*math.cos(t) for t in samples]))
initial_x_trajectory = PiecewisePolynomial.FirstOrderHold(samples, x_guess)
dircol.SetInitialTrajectory(PiecewisePolynomial(), initial_x_trajectory)
fig = plt.figure()
h, = plt.plot([], [], '.-')
plt.xlim((-2.5, 2.5))
plt.ylim((-3., 3.))
def draw_trajectory(t, x):
h.set_xdata(x[0, :])
h.set_ydata(x[1, :])
fig.canvas.draw()
if plt.get_backend() == u'MacOSX':
plt.pause(1e-10)
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
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]
# Add initial and final state constraints
x0 = [0, 0, 0, 0]
xf = [pi, 0, 0, 0]
prog.AddBoundingBoxConstraint(x0, x0, prog.initial_state())
prog.AddBoundingBoxConstraint(xf, xf, prog.final_state())
# Add running cost
u = prog.input()
R = 10
prog.AddRunningCost(R * u[0]**2)
# Add final cost
prog.AddFinalCost(prog.time())
# Create an initial guess at the trajectory
init_x = PiecewisePolynomial.FirstOrderHold([0, 10.0], np.column_stack(
(x0, xf)))
init_u = PiecewisePolynomial.FirstOrderHold([0, 10.0], np.zeros(shape=(1, 2)))
prog.SetInitialTrajectory(traj_init_u=init_u, traj_init_x=init_x)
# Set SNOPT options
prog.SetSolverOption(SnoptSolver().solver_id(), "Major Iterations Limit", 1000)
prog.SetSolverOption(SnoptSolver().solver_id(), "Major Feasibility Tolerance",
1e-4)
prog.SetSolverOption(SnoptSolver().solver_id(), "Major Optimality Tolerance",
1e-4)
prog.SetSolverOption(SnoptSolver().solver_id(), "Scale Option", 2)
solver = SnoptSolver()
# 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
def do_single_basic_pend_traj_opt(should_init):
plant = PendulumPlant()
context = plant.CreateDefaultContext()
kNumTimeSamples = 15
kMinimumTimeStep = 0.01
kMaximumTimeStep = 0.2
dircol = DirectCollocation(plant, context, kNumTimeSamples, kMinimumTimeStep, kMaximumTimeStep)
dircol.AddEqualTimeIntervalsConstraints()
kTorqueLimit = 3.0 # N*m.
u = dircol.input()
dircol.AddConstraintToAllKnotPoints(-kTorqueLimit <= u[0])
dircol.AddConstraintToAllKnotPoints(u[0] <= kTorqueLimit)
initial_state = PendulumState()
initial_state.set_theta(0.0)
initial_state.set_thetadot(0.0)
dircol.AddBoundingBoxConstraint(initial_state.get_value(), initial_state.get_value(), dircol.initial_state())
# dircol.AddLinearConstraint(dircol.initial_state() == initial_state.get_value())
final_state = PendulumState()
final_state.set_theta(math.pi)
final_state.set_thetadot(0.0)
dircol.AddBoundingBoxConstraint(final_state.get_value(), final_state.get_value(), dircol.final_state())
# dircol.AddLinearConstraint(dircol.final_state() == final_state.get_value())
R = 10 # Cost on input "effort".
dircol.AddRunningCost(R*u[0]**2)
if should_init:
initial_x_trajectory = PiecewisePolynomial.FirstOrderHold([0., 4.], [initial_state.get_value(), final_state.get_value()])
dircol.SetInitialTrajectory(PiecewisePolynomial(), initial_x_trajectory)
def cb(decision_vars):
global cb_counter
cb_counter += 1
if cb_counter % 10 != 1:
return
# Get the total cost
all_costs = dircol.EvalBindings(dircol.GetAllCosts(), decision_vars)
# :all_constraints = dircol.EvalBindings(dircol.GetAllConstraints(), 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)
# TODO: switch to DoCheckSatisfied...
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("val,lb,ub: ", val, lb, ub)
violated_constraint_count += 1
violated_constraint_cost += np.sum(val)
constraint_cost += np.sum(val)
print("total cost: {:.2f} | constraint {:.2f}, bad {}, {:.2f}".format(
sum(all_costs), constraint_cost, violated_constraint_count, violated_constraint_cost))
# dircol.AddVisualizationCallback(cb, np.array(dircol.decision_variables()))
result = dircol.Solve()
assert(result == SolutionResult.kSolutionFound)
sol_costs = np.hstack([dircol.EvalBindingAtSolution(cost) for cost in dircol.GetAllCosts()])
sol_constraints = np.hstack([dircol.EvalBindingAtSolution(constraint) for constraint in dircol.GetAllConstraints()])
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 make_real_dircol_mp(expmt="cartpole", seed=1776):
global tree
global plant
global context
global dircol
# TODO: use the seed in some meaningful way:
# https://github.com/RobotLocomotion/drake/blob/master/systems/stochastic_systems.h
assert expmt in ("cartpole", "acrobot")
# expmt = "cartpole" # State: (x, theta, x_dot, theta_dot) Input: x force
# expmt = "acrobot" # State: (theta1, theta2, theta1_dot, theta2_dot) Input: Elbow torque
if expmt == "cartpole":
tree = RigidBodyTree("/opt/underactuated/src/cartpole/cartpole.urdf",
FloatingBaseType.kFixed)
plant = RigidBodyPlant(tree)
else:
tree = RigidBodyTree("/opt/underactuated/src/acrobot/acrobot.urdf",
FloatingBaseType.kFixed)
plant = AcrobotPlant()
context = plant.CreateDefaultContext()
if expmt == "cartpole":
dircol = DirectCollocation(plant,
context,
num_time_samples=21,
minimum_timestep=0.1,
maximum_timestep=0.4)
else:
dircol = DirectCollocation(plant,
context,
num_time_samples=21,
minimum_timestep=0.05,
maximum_timestep=0.2)
dircol.AddEqualTimeIntervalsConstraints()
if expmt == "acrobot":
# Add input limits.
torque_limit = 8.0 # N*m.
u = dircol.input()
dircol.AddConstraintToAllKnotPoints(-torque_limit <= u[0])
dircol.AddConstraintToAllKnotPoints(u[0] <= torque_limit)
initial_state = (0., 0., 0., 0.)
dircol.AddBoundingBoxConstraint(initial_state, initial_state,
dircol.initial_state())
# More elegant version is blocked on drake #8315:
# dircol.AddLinearConstraint(dircol.initial_state() == initial_state)
if expmt == "cartpole":
final_state = (0., math.pi, 0., 0.)
else:
final_state = (math.pi, 0., 0., 0.)
dircol.AddBoundingBoxConstraint(final_state, final_state,
dircol.final_state())
# dircol.AddLinearConstraint(dircol.final_state() == final_state)
# R = 10 # Cost on input "effort".
# 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)))
dircol.SetInitialTrajectory(PiecewisePolynomial(), initial_x_trajectory)
return dircol, tree
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