def test_minimal_simulation(self): # Create a simple system. system = ConstantVectorSource([1.]) def make_simulator(generator): simulator = Simulator(system) simulator.Initialize() simulator.set_target_realtime_rate(0) return simulator def calc_output(system, context): return 42. result = RandomSimulation(make_simulator=make_simulator, output=calc_output, final_time=1.0, generator=RandomGenerator()) self.assertEqual(result, 42.) result = MonteCarloSimulation(make_simulator=make_simulator, output=calc_output, final_time=1.0, num_samples=10, generator=RandomGenerator()) self.assertIsInstance(result, list) self.assertEqual(len(result), 10) self.assertIsInstance(result[0], RandomSimulationResult) self.assertIsInstance(result[0].generator_snapshot, RandomGenerator) self.assertEqual(result[0].output, 42.) for i in range(1, len(result)): self.assertIsNot(result[0].generator_snapshot, result[i].generator_snapshot)
def _check_distribution_vector(self, dut): """Confirms that subclasses of DistributionVector bind the base methods. """ self.assertIsInstance(dut, mut.DistributionVector) dut.Sample(generator=RandomGenerator()) dut.Mean() dut.ToSymbolic()
def test_transform(self): mut.Transform() dut = mut.Transform(pydrake.math.RigidTransform()) dut.set_rotation_rpy_deg([0.1, 0.2, 0.3]) dut.IsDeterministic() dut.GetDeterministicValue() dut.ToSymbolic() dut.Sample(generator=RandomGenerator()) dut.base_frame = "name"
def test_random_rotations(self): g = RandomGenerator() quat = mut.UniformlyRandomQuaternion(g) self.assertIsInstance(quat, Quaternion_[float]) angle_axis = mut.UniformlyRandomAngleAxis(g) self.assertIsInstance(angle_axis, AngleAxis_[float]) rot_mat = mut.UniformlyRandomRotationMatrix(g) self.assertIsInstance(rot_mat, mut.RotationMatrix) rpy = mut.UniformlyRandomRPY(g) self.assertIsInstance(rpy, np.ndarray) self.assertEqual(len(rpy), 3)
def _check_distribution(self, dut): """Confirms that a subclass instance of Distribution * binds the base methods, and * supports copy/deepcopy (even though the superclass does not). """ self.assertIsInstance(dut, mut.Distribution) copy.copy(dut) copy.deepcopy(dut) dut.Sample(generator=RandomGenerator()) dut.Mean() dut.ToSymbolic()
def test_distribution_variant(self): """Confirms that the free functions that operate on a variant are bound.""" items = [ mut.Deterministic(1.0), mut.Gaussian(1.0, 0.1), mut.Uniform(-1.0, 1.0), mut.UniformDiscrete([0.0, 1.0]), ] for item in items: copied = mut.ToDistribution(item) self._check_distribution(copied) mut.Sample(var=item, generator=RandomGenerator()) mut.Mean(var=item) mut.ToSymbolic(var=item) if mut.IsDeterministic(var=item): mut.GetDeterministicValue(var=item)
def test_linear_affine_system(self): # Just make sure linear system is spelled correctly. A = np.identity(2) B = np.array([[0], [1]]) f0 = np.array([[0], [0]]) C = np.array([[0, 1]]) D = [0] y0 = [0] system = LinearSystem(A, B, C, D) context = system.CreateDefaultContext() self.assertEqual(system.get_input_port(0).size(), 1) self.assertEqual(context.get_mutable_continuous_state_vector().size(), 2) self.assertEqual(system.get_output_port(0).size(), 1) self.assertTrue((system.A() == A).all()) self.assertTrue((system.B() == B).all()) self.assertTrue((system.f0() == f0).all()) self.assertTrue((system.C() == C).all()) self.assertEqual(system.D(), D) self.assertEqual(system.y0(), y0) self.assertEqual(system.time_period(), 0.) x0 = np.array([1, 2]) system.configure_default_state(x0=x0) system.SetDefaultContext(context) np.testing.assert_equal( context.get_continuous_state_vector().CopyToVector(), x0) generator = RandomGenerator() system.SetRandomContext(context, generator) np.testing.assert_equal( context.get_continuous_state_vector().CopyToVector(), x0) system.configure_random_state(covariance=np.eye(2)) system.SetRandomContext(context, generator) self.assertNotEqual( context.get_continuous_state_vector().CopyToVector()[1], x0[1]) Co = ControllabilityMatrix(system) self.assertEqual(Co.shape, (2, 2)) self.assertFalse(IsControllable(system)) self.assertFalse(IsControllable(system, 1e-6)) Ob = ObservabilityMatrix(system) self.assertEqual(Ob.shape, (2, 2)) self.assertFalse(IsObservable(system)) system = AffineSystem(A, B, f0, C, D, y0, .1) self.assertEqual(system.get_input_port(0), system.get_input_port()) self.assertEqual(system.get_output_port(0), system.get_output_port()) context = system.CreateDefaultContext() self.assertEqual(system.get_input_port(0).size(), 1) self.assertEqual(context.get_discrete_state_vector().size(), 2) self.assertEqual(system.get_output_port(0).size(), 1) self.assertTrue((system.A() == A).all()) self.assertTrue((system.B() == B).all()) self.assertTrue((system.f0() == f0).all()) self.assertTrue((system.C() == C).all()) self.assertEqual(system.D(), D) self.assertEqual(system.y0(), y0) self.assertEqual(system.time_period(), .1) system.get_input_port(0).FixValue(context, 0) linearized = Linearize(system, context) self.assertTrue((linearized.A() == A).all()) taylor = FirstOrderTaylorApproximation(system, context) self.assertTrue((taylor.y0() == y0).all()) system = MatrixGain(D=A) self.assertTrue((system.D() == A).all())
def main(): parser = argparse.ArgumentParser(description=__doc__) parser.add_argument( "--num_samples", type=int, default=50, help="Number of Monte Carlo samples to use to estimate performance.") parser.add_argument("--torque_limit", type=float, default=2.0, help="Torque limit of the pendulum.") args = parser.parse_args() if args.torque_limit < 0: raise InvalidArgumentError("Please supply a nonnegative torque limit.") # Assemble the Pendulum plant. builder = DiagramBuilder() pendulum = builder.AddSystem(MultibodyPlant(0.0)) file_name = FindResourceOrThrow("drake/examples/pendulum/Pendulum.urdf") Parser(pendulum).AddModelFromFile(file_name) pendulum.WeldFrames(pendulum.world_frame(), pendulum.GetFrameByName("base")) pendulum.Finalize() # Set the pendulum to start at uniformly random # positions (but always zero velocity). elbow = pendulum.GetMutableJointByName("theta") upright_theta = np.pi theta_expression = Variable(name="theta", type=Variable.Type.RANDOM_UNIFORM) * 2. * np.pi elbow.set_random_angle_distribution(theta_expression) # Set up LQR, with high position gains to try to ensure the # ROA is close to the theoretical torque-limited limit. Q = np.diag([100., 1.]) R = np.identity(1) * 0.01 linearize_context = pendulum.CreateDefaultContext() linearize_context.SetContinuousState(np.array([upright_theta, 0.])) actuation_port = pendulum.get_actuation_input_port() actuation_port.FixValue(linearize_context, 0) controller = builder.AddSystem( LinearQuadraticRegulator(pendulum, linearize_context, Q, R, np.zeros(0), actuation_port.get_index())) # Apply the torque limit. torque_limit = args.torque_limit torque_limiter = builder.AddSystem( Saturation(min_value=np.array([-torque_limit]), max_value=np.array([torque_limit]))) builder.Connect(controller.get_output_port(0), torque_limiter.get_input_port(0)) builder.Connect(torque_limiter.get_output_port(0), pendulum.get_actuation_input_port()) builder.Connect(pendulum.get_state_output_port(), controller.get_input_port(0)) diagram = builder.Build() # Perform the Monte Carlo simulation. def make_simulator(generator): ''' Create a simulator for the system using the given generator. ''' simulator = Simulator(diagram) simulator.set_target_realtime_rate(0) simulator.Initialize() return simulator def calc_wrapped_error(system, context): ''' Given a context from the end of the simulation, calculate an error -- which for this stabilizing task is the distance from the fixed point. ''' state = diagram.GetSubsystemContext( pendulum, context).get_continuous_state_vector() error = state.GetAtIndex(0) - upright_theta # Wrap error to [-pi, pi]. return (error + np.pi) % (2 * np.pi) - np.pi num_samples = args.num_samples results = MonteCarloSimulation(make_simulator=make_simulator, output=calc_wrapped_error, final_time=1.0, num_samples=num_samples, generator=RandomGenerator()) # Compute results. # The "success" region is fairly large since some "stabilized" trials # may still be oscillating around the fixed point. Failed examples are # consistently much farther from the fixed point than this. binary_results = np.array([abs(res.output) < 0.1 for res in results]) passing_ratio = float(sum(binary_results)) / len(results) # 95% confidence interval for the passing ratio. passing_ratio_var = 1.96 * np.sqrt(passing_ratio * (1. - passing_ratio) / len(results)) print("Monte-Carlo estimated performance across %d samples: " "%.2f%% +/- %0.2f%%" % (num_samples, passing_ratio * 100, passing_ratio_var * 100)) # Analytically compute the best possible ROA, for comparison, but # calculating where the torque needed to lift the pendulum exceeds # the torque limit. arm_radius = 0.5 arm_mass = 1.0 # torque = r x f = r * (m * 9.81 * sin(theta)) # theta = asin(torque / (r * m)) if torque_limit <= (arm_radius * arm_mass * 9.81): roa_half_width = np.arcsin(torque_limit / (arm_radius * arm_mass * 9.81)) else: roa_half_width = np.pi roa_as_fraction_of_state_space = roa_half_width / np.pi print("Max possible ROA = %0.2f%% of state space, which should" " match with the above estimate." % (100 * roa_as_fraction_of_state_space))
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_multilayer_perceptron(self): mlp = MultilayerPerceptron( layers=[1, 2, 3], activation_type=PerceptronActivationType.kReLU) self.assertEqual(mlp.get_input_port().size(), 1) self.assertEqual(mlp.get_output_port().size(), 3) context = mlp.CreateDefaultContext() params = np.zeros((mlp.num_parameters(), 1)) self.assertEqual(mlp.num_parameters(), 13) self.assertEqual(mlp.layers(), [1, 2, 3]) self.assertEqual(mlp.activation_type(layer=0), PerceptronActivationType.kReLU) self.assertEqual(len(mlp.GetParameters(context=context)), mlp.num_parameters()) mlp.SetWeights(context=context, layer=0, W=np.array([[1], [2]])) mlp.SetBiases(context=context, layer=0, b=[3, 4]) np.testing.assert_array_equal(mlp.GetWeights(context=context, layer=0), np.array([[1], [2]])) np.testing.assert_array_equal(mlp.GetBiases(context=context, layer=0), np.array([3, 4])) params = np.zeros(mlp.num_parameters()) mlp.SetWeights(params=params, layer=0, W=np.array([[1], [2]])) mlp.SetBiases(params=params, layer=0, b=[3, 4]) np.testing.assert_array_equal(mlp.GetWeights(params=params, layer=0), np.array([[1], [2]])) np.testing.assert_array_equal(mlp.GetBiases(params=params, layer=0), np.array([3, 4])) mutable_params = mlp.GetMutableParameters(context=context) mutable_params[:] = 3.0 np.testing.assert_array_equal(mlp.GetParameters(context), np.full(mlp.num_parameters(), 3.0)) global called_loss called_loss = False def silly_loss(Y, dloss_dY): global called_loss called_loss = True # We must be careful to update the dloss in place, rather than bind # a new matrix to the same variable name. dloss_dY[:] = 1 # dloss_dY = np.array(...etc...) # <== wrong return Y.sum() dloss_dparams = np.zeros((13, )) generator = RandomGenerator(23) mlp.SetRandomContext(context, generator) mlp.Backpropagation(context=context, X=np.array([1, 3, 4]).reshape((1, 3)), loss=silly_loss, dloss_dparams=dloss_dparams) self.assertTrue(called_loss) self.assertTrue(dloss_dparams.any()) # No longer all zero. dloss_dparams = np.zeros((13, )) mlp.BackpropagationMeanSquaredError(context=context, X=np.array([1, 3, 4]).reshape( (1, 3)), Y_desired=np.eye(3), dloss_dparams=dloss_dparams) self.assertTrue(dloss_dparams.any()) # No longer all zero. Y = np.asfortranarray(np.eye(3)) mlp.BatchOutput(context=context, X=np.array([[0.1, 0.3, 0.4]]), Y=Y) self.assertFalse(np.allclose(Y, np.eye(3))) Y2 = mlp.BatchOutput(context=context, X=np.array([[0.1, 0.3, 0.4]])) np.testing.assert_array_equal(Y, Y2) mlp2 = MultilayerPerceptron(layers=[3, 2, 1], activation_types=[ PerceptronActivationType.kReLU, PerceptronActivationType.kTanh ]) self.assertEqual(mlp2.activation_type(0), PerceptronActivationType.kReLU) self.assertEqual(mlp2.activation_type(1), PerceptronActivationType.kTanh) Y = np.asfortranarray(np.full((1, 3), 2.4)) dYdX = np.asfortranarray(np.full((3, 3), 5.3)) context2 = mlp2.CreateDefaultContext() mlp2.BatchOutput(context=context2, X=np.eye(3), Y=Y, dYdX=dYdX) # The default context sets the weights and biases to zero, so the # output (and gradients) should be zero. np.testing.assert_array_almost_equal(Y, np.zeros((1, 3))) np.testing.assert_array_almost_equal(dYdX, np.zeros((3, 3))) mlp = MultilayerPerceptron(use_sin_cos_for_input=[True, False], remaining_layers=[3, 2], activation_types=[ PerceptronActivationType.kReLU, PerceptronActivationType.kTanh ]) self.assertEqual(mlp.get_input_port().size(), 2) np.testing.assert_array_equal(mlp.layers(), [3, 3, 2])
def test_h_polyhedron(self): hpoly = mut.HPolyhedron(A=self.A, b=self.b) self.assertEqual(hpoly.ambient_dimension(), 3) np.testing.assert_array_equal(hpoly.A(), self.A) np.testing.assert_array_equal(hpoly.b(), self.b) self.assertTrue(hpoly.PointInSet(x=[0, 0, 0], tol=0.0)) self.assertFalse(hpoly.IsBounded()) hpoly.AddPointInSetConstraints(self.prog, self.x) constraints = hpoly.AddPointInNonnegativeScalingConstraints( prog=self.prog, x=self.x, t=self.t) self.assertGreaterEqual(len(constraints), 2) self.assertIsInstance(constraints[0], Binding[Constraint]) constraints = hpoly.AddPointInNonnegativeScalingConstraints( prog=self.prog, A=self.Ay, b=self.by, c=self.cz, d=self.dz, x=self.y, t=self.z) self.assertGreaterEqual(len(constraints), 2) self.assertIsInstance(constraints[0], Binding[Constraint]) with self.assertRaisesRegex(RuntimeError, ".*not implemented yet for HPolyhedron.*"): hpoly.ToShapeWithPose() assert_pickle(self, hpoly, lambda S: np.vstack((S.A(), S.b()))) h_box = mut.HPolyhedron.MakeBox(lb=[-1, -1, -1], ub=[1, 1, 1]) self.assertTrue(h_box.IntersectsWith(hpoly)) h_unit_box = mut.HPolyhedron.MakeUnitBox(dim=3) np.testing.assert_array_equal(h_box.A(), h_unit_box.A()) np.testing.assert_array_equal(h_box.b(), h_unit_box.b()) A_l1 = np.array([[1, 1, 1], [-1, 1, 1], [1, -1, 1], [-1, -1, 1], [1, 1, -1], [-1, 1, -1], [1, -1, -1], [-1, -1, -1]]) b_l1 = np.ones(8) h_l1_ball = mut.HPolyhedron.MakeL1Ball(dim=3) np.testing.assert_array_equal(A_l1, h_l1_ball.A()) np.testing.assert_array_equal(b_l1, h_l1_ball.b()) self.assertIsInstance(h_box.MaximumVolumeInscribedEllipsoid(), mut.Hyperellipsoid) np.testing.assert_array_almost_equal(h_box.ChebyshevCenter(), [0, 0, 0]) h2 = h_box.CartesianProduct(other=h_unit_box) self.assertIsInstance(h2, mut.HPolyhedron) self.assertEqual(h2.ambient_dimension(), 6) h3 = h_box.CartesianPower(n=3) self.assertIsInstance(h3, mut.HPolyhedron) self.assertEqual(h3.ambient_dimension(), 9) h4 = h_box.Intersection(other=h_unit_box) self.assertIsInstance(h4, mut.HPolyhedron) self.assertEqual(h4.ambient_dimension(), 3) h5 = h_box.PontryaginDifference(other=h_unit_box) self.assertIsInstance(h5, mut.HPolyhedron) np.testing.assert_array_equal(h5.A(), h_box.A()) np.testing.assert_array_equal(h5.b(), np.zeros(6)) generator = RandomGenerator() sample = h_box.UniformSample(generator=generator) self.assertEqual(sample.shape, (3, )) self.assertEqual( h_box.UniformSample(generator=generator, previous_sample=sample).shape, (3, )) h_half_box = mut.HPolyhedron.MakeBox(lb=[-0.5, -0.5, -0.5], ub=[0.5, 0.5, 0.5]) self.assertTrue(h_half_box.ContainedIn(other=h_unit_box)) h_half_box2 = h_half_box.Intersection(other=h_unit_box, check_for_redundancy=True) self.assertIsInstance(h_half_box2, mut.HPolyhedron) self.assertEqual(h_half_box2.ambient_dimension(), 3) np.testing.assert_array_almost_equal(h_half_box2.A(), h_half_box.A()) np.testing.assert_array_almost_equal(h_half_box2.b(), h_half_box.b()) # Intersection of 1/2*unit_box and unit_box and reducing the redundant # inequalities should result in the 1/2*unit_box. h_half_box_intersect_unit_box = h_half_box.Intersection( other=h_unit_box, check_for_redundancy=False) # Check that the ReduceInequalities binding works. h_half_box3 = h_half_box_intersect_unit_box.ReduceInequalities()