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. 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_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) 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")) x = np.array([0.1, 0.2]) context.SetContinuousState(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) # 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_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(0) x = dircol.state() x2 = dircol.state(2) x0 = dircol.initial_state() xf = dircol.final_state() u = dircol.input() u2 = dircol.input(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(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 global complete_was_called complete_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 def complete_callback(t, x, u, v): global complete_was_called complete_was_called = True dircol.AddInputTrajectoryCallback(input_callback) dircol.AddStateTrajectoryCallback(state_callback) dircol.AddCompleteTrajectoryCallback(callback=complete_callback, names=["test"]) result = mp.Solve(dircol) self.assertTrue(input_was_called) self.assertTrue(state_was_called) self.assertTrue(complete_was_called) times = dircol.GetSampleTimes(result) inputs = dircol.GetInputSamples(result) states = dircol.GetStateSamples(result) variables = dircol.GetSequentialVariableSamples(result=result, name="test") 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_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))
import math import numpy as np import matplotlib.pyplot as plt from mpl_toolkits.mplot3d import Axes3D from matplotlib import cm from pydrake.all import (DiagramBuilder, SignalLogger, Simulator, VectorSystem) from pydrake.examples.pendulum import PendulumPlant from pydrake.systems.controllers import (DynamicProgrammingOptions, FittedValueIteration, PeriodicBoundaryCondition) from visualizer import PendulumVisualizer plant = PendulumPlant() simulator = Simulator(plant) options = DynamicProgrammingOptions() def min_time_cost(context): x = context.get_continuous_state_vector().CopyToVector() x[0] = x[0] - math.pi if x.dot(x) < .05: return 0. return 1. def quadratic_regulator_cost(context): x = context.get_continuous_state_vector().CopyToVector() x[0] = x[0] - math.pi u = plant.EvalVectorInput(context, 0).CopyToVector() return 2 * x.dot(x) + u.dot(u)
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 _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) # 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 warnings.catch_warnings(record=True) as w: warnings.simplefilter('always', DrakeDeprecationWarning) dircol.Solve() self.assertEqual(len(w), 1) result = None else: result = mp.Solve(dircol) self.assertTrue(input_was_called) self.assertTrue(state_was_called) if use_deprecated_solve: with warnings.catch_warnings(record=True) as w: warnings.simplefilter('always', DrakeDeprecationWarning) times = dircol.GetSampleTimes() inputs = dircol.GetInputSamples() states = dircol.GetStateSamples() input_traj = dircol.ReconstructInputTrajectory() state_traj = dircol.ReconstructStateTrajectory() self.assertEqual(len(w), 5) 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 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