From 2fa75adc0c361685554a3b5b7ed6f4cc15223a23 Mon Sep 17 00:00:00 2001 From: Russ Tedrake Date: Sun, 4 Nov 2018 17:24:39 -0500 Subject: [PATCH] Add multibody_position_to_geometry_pose and a geometry_inspector tool with python sliders. --- bindings/pydrake/manipulation/BUILD.bazel | 1 + bindings/pydrake/manipulation/simple_ui.py | 35 +++++-- .../manipulation/test/simple_ui_test.py | 3 +- bindings/pydrake/systems/BUILD.bazel | 1 + bindings/pydrake/systems/rendering_py.cc | 15 +++ .../pydrake/systems/test/rendering_test.py | 24 +++++ examples/manipulation_station/joint_teleop.py | 2 +- manipulation/util/BUILD.bazel | 11 +++ manipulation/util/geometry_inspector.py | 99 +++++++++++++++++++ systems/rendering/BUILD.bazel | 25 +++++ .../multibody_position_to_geometry_pose.cc | 53 ++++++++++ .../multibody_position_to_geometry_pose.h | 76 ++++++++++++++ ...ultibody_position_to_geometry_pose_test.cc | 55 +++++++++++ 13 files changed, 392 insertions(+), 8 deletions(-) create mode 100644 manipulation/util/geometry_inspector.py create mode 100644 systems/rendering/multibody_position_to_geometry_pose.cc create mode 100644 systems/rendering/multibody_position_to_geometry_pose.h create mode 100644 systems/rendering/test/multibody_position_to_geometry_pose_test.cc diff --git a/bindings/pydrake/manipulation/BUILD.bazel b/bindings/pydrake/manipulation/BUILD.bazel index 5f1249c535f9..fb407ca99fb5 100644 --- a/bindings/pydrake/manipulation/BUILD.bazel +++ b/bindings/pydrake/manipulation/BUILD.bazel @@ -10,6 +10,7 @@ load( ) load( "@drake//tools/skylark:drake_py.bzl", + "drake_py_binary", "drake_py_library", "drake_py_unittest", ) diff --git a/bindings/pydrake/manipulation/simple_ui.py b/bindings/pydrake/manipulation/simple_ui.py index 60f7771f78ce..49468ab73fb8 100644 --- a/bindings/pydrake/manipulation/simple_ui.py +++ b/bindings/pydrake/manipulation/simple_ui.py @@ -14,11 +14,13 @@ class JointSliders(VectorSystem): """ Provides a simple tcl/tk gui with one slider per joint of the - MultibodyPlant. + MultibodyPlant. Any positions that are not associated with joints (e.g. + floating-base "mobilizers") are held constant at the default value + obtained from robot.CreateDefaultContext(). @system{ JointSliders, , # no input ports - @output_port{joint positions} } + @output_port{positions} } """ def __init__(self, robot, lower_limit=-10., upper_limit=10., @@ -74,12 +76,18 @@ def _reshape(x, num): self._DeclarePeriodicPublish(update_period_sec, 0.0) self._slider = [] + self._slider_position_start = [] + context = robot.CreateDefaultContext() + state = robot.tree().get_multibody_state_vector(context) + self._default_position = state[:robot.num_positions()] + k = 0 for i in range(0, robot.num_joints()): joint = robot.tree().get_joint(JointIndex(i)) low = joint.lower_limits() upp = joint.upper_limits() for j in range(0, joint.num_positions()): + self._slider_position_start.append(joint.position_start() + j) self._slider.append(tk.Scale(self.window, from_=max(low[j], lower_limit[k]), @@ -94,14 +102,28 @@ def _reshape(x, num): # TODO(russt): Consider resolving constraints in a slider event # callback. - def set(self, q): + def set_position(self, q): + """ + Set all robot positions (corresponding to joint positions and + potentially positions not associated with any joint) to the + values in q. + + Args: + q: a vector whose length is robot.num_positions(). + """ + self._default_position = q + for i in range(len(self._slider)): + self._slider[i].set(q[self._slider_position_start[i]]) + + def set_joint_position(self, q): """ Set the slider positions to the values in q. Args: - q: a vector of length robot.num_positions() + q: a vector whose length is the same as the number of joint + positions (also the number of sliders) for the robot. """ - assert(len(q) == len(self._slider)) + assert(len(q) == len(self._default_position)) for i in range(len(self._slider)): self._slider[i].set(q[i]) @@ -110,8 +132,9 @@ def _DoPublish(self, context, event): self.window.update() def _DoCalcVectorOutput(self, context, unused, unused2, output): + output[:] = self._default_position for i in range(0, len(self._slider)): - output[i] = self._slider[i].get() + output[self._slider_position_start[i]] = self._slider[i].get() class SchunkWsgButtons(LeafSystem): diff --git a/bindings/pydrake/manipulation/test/simple_ui_test.py b/bindings/pydrake/manipulation/test/simple_ui_test.py index d2294862d744..ddd77dd4ea46 100644 --- a/bindings/pydrake/manipulation/test/simple_ui_test.py +++ b/bindings/pydrake/manipulation/test/simple_ui_test.py @@ -29,7 +29,8 @@ def test_joint_slider(self): output = slider.AllocateOutput() q = [.1, .2] - slider.set(q) + slider.set_position(q) + slider.set_joint_position(q) slider.CalcOutput(context, output) np.testing.assert_array_equal(output.get_vector_data(0).get_value(), q) diff --git a/bindings/pydrake/systems/BUILD.bazel b/bindings/pydrake/systems/BUILD.bazel index 214916fbf862..318b9b69d8c7 100644 --- a/bindings/pydrake/systems/BUILD.bazel +++ b/bindings/pydrake/systems/BUILD.bazel @@ -361,6 +361,7 @@ drake_py_unittest( drake_py_unittest( name = "rendering_test", + data = ["//multibody/benchmarks/acrobot:models"], deps = [ ":rendering_py", "//bindings/pydrake/multibody:multibody_tree_py", diff --git a/bindings/pydrake/systems/rendering_py.cc b/bindings/pydrake/systems/rendering_py.cc index 7be27688332c..67a812528609 100644 --- a/bindings/pydrake/systems/rendering_py.cc +++ b/bindings/pydrake/systems/rendering_py.cc @@ -8,6 +8,7 @@ #include "drake/bindings/pydrake/util/eigen_geometry_pybind.h" #include "drake/multibody/multibody_tree/math/spatial_velocity.h" #include "drake/systems/rendering/frame_velocity.h" +#include "drake/systems/rendering/multibody_position_to_geometry_pose.h" #include "drake/systems/rendering/pose_aggregator.h" #include "drake/systems/rendering/pose_vector.h" @@ -105,6 +106,20 @@ PYBIND11_MODULE(rendering, m) { .def("AddBundleInput", &PoseAggregator::AddBundleInput, py_reference_internal, doc.PoseAggregator.AddBundleInput.doc); + py::class_, LeafSystem>(m, + "MultibodyPositionToGeometryPose", doc.MultibodyPositionToGeometryPose + .doc) + .def(py::init&>(), + doc.MultibodyPositionToGeometryPose.ctor.doc_3) + .def("get_input_port", + &MultibodyPositionToGeometryPose::get_input_port, + py_reference_internal, + doc.MultibodyPositionToGeometryPose.get_input_port.doc) + .def("get_output_port", + &MultibodyPositionToGeometryPose::get_output_port, + py_reference_internal, + doc.MultibodyPositionToGeometryPose.get_output_port.doc); + // TODO(eric.cousineau): Add more systems as needed. } diff --git a/bindings/pydrake/systems/test/rendering_test.py b/bindings/pydrake/systems/test/rendering_test.py index 35eebc86307d..6200626e6ad6 100644 --- a/bindings/pydrake/systems/test/rendering_test.py +++ b/bindings/pydrake/systems/test/rendering_test.py @@ -2,6 +2,7 @@ from pydrake.systems.rendering import ( FrameVelocity, + MultibodyPositionToGeometryPose, PoseBundle, PoseAggregator, PoseVector, @@ -11,6 +12,10 @@ import unittest import numpy as np +from pydrake.common import FindResourceOrThrow +from pydrake.geometry import SceneGraph +from pydrake.multibody.multibody_tree.multibody_plant import MultibodyPlant +from pydrake.multibody.multibody_tree.parsing import AddModelFromSdfFile from pydrake.multibody.multibody_tree.math import ( SpatialVelocity, ) @@ -187,3 +192,22 @@ def test_pose_aggregator(self): self.assertTrue(np.allclose(vel_actual.translational(), v)) self.assertTrue( (value.get_pose(2).matrix() == Isometry3(q3, p3).matrix()).all()) + + def testMultibodyPositionToGeometryPose(self): + file_name = FindResourceOrThrow( + "drake/multibody/benchmarks/acrobot/acrobot.sdf") + plant = MultibodyPlant(time_step=0.01) + model_instance = AddModelFromSdfFile( + file_name=file_name, plant=plant) + scene_graph = SceneGraph() + plant.RegisterAsSourceForSceneGraph(scene_graph) + plant.Finalize() + + to_pose = MultibodyPositionToGeometryPose(plant) + + # Check the size of the input. + self.assertEqual(to_pose.get_input_port().size(), 2) + + # Just check the spelling of the output port (size is not meaningful + # for Abstract-valued ports). + to_pose.get_output_port() diff --git a/examples/manipulation_station/joint_teleop.py b/examples/manipulation_station/joint_teleop.py index 6c4ee43a78d9..0b81eeee7740 100644 --- a/examples/manipulation_station/joint_teleop.py +++ b/examples/manipulation_station/joint_teleop.py @@ -96,7 +96,7 @@ # Eval the output port once to read the initial positions of the IIWA. q0 = station.GetOutputPort("iiwa_position_measured").Eval( station_context).get_value() -teleop.set(q0) +teleop.set_position(q0) # This is important to avoid duplicate publishes to the hardware interface: simulator.set_publish_every_time_step(False) diff --git a/manipulation/util/BUILD.bazel b/manipulation/util/BUILD.bazel index 1e3c8b05b68a..5fb98e60af1e 100644 --- a/manipulation/util/BUILD.bazel +++ b/manipulation/util/BUILD.bazel @@ -54,6 +54,17 @@ drake_cc_library( ], ) +drake_py_binary( + name = "geometry_inspector", + srcs = ["geometry_inspector.py"], + add_test_rule = 1, + data = ["//multibody/benchmarks/acrobot:acrobot.sdf"], + test_rule_args = ["$(location //multibody/benchmarks/acrobot:acrobot.sdf) --test --position 0.1 0.2"], # noqa + deps = [ + "//bindings/pydrake", + ], +) + drake_cc_library( name = "robot_state_msg_translator", srcs = ["robot_state_msg_translator.cc"], diff --git a/manipulation/util/geometry_inspector.py b/manipulation/util/geometry_inspector.py new file mode 100644 index 000000000000..e2be1ff6fa27 --- /dev/null +++ b/manipulation/util/geometry_inspector.py @@ -0,0 +1,99 @@ +""" +Simple tool that parses an SDF file from the command line and runs a simple +system which takes joint positions from a JointSlider gui and publishes the +resulting geometry poses to drake_visualizer. + +Make sure to `bazel run //tools:drake_visualizer` before executing this to see +the visualization. + +Example usages (each on one line): +bazel run //bindings/pydrake/multibody:geometry_inspector -- + $HOME/drake/manipulation/models/iiwa_description/sdf/iiwa14_no_collision.sdf + +bazel run geometry_inspector -- + $HOME/drake/multibody/benchmarks/acrobot/acrobot.sdf --position 0.1 0.2 +""" +# TODO(russt): Add support for URDF, too. + +import argparse +import numpy as np + +from pydrake.geometry import ConnectDrakeVisualizer, SceneGraph +from pydrake.manipulation.simple_ui import JointSliders +from pydrake.multibody.multibody_tree.multibody_plant import MultibodyPlant +from pydrake.multibody.multibody_tree.parsing import AddModelFromSdfFile +from pydrake.systems.analysis import Simulator +from pydrake.systems.framework import DiagramBuilder +from pydrake.systems.rendering import MultibodyPositionToGeometryPose + + +parser = argparse.ArgumentParser() +parser.add_argument( + "filename", + type=str, + help="Full path to an SDF file.") +position_group = parser.add_mutually_exclusive_group() +position_group.add_argument( + "--position", + type=float, + nargs="+", + help="A list of positions which must be the same length as the number of " + "positions in the sdf model. Note that most models have a " + "floating-base joint by default (unless the sdf explicitly welds the " + "base to the world, and so have 7 positions corresponding to the " + "quaternion representation of that floating-base position.", + default=[]) +position_group.add_argument( + "--joint_position", + type=float, + nargs="+", + help="A list of positions which must be the same length as the number of " + "positions ASSOCIATED WITH JOINTS in the sdf model. This does not " + "include, e.g., floating-base coordinates, which will be assigned a " + "default value.", + default=[]) +parser.add_argument( + "--test", + action='store_true', + help="Disable opening the gui window for testing." +) +# TODO(russt): Add option to weld the base to the world pending the +# availability of GetUniqueBaseBody requested in #9747. +args = parser.parse_args() + +builder = DiagramBuilder() +scene_graph = builder.AddSystem(SceneGraph()) + +# Construct a MultibodyPlant and load the SDF into it. +plant = MultibodyPlant() +plant.RegisterAsSourceForSceneGraph(scene_graph) +AddModelFromSdfFile(args.filename, plant) +plant.Finalize(scene_graph) + +# Add sliders to set positions of the joints. +sliders = builder.AddSystem(JointSliders(robot=plant)) +to_pose = builder.AddSystem(MultibodyPositionToGeometryPose(plant)) +builder.Connect(sliders.get_output_port(0), to_pose.get_input_port()) +builder.Connect(to_pose.get_output_port(), scene_graph.get_source_pose_port( + plant.get_source_id())) + +# Connect this to drake_visualizer. +ConnectDrakeVisualizer(builder=builder, scene_graph=scene_graph) + +if len(args.position): + sliders.set_position(args.position) +elif len(args.joint_position): + sliders.set_joint_position(args.joint_position) + +# Make the diagram and run it. +diagram = builder.Build() +simulator = Simulator(diagram) + +simulator.set_publish_every_time_step(False) + +if args.test: + sliders.window.withdraw() + simulator.StepTo(0.1) +else: + simulator.set_target_realtime_rate(1.0) + simulator.StepTo(np.inf) diff --git a/systems/rendering/BUILD.bazel b/systems/rendering/BUILD.bazel index 752529a86b79..6442794f7ab5 100644 --- a/systems/rendering/BUILD.bazel +++ b/systems/rendering/BUILD.bazel @@ -15,6 +15,7 @@ drake_cc_package_library( deps = [ ":drake_visualizer_client", ":frame_velocity", + ":multibody_position_to_geometry_pose", ":pose_aggregator", ":pose_bundle", ":pose_bundle_to_draw_message", @@ -106,6 +107,18 @@ drake_cc_library( ], ) +drake_cc_library( + name = "multibody_position_to_geometry_pose", + srcs = ["multibody_position_to_geometry_pose.cc"], + hdrs = ["multibody_position_to_geometry_pose.h"], + deps = [ + "//common:pointer_cast", + "//geometry:frame_kinematics", + "//multibody/multibody_tree/multibody_plant", + "//systems/framework:leaf_system", + ], +) + drake_cc_library( name = "render_pose_to_geometry_pose", srcs = ["render_pose_to_geometry_pose.cc"], @@ -169,6 +182,18 @@ drake_cc_googletest( ], ) +drake_cc_googletest( + name = "multibody_position_to_geometry_pose_test", + data = ["//manipulation/models/iiwa_description:models"], + deps = [ + ":multibody_position_to_geometry_pose", + "//common:find_resource", + "//common/test_utilities", + "//multibody/multibody_tree/parsing:multibody_plant_sdf_parser", + "//systems/framework/test_utilities", + ], +) + drake_cc_googletest( name = "render_pose_to_geometry_pose_test", deps = [ diff --git a/systems/rendering/multibody_position_to_geometry_pose.cc b/systems/rendering/multibody_position_to_geometry_pose.cc new file mode 100644 index 000000000000..1533d6f703c1 --- /dev/null +++ b/systems/rendering/multibody_position_to_geometry_pose.cc @@ -0,0 +1,53 @@ +#include "drake/systems/rendering/multibody_position_to_geometry_pose.h" + +#include + +#include "drake/common/drake_assert.h" +#include "drake/common/pointer_cast.h" + +namespace drake { +namespace systems { +namespace rendering { + +template +MultibodyPositionToGeometryPose::MultibodyPositionToGeometryPose( + const multibody::multibody_plant::MultibodyPlant& plant) + : plant_(plant), + plant_context_( + dynamic_pointer_cast_or_throw>( + plant.CreateDefaultContext())) { + DRAKE_DEMAND(plant.is_finalized()); + DRAKE_DEMAND(plant.geometry_source_is_registered()); + + this->DeclareInputPort("position", kVectorValued, plant.num_positions()); + this->DeclareAbstractOutputPort( + "geometry_pose", + [this]() { + return this->plant_.get_geometry_poses_output_port().Allocate(); + }, + [this](const Context& context, AbstractValue* output) { + return this->CalcGeometryPose(context, output); + }); + + // Fix all input ports in the Context to avoid leaving them unassigned. + // They should not impact the output values. + plant_.AllocateFixedInputs(plant_context_.get()); +} + +template +void MultibodyPositionToGeometryPose::CalcGeometryPose( + const Context& context, AbstractValue* output) const { + // Set the positions in the owned (mutable) context so that we can ask the + // MultibodyPlant to compute the outputs. + plant_context_->get_mutable_positions() = + this->EvalEigenVectorInput(context, 0); + + // Evaluate the plant's output port. + plant_.get_geometry_poses_output_port().Calc(*plant_context_, output); +} + +template class MultibodyPositionToGeometryPose; + +} // namespace rendering +} // namespace systems +} // namespace drake diff --git a/systems/rendering/multibody_position_to_geometry_pose.h b/systems/rendering/multibody_position_to_geometry_pose.h new file mode 100644 index 000000000000..8cab1716a500 --- /dev/null +++ b/systems/rendering/multibody_position_to_geometry_pose.h @@ -0,0 +1,76 @@ +#pragma once + +#include + +#include "drake/multibody/multibody_tree/multibody_plant/multibody_plant.h" +#include "drake/systems/framework/leaf_system.h" + +namespace drake { +namespace systems { +namespace rendering { + +/** + * A direct-feedthrough system that converts a vector of joint positions + * directly to a geometry::FramePoseVector to behave like a + * MultibodyPlant::get_geometry_pose_output_port(). + * + * @system{ MultibodyPositionToGeometryPose, + * @input_port{position}, + * @output_port{geometry_pose} + * } + * + * The position input must be a vector whose length matches the number of + * positions in the MultibodyPlant. + * + * @tparam T The vector element type, which must be a valid Eigen scalar. + * + * Instantiated templates for the following kinds of T's are provided: + * + * - double + * + * @ingroup visualization + * } + */ +template +class MultibodyPositionToGeometryPose final : public LeafSystem { + public: + DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(MultibodyPositionToGeometryPose) + + /*** + * The %MultibodyPositionToGeometryPose holds an internal, non-owned + * reference to the MultibodyPlant object so you must ensure that @p plant + * has a longer lifetime than `this` %MultibodyPositionToGeometryPose system. + * + * @pre @p plant must be registered with a SceneGraph. + * @pre @p plant must be finalized. + */ + explicit MultibodyPositionToGeometryPose( + const multibody::multibody_plant::MultibodyPlant& plant); + + ~MultibodyPositionToGeometryPose() override = default; + + /** Returns the multibody position input port. */ + const InputPort& get_input_port() const { + return LeafSystem::get_input_port(0); + } + + /** Returns the geometry::FramePoseVector output port. */ + const OutputPort& get_output_port() const { + return LeafSystem::get_output_port(0); + } + + private: + optional DoHasDirectFeedthrough(int, int) const final { return true; } + + void CalcGeometryPose(const Context& context, AbstractValue* poses) const; + + const multibody::multibody_plant::MultibodyPlant& plant_; + + // This is a context of the plant_ system, which is only owned here to avoid + // runtime allocation. It contains no relevant state. + mutable std::unique_ptr> plant_context_; +}; + +} // namespace rendering +} // namespace systems +} // namespace drake diff --git a/systems/rendering/test/multibody_position_to_geometry_pose_test.cc b/systems/rendering/test/multibody_position_to_geometry_pose_test.cc new file mode 100644 index 000000000000..0a1b55e8f717 --- /dev/null +++ b/systems/rendering/test/multibody_position_to_geometry_pose_test.cc @@ -0,0 +1,55 @@ +#include "drake/systems/rendering/multibody_position_to_geometry_pose.h" + +#include + +#include "drake/common/find_resource.h" +#include "drake/common/test_utilities/eigen_matrix_compare.h" +#include "drake/multibody/multibody_tree/multibody_plant/multibody_plant.h" +#include "drake/multibody/multibody_tree/parsing/multibody_plant_sdf_parser.h" + +namespace drake { +namespace systems { +namespace rendering { +namespace { + +GTEST_TEST(MultibodyPositionToGeometryPoseTest, InputOutput) { + multibody::multibody_plant::MultibodyPlant mbp; + geometry::SceneGraph scene_graph; + mbp.RegisterAsSourceForSceneGraph(&scene_graph); + multibody::parsing::AddModelFromSdfFile( + FindResourceOrThrow("drake/manipulation/models/iiwa_description/iiwa7" + "/iiwa7_no_collision.sdf"), + &mbp); + mbp.Finalize(&scene_graph); + + const MultibodyPositionToGeometryPose dut(mbp); + + EXPECT_EQ(dut.get_input_port().get_index(), 0); + EXPECT_EQ(dut.get_output_port().get_index(), 0); + EXPECT_TRUE(dut.HasAnyDirectFeedthrough()); + + auto context = dut.CreateDefaultContext(); + + const Eigen::VectorXd position = + Eigen::VectorXd::LinSpaced(mbp.num_positions(), 0.123, 0.456); + context->FixInputPort(dut.get_input_port().get_index(), position); + + const auto& output = + dut.get_output_port().Eval>(*context); + EXPECT_EQ(output.source_id(), mbp.get_source_id()); + for (multibody::BodyIndex i(0); i < mbp.num_bodies(); i++) { + if (i == mbp.world_body().index()) { + // The world geometry will not appear in the poses. + continue; + } + const optional id = mbp.GetBodyFrameIdIfExists(i); + EXPECT_TRUE(id.has_value()); + EXPECT_TRUE(output.has_id(id.value())); + } + EXPECT_EQ(output.size(), mbp.num_bodies()-1); +} + +} // namespace +} // namespace rendering +} // namespace systems +} // namespace drake