From 41a756e9bed8d5c785d0cb46a9f1f5c667e1a723 Mon Sep 17 00:00:00 2001 From: Geoffrey Biggs Date: Mon, 23 May 2022 19:47:25 +0900 Subject: [PATCH] Add C++ version of multirobot sample Signed-off-by: Geoffrey Biggs --- .../examples/multirobot/CMakeLists.txt | 16 +++ .../examples/multirobot/README.md | 15 ++- .../examples/multirobot/multirobot.cc | 121 ++++++++++++++++++ 3 files changed, 151 insertions(+), 1 deletion(-) create mode 100644 drake_ros_examples/examples/multirobot/multirobot.cc diff --git a/drake_ros_examples/examples/multirobot/CMakeLists.txt b/drake_ros_examples/examples/multirobot/CMakeLists.txt index 23482804..c2787311 100644 --- a/drake_ros_examples/examples/multirobot/CMakeLists.txt +++ b/drake_ros_examples/examples/multirobot/CMakeLists.txt @@ -1,3 +1,19 @@ +find_package(ament_index_cpp REQUIRED) + +add_executable(multirobot multirobot.cc) +target_link_libraries(multirobot + drake::drake + drake_ros_core::drake_ros_core + drake_ros_tf2::drake_ros_tf2 + drake_ros_viz::drake_ros_viz + ament_index_cpp::ament_index_cpp +) + +install( + TARGETS multirobot + DESTINATION lib/${PROJECT_NAME} +) + install( PROGRAMS multirobot.py DESTINATION lib/${PROJECT_NAME} diff --git a/drake_ros_examples/examples/multirobot/README.md b/drake_ros_examples/examples/multirobot/README.md index 8f46857b..9545ce7f 100644 --- a/drake_ros_examples/examples/multirobot/README.md +++ b/drake_ros_examples/examples/multirobot/README.md @@ -14,10 +14,23 @@ The simulation publishes the following topics: ## How to run the example -Run the Python `multirobot.py` script as explained [here](../../README.md#running). +Run either the C++ executable or the Python script. +For the C++ version of the example, run the executable. + +``` +ros2 run drake_ros_examples multirobot +``` + +For the Python version of the example, run the Python script. + +``` +ros2 run drake_ros_examples multirobot.py +``` In a separate terminal, launch RViz and provide the path to the configuration file to visualise the robots. ``` ros2 run rviz2 rviz2 -d $(ros2 pkg prefix drake_ros_examples)/share/drake_ros_examples/multirobot.rviz ``` + +You should observe a 10x10 array of manipulators flopping about under the influence of gravity. diff --git a/drake_ros_examples/examples/multirobot/multirobot.cc b/drake_ros_examples/examples/multirobot/multirobot.cc new file mode 100644 index 00000000..f704927d --- /dev/null +++ b/drake_ros_examples/examples/multirobot/multirobot.cc @@ -0,0 +1,121 @@ +// Copyright 2022 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include +#include + +using drake_ros_core::DrakeRos; +using drake_ros_core::RosInterfaceSystem; + +using drake::systems::ConstantVectorSource; +using drake::systems::Simulator; + + +int main() +{ + drake::systems::DiagramBuilder builder; + + drake_ros_core::init(); + auto ros_interface_system = + builder.AddSystem(std::make_unique("multirobot_node")); + + auto [plant, scene_graph] = drake::multibody::AddMultibodyPlantSceneGraph(&builder, 0.001); + + auto scene_tf_broadcaster = builder.AddSystem( + ros_interface_system->get_ros_interface(), + drake_ros_tf2::SceneTfBroadcasterParams{ + {drake::systems::TriggerType::kForced}, 0., "/tf"}); + + auto scene_visualizer = builder.AddSystem( + ros_interface_system->get_ros_interface(), + drake_ros_viz::RvizVisualizerParams{ + {drake::systems::TriggerType::kPeriodic}, 0.05, true}); + + builder.Connect(scene_graph.get_query_output_port(), + scene_tf_broadcaster->get_graph_query_port()); + builder.Connect(scene_graph.get_query_output_port(), + scene_visualizer->get_graph_query_port()); + + auto parser = drake::multibody::Parser(&plant); + parser.package_map().PopulateFromEnvironment("AMENT_PREFIX_PATH"); + std::filesystem::path model_file_path = + ament_index_cpp::get_package_share_directory("drake_ros_examples"); + model_file_path /= "iiwa_description/urdf/iiwa14_polytope_collision.urdf"; + + std::string model_name = "kuka_iiwa"; + + size_t kNumRows = 10; + size_t kNumCols = 10; + std::vector> models; + for (uint8_t xx = 0; xx < kNumRows; ++xx) { + std::vector models_xx; + for (uint8_t yy = 0; yy < kNumCols; ++yy) { + std::stringstream model_instance_name; + model_instance_name << model_name << xx << '_' << yy; + auto model_instance = parser.AddModelFromFile(model_file_path, model_instance_name.str()); + + // Weld the model to the world so it doesn't fall through the floor + auto & base_frame = plant.GetFrameByName("base", model_instance); + auto X_WB = drake::math::RigidTransform(drake::Vector3{ + static_cast(xx), + static_cast(yy), + 0.}); + plant.WeldFrames(plant.world_frame(), base_frame, X_WB); + + models_xx.push_back(model_instance); + } + models.push_back(models_xx); + } + + plant.Finalize(); + + for (size_t xx = 0; xx < kNumRows; ++xx) { + for (size_t yy = 0; yy < kNumCols; ++yy) { + auto num_dofs = plant.num_actuated_dofs(models[xx][yy]); + auto u0 = drake::VectorX::Zero(num_dofs); + auto constant = builder.AddSystem(u0); + builder.Connect( + constant->get_output_port(), + plant.get_actuation_input_port(models[xx][yy])); + } + } + + drake::geometry::DrakeVisualizer::AddToBuilder(&builder, scene_graph); + + auto diagram = builder.Build(); + auto simulator = std::make_unique>(*diagram); + auto & simulator_context = simulator->get_mutable_context(); + simulator->set_target_realtime_rate(1.0); + + while (true) { + simulator->AdvanceTo(simulator_context.get_time() + 0.1); + diagram->Publish(simulator_context); + } + + return 0; +}