From 9568bd5df5fa3e9e7f237db396eca6e7774cad6f Mon Sep 17 00:00:00 2001 From: Mabel Zhang Date: Fri, 28 May 2021 19:52:48 -0400 Subject: [PATCH 1/4] Multi-LRAUV swimming race example world and controller Signed-off-by: Mabel Zhang --- .../multi_lrauv_race/CMakeLists.txt | 12 + .../multi_lrauv_race/multi_lrauv_race.cc | 109 ++++++ examples/worlds/multi_lrauv_race.sdf | 365 ++++++++++++++++++ 3 files changed, 486 insertions(+) create mode 100644 examples/standalone/multi_lrauv_race/CMakeLists.txt create mode 100644 examples/standalone/multi_lrauv_race/multi_lrauv_race.cc create mode 100644 examples/worlds/multi_lrauv_race.sdf diff --git a/examples/standalone/multi_lrauv_race/CMakeLists.txt b/examples/standalone/multi_lrauv_race/CMakeLists.txt new file mode 100644 index 0000000000..5de19f3212 --- /dev/null +++ b/examples/standalone/multi_lrauv_race/CMakeLists.txt @@ -0,0 +1,12 @@ +cmake_minimum_required(VERSION 3.10.2 FATAL_ERROR) + +find_package(ignition-transport11 QUIET REQUIRED OPTIONAL_COMPONENTS log) +set(IGN_TRANSPORT_VER ${ignition-transport11_VERSION_MAJOR}) + +find_package(ignition-gazebo6 REQUIRED) +set(IGN_GAZEBO_VER ${ignition-gazebo6_VERSION_MAJOR}) + +add_executable(multi_lrauv_race multi_lrauv_race.cc) +target_link_libraries(multi_lrauv_race + ignition-transport${IGN_TRANSPORT_VER}::core + ignition-gazebo${IGN_GAZEBO_VER}::ignition-gazebo${IGN_GAZEBO_VER}) diff --git a/examples/standalone/multi_lrauv_race/multi_lrauv_race.cc b/examples/standalone/multi_lrauv_race/multi_lrauv_race.cc new file mode 100644 index 0000000000..837abff4df --- /dev/null +++ b/examples/standalone/multi_lrauv_race/multi_lrauv_race.cc @@ -0,0 +1,109 @@ +/* + * Copyright (C) 2021 Open Source Robotics Foundation + * + * 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. + * + */ + +/* + * In each iteration, for each vehicle, generate a random fin angle and thrust + * within reasonable limits, and send the command to the vehicle. + * + * Usage: + * $ multi_lrauv_race + */ + +#include +#include + +#include +#include + +// Fin joint limits from tethys model.sdf +double random_angle_within_limits(double min=-0.261799, double max=0.261799) +{ + return min + static_cast(rand()) / + (static_cast(RAND_MAX / (max - min))); +} + +// Nominal speed is thruster 300 rpm = 31.4 radians +double random_thrust_within_limits(double min=-31.4, double max=31.4) +{ + return min + static_cast(rand()) / + (static_cast(RAND_MAX / (max - min))); +} + +int main(int argc, char** argv) +{ + // Initialize random seed + srand(time(NULL)); + + std::vector ns; + ns.push_back("tethys"); + ns.push_back("triton"); + ns.push_back("daphne"); + + ignition::transport::Node node; + + std::vector rudderTopics; + rudderTopics.resize(ns.size(), ""); + std::vector rudderPubs; + rudderPubs.resize(ns.size()); + + std::vector propellerTopics; + propellerTopics.resize(ns.size(), ""); + std::vector propellerPubs; + propellerPubs.resize(ns.size()); + + // Set up topic names and publishers + for (int i = 0; i < ns.size(); i++) + { + rudderTopics[i] = ignition::transport::TopicUtils::AsValidTopic( + "/model/" + ns[i] + "/joint/vertical_fins_joint/0/cmd_pos"); + rudderPubs[i] = node.Advertise(rudderTopics[i]); + + propellerTopics[i] = ignition::transport::TopicUtils::AsValidTopic( + "/model/" + ns[i] + "/joint/propeller_joint/cmd_pos"); + propellerPubs[i] = node.Advertise( + propellerTopics[i]); + } + + std::vector rudderCmds; + rudderCmds.resize(ns.size(), 0.0); + std::vector propellerCmds; + propellerCmds.resize(ns.size(), 0.0); + + float artificial_speedup = 1; + + while (true) + { + for (int i = 0; i < ns.size(); i++) + { + rudderCmds[i] = random_angle_within_limits(-0.01, 0.01); + ignition::msgs::Double rudderMsg; + rudderMsg.set_data(rudderCmds[i]); + rudderPubs[i].Publish(rudderMsg); + + propellerCmds[i] = random_thrust_within_limits( + -31.4 * artificial_speedup, 0); + ignition::msgs::Double propellerMsg; + propellerMsg.set_data(propellerCmds[i]); + propellerPubs[i].Publish(propellerMsg); + + std::cout << "Commanding " << ns[i] << " rudder angle " << rudderCmds[i] + << ", thrust " << propellerCmds[i] << std::endl; + } + + std::this_thread::sleep_for(std::chrono::milliseconds(200)); + } +} diff --git a/examples/worlds/multi_lrauv_race.sdf b/examples/worlds/multi_lrauv_race.sdf new file mode 100644 index 0000000000..717bf86180 --- /dev/null +++ b/examples/worlds/multi_lrauv_race.sdf @@ -0,0 +1,365 @@ + + + + + + + + + 0.0 1.0 1.0 + 0.0 0.7 0.8 + + + + 0.001 + 1.0 + + + + + + + + + + + + 1000 + + + + + + + + true + 0 0 10 0 0 0 + 1 1 1 1 + 0.5 0.5 0.5 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + + + + -5 0 0 0 0 0 + https://fuel.ignitionrobotics.org/1.0/mabel/models/Turquoise turbidity generator + + + + 0 0 1 0 0 1.57 + https://fuel.ignitionrobotics.org/1.0/accurrent/models/MBARI Tethys LRAUV + + + + horizontal_fins_joint + 0.1 + + + + vertical_fins_joint + 0.1 + + + + tethys + propeller_joint + 0.004422 + 1000 + 0.2 + + + + + + + 1000 + 4.13 + -1.1 + 0.2 + 0.03 + 0.17 + 0 + 0.0244 + 0 1 0 + 1 0 0 + vertical_fins + 0 0 0 + + + + + 1000 + 4.13 + -1.1 + 0.2 + 0.03 + 0.17 + 0 + 0.0244 + 0 0 1 + 1 0 0 + horizontal_fins + 0 0 0 + + + + base_link + -4.876161 + -126.324739 + -126.324739 + 0 + -33.46 + -33.46 + -6.2282 + 0 + -601.27 + 0 + -601.27 + 0 + -0.1916 + 0 + -632.698957 + 0 + -632.698957 + 0 + + + + + + 5 0 1 0 0 1.57 + https://fuel.ignitionrobotics.org/1.0/accurrent/models/MBARI Tethys LRAUV + triton + + + + horizontal_fins_joint + 0.1 + + + + vertical_fins_joint + 0.1 + + + + triton + propeller_joint + 0.004422 + 1000 + 0.2 + + + + + + + 1000 + 4.13 + -1.1 + 0.2 + 0.03 + 0.17 + 0 + 0.0244 + 0 1 0 + 1 0 0 + vertical_fins + 0 0 0 + + + + + 1000 + 4.13 + -1.1 + 0.2 + 0.03 + 0.17 + 0 + 0.0244 + 0 0 1 + 1 0 0 + horizontal_fins + 0 0 0 + + + + base_link + -4.876161 + -126.324739 + -126.324739 + 0 + -33.46 + -33.46 + -6.2282 + 0 + -601.27 + 0 + -601.27 + 0 + -0.1916 + 0 + -632.698957 + 0 + -632.698957 + 0 + + + + + + -5 0 1 0 0 1.57 + https://fuel.ignitionrobotics.org/1.0/accurrent/models/MBARI Tethys LRAUV + daphne + + + + horizontal_fins_joint + 0.1 + + + + vertical_fins_joint + 0.1 + + + + daphne + propeller_joint + 0.004422 + 1000 + 0.2 + + + + + + + 1000 + 4.13 + -1.1 + 0.2 + 0.03 + 0.17 + 0 + 0.0244 + 0 1 0 + 1 0 0 + vertical_fins + 0 0 0 + + + + + 1000 + 4.13 + -1.1 + 0.2 + 0.03 + 0.17 + 0 + 0.0244 + 0 0 1 + 1 0 0 + horizontal_fins + 0 0 0 + + + + base_link + -4.876161 + -126.324739 + -126.324739 + 0 + -33.46 + -33.46 + -6.2282 + 0 + -601.27 + 0 + -601.27 + 0 + -0.1916 + 0 + -632.698957 + 0 + -632.698957 + 0 + + + + + + + 0 0 -1 0 0 3.1415926 + https://fuel.ignitionrobotics.org/1.0/mabel/models/ABCSign_5m + start_line + + + 0 -25 -1 0 0 3.1415926 + https://fuel.ignitionrobotics.org/1.0/mabel/models/ABCSign_5m + finish_line + + + + From b505440375b55a78f7b87793459ae374947973e2 Mon Sep 17 00:00:00 2001 From: Mabel Zhang Date: Fri, 28 May 2021 20:35:25 -0400 Subject: [PATCH 2/4] specifiy units Signed-off-by: Mabel Zhang --- examples/standalone/multi_lrauv_race/multi_lrauv_race.cc | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/examples/standalone/multi_lrauv_race/multi_lrauv_race.cc b/examples/standalone/multi_lrauv_race/multi_lrauv_race.cc index 837abff4df..6854da139d 100644 --- a/examples/standalone/multi_lrauv_race/multi_lrauv_race.cc +++ b/examples/standalone/multi_lrauv_race/multi_lrauv_race.cc @@ -36,7 +36,7 @@ double random_angle_within_limits(double min=-0.261799, double max=0.261799) (static_cast(RAND_MAX / (max - min))); } -// Nominal speed is thruster 300 rpm = 31.4 radians +// Nominal speed is thruster 300 rpm = 31.4 radians per second double random_thrust_within_limits(double min=-31.4, double max=31.4) { return min + static_cast(rand()) / @@ -101,7 +101,7 @@ int main(int argc, char** argv) propellerPubs[i].Publish(propellerMsg); std::cout << "Commanding " << ns[i] << " rudder angle " << rudderCmds[i] - << ", thrust " << propellerCmds[i] << std::endl; + << " rad, thrust " << propellerCmds[i] << " rad/s" << std::endl; } std::this_thread::sleep_for(std::chrono::milliseconds(200)); From 585f31e2bafd13cda7752eb77c2fab6bbbf1ab74 Mon Sep 17 00:00:00 2001 From: Mabel Zhang Date: Mon, 7 Jun 2021 14:32:43 -0400 Subject: [PATCH 3/4] change thruster unit to Newtons Signed-off-by: Mabel Zhang --- examples/standalone/multi_lrauv_race/multi_lrauv_race.cc | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/examples/standalone/multi_lrauv_race/multi_lrauv_race.cc b/examples/standalone/multi_lrauv_race/multi_lrauv_race.cc index 6854da139d..a180748fd1 100644 --- a/examples/standalone/multi_lrauv_race/multi_lrauv_race.cc +++ b/examples/standalone/multi_lrauv_race/multi_lrauv_race.cc @@ -36,8 +36,8 @@ double random_angle_within_limits(double min=-0.261799, double max=0.261799) (static_cast(RAND_MAX / (max - min))); } -// Nominal speed is thruster 300 rpm = 31.4 radians per second -double random_thrust_within_limits(double min=-31.4, double max=31.4) +// Nominal speed is thruster 300 rpm ~ 31.4 radians per second ~ 6.14 Newtons +double random_thrust_within_limits(double min=-6.14, double max=6.14) { return min + static_cast(rand()) / (static_cast(RAND_MAX / (max - min))); @@ -95,13 +95,13 @@ int main(int argc, char** argv) rudderPubs[i].Publish(rudderMsg); propellerCmds[i] = random_thrust_within_limits( - -31.4 * artificial_speedup, 0); + -6.14 * artificial_speedup, 0); ignition::msgs::Double propellerMsg; propellerMsg.set_data(propellerCmds[i]); propellerPubs[i].Publish(propellerMsg); std::cout << "Commanding " << ns[i] << " rudder angle " << rudderCmds[i] - << " rad, thrust " << propellerCmds[i] << " rad/s" << std::endl; + << " rad, thrust " << propellerCmds[i] << " Newtons" << std::endl; } std::this_thread::sleep_for(std::chrono::milliseconds(200)); From 615069a4a3c6d8570605540616618a67e12fa0cf Mon Sep 17 00:00:00 2001 From: Mabel Zhang Date: Fri, 11 Jun 2021 21:07:29 -0400 Subject: [PATCH 4/4] add README Signed-off-by: Mabel Zhang --- .../standalone/multi_lrauv_race/README.md | 25 +++++++++++++++++++ 1 file changed, 25 insertions(+) create mode 100644 examples/standalone/multi_lrauv_race/README.md diff --git a/examples/standalone/multi_lrauv_race/README.md b/examples/standalone/multi_lrauv_race/README.md new file mode 100644 index 0000000000..d9a2e2ee2b --- /dev/null +++ b/examples/standalone/multi_lrauv_race/README.md @@ -0,0 +1,25 @@ +# Multi-LRAUV Swimming Race Example + +This example shows the usage of the Thruster plugin and rudder joint control on +multiple autonomous underwater vehicles (AUV) with buoyancy, lift drag, and +hydrodynamics plugins. The multiple vehicles are differentiated by namespaces. + +## Build Instructions + +From this directory, run the following to compile: + + mkdir build + cd build + cmake .. + make + +## Execute Instructions + +From the `build` directory, run Ignition and the example controller: + + ign gazebo -r ../../../worlds/multi_lrauv_race.sdf + ./multi_lrauv_race + +The example controller will output pseudorandom propeller and rudder commands +to move the vehicles forward. The low speed is by design to model the actual +vehicle velocity.