def test_geometry(self):
builder = DiagramBuilder()
plant = builder.AddSystem(CompassGait())
scene_graph = builder.AddSystem(SceneGraph())
geom = CompassGaitGeometry.AddToBuilder(
builder=builder,
floating_base_state_port=plant.get_floating_base_state_output_port(
), # noqa
scene_graph=scene_graph)
# Confirming that the resulting diagram builds.
builder.Build()
self.assertIsInstance(geom, CompassGaitGeometry)
def test_simulation(self):
# Basic compass_gait simulation.
compass_gait = CompassGait()
# Create the simulator.
simulator = Simulator(compass_gait)
context = simulator.get_mutable_context()
context.SetAccuracy(1e-8)
# Zero the input
compass_gait.get_input_port(0).FixValue(context, 0.0)
# Set the initial state.
state = context.get_mutable_continuous_state_vector()
state.set_stance(0.)
state.set_swing(0.)
state.set_stancedot(0.4)
state.set_swingdot(-2.0)
# Simulate (and make sure the state actually changes).
initial_state = state.CopyToVector()
simulator.AdvanceTo(1.0)
self.assertFalse((state.CopyToVector() == initial_state).any())
tree = RigidBodyTree(
FindResourceOrThrow("drake/examples/compass_gait/CompassGait.urdf"),
FloatingBaseType.kRollPitchYaw)
params = CompassGaitParams()
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()
compass_gait = builder.AddSystem(CompassGait())
parser = argparse.ArgumentParser()
parser.add_argument("-T",
"--duration",
type=float,
help="Duration to run sim.",
default=10.0)
args = parser.parse_args()
visualizer = builder.AddSystem(
PlanarRigidBodyVisualizer(tree,
xlim=[-1., 5.],
ylim=[-1., 2.],
figsize_multiplier=2))
builder.Connect(compass_gait.get_output_port(1), visualizer.get_input_port(0))
def test_compass_gait(self):
plant = CompassGait()
# Confirm the spelling on the output ports.
plant.get_minimal_state_output_port()
plant.get_floating_base_state_output_port()
