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
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)
dircol.AddStateTrajectoryCallback(draw_trajectory)
result = dircol.Solve()
assert(result == SolutionResult.kSolutionFound)
x_trajectory = dircol.ReconstructStateTrajectory()
x_knots = np.hstack([x_trajectory.value(t) for t in
np.linspace(x_trajectory.start_time(),
x_trajectory.end_time(), 100)])
plt.plot(x_knots[0, :], x_knots[1, :])
plt.show()
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