def test_context_api(self):
system = Adder(3, 10)
context = system.CreateDefaultContext()
self.assertIsInstance(context.get_continuous_state(), ContinuousState)
self.assertIsInstance(context.get_mutable_continuous_state(),
ContinuousState)
self.assertIsInstance(context.get_continuous_state_vector(),
VectorBase)
self.assertIsInstance(context.get_mutable_continuous_state_vector(),
VectorBase)
# TODO(eric.cousineau): Consolidate main API tests for `Context` here.
pendulum = PendulumPlant()
context = pendulum.CreateDefaultContext()
self.assertEqual(context.num_numeric_parameter_groups(), 1)
self.assertTrue(context.get_parameters().get_numeric_parameter(0) is
context.get_numeric_parameter(index=0))
self.assertEqual(context.num_abstract_parameters(), 0)
# TODO(russt): Bind _Declare*Parameter or find an example with an
# abstract parameter to actually call this method.
self.assertTrue(hasattr(context, "get_abstract_parameter"))
x = np.array([0.1, 0.2])
context.SetContinuousState(x)
np.testing.assert_equal(
context.get_continuous_state_vector().CopyToVector(), x)
def test_plant(self):
pendulum = PendulumPlant()
self.assertEqual(pendulum.get_input_port(), pendulum.get_input_port(0))
self.assertEqual(pendulum.get_state_output_port(),
pendulum.get_output_port(0))
context = pendulum.CreateDefaultContext()
self.assertIsInstance(pendulum.get_state(context=context),
PendulumState)
self.assertIsInstance(pendulum.get_mutable_state(context=context),
PendulumState)
self.assertIsInstance(pendulum.get_parameters(context=context),
PendulumParams)
self.assertIsInstance(pendulum.get_mutable_parameters(context=context),
PendulumParams)
def test_context_api(self):
system = Adder(3, 10)
context = system.CreateDefaultContext()
self.assertIsInstance(
context.get_continuous_state(), ContinuousState)
self.assertIsInstance(
context.get_mutable_continuous_state(), ContinuousState)
self.assertIsInstance(
context.get_continuous_state_vector(), VectorBase)
self.assertIsInstance(
context.get_mutable_continuous_state_vector(), VectorBase)
# TODO(eric.cousineau): Consolidate main API tests for `Context` here.
pendulum = PendulumPlant()
context = pendulum.CreateDefaultContext()
self.assertEqual(context.num_numeric_parameter_groups(), 1)
self.assertEqual(pendulum.num_numeric_parameter_groups(), 1)
self.assertTrue(
context.get_parameters().get_numeric_parameter(0) is
context.get_numeric_parameter(index=0))
self.assertEqual(context.num_abstract_parameters(), 0)
self.assertEqual(pendulum.num_numeric_parameter_groups(), 1)
# TODO(russt): Bind _Declare*Parameter or find an example with an
# abstract parameter to actually call this method.
self.assertTrue(hasattr(context, "get_abstract_parameter"))
x = np.array([0.1, 0.2])
context.SetContinuousState(x)
np.testing.assert_equal(
context.get_continuous_state_vector().CopyToVector(), x)
# RimlessWheel has a single discrete variable and a bool abstract
# variable.
rimless = RimlessWheel()
context = rimless.CreateDefaultContext()
x = np.array([1.125])
context.SetDiscreteState(xd=2 * x)
np.testing.assert_equal(
context.get_discrete_state_vector().CopyToVector(), 2 * x)
context.SetDiscreteState(group_index=0, xd=3 * x)
np.testing.assert_equal(
context.get_discrete_state_vector().CopyToVector(), 3 * x)
context.SetAbstractState(index=0, value=True)
value = context.get_abstract_state(0)
self.assertTrue(value.get_value())
context.SetAbstractState(index=0, value=False)
value = context.get_abstract_state(0)
self.assertFalse(value.get_value())
def test_direct_collocation(self):
plant = PendulumPlant()
context = plant.CreateDefaultContext()
dircol = DirectCollocation(plant,
context,
num_time_samples=21,
minimum_timestep=0.2,
maximum_timestep=0.5)
# Spell out most of the methods, regardless of whether they make sense
# as a consistent optimization. The goal is to check the bindings,
# not the implementation.
t = dircol.time()
dt = dircol.timestep(0)
x = dircol.state()
x2 = dircol.state(2)
x0 = dircol.initial_state()
xf = dircol.final_state()
u = dircol.input()
u2 = dircol.input(2)
dircol.AddRunningCost(x.dot(x))
dircol.AddConstraintToAllKnotPoints(u[0] == 0)
dircol.AddTimeIntervalBounds(0.3, 0.4)
dircol.AddEqualTimeIntervalsConstraints()
dircol.AddDurationBounds(.3 * 21, 0.4 * 21)
dircol.AddFinalCost(2 * x.dot(x))
initial_u = PiecewisePolynomial.ZeroOrderHold([0, .3 * 21],
np.zeros((1, 2)))
initial_x = PiecewisePolynomial()
dircol.SetInitialTrajectory(initial_u, initial_x)
dircol.Solve()
times = dircol.GetSampleTimes()
inputs = dircol.GetInputSamples()
states = dircol.GetStateSamples()
input_traj = dircol.ReconstructInputTrajectory()
state_traj = dircol.ReconstructStateTrajectory()
constraint = DirectCollocationConstraint(plant, context)
AddDirectCollocationConstraint(constraint, dircol.timestep(0),
dircol.state(0), dircol.state(1),
dircol.input(0), dircol.input(1),
dircol)
def BalancingLQR():
# Design an LQR controller for stabilizing the Acrobot around the upright.
# Returns a (static) AffineSystem that implements the controller (in
# the original AcrobotState coordinates).
pendulum = PendulumPlant()
context = pendulum.CreateDefaultContext()
input = PendulumInput()
input.set_tau(0.)
context.FixInputPort(0, input)
context.get_mutable_continuous_state_vector().SetFromVector(
UprightState().CopyToVector())
Q = np.diag((10., 1.))
R = [1]
linearized_pendulum = Linearize(pendulum, context)
(K, S) = LinearQuadraticRegulator(linearized_pendulum.A(),
linearized_pendulum.B(), Q, R)
return (K, S)
def test_direct_collocation(self):
plant = PendulumPlant()
context = plant.CreateDefaultContext()
num_time_samples = 21
dircol = DirectCollocation(
plant,
context,
num_time_samples=num_time_samples,
minimum_timestep=0.2,
maximum_timestep=0.5,
input_port_index=InputPortSelection.kUseFirstInputIfItExists,
assume_non_continuous_states_are_fixed=False)
prog = dircol.prog()
# Spell out most of the methods, regardless of whether they make sense
# as a consistent optimization. The goal is to check the bindings,
# not the implementation.
t = dircol.time()
dt = dircol.timestep(index=0)
x = dircol.state()
x2 = dircol.state(index=2)
x0 = dircol.initial_state()
xf = dircol.final_state()
u = dircol.input()
u2 = dircol.input(index=2)
v = dircol.NewSequentialVariable(rows=1, name="test")
v2 = dircol.GetSequentialVariableAtIndex(name="test", index=2)
dircol.AddRunningCost(x.dot(x))
input_con = dircol.AddConstraintToAllKnotPoints(u[0] == 0)
self.assertEqual(len(input_con), 21)
interval_bound = dircol.AddTimeIntervalBounds(lower_bound=0.3,
upper_bound=0.4)
self.assertIsInstance(interval_bound.evaluator(),
mp.BoundingBoxConstraint)
equal_time_con = dircol.AddEqualTimeIntervalsConstraints()
self.assertEqual(len(equal_time_con), 19)
duration_bound = dircol.AddDurationBounds(lower_bound=.3 * 21,
upper_bound=0.4 * 21)
self.assertIsInstance(duration_bound.evaluator(), mp.LinearConstraint)
final_cost = dircol.AddFinalCost(2 * x.dot(x))
self.assertIsInstance(final_cost.evaluator(), mp.Cost)
initial_u = PiecewisePolynomial.ZeroOrderHold([0, .3 * 21],
np.zeros((1, 2)))
initial_x = PiecewisePolynomial()
dircol.SetInitialTrajectory(traj_init_u=initial_u,
traj_init_x=initial_x)
was_called = dict(input=False, state=False, complete=False)
def input_callback(t, u):
was_called["input"] = True
def state_callback(t, x):
was_called["state"] = True
def complete_callback(t, x, u, v):
was_called["complete"] = True
dircol.AddInputTrajectoryCallback(callback=input_callback)
dircol.AddStateTrajectoryCallback(callback=state_callback)
dircol.AddCompleteTrajectoryCallback(callback=complete_callback,
names=["test"])
result = mp.Solve(dircol.prog())
self.assertTrue(was_called["input"])
self.assertTrue(was_called["state"])
self.assertTrue(was_called["complete"])
dircol.GetSampleTimes(result=result)
dircol.GetInputSamples(result=result)
dircol.GetStateSamples(result=result)
dircol.GetSequentialVariableSamples(result=result, name="test")
dircol.ReconstructInputTrajectory(result=result)
dircol.ReconstructStateTrajectory(result=result)
constraint = DirectCollocationConstraint(plant, context)
AddDirectCollocationConstraint(constraint, dircol.timestep(0),
dircol.state(0), dircol.state(1),
dircol.input(0), dircol.input(1), prog)
# Test AddConstraintToAllKnotPoints variants.
nc = len(prog.bounding_box_constraints())
c = dircol.AddConstraintToAllKnotPoints(
constraint=mp.BoundingBoxConstraint([0], [1]), vars=u)
self.assertIsInstance(c[0], mp.Binding[mp.BoundingBoxConstraint])
self.assertEqual(len(prog.bounding_box_constraints()),
nc + num_time_samples)
nc = len(prog.linear_equality_constraints())
c = dircol.AddConstraintToAllKnotPoints(
constraint=mp.LinearEqualityConstraint([1], [0]), vars=u)
self.assertIsInstance(c[0], mp.Binding[mp.LinearEqualityConstraint])
self.assertEqual(len(prog.linear_equality_constraints()),
nc + num_time_samples)
nc = len(prog.linear_constraints())
c = dircol.AddConstraintToAllKnotPoints(constraint=mp.LinearConstraint(
[1], [0], [1]),
vars=u)
self.assertIsInstance(c[0], mp.Binding[mp.LinearConstraint])
self.assertEqual(len(prog.linear_constraints()), nc + num_time_samples)
nc = len(prog.linear_equality_constraints())
# eq(x, 2) produces a 2-dimensional vector of Formula.
c = dircol.AddConstraintToAllKnotPoints(eq(x, 2))
self.assertIsInstance(c[0].evaluator(), mp.LinearEqualityConstraint)
self.assertEqual(len(prog.linear_equality_constraints()),
nc + 2 * num_time_samples)
def test_context_api(self):
system = Adder(3, 10)
context = system.AllocateContext()
self.assertIsInstance(
context.get_continuous_state(), ContinuousState)
self.assertIsInstance(
context.get_mutable_continuous_state(), ContinuousState)
self.assertIsInstance(
context.get_continuous_state_vector(), VectorBase)
self.assertIsInstance(
context.get_mutable_continuous_state_vector(), VectorBase)
system.SetDefaultContext(context)
# Check random context method.
system.SetRandomContext(context=context, generator=RandomGenerator())
context = system.CreateDefaultContext()
self.assertIsInstance(
context.get_continuous_state(), ContinuousState)
self.assertIsInstance(
context.get_mutable_continuous_state(), ContinuousState)
self.assertIsInstance(
context.get_continuous_state_vector(), VectorBase)
self.assertIsInstance(
context.get_mutable_continuous_state_vector(), VectorBase)
self.assertTrue(context.is_stateless())
self.assertFalse(context.has_only_continuous_state())
self.assertFalse(context.has_only_discrete_state())
self.assertEqual(context.num_total_states(), 0)
# TODO(eric.cousineau): Consolidate main API tests for `Context` here.
# Test methods with two scalar types.
for T in [float, AutoDiffXd, Expression]:
systemT = Adder_[T](3, 10)
contextT = systemT.CreateDefaultContext()
for U in [float, AutoDiffXd, Expression]:
systemU = Adder_[U](3, 10)
contextU = systemU.CreateDefaultContext()
contextU.SetTime(0.5)
contextT.SetTimeStateAndParametersFrom(contextU)
if T == float:
self.assertEqual(contextT.get_time(), 0.5)
elif T == AutoDiffXd:
self.assertEqual(contextT.get_time().value(), 0.5)
else:
self.assertEqual(contextT.get_time().Evaluate(), 0.5)
pendulum = PendulumPlant()
context = pendulum.CreateDefaultContext()
self.assertEqual(context.num_numeric_parameter_groups(), 1)
self.assertEqual(pendulum.num_numeric_parameter_groups(), 1)
self.assertTrue(
context.get_parameters().get_numeric_parameter(0) is
context.get_numeric_parameter(index=0))
self.assertTrue(
context.get_mutable_parameters().get_mutable_numeric_parameter(
0) is context.get_mutable_numeric_parameter(index=0))
self.assertEqual(context.num_abstract_parameters(), 0)
self.assertEqual(pendulum.num_numeric_parameter_groups(), 1)
# TODO(russt): Bind _Declare*Parameter or find an example with an
# abstract parameter to actually call this method.
self.assertTrue(hasattr(context, "get_abstract_parameter"))
self.assertTrue(hasattr(context, "get_mutable_abstract_parameter"))
context.DisableCaching()
context.EnableCaching()
context.SetAllCacheEntriesOutOfDate()
context.FreezeCache()
self.assertTrue(context.is_cache_frozen())
context.UnfreezeCache()
self.assertFalse(context.is_cache_frozen())
x = np.array([0.1, 0.2])
context.SetContinuousState(x)
np.testing.assert_equal(
context.get_continuous_state().CopyToVector(), x)
np.testing.assert_equal(
context.get_continuous_state_vector().CopyToVector(), x)
context.SetTimeAndContinuousState(0.3, 2*x)
np.testing.assert_equal(context.get_time(), 0.3)
np.testing.assert_equal(
context.get_continuous_state_vector().CopyToVector(), 2*x)
self.assertNotEqual(pendulum.EvalPotentialEnergy(context=context), 0)
self.assertNotEqual(pendulum.EvalKineticEnergy(context=context), 0)
# RimlessWheel has a single discrete variable and a bool abstract
# variable.
rimless = RimlessWheel()
context = rimless.CreateDefaultContext()
x = np.array([1.125])
context.SetDiscreteState(xd=2 * x)
np.testing.assert_equal(
context.get_discrete_state_vector().CopyToVector(), 2 * x)
context.SetDiscreteState(group_index=0, xd=3 * x)
np.testing.assert_equal(
context.get_discrete_state_vector().CopyToVector(), 3 * x)
def check_abstract_value_zero(context, expected_value):
# Check through Context, State, and AbstractValues APIs.
self.assertEqual(context.get_abstract_state(index=0).get_value(),
expected_value)
self.assertEqual(context.get_abstract_state().get_value(
index=0).get_value(), expected_value)
self.assertEqual(context.get_state().get_abstract_state()
.get_value(index=0).get_value(), expected_value)
context.SetAbstractState(index=0, value=True)
check_abstract_value_zero(context, True)
context.SetAbstractState(index=0, value=False)
check_abstract_value_zero(context, False)
value = context.get_mutable_state().get_mutable_abstract_state()\
.get_mutable_value(index=0)
value.set_value(True)
check_abstract_value_zero(context, True)
def test_direct_collocation(self):
plant = PendulumPlant()
context = plant.CreateDefaultContext()
dircol = DirectCollocation(
plant, context, num_time_samples=21, minimum_timestep=0.2,
maximum_timestep=0.5,
input_port_index=InputPortSelection.kUseFirstInputIfItExists,
assume_non_continuous_states_are_fixed=False)
# Spell out most of the methods, regardless of whether they make sense
# as a consistent optimization. The goal is to check the bindings,
# not the implementation.
t = dircol.time()
dt = dircol.timestep(index=0)
x = dircol.state()
x2 = dircol.state(index=2)
x0 = dircol.initial_state()
xf = dircol.final_state()
u = dircol.input()
u2 = dircol.input(index=2)
v = dircol.NewSequentialVariable(rows=1, name="test")
v2 = dircol.GetSequentialVariableAtIndex(name="test", index=2)
dircol.AddRunningCost(x.dot(x))
dircol.AddConstraintToAllKnotPoints(u[0] == 0)
dircol.AddTimeIntervalBounds(lower_bound=0.3, upper_bound=0.4)
dircol.AddEqualTimeIntervalsConstraints()
dircol.AddDurationBounds(lower_bound=.3*21, upper_bound=0.4*21)
dircol.AddFinalCost(2*x.dot(x))
initial_u = PiecewisePolynomial.ZeroOrderHold([0, .3*21],
np.zeros((1, 2)))
initial_x = PiecewisePolynomial()
dircol.SetInitialTrajectory(traj_init_u=initial_u,
traj_init_x=initial_x)
was_called = dict(
input=False,
state=False,
complete=False
)
def input_callback(t, u):
was_called["input"] = True
def state_callback(t, x):
was_called["state"] = True
def complete_callback(t, x, u, v):
was_called["complete"] = True
dircol.AddInputTrajectoryCallback(callback=input_callback)
dircol.AddStateTrajectoryCallback(callback=state_callback)
dircol.AddCompleteTrajectoryCallback(callback=complete_callback,
names=["test"])
result = mp.Solve(dircol)
self.assertTrue(was_called["input"])
self.assertTrue(was_called["state"])
self.assertTrue(was_called["complete"])
times = dircol.GetSampleTimes(result=result)
inputs = dircol.GetInputSamples(result=result)
states = dircol.GetStateSamples(result=result)
variables = dircol.GetSequentialVariableSamples(result=result,
name="test")
input_traj = dircol.ReconstructInputTrajectory(result=result)
state_traj = dircol.ReconstructStateTrajectory(result=result)
constraint = DirectCollocationConstraint(plant, context)
AddDirectCollocationConstraint(constraint, dircol.timestep(0),
dircol.state(0), dircol.state(1),
dircol.input(0), dircol.input(1),
dircol)
import math
import numpy as np
import matplotlib.pyplot as plt
from pydrake.examples.pendulum import (PendulumPlant, PendulumState)
from pydrake.all import (DirectCollocation, PiecewisePolynomial,
Solve)
from visualizer import PendulumVisualizer
plant = PendulumPlant()
context = plant.CreateDefaultContext()
N = 21
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)
dircol.AddEqualTimeIntervalsConstraints()
torque_limit = 3.0 # N*m.
u = dircol.input()
dircol.AddConstraintToAllKnotPoints(-torque_limit <= u[0])
dircol.AddConstraintToAllKnotPoints(u[0] <= torque_limit)
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())
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 _do_test_direct_collocation(self, use_deprecated_solve):
plant = PendulumPlant()
context = plant.CreateDefaultContext()
dircol = DirectCollocation(
plant, context, num_time_samples=21, minimum_timestep=0.2,
maximum_timestep=0.5,
input_port_index=InputPortSelection.kUseFirstInputIfItExists,
assume_non_continuous_states_are_fixed=False)
# Spell out most of the methods, regardless of whether they make sense
# as a consistent optimization. The goal is to check the bindings,
# not the implementation.
t = dircol.time()
dt = dircol.timestep(0)
x = dircol.state()
x2 = dircol.state(2)
x0 = dircol.initial_state()
xf = dircol.final_state()
u = dircol.input()
u2 = dircol.input(2)
dircol.AddRunningCost(x.dot(x))
dircol.AddConstraintToAllKnotPoints(u[0] == 0)
dircol.AddTimeIntervalBounds(0.3, 0.4)
dircol.AddEqualTimeIntervalsConstraints()
dircol.AddDurationBounds(.3*21, 0.4*21)
dircol.AddFinalCost(2*x.dot(x))
initial_u = PiecewisePolynomial.ZeroOrderHold([0, .3*21],
np.zeros((1, 2)))
initial_x = PiecewisePolynomial()
dircol.SetInitialTrajectory(initial_u, initial_x)
global input_was_called
input_was_called = False
global state_was_called
state_was_called = False
def input_callback(t, u):
global input_was_called
input_was_called = True
def state_callback(t, x):
global state_was_called
state_was_called = True
dircol.AddInputTrajectoryCallback(input_callback)
dircol.AddStateTrajectoryCallback(state_callback)
if use_deprecated_solve:
with catch_drake_warnings(expected_count=1):
dircol.Solve()
result = None
else:
result = mp.Solve(dircol)
self.assertTrue(input_was_called)
self.assertTrue(state_was_called)
if use_deprecated_solve:
with catch_drake_warnings(expected_count=5):
times = dircol.GetSampleTimes()
inputs = dircol.GetInputSamples()
states = dircol.GetStateSamples()
input_traj = dircol.ReconstructInputTrajectory()
state_traj = dircol.ReconstructStateTrajectory()
else:
times = dircol.GetSampleTimes(result)
inputs = dircol.GetInputSamples(result)
states = dircol.GetStateSamples(result)
input_traj = dircol.ReconstructInputTrajectory(result)
state_traj = dircol.ReconstructStateTrajectory(result)
constraint = DirectCollocationConstraint(plant, context)
AddDirectCollocationConstraint(constraint, dircol.timestep(0),
dircol.state(0), dircol.state(1),
dircol.input(0), dircol.input(1),
dircol)
def test_context_api(self):
system = Adder(3, 10)
context = system.AllocateContext()
self.assertIsInstance(context.get_continuous_state(), ContinuousState)
self.assertIsInstance(context.get_mutable_continuous_state(),
ContinuousState)
self.assertIsInstance(context.get_continuous_state_vector(),
VectorBase)
self.assertIsInstance(context.get_mutable_continuous_state_vector(),
VectorBase)
context = system.CreateDefaultContext()
self.assertIsInstance(context.get_continuous_state(), ContinuousState)
self.assertIsInstance(context.get_mutable_continuous_state(),
ContinuousState)
self.assertIsInstance(context.get_continuous_state_vector(),
VectorBase)
self.assertIsInstance(context.get_mutable_continuous_state_vector(),
VectorBase)
# TODO(eric.cousineau): Consolidate main API tests for `Context` here.
# Test methods with two scalar types.
for T in [float, AutoDiffXd, Expression]:
systemT = Adder_[T](3, 10)
contextT = systemT.CreateDefaultContext()
for U in [float, AutoDiffXd, Expression]:
systemU = Adder_[U](3, 10)
contextU = systemU.CreateDefaultContext()
contextU.SetTime(0.5)
contextT.SetTimeStateAndParametersFrom(contextU)
if T == float:
self.assertEqual(contextT.get_time(), 0.5)
elif T == AutoDiffXd:
self.assertEqual(contextT.get_time().value(), 0.5)
else:
self.assertEqual(contextT.get_time().Evaluate(), 0.5)
pendulum = PendulumPlant()
context = pendulum.CreateDefaultContext()
self.assertEqual(context.num_numeric_parameter_groups(), 1)
self.assertEqual(pendulum.num_numeric_parameter_groups(), 1)
self.assertTrue(context.get_parameters().get_numeric_parameter(0) is
context.get_numeric_parameter(index=0))
self.assertEqual(context.num_abstract_parameters(), 0)
self.assertEqual(pendulum.num_numeric_parameter_groups(), 1)
# TODO(russt): Bind _Declare*Parameter or find an example with an
# abstract parameter to actually call this method.
self.assertTrue(hasattr(context, "get_abstract_parameter"))
x = np.array([0.1, 0.2])
context.SetContinuousState(x)
np.testing.assert_equal(
context.get_continuous_state_vector().CopyToVector(), x)
# RimlessWheel has a single discrete variable and a bool abstract
# variable.
rimless = RimlessWheel()
context = rimless.CreateDefaultContext()
x = np.array([1.125])
context.SetDiscreteState(xd=2 * x)
np.testing.assert_equal(
context.get_discrete_state_vector().CopyToVector(), 2 * x)
context.SetDiscreteState(group_index=0, xd=3 * x)
np.testing.assert_equal(
context.get_discrete_state_vector().CopyToVector(), 3 * x)
context.SetAbstractState(index=0, value=True)
value = context.get_abstract_state(0)
self.assertTrue(value.get_value())
context.SetAbstractState(index=0, value=False)
value = context.get_abstract_state(0)
self.assertFalse(value.get_value())
def generate_prog(self):
self.nn_conss = []
self.nn_costs = []
plant = PendulumPlant()
context = plant.CreateDefaultContext()
dircol_constraint = DirectCollocationConstraint(plant, context)
prog = MathematicalProgram()
# K = prog.NewContinuousVariables(1,7,'K')
def final_cost(x):
return 100.*(cos(.5*x[0])**2 + x[1]**2)
h = [];
u = [];
x = [];
xf = np.array([math.pi, 0.])
# Declare the MathematicalProgram vars up front in a good order, so that
# prog.decision_variables will be result of np.hstack(h.flatten(), u.flatten(), x.flatten(), T.flatten())
# for the following shapes: unfortunately, prog.decision_variables() will have these shapes:
# h = (num_trajectories, 1) | h = (num_trajectories, 1)
# u = (num_trajectories, num_inputs, num_samples) | u = (num_trajectories, num_samples, num_inputs)
# x = (num_trajectories, num_states, num_samples) | x = (num_trajectories, num_samples, num_states)
# T = (num_params) | T = (num_params)
for ti in range(self.num_trajectories):
h.append(prog.NewContinuousVariables(1,'h'+str(ti)))
for ti in range(self.num_trajectories):
u.append(prog.NewContinuousVariables(1, self.num_samples,'u'+str(ti)))
for ti in range(self.num_trajectories):
x.append(prog.NewContinuousVariables(2, self.num_samples,'x'+str(ti)))
# Add in constraints
for ti in range(self.num_trajectories):
prog.AddBoundingBoxConstraint(self.kMinimumTimeStep, self.kMaximumTimeStep, h[ti])
# prog.AddQuadraticCost([1.], [0.], h[ti]) # Added by me, penalize long timesteps
x0 = np.array(self.ic_list[ti]) # TODO: hopefully this isn't subtley bad...
prog.AddBoundingBoxConstraint(x0, x0, x[ti][:,0])
# nudge = np.array([.2, .2])
# prog.AddBoundingBoxConstraint(xf-nudge, xf+nudge, x[ti][:,-1])
prog.AddBoundingBoxConstraint(xf, xf, x[ti][:,-1])
# Do optional warm start here
if self.warm_start:
prog.SetInitialGuess(h[ti], [(self.kMinimumTimeStep+self.kMaximumTimeStep)/2])
for i in range(self.num_samples):
prog.SetInitialGuess(u[ti][:,i], [0.])
x_interp = (xf-x0)*i/self.num_samples + x0
prog.SetInitialGuess(x[ti][:,i], x_interp)
# prog.SetInitialGuess(u[ti][:,i], np.array(1.0))
for i in range(self.num_samples-1):
AddDirectCollocationConstraint(dircol_constraint, h[ti], x[ti][:,i], x[ti][:,i+1], u[ti][:,i], u[ti][:,i+1], prog)
for i in range(self.num_samples):
#prog.AddQuadraticCost([[2., 0.], [0., 2.]], [0., 0.], x[ti][:,i])
#prog.AddQuadraticCost([25.], [0.], u[ti][:,i])
u_var = u[ti][:,i]
x_var = x[ti][:,i] - xf
h_var = h[ti][0]
#print(u_var.shape, x_var.shape, xf.shape)
prog.AddCost( h_var * (2*x_var.dot(x_var) + 25*u_var.dot(u_var)) )
#prog.AddCost( 2*((x[0]-math.pi)*(x[0]-math.pi) + x[1]*x[1]) + 25*u.dot(u))
kTorqueLimit = 5
prog.AddBoundingBoxConstraint([-kTorqueLimit], [kTorqueLimit], u[ti][:,i])
# prog.AddConstraint(control, [0.], [0.], np.hstack([x[ti][:,i], u[ti][:,i], K.flatten()]))
# prog.AddConstraint(u[ti][0,i] == (3.*sym.tanh(K.dot(control_basis(x[ti][:,i]))[0]))) # u = 3*tanh(K * m(x))
# prog.AddCost(final_cost, x[ti][:,-1])
# prog.AddCost(h[ti][0]*100) # Try to penalize using more time than it needs?
# Setting solver options
#prog.SetSolverOption(SolverType.kSnopt, 'Verify level', -1) # Derivative checking disabled. (otherwise it complains on the saturation)
#prog.SetSolverOption(SolverType.kSnopt, 'Print file', "/tmp/snopt.out")
# Save references
self.prog = prog
self.h = h
self.u = u
self.x = x
self.T = []
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 make_multiple_dircol_trajectories(num_trajectories,
num_samples,
initial_conditions=None):
from pydrake.all import (
AutoDiffXd,
Expression,
Variable,
MathematicalProgram,
SolverType,
SolutionResult,
DirectCollocationConstraint,
AddDirectCollocationConstraint,
PiecewisePolynomial,
)
import pydrake.symbolic as sym
from pydrake.examples.pendulum import (PendulumPlant)
# initial_conditions maps (ti) -> [1xnum_states] initial state
if initial_conditions is not None:
initial_conditions = intial_cond_dict[initial_conditions]
assert callable(initial_conditions)
plant = PendulumPlant()
context = plant.CreateDefaultContext()
dircol_constraint = DirectCollocationConstraint(plant, context)
# num_trajectories = 15;
# num_samples = 15;
prog = MathematicalProgram()
# K = prog.NewContinuousVariables(1,7,'K')
def cos(x):
if isinstance(x, AutoDiffXd):
return x.cos()
elif isinstance(x, Variable):
return sym.cos(x)
return math.cos(x)
def sin(x):
if isinstance(x, AutoDiffXd):
return x.sin()
elif isinstance(x, Variable):
return sym.sin(x)
return math.sin(x)
def final_cost(x):
return 100. * (cos(.5 * x[0])**2 + x[1]**2)
h = []
u = []
x = []
xf = (math.pi, 0.)
for ti in range(num_trajectories):
h.append(prog.NewContinuousVariables(1))
prog.AddBoundingBoxConstraint(.01, .2, h[ti])
# prog.AddQuadraticCost([1.], [0.], h[ti]) # Added by me, penalize long timesteps
u.append(prog.NewContinuousVariables(1, num_samples, 'u' + str(ti)))
x.append(prog.NewContinuousVariables(2, num_samples, 'x' + str(ti)))
# Use Russ's initial conditions, unless I pass in a function myself.
if initial_conditions is None:
x0 = (.8 + math.pi - .4 * ti, 0.0)
else:
x0 = initial_conditions(ti)
assert len(x0) == 2 #TODO: undo this hardcoding.
prog.AddBoundingBoxConstraint(x0, x0, x[ti][:, 0])
nudge = np.array([.2, .2])
prog.AddBoundingBoxConstraint(xf - nudge, xf + nudge, x[ti][:, -1])
# prog.AddBoundingBoxConstraint(xf, xf, x[ti][:,-1])
for i in range(num_samples - 1):
AddDirectCollocationConstraint(dircol_constraint, h[ti],
x[ti][:, i], x[ti][:, i + 1],
u[ti][:, i], u[ti][:, i + 1], prog)
for i in range(num_samples):
prog.AddQuadraticCost([1.], [0.], u[ti][:, i])
# prog.AddConstraint(control, [0.], [0.], np.hstack([x[ti][:,i], u[ti][:,i], K.flatten()]))
# prog.AddBoundingBoxConstraint([-3.], [3.], u[ti][:,i])
# prog.AddConstraint(u[ti][0,i] == (3.*sym.tanh(K.dot(control_basis(x[ti][:,i]))[0]))) # u = 3*tanh(K * m(x))
# prog.AddCost(final_cost, x[ti][:,-1])
# prog.AddCost(h[ti][0]*100) # Try to penalize using more time than it needs?
#prog.SetSolverOption(SolverType.kSnopt, 'Verify level', -1) # Derivative checking disabled. (otherwise it complains on the saturation)
prog.SetSolverOption(SolverType.kSnopt, 'Print file', "/tmp/snopt.out")
# result = prog.Solve()
# print(result)
# print(prog.GetSolution(K))
# print(prog.GetSolution(K).dot(control_basis(x[0][:,0])))
#if (result != SolutionResult.kSolutionFound):
# f = open('/tmp/snopt.out', 'r')
# print(f.read())
# f.close()
return prog, h, u, x
