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_geometry_with_params(self):
builder = DiagramBuilder()
plant = builder.AddSystem(RimlessWheel())
scene_graph = builder.AddSystem(SceneGraph())
geom = RimlessWheelGeometry.AddToBuilder(
builder=builder,
floating_base_state_port=plant.get_floating_base_state_output_port(), # noqa
rimless_wheel_params=RimlessWheelParams(), scene_graph=scene_graph)
# Confirming that the resulting diagram builds.
builder.Build()
self.assertIsInstance(geom, RimlessWheelGeometry)
def test_params(self):
params = RimlessWheelParams()
params.set_mass(1.)
params.set_length(2.)
params.set_gravity(4.)
params.set_number_of_spokes(7)
params.set_slope(.15)
self.assertEqual(params.mass(), 1.)
self.assertEqual(params.length(), 2.)
self.assertEqual(params.gravity(), 4.)
self.assertEqual(params.number_of_spokes(), 7)
self.assertEqual(params.slope(), .15)
self.assertEqual(RimlessWheel.calc_alpha(params=params),
np.pi / params.number_of_spokes())
def test_simulation(self):
# Basic constant-torque rimless_wheel simulation.
rimless_wheel = RimlessWheel()
# Create the simulator.
simulator = Simulator(rimless_wheel)
context = simulator.get_mutable_context()
context.set_accuracy(1e-8)
# Set the initial state.
state = context.get_mutable_continuous_state_vector()
state.set_theta(0.)
state.set_thetadot(4.)
# Simulate (and make sure the state actually changes).
initial_state = state.CopyToVector()
simulator.StepTo(1.0)
self.assertFalse((state.CopyToVector() == initial_state).any())
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)
FindResourceOrThrow("drake/examples/rimless_wheel/RimlessWheel.urdf"),
FloatingBaseType.kRollPitchYaw)
params = RimlessWheelParams()
params.set_slope(args.slope)
R = np.identity(3)
R[0, 0] = math.cos(params.slope())
R[0, 2] = math.sin(params.slope())
R[2, 0] = -math.sin(params.slope())
R[2, 2] = math.cos(params.slope())
X = Isometry3(rotation=R, translation=[0, 0, -5.])
color = np.array([0.9297, 0.7930, 0.6758, 1])
tree.world().AddVisualElement(VisualElement(Box([100., 1., 10.]), X, color))
tree.compile()
builder = DiagramBuilder()
rimless_wheel = builder.AddSystem(RimlessWheel())
visualizer = builder.AddSystem(
PlanarRigidBodyVisualizer(tree,
xlim=[-8., 8.],
ylim=[-2., 3.],
figsize_multiplier=3))
builder.Connect(rimless_wheel.get_output_port(1), visualizer.get_input_port(0))
diagram = builder.Build()
simulator = Simulator(diagram)
simulator.set_target_realtime_rate(1.0)
context = simulator.get_mutable_context()
diagram.Publish(context) # draw once to get the window open
diagram.GetMutableSubsystemContext(
import numpy as np
import matplotlib.pyplot as plt
from pydrake.examples.rimless_wheel import RimlessWheel
from pydrake.all import DirectCollocation, Solve
plant = RimlessWheel()
context = plant.CreateDefaultContext()
params = context.get_numeric_parameter(0)
slope = params.slope()
alpha = np.pi / params.number_of_spokes()
dircol = DirectCollocation(plant,
context,
num_time_samples=15,
minimum_timestep=0.01,
maximum_timestep=0.1,
assume_non_continuous_states_are_fixed=True)
dircol.AddEqualTimeIntervalsConstraints()
dircol.AddConstraintToAllKnotPoints(dircol.state()[0] >= slope - alpha)
dircol.AddConstraintToAllKnotPoints(dircol.state()[0] <= slope + alpha)
dircol.AddConstraint(dircol.initial_state()[0] == slope - alpha)
dircol.AddConstraint(dircol.final_state()[0] == slope + alpha)
dircol.AddConstraint(dircol.initial_state()[1] == dircol.final_state()[1] *
np.cos(2 * alpha))
def test_rimless_wheel(self):
plant = RimlessWheel()
# Confirm the spelling on the output ports.
plant.get_minimal_state_output_port()
plant.get_floating_base_state_output_port()
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())
