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()
