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
#visualizer = builder.AddSystem(PlanarRigidBodyVisualizer(tree, xlim=[-4., 4.], ylim=[-4., 4.])) #builder.Connect(acrobot.get_output_port(0), visualizer.get_input_port(0)) #ax = visualizer.fig.add_axes([.2, .95, .6, .025]) #torque_system = builder.AddSystem(SliderSystem(ax, 'Torque', -5., 5.)) #builder.Connect(torque_system.get_output_port(0), # acrobot.get_input_port(0)) diagram = builder.Build() #simulator = Simulator(diagram) #simulator.set_target_realtime_rate(1.0) #simulator.set_publish_every_time_step(False) #state = simulator.get_mutable_context().get_mutable_continuous_state_vector() #state.SetFromVector([1., 0., 0., 0.]) q = np.ones(2) v = -np.ones(2) kinsol = tree.doKinematics(q, v) point = np.ones(3) tree.transformPoints(kinsol, point, 3, 0) context = plant.CreateDefaultContext() state = context.get_mutable_continuous_state_vector() state.SetFromVector([0, 1., 0., 1., 2., 0., 0., 0., 0., 0., 0., 0.]) contact_results = plant.contact_results_output_port() #simulator.StepTo(args.duration)
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
