Skip to content

Commit

Permalink
Purge RigidBody* from examples/compass_gait (RobotLocomotion#12618)
Browse files Browse the repository at this point in the history
Adds CompassGaitGeometry (which generalizes the previous visualization by supporting the parameterized geometry)
Removes CompassGait.urdf (and .xacro), but git remembers
  • Loading branch information
RussTedrake authored Jan 22, 2020
1 parent cdc37f4 commit 489048b
Show file tree
Hide file tree
Showing 12 changed files with 363 additions and 262 deletions.
35 changes: 34 additions & 1 deletion bindings/pydrake/examples/compass_gait_py.cc
Original file line number Diff line number Diff line change
Expand Up @@ -4,6 +4,7 @@
#include "drake/bindings/pydrake/documentation_pybind.h"
#include "drake/bindings/pydrake/pydrake_pybind.h"
#include "drake/examples/compass_gait/compass_gait.h"
#include "drake/examples/compass_gait/compass_gait_geometry.h"
#include "drake/examples/compass_gait/gen/compass_gait_continuous_state.h"
#include "drake/examples/compass_gait/gen/compass_gait_params.h"

Expand All @@ -28,7 +29,14 @@ PYBIND11_MODULE(compass_gait, m) {

py::class_<CompassGait<T>, LeafSystem<T>>(
m, "CompassGait", doc.CompassGait.doc)
.def(py::init<>(), doc.CompassGait.ctor.doc);
.def(py::init<>(), doc.CompassGait.ctor.doc)
.def("get_minimal_state_output_port",
&CompassGait<T>::get_minimal_state_output_port, py_reference_internal,
doc.CompassGait.get_minimal_state_output_port.doc)
.def("get_floating_base_state_output_port",
&CompassGait<T>::get_floating_base_state_output_port,
py_reference_internal,
doc.CompassGait.get_floating_base_state_output_port.doc);

// TODO(russt): Remove custom bindings once #8096 is resolved.
py::class_<CompassGaitParams<T>, BasicVector<T>>(
Expand Down Expand Up @@ -79,6 +87,31 @@ PYBIND11_MODULE(compass_gait, m) {
doc.CompassGaitContinuousState.set_stancedot.doc)
.def("set_swingdot", &CompassGaitContinuousState<T>::set_swingdot,
doc.CompassGaitContinuousState.set_swingdot.doc);

py::class_<CompassGaitGeometry, LeafSystem<double>>(
m, "CompassGaitGeometry", doc.CompassGaitGeometry.doc)
.def_static("AddToBuilder",
py::overload_cast<systems::DiagramBuilder<double>*,
const systems::OutputPort<double>&,
const CompassGaitParams<double>&, geometry::SceneGraph<double>*>(
&CompassGaitGeometry::AddToBuilder),
py::arg("builder"), py::arg("floating_base_state_port"),
py::arg("compass_gait_params"), py::arg("scene_graph"),
// Keep alive, ownership: `return` keeps `builder` alive.
py::keep_alive<0, 1>(),
// See #11531 for why `py_reference` is needed.
py_reference, doc.CompassGaitGeometry.AddToBuilder.doc_4args)
.def_static("AddToBuilder",
py::overload_cast<systems::DiagramBuilder<double>*,
const systems::OutputPort<double>&,
geometry::SceneGraph<double>*>(
&CompassGaitGeometry::AddToBuilder),
py::arg("builder"), py::arg("floating_base_state_port"),
py::arg("scene_graph"),
// Keep alive, ownership: `return` keeps `builder` alive.
py::keep_alive<0, 1>(),
// See #11531 for why `py_reference` is needed.
py_reference, doc.CompassGaitGeometry.AddToBuilder.doc_3args);
}

} // namespace pydrake
Expand Down
41 changes: 36 additions & 5 deletions bindings/pydrake/examples/test/compass_gait_test.py
Original file line number Diff line number Diff line change
@@ -1,14 +1,21 @@
import unittest

from pydrake.examples.compass_gait import (
CompassGaitParams, CompassGait, CompassGaitContinuousState
)
from pydrake.systems.analysis import (
Simulator
)
CompassGait, CompassGaitContinuousState,
CompassGaitGeometry, CompassGaitParams
)
from pydrake.geometry import SceneGraph
from pydrake.systems.analysis import Simulator
from pydrake.systems.framework import DiagramBuilder


class TestCompassGait(unittest.TestCase):
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()

def test_params(self):
params = CompassGaitParams()
params.set_mass_hip(1.)
Expand All @@ -35,6 +42,30 @@ def test_state(self):
self.assertEqual(state.stancedot(), 3.)
self.assertEqual(state.swingdot(), 4.)

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_geometry_with_params(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
compass_gait_params=CompassGaitParams(), 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()
Expand Down
6 changes: 2 additions & 4 deletions bindings/pydrake/examples/test/rimless_wheel_test.py
Original file line number Diff line number Diff line change
Expand Up @@ -42,8 +42,7 @@ def test_geometry(self):
scene_graph = builder.AddSystem(SceneGraph())
geom = RimlessWheelGeometry.AddToBuilder(
builder=builder,
floating_base_state_port=plant
.get_floating_base_state_output_port(),
floating_base_state_port=plant.get_floating_base_state_output_port(), # noqa
scene_graph=scene_graph)
# Confirming that the resulting diagram builds.
builder.Build()
Expand All @@ -55,8 +54,7 @@ def test_geometry_with_params(self):
scene_graph = builder.AddSystem(SceneGraph())
geom = RimlessWheelGeometry.AddToBuilder(
builder=builder,
floating_base_state_port=plant
.get_floating_base_state_output_port(),
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()
Expand Down
33 changes: 25 additions & 8 deletions examples/compass_gait/BUILD.bazel
Original file line number Diff line number Diff line change
Expand Up @@ -39,20 +39,30 @@ drake_cc_library(
],
)

drake_cc_library(
name = "compass_gait_geometry",
srcs = ["compass_gait_geometry.cc"],
hdrs = ["compass_gait_geometry.h"],
visibility = ["//visibility:public"],
deps = [
":compass_gait",
"//geometry:geometry_roles",
"//geometry:scene_graph",
"//math:geometric_transform",
"//systems/framework:diagram_builder",
"//systems/framework:leaf_system",
],
)

drake_cc_binary(
name = "simulate",
srcs = ["simulate.cc"],
add_test_rule = 1,
data = ["CompassGait.urdf"],
test_rule_args = ["--target_realtime_rate=0.0"],
deps = [
":compass_gait",
"//attic/multibody:rigid_body_tree",
"//attic/multibody/parsers",
"//attic/multibody/rigid_body_plant:drake_visualizer",
"//common:find_resource",
"//lcm",
"//math:geometric_transform",
":compass_gait_geometry",
"//geometry:geometry_visualization",
"//systems/analysis:simulator",
"//systems/framework:diagram_builder",
"@gflags",
Expand All @@ -61,7 +71,6 @@ drake_cc_binary(

drake_cc_googletest(
name = "compass_gait_test",
data = ["CompassGait.urdf"],
deps = [
":compass_gait",
"//common/test_utilities:eigen_matrix_compare",
Expand All @@ -70,6 +79,14 @@ drake_cc_googletest(
],
)

drake_cc_googletest(
name = "compass_gait_geometry_test",
deps = [
":compass_gait",
":compass_gait_geometry",
],
)

install_data()

add_lint_tests()
112 changes: 0 additions & 112 deletions examples/compass_gait/CompassGait.urdf

This file was deleted.

90 changes: 0 additions & 90 deletions examples/compass_gait/CompassGait.xacro

This file was deleted.

Loading

0 comments on commit 489048b

Please sign in to comment.