SimDiagramBuilder, WorldSimTreeBuilder, and RobotPlanInterpolator rel…

…ocated (RobotLocomotion#6864)

* SimDiagramBuilder, WorldSimTreeBuilder, and RobotPlanInterpolator relocated to drake/manipulation

drake/examples/kuka_iiwa_arm/BUILD

@@ -22,6 +22,7 @@ drake_cc_library(
     hdrs = [
         "iiwa_common.h",
     ],
+    visibility = ["//visibility:public"],
     deps = [
         "//drake/common:find_resource",
         "//drake/common/trajectories:piecewise_polynomial_trajectory",
@@ -59,32 +60,6 @@ drake_cc_library(
     ],
 )
 
-drake_cc_library(
-    name = "robot_plan_interpolator",
-    srcs = ["robot_plan_interpolator.cc"],
-    hdrs = ["robot_plan_interpolator.h"],
-    deps = [
-        "//drake/common/trajectories:piecewise_polynomial",
-        "//drake/multibody:rigid_body_tree",
-        "//drake/multibody/parsers",
-        "//drake/systems/framework:leaf_system",
-        "@lcmtypes_bot2_core",
-        "@lcmtypes_robotlocomotion",
-    ],
-)
-
-drake_cc_library(
-    name = "sim_diagram_builder",
-    srcs = ["sim_diagram_builder.cc"],
-    hdrs = ["sim_diagram_builder.h"],
-    deps = [
-        "//drake/multibody/rigid_body_plant",
-        "//drake/multibody/rigid_body_plant:drake_visualizer",
-        "//drake/systems/controllers:state_feedback_controller_interface",
-        "//drake/systems/framework",
-    ],
-)
-
 drake_cc_binary(
     name = "iiwa_controller",
     srcs = ["iiwa_controller.cc"],
@@ -95,10 +70,10 @@ drake_cc_binary(
     deps = [
         ":iiwa_common",
         ":iiwa_lcm",
-        ":robot_plan_interpolator",
         "//drake/common:find_resource",
         "//drake/common:text_logging_gflags",
         "//drake/lcm",
+        "//drake/manipulation/planner:robot_plan_interpolator",
         "//drake/systems/analysis:simulator",
         "//drake/systems/lcm",
         "//drake/systems/lcm:lcm_driven_loop",
@@ -124,11 +99,11 @@ drake_cc_binary(
         ":iiwa_lcm",
         ":oracular_state_estimator",
         "//drake/examples/kuka_iiwa_arm/iiwa_world:iiwa_wsg_diagram_factory",
-        "//drake/examples/kuka_iiwa_arm/iiwa_world:world_sim_tree_builder",
         "//drake/lcm",
         "//drake/manipulation/schunk_wsg:schunk_wsg_constants",
         "//drake/manipulation/schunk_wsg:schunk_wsg_controller",
         "//drake/manipulation/schunk_wsg:schunk_wsg_lcm",
+        "//drake/manipulation/util:world_sim_tree_builder",
         "//drake/multibody/rigid_body_plant",
         "//drake/systems/analysis",
         "//drake/systems/controllers:inverse_dynamics_controller",
@@ -152,10 +127,10 @@ drake_cc_binary(
     deps = [
         ":iiwa_common",
         ":iiwa_lcm",
-        ":sim_diagram_builder",
         "//drake/common:find_resource",
-        "//drake/examples/kuka_iiwa_arm/iiwa_world:world_sim_tree_builder",
         "//drake/lcm",
+        "//drake/manipulation/util:sim_diagram_builder",
+        "//drake/manipulation/util:world_sim_tree_builder",
         "//drake/multibody/rigid_body_plant",
         "//drake/multibody/rigid_body_plant:frame_visualizer",
         "//drake/systems/analysis:simulator",
@@ -192,41 +167,6 @@ drake_cc_googletest(
     ],
 )
 
-drake_cc_googletest(
-    name = "robot_plan_interpolator_test",
-    data = [
-        ":models",
-        "//drake/manipulation/models/iiwa_description:models",
-    ],
-    deps = [
-        ":iiwa_common",
-        ":robot_plan_interpolator",
-        "//drake/common:find_resource",
-        "//drake/systems/framework",
-    ],
-)
-
-drake_cc_googletest(
-    name = "sim_diagram_builder_test",
-    data = [
-        ":models",
-        "//drake/manipulation/models/iiwa_description:models",
-    ],
-    deps = [
-        ":iiwa_common",
-        ":sim_diagram_builder",
-        "//drake/common:eigen_matrix_compare",
-        "//drake/examples/kuka_iiwa_arm/iiwa_world:world_sim_tree_builder",
-        "//drake/lcm",
-        "//drake/multibody/rigid_body_plant",
-        "//drake/systems/analysis",
-        "//drake/systems/controllers:inverse_dynamics_controller",
-        "//drake/systems/controllers:pid_controller",
-        "//drake/systems/framework",
-        "//drake/systems/primitives:constant_vector_source",
-    ],
-)
-
 drake_cc_googletest(
     name = "optitrack_test",
     deps = [

drake/examples/kuka_iiwa_arm/CMakeLists.txt

@@ -1,7 +1,7 @@
 if(lcm_FOUND)
   # Defines a shared library for use by KUKA iiwa demos based on System 2.0.
   add_library_with_exports(LIB_NAME drakeKukaIiwaArmCommon SOURCE_FILES
-      iiwa_common.cc iiwa_lcm.cc robot_plan_interpolator.cc)
+      iiwa_common.cc iiwa_lcm.cc)
   target_link_libraries(drakeKukaIiwaArmCommon
       drakeIK
       drakeLcmSystem
@@ -11,7 +11,7 @@ if(lcm_FOUND)
       Eigen3::Eigen)
   drake_install_libraries(drakeKukaIiwaArmCommon)
   drake_install_headers(
-      iiwa_common.h iiwa_lcm.h robot_plan_interpolator.h)
+      iiwa_common.h iiwa_lcm.h)
   drake_install_pkg_config_file(drake-kuka-iiwa-arm-common
       TARGET drakeKukaIiwaArmCommon
       LIBS -ldrakeKukaIiwaArmCommon

drake/examples/kuka_iiwa_arm/controlled_kuka/BUILD

@@ -21,8 +21,8 @@ drake_cc_binary(
     deps = [
         "//drake/common:find_resource",
         "//drake/examples/kuka_iiwa_arm:iiwa_common",
-        "//drake/examples/kuka_iiwa_arm:sim_diagram_builder",
         "//drake/lcm",
+        "//drake/manipulation/util:sim_diagram_builder",
         "//drake/systems/analysis:simulator",
         "//drake/systems/controllers:inverse_dynamics_controller",
         "//drake/systems/primitives:trajectory_source",

drake/examples/kuka_iiwa_arm/controlled_kuka/controlled_kuka_demo.cc

@@ -15,8 +15,8 @@
 #include "drake/common/find_resource.h"
 #include "drake/common/trajectories/piecewise_polynomial_trajectory.h"
 #include "drake/examples/kuka_iiwa_arm/iiwa_common.h"
-#include "drake/examples/kuka_iiwa_arm/sim_diagram_builder.h"
 #include "drake/lcm/drake_lcm.h"
+#include "drake/manipulation/util/sim_diagram_builder.h"
 #include "drake/math/roll_pitch_yaw.h"
 #include "drake/multibody/ik_options.h"
 #include "drake/multibody/joints/floating_base_types.h"
@@ -42,6 +42,7 @@ namespace drake {
 namespace examples {
 namespace kuka_iiwa_arm {
 namespace {
+using manipulation::util::SimDiagramBuilder;
 
 const char kUrdfPath[] =
     "drake/manipulation/models/iiwa_description/urdf/"

drake/examples/kuka_iiwa_arm/dev/monolithic_pick_and_place/BUILD

@@ -40,12 +40,12 @@ drake_cc_binary(
         "//drake/common:text_logging_gflags",
         "//drake/examples/kuka_iiwa_arm:iiwa_common",
         "//drake/examples/kuka_iiwa_arm:iiwa_lcm",
-        "//drake/examples/kuka_iiwa_arm:robot_plan_interpolator",
         "//drake/examples/kuka_iiwa_arm/iiwa_world:iiwa_wsg_diagram_factory",
-        "//drake/examples/kuka_iiwa_arm/iiwa_world:world_sim_tree_builder",
         "//drake/lcm",
         "//drake/lcmtypes:iiwa",
+        "//drake/manipulation/planner:robot_plan_interpolator",
         "//drake/manipulation/schunk_wsg:schunk_wsg_controller",
+        "//drake/manipulation/util:world_sim_tree_builder",
         "//drake/multibody/rigid_body_plant:contact_results_to_lcm",
         "//drake/systems/analysis:simulator",
         "@com_github_gflags_gflags//:gflags",

drake/examples/kuka_iiwa_arm/dev/monolithic_pick_and_place/monolithic_pick_and_place_demo.cc

@@ -11,13 +11,14 @@
 #include "drake/examples/kuka_iiwa_arm/dev/monolithic_pick_and_place/state_machine_system.h"
 #include "drake/examples/kuka_iiwa_arm/iiwa_common.h"
 #include "drake/examples/kuka_iiwa_arm/iiwa_world/iiwa_wsg_diagram_factory.h"
-#include "drake/examples/kuka_iiwa_arm/robot_plan_interpolator.h"
 #include "drake/lcm/drake_lcm.h"
 #include "drake/lcmt_contact_results_for_viz.hpp"
 #include "drake/lcmt_schunk_wsg_status.hpp"
 #include "drake/lcmtypes/drake/lcmt_schunk_wsg_command.hpp"
+#include "drake/manipulation/planner/robot_plan_interpolator.h"
 #include "drake/manipulation/schunk_wsg/schunk_wsg_controller.h"
 #include "drake/manipulation/schunk_wsg/schunk_wsg_lcm.h"
+#include "drake/manipulation/util/world_sim_tree_builder.h"
 #include "drake/math/rotation_matrix.h"
 #include "drake/multibody/parsers/urdf_parser.h"
 #include "drake/multibody/rigid_body_plant/contact_results_to_lcm.h"
@@ -35,24 +36,26 @@ DEFINE_double(orientation, 2 * M_PI, "Yaw angle of the box.");
 DEFINE_int32(start_position, 1, "Position index to start from");
 DEFINE_int32(end_position, 2, "Position index to end at");
 DEFINE_double(dt, 1e-3, "Integration step size");
-DEFINE_double(realtime_rate, 0.0,
-              "Rate at which to run the simulation, relative to realtime");
+DEFINE_double(realtime_rate, 0.0, "Rate at which to run the simulation, "
+    "relative to realtime");
 DEFINE_bool(quick, false, "Run only a brief simulation and return success "
-            "without executing the entire task");
+    "without executing the entire task");
 
 using robotlocomotion::robot_plan_t;
 
 namespace drake {
+namespace examples {
+namespace kuka_iiwa_arm {
+namespace monolithic_pick_and_place {
+namespace {
 using manipulation::schunk_wsg::SchunkWsgController;
 using manipulation::schunk_wsg::SchunkWsgStatusSender;
 using systems::RigidBodyPlant;
 using systems::RungeKutta2Integrator;
 using systems::Simulator;
-
-namespace examples {
-namespace kuka_iiwa_arm {
-namespace monolithic_pick_and_place {
-namespace {
+using manipulation::util::ModelInstanceInfo;
+using manipulation::planner::RobotPlanInterpolator;
+using manipulation::util::WorldSimTreeBuilder;
 
 const char kIiwaUrdf[] =
     "drake/manipulation/models/iiwa_description/urdf/"
@@ -142,8 +145,7 @@ std::unique_ptr<systems::RigidBodyPlant<double>> BuildCombinedPlant(
   *box_instance = tree_builder->get_model_info_for_instance(box_id);
 
   int wsg_id = tree_builder->AddModelInstanceToFrame(
-      "wsg", Eigen::Vector3d::Zero(), Eigen::Vector3d::Zero(),
-      tree_builder->tree().findFrame("iiwa_frame_ee"),
+      "wsg", tree_builder->tree().findFrame("iiwa_frame_ee"),
       drake::multibody::joints::kFixed);
   *wsg_instance = tree_builder->get_model_info_for_instance(wsg_id);
 

drake/examples/kuka_iiwa_arm/iiwa_controller.cc

@@ -11,10 +11,10 @@
 #include "drake/common/text_logging.h"
 #include "drake/common/text_logging_gflags.h"
 #include "drake/examples/kuka_iiwa_arm/iiwa_lcm.h"
-#include "drake/examples/kuka_iiwa_arm/robot_plan_interpolator.h"
 #include "drake/lcm/drake_lcm.h"
 #include "drake/lcmt_iiwa_command.hpp"
 #include "drake/lcmt_iiwa_status.hpp"
+#include "drake/manipulation/planner/robot_plan_interpolator.h"
 #include "drake/systems/analysis/simulator.h"
 #include "drake/systems/framework/context.h"
 #include "drake/systems/framework/diagram.h"
@@ -32,8 +32,10 @@ namespace drake {
 namespace examples {
 namespace kuka_iiwa_arm {
 namespace {
+using manipulation::planner::RobotPlanInterpolator;
 
-const char* const kIiwaUrdf = "drake/manipulation/models/iiwa_description/urdf/"
+const char* const kIiwaUrdf =
+    "drake/manipulation/models/iiwa_description/urdf/"
     "iiwa14_polytope_collision.urdf";
 const char* const kLcmStatusChannel = "IIWA_STATUS";
 const char* const kLcmCommandChannel = "IIWA_COMMAND";
@@ -50,10 +52,9 @@ int DoMain() {
           kLcmPlanChannel, &lcm));
   plan_sub->set_name("plan_sub");
 
-  const std::string urdf = (!FLAGS_urdf.empty() ? FLAGS_urdf :
-                            FindResourceOrThrow(kIiwaUrdf));
-  auto plan_source =
-      builder.AddSystem<RobotPlanInterpolator>(urdf);
+  const std::string urdf =
+      (!FLAGS_urdf.empty() ? FLAGS_urdf : FindResourceOrThrow(kIiwaUrdf));
+  auto plan_source = builder.AddSystem<RobotPlanInterpolator>(urdf);
   plan_source->set_name("plan_source");
   const int num_joints = plan_source->tree().get_num_positions();
 

drake/examples/kuka_iiwa_arm/iiwa_world/BUILD

@@ -8,21 +8,6 @@ load(
 )
 load("//tools:lint.bzl", "add_lint_tests")
 
-drake_cc_library(
-    name = "world_sim_tree_builder",
-    srcs = ["world_sim_tree_builder.cc"],
-    hdrs = ["world_sim_tree_builder.h"],
-    visibility = ["//drake/examples/kuka_iiwa_arm:__subpackages__"],
-    deps = [
-        "//drake/common:find_resource",
-        "//drake/common:unused",
-        "//drake/multibody:rigid_body_tree",
-        "//drake/multibody:rigid_body_tree_construction",
-        "//drake/multibody/parsers",
-        "//drake/multibody/rigid_body_plant:drake_visualizer",
-    ],
-)
-
 drake_cc_library(
     name = "iiwa_wsg_diagram_factory",
     srcs = [
@@ -33,11 +18,11 @@ drake_cc_library(
     ],
     visibility = ["//drake/examples/kuka_iiwa_arm:__subpackages__"],
     deps = [
-        ":world_sim_tree_builder",
         "//drake/examples/kuka_iiwa_arm:iiwa_common",
         "//drake/examples/kuka_iiwa_arm:oracular_state_estimator",
-        "//drake/examples/kuka_iiwa_arm:sim_diagram_builder",
         "//drake/manipulation/schunk_wsg:schunk_wsg_constants",
+        "//drake/manipulation/util:sim_diagram_builder",
+        "//drake/manipulation/util:world_sim_tree_builder",
         "//drake/multibody/rigid_body_plant",
         "//drake/systems/controllers:inverse_dynamics_controller",
         "//drake/systems/controllers:pid_controller",
@@ -57,11 +42,11 @@ drake_cc_binary(
         "//drake/manipulation/models/wsg_50_description:models",
     ],
     deps = [
-        ":world_sim_tree_builder",
         "//drake/examples/kuka_iiwa_arm:iiwa_common",
-        "//drake/examples/kuka_iiwa_arm:sim_diagram_builder",
         "//drake/lcm",
         "//drake/manipulation/schunk_wsg:schunk_wsg_constants",
+        "//drake/manipulation/util:sim_diagram_builder",
+        "//drake/manipulation/util:world_sim_tree_builder",
         "//drake/systems/analysis",
         "//drake/systems/controllers:inverse_dynamics_controller",
         "//drake/systems/controllers:pid_controller",

drake/examples/kuka_iiwa_arm/iiwa_world/iiwa_wsg_diagram_factory.cc

@@ -7,10 +7,10 @@
 #include <utility>
 
 #include "drake/examples/kuka_iiwa_arm/iiwa_common.h"
-#include "drake/examples/kuka_iiwa_arm/iiwa_world/world_sim_tree_builder.h"
 #include "drake/examples/kuka_iiwa_arm/oracular_state_estimator.h"
-#include "drake/examples/kuka_iiwa_arm/sim_diagram_builder.h"
 #include "drake/manipulation/schunk_wsg/schunk_wsg_constants.h"
+#include "drake/manipulation/util/sim_diagram_builder.h"
+#include "drake/manipulation/util/world_sim_tree_builder.h"
 #include "drake/multibody/parsers/urdf_parser.h"
 #include "drake/multibody/rigid_body_plant/rigid_body_plant.h"
 #include "drake/systems/controllers/inverse_dynamics_controller.h"
@@ -22,16 +22,15 @@
 #include "drake/util/drakeGeometryUtil.h"
 
 namespace drake {
-
+namespace examples {
+namespace kuka_iiwa_arm {
 using systems::Context;
 using systems::Diagram;
 using systems::DiagramBuilder;
 using systems::InputPortDescriptor;
 using systems::OutputPort;
 using systems::RigidBodyPlant;
-
-namespace examples {
-namespace kuka_iiwa_arm {
+using manipulation::util::SimDiagramBuilder;
 
 template <typename T>
 IiwaAndWsgPlantWithStateEstimator<T>::IiwaAndWsgPlantWithStateEstimator(