diff --git a/.github/ISSUE_TEMPLATE/bug_report.md b/.github/ISSUE_TEMPLATE/bug_report.md index 9fa3ba7da6..7bdc022512 100644 --- a/.github/ISSUE_TEMPLATE/bug_report.md +++ b/.github/ISSUE_TEMPLATE/bug_report.md @@ -5,7 +5,7 @@ labels: bug --- +https://robotics.stackexchange.com instead.--> ## Environment * OS Version: diff --git a/.github/ISSUE_TEMPLATE/feature_request.md b/.github/ISSUE_TEMPLATE/feature_request.md index 87233a479a..52b56e3365 100644 --- a/.github/ISSUE_TEMPLATE/feature_request.md +++ b/.github/ISSUE_TEMPLATE/feature_request.md @@ -6,7 +6,7 @@ labels: enhancement +https://robotics.stackexchange.com instead.--> ## Desired behavior diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 15fd1be29c..a0ce61b450 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -4,10 +4,9 @@ on: pull_request: push: branches: - - 'gz-sim8' + - 'ign-gazebo[0-9]' + - 'gz-sim[0-9]?' - 'main' - -# Every time you make a push to your PR, it cancel immediately the previous checks, # and start a new one. The other runner will be available more quickly to your PR. concurrency: group: ${{ github.workflow }}-${{ github.head_ref || github.run_id }} @@ -19,7 +18,7 @@ jobs: name: Ubuntu Jammy CI steps: - name: Checkout - uses: actions/checkout@v3 + uses: actions/checkout@v4 - uses: actions/setup-python@v3 - uses: pre-commit/action@v3.0.0 with: diff --git a/Changelog.md b/Changelog.md index 4d7d22df7a..0f36cb3f50 100644 --- a/Changelog.md +++ b/Changelog.md @@ -131,6 +131,83 @@ ## Gazebo Sim 7.x +### Gazebo Sim 7.7.0 (2024-01-17) + +1. Allow using plugin file names and environment variables compatible with Garden and later + * [Pull request #2275](https://github.com/gazebosim/gz-sim/pull/2275) + +1. Added tutorial for Gazebo joint controller plugin + * [Pull request #2263](https://github.com/gazebosim/gz-sim/pull/2263) + +1. Fix incorrect light direction in tunnel.sdf example + * [Pull request #2264](https://github.com/gazebosim/gz-sim/pull/2264) + +1. Fix DLL linkage/visibility issues + * [Pull request #2254](https://github.com/gazebosim/gz-sim/pull/2254) + +1. mecanum_drive: use mesh wheels in example world + * [Pull request #2250](https://github.com/gazebosim/gz-sim/pull/2250) + +1. environment_preload: fix windows compiler warnings + * [Pull request #2246](https://github.com/gazebosim/gz-sim/pull/2246) + +1. EnvironmentPreload: ignerr -> gzerr + * [Pull request #2245](https://github.com/gazebosim/gz-sim/pull/2245) + +1. Update friction parameters for skid steer example + * [Pull request #2235](https://github.com/gazebosim/gz-sim/pull/2235) + +1. Use sdf FindElement API to avoid const_cast + * [Pull request #2236](https://github.com/gazebosim/gz-sim/pull/2236) + +1. Use `GZ_PI` instead of `M_PI` to fix windows builds + * [Pull request #2230](https://github.com/gazebosim/gz-sim/pull/2230) + +1. Add note about elevator example + * [Pull request #2227](https://github.com/gazebosim/gz-sim/pull/2227) + +1. Porting Advanced Lift Drag Plugin to Gazebo + * [Pull request #2185](https://github.com/gazebosim/gz-sim/pull/2185) + * [Pull request #2226](https://github.com/gazebosim/gz-sim/pull/2226) + +1. Fix macOS test failures by registering components in the core library + * [Pull request #2220](https://github.com/gazebosim/gz-sim/pull/2220) + +1. Fix for sensor pointer null when navsat plugin in included in sdf + * [Pull request #2176](https://github.com/gazebosim/gz-sim/pull/2176) + +1. Fix another deadlock in sensors system + * [Pull request #2200](https://github.com/gazebosim/gz-sim/pull/2200) + +1. Fix sensors system parallel updates + * [Pull request #2201](https://github.com/gazebosim/gz-sim/pull/2201) + +1. Relax pose check in actor no mesh test + * [Pull request #2196](https://github.com/gazebosim/gz-sim/pull/2196) + +1. backport component inspector Vector3d width fix + * [Pull request #2195](https://github.com/gazebosim/gz-sim/pull/2195) + +1. fix INTEGRATION_save_world's SdfGeneratorFixture.ModelWithNestedIncludes test + * [Pull request #2197](https://github.com/gazebosim/gz-sim/pull/2197) + +1. Lift Drag Bug Fix + * [Pull request #2189](https://github.com/gazebosim/gz-sim/pull/2189) + * [Pull request #2272](https://github.com/gazebosim/gz-sim/pull/2272) + * [Pull request #2273](https://github.com/gazebosim/gz-sim/pull/2273) + * [Issue #2188](https://github.com/gazebosim/gz-sim/issues/2188) + +1. Bump Fuel model version in test + * [Pull request #2190](https://github.com/gazebosim/gz-sim/pull/2190) + +1. Fix enviroment system loading mechanism + * [Pull request #1842](https://github.com/gazebosim/gz-sim/pull/1842) + +1. Infrastructure + * [Pull request #2237](https://github.com/gazebosim/gz-sim/pull/2237) + * [Pull request #2222](https://github.com/gazebosim/gz-sim/pull/2222) + + ### Gazebo Sim 7.6.0 (2023-09-26) 1. Documentation updates @@ -1018,6 +1095,35 @@ ## Gazebo Sim 6.x +### Gazebo Sim 6.16.0 (2024-01-12) + +1. Allow using plugin file names and environment variables compatible with Garden and later + * [Pull request #2275](https://github.com/gazebosim/gz-sim/pull/2275) + +1. Update friction parameters for skid steer example + * [Pull request #2235](https://github.com/gazebosim/gz-sim/pull/2235) + +1. Relax pose check in actor no mesh test + * [Pull request #2196](https://github.com/gazebosim/gz-sim/pull/2196) + +1. Fix macOS test failures by registering components in the core library + * [Pull request #2220](https://github.com/gazebosim/gz-sim/pull/2220) + +1. Fix for sensor pointer null when navsat plugin in included in sdf + * [Pull request #2176](https://github.com/gazebosim/gz-sim/pull/2176) + +1. Fix another deadlock in sensors system + * [Pull request #2200](https://github.com/gazebosim/gz-sim/pull/2200) + +1. Backport component inspector Vector3d width fix + * [Pull request #2195](https://github.com/gazebosim/gz-sim/pull/2195) + +1. Bump Fuel model version in test + * [Pull request #2190](https://github.com/gazebosim/gz-sim/pull/2190) + +1. Infrastructure + * [Pull request #2237](https://github.com/gazebosim/gz-sim/pull/2237) + * [Pull request #2222](https://github.com/gazebosim/gz-sim/pull/2222) ### Gazebo Sim 6.15.0 (2023-08-16) diff --git a/examples/worlds/diff_drive_skid.sdf b/examples/worlds/diff_drive_skid.sdf index fe1fdbe833..58cce3ca04 100644 --- a/examples/worlds/diff_drive_skid.sdf +++ b/examples/worlds/diff_drive_skid.sdf @@ -155,8 +155,10 @@ - 0.5 - 1.0 + 1 + 1 + 0.035 + 0 0 0 1 @@ -203,8 +205,10 @@ - 0.5 - 1.0 + 1 + 1 + 0.035 + 0 0 0 1 @@ -251,8 +255,10 @@ - 0.5 - 1.0 + 1 + 1 + 0.035 + 0 0 0 1 @@ -299,8 +305,10 @@ - 0.5 - 1.0 + 1 + 1 + 0.035 + 0 0 0 1 diff --git a/examples/worlds/joint_controller.sdf b/examples/worlds/joint_controller.sdf index 65cf6fe938..a02ff06900 100644 --- a/examples/worlds/joint_controller.sdf +++ b/examples/worlds/joint_controller.sdf @@ -91,8 +91,7 @@ 0.2 0.8 0.2 1 - 0.2 0.8 0.2 1 - 0.2 0.8 0.2 1 + 0.8 0 0 1 @@ -179,8 +178,7 @@ 0.2 0.8 0.2 1 - 0.2 0.8 0.2 1 - 0.2 0.8 0.2 1 + 0.8 0 0 1 diff --git a/examples/worlds/joint_position_controller.sdf b/examples/worlds/joint_position_controller.sdf index c39256aa1b..8265059ad7 100644 --- a/examples/worlds/joint_position_controller.sdf +++ b/examples/worlds/joint_position_controller.sdf @@ -85,6 +85,7 @@ 0.2 0.8 0.2 1 + 0.8 0 0 1 diff --git a/examples/worlds/mecanum_drive.sdf b/examples/worlds/mecanum_drive.sdf index 6109eafc48..6d39c77ce1 100644 --- a/examples/worlds/mecanum_drive.sdf +++ b/examples/worlds/mecanum_drive.sdf @@ -128,15 +128,13 @@ + 0 0 0 1.5707 0 0 - - 0.3 - - - + + + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Mecanum lift/tip/files/meshes/mecanum_wheel_left.STL + 0.006 0.006 0.006 + 0.2 0.2 0.2 1 @@ -176,11 +174,13 @@ + 0 0 0 1.5707 0 0 - - 0.3 - - + + + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Mecanum lift/tip/files/meshes/mecanum_wheel_right.STL + 0.006 0.006 0.006 + 0.2 0.2 0.2 1 @@ -220,11 +220,13 @@ + 0 0 0 1.5707 0 0 - - 0.3 - - + + + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Mecanum lift/tip/files/meshes/mecanum_wheel_right.STL + 0.006 0.006 0.006 + 0.2 0.2 0.2 1 @@ -264,11 +266,13 @@ + 0 0 0 1.5707 0 0 - - 0.3 - - + + + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Mecanum lift/tip/files/meshes/mecanum_wheel_left.STL + 0.006 0.006 0.006 + 0.2 0.2 0.2 1 diff --git a/examples/worlds/tunnel.sdf b/examples/worlds/tunnel.sdf index a902e40f03..91c069437a 100644 --- a/examples/worlds/tunnel.sdf +++ b/examples/worlds/tunnel.sdf @@ -1165,7 +1165,7 @@ 1.1 1 - 0 -1 0 + 0 0 -1 1 diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index c8bb1e1519..667562dc45 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -188,7 +188,7 @@ target_link_libraries(${gz_lib_target} ) # Executable target that runs the GUI without ruby for debugging purposes. -add_executable(runGui gz.cc) +add_executable(runGui cmd/runGui_main.cc) target_link_libraries(runGui PRIVATE ${gz_lib_target}) # Create the library target diff --git a/src/Conversions.cc b/src/Conversions.cc index 519593ff79..fbe7587141 100644 --- a/src/Conversions.cc +++ b/src/Conversions.cc @@ -1363,6 +1363,41 @@ sdf::Sensor gz::sim::convert(const msgs::Sensor &_in) out.SetCameraSensor(sensor); } + else if (out.Type() == sdf::SensorType::GPS || + out.Type() == sdf::SensorType::NAVSAT) + { + sdf::NavSat sensor; + if (_in.has_gps()) + { + if (_in.gps().position().has_horizontal_noise()) + { + sensor.SetHorizontalPositionNoise(sim::convert( + _in.gps().position().horizontal_noise())); + } + if (_in.gps().position().has_vertical_noise()) + { + sensor.SetVerticalPositionNoise(sim::convert( + _in.gps().position().vertical_noise())); + } + if (_in.gps().velocity().has_horizontal_noise()) + { + sensor.SetHorizontalVelocityNoise(sim::convert( + _in.gps().velocity().horizontal_noise())); + } + if (_in.gps().velocity().has_vertical_noise()) + { + sensor.SetVerticalVelocityNoise(sim::convert( + _in.gps().velocity().vertical_noise())); + } + } + else + { + gzerr << "Attempting to convert a navsat sensor message, but the " + << "message does not have a navsat nested message.\n"; + } + + out.SetNavSatSensor(sensor); + } else if (out.Type() == sdf::SensorType::ALTIMETER) { sdf::Altimeter sensor; @@ -1637,8 +1672,8 @@ msgs::ParticleEmitter gz::sim::convert(const sdf::ParticleEmitter &_in) std::string path = asFullPath(_in.ColorRangeImage(), _in.FilePath()); common::SystemPaths systemPaths; - systemPaths.SetFilePathEnv(kResourcePathEnv); - std::string absolutePath = systemPaths.FindFile(path); + std::string absolutePath = + common::SystemPaths::LocateLocalFile(path, sim::resourcePaths()); if (!absolutePath.empty()) { diff --git a/src/Server_TEST.cc b/src/Server_TEST.cc index df4373521c..c3efe00123 100644 --- a/src/Server_TEST.cc +++ b/src/Server_TEST.cc @@ -46,6 +46,7 @@ #include "plugins/MockSystem.hh" #include "../test/helpers/Relay.hh" #include "../test/helpers/EnvTestFixture.hh" +#include "../test/helpers/Util.hh" using namespace gz; using namespace gz::sim; @@ -278,14 +279,10 @@ TEST_P(ServerFixture, GZ_UTILS_TEST_DISABLED_ON_WIN32(ServerConfigRealPlugin)) msgs::StringMsg rep; bool result{false}; bool executed{false}; - int sleep{0}; - int maxSleep{30}; - while (!executed && sleep < maxSleep) - { - gzdbg << "Requesting /test/service" << std::endl; - executed = node.Request("/test/service", 100, rep, result); - sleep++; - } + const std::string service = "/test/service"; + ASSERT_TRUE(test::waitForService(node, service)); + gzdbg << "Requesting " << service << std::endl; + executed = node.Request(service, 1000, rep, result); EXPECT_TRUE(executed); EXPECT_TRUE(result); EXPECT_EQ("TestModelSystem", rep.data()); @@ -337,14 +334,10 @@ TEST_P(ServerFixture, msgs::StringMsg rep; bool result{false}; bool executed{false}; - int sleep{0}; - int maxSleep{30}; - while (!executed && sleep < maxSleep) - { - gzdbg << "Requesting /test/service/sensor" << std::endl; - executed = node.Request("/test/service/sensor", 100, rep, result); - sleep++; - } + const std::string service ="/test/service/sensor"; + ASSERT_TRUE(test::waitForService(node, service)); + gzdbg << "Requesting " << service << std::endl; + executed = node.Request(service, 1000, rep, result); EXPECT_TRUE(executed); EXPECT_TRUE(result); EXPECT_EQ("TestSensorSystem", rep.data()); @@ -780,16 +773,12 @@ TEST_P(ServerFixture, ServerControlStop) msgs::Boolean res; bool result{false}; bool executed{false}; - int sleep{0}; - int maxSleep{30}; + const std::string service = "/server_control"; + ASSERT_TRUE(test::waitForService(node, service)); // first, call with stop = false; the server should keep running - while (!executed && sleep < maxSleep) - { - gzdbg << "Requesting /server_control" << std::endl; - executed = node.Request("/server_control", req, 100, res, result); - sleep++; - } + gzdbg << "Requesting " << service << std::endl; + executed = node.Request(service, req, 1000, res, result); EXPECT_TRUE(executed); EXPECT_TRUE(result); EXPECT_FALSE(res.data()); @@ -802,8 +791,8 @@ TEST_P(ServerFixture, ServerControlStop) // now call with stop = true; the server should stop req.set_stop(true); - gzdbg << "Requesting /server_control" << std::endl; - executed = node.Request("/server_control", req, 100, res, result); + gzdbg << "Requesting " << service << std::endl; + executed = node.Request(service, req, 1000, res, result); EXPECT_TRUE(executed); EXPECT_TRUE(result); @@ -1047,14 +1036,10 @@ TEST_P(ServerFixture, GetResourcePaths) msgs::StringMsg_V res; bool result{false}; bool executed{false}; - int sleep{0}; - int maxSleep{30}; - while (!executed && sleep < maxSleep) - { - gzdbg << "Requesting /gazebo/resource_paths/get" << std::endl; - executed = node.Request("/gazebo/resource_paths/get", 100, res, result); - sleep++; - } + const std::string service = "/gazebo/resource_paths/get"; + ASSERT_TRUE(test::waitForService(node, service)); + gzdbg << "Requesting " << service << std::endl; + executed = node.Request(service, 1000, res, result); EXPECT_TRUE(executed); EXPECT_TRUE(result); EXPECT_EQ(2, res.data_size()); @@ -1101,7 +1086,9 @@ TEST_P(ServerFixture, AddResourcePaths) common::SystemPaths::Delimiter() + std::string("/tmp/even_more")); req.add_data("/tmp/some/path"); - bool executed = node.Request("/gazebo/resource_paths/add", req); + const std::string service = "/gazebo/resource_paths/add"; + ASSERT_TRUE(test::waitForService(node, service)); + bool executed = node.Request(service, req); EXPECT_TRUE(executed); int sleep{0}; @@ -1155,17 +1142,12 @@ TEST_P(ServerFixture, ResolveResourcePaths) msgs::StringMsg req, res; bool result{false}; bool executed{false}; - int sleep{0}; - int maxSleep{30}; req.set_data(_uri); - while (!executed && sleep < maxSleep) - { - gzdbg << "Requesting /gazebo/resource_paths/resolve" << std::endl; - executed = node.Request("/gazebo/resource_paths/resolve", req, 100, - res, result); - sleep++; - } + const std::string service ="/gazebo/resource_paths/resolve"; + ASSERT_TRUE(test::waitForService(node, service)); + gzdbg << "Requesting " << service << std::endl; + executed = node.Request(service, req, 1000, res, result); EXPECT_TRUE(executed); EXPECT_EQ(_found, result); EXPECT_EQ(_expected, res.data()) << "Expected[" << _expected diff --git a/src/SystemLoader_TEST.cc b/src/SystemLoader_TEST.cc index 4709af5fd8..6550e2b233 100644 --- a/src/SystemLoader_TEST.cc +++ b/src/SystemLoader_TEST.cc @@ -29,29 +29,38 @@ using namespace gz; using namespace sim; +#ifdef _WIN32 + constexpr const char *kPluginDir = "bin"; +#else + constexpr const char *kPluginDir = "lib"; +#endif ///////////////////////////////////////////////// TEST(SystemLoader, Constructor) { - sim::SystemLoader sm; - // Add test plugin to path (referenced in config) auto testBuildPath = gz::common::joinPaths( - std::string(PROJECT_BINARY_PATH), "lib"); - sm.AddSystemPluginPath(testBuildPath); + std::string(PROJECT_BINARY_PATH), kPluginDir); sdf::Root root; root.LoadSdfString(std::string("" - "" - "" + "") + + "" + "" + "" ""); - + ASSERT_NE(root.WorldByIndex(0), nullptr); auto worldElem = root.WorldByIndex(0)->Element(); if (worldElem->HasElement("plugin")) { sdf::ElementPtr pluginElem = worldElem->GetElement("plugin"); while (pluginElem) { + gz::sim::SystemLoader sm; + sm.AddSystemPluginPath(testBuildPath); sdf::Plugin plugin; plugin.Load(pluginElem); auto system = sm.LoadPlugin(plugin); @@ -60,6 +69,40 @@ TEST(SystemLoader, Constructor) } } } +///////////////////////////////////////////////// +TEST(SystemLoader, FromPluginPathEnv) +{ + sdf::Root root; + root.LoadSdfString(R"( + + + + + )"); + + ASSERT_NE(root.WorldCount(), 0u); + auto world = root.WorldByIndex(0); + ASSERT_TRUE(world != nullptr); + ASSERT_FALSE(world->Plugins().empty()); + auto plugin = world->Plugins()[0]; + + { + gz::sim::SystemLoader sm; + auto system = sm.LoadPlugin(plugin); + EXPECT_FALSE(system.has_value()); + } + + const auto libPath = common::joinPaths(PROJECT_BINARY_PATH, kPluginDir); + + { + common::setenv("GZ_SIM_SYSTEM_PLUGIN_PATH", libPath.c_str()); + + gz::sim::SystemLoader sm; + auto system = sm.LoadPlugin(plugin); + EXPECT_TRUE(system.has_value()); + EXPECT_TRUE(common::unsetenv("GZ_SIM_SYSTEM_PLUGIN_PATH")); + } +} ///////////////////////////////////////////////// TEST(SystemLoader, EmptyNames) diff --git a/src/Util.cc b/src/Util.cc index 85e21e4e4f..e7b444bfe7 100644 --- a/src/Util.cc +++ b/src/Util.cc @@ -435,15 +435,31 @@ std::string asFullPath(const std::string &_uri, const std::string &_filePath) return common::joinPaths(path, uri); } +namespace +{ ////////////////////////////////////////////////// -std::vector resourcePaths() +/// \brief Helper function to extract paths form an environment variable +/// refactored from `resourcePaths` below. +/// common::SystemPaths::PathsFromEnv is available, but it's behavior is +/// slightly different from this in that it adds trailing `/` to the end of a +/// path if it doesn't have it already. +std::vector extractPathsFromEnv(const std::string &_envVar) { - std::vector gzPaths; - char *gzPathCStr = std::getenv(kResourcePathEnv.c_str()); - if (gzPathCStr && *gzPathCStr != '\0') + std::vector pathsFromEnv; + char *pathFromEnvCStr = std::getenv(_envVar.c_str()); + if (pathFromEnvCStr && *pathFromEnvCStr != '\0') { - gzPaths = common::Split(gzPathCStr, common::SystemPaths::Delimiter()); + pathsFromEnv = + common::Split(pathFromEnvCStr, common::SystemPaths::Delimiter()); } + return pathsFromEnv; +} +} // namespace + +////////////////////////////////////////////////// +std::vector resourcePaths() +{ + auto gzPaths = extractPathsFromEnv(kResourcePathEnv); gzPaths.erase(std::remove_if(gzPaths.begin(), gzPaths.end(), [](const std::string &_path) @@ -476,36 +492,28 @@ void addResourcePaths(const std::vector &_paths) } // Gazebo resource paths - std::vector gzPaths; - char *gzPathCStr = std::getenv(kResourcePathEnv.c_str()); - if (gzPathCStr && *gzPathCStr != '\0') - { - gzPaths = common::Split(gzPathCStr, common::SystemPaths::Delimiter()); - } + auto gzPaths = extractPathsFromEnv(kResourcePathEnv); - // Add new paths to gzPaths - for (const auto &path : _paths) + auto addUniquePaths = [](std::vector &_container, + const std::vector _pathsToAdd) { - if (std::find(gzPaths.begin(), gzPaths.end(), path) == gzPaths.end()) + for (const auto &path : _pathsToAdd) { - gzPaths.push_back(path); + if (std::find(_container.begin(), _container.end(), path) == + _container.end()) + { + _container.push_back(path); + } } - } + }; + // Add new paths to gzPaths + addUniquePaths(gzPaths, _paths); // Append Gz paths to SDF / Ign paths - for (const auto &path : gzPaths) - { - if (std::find(sdfPaths.begin(), sdfPaths.end(), path) == sdfPaths.end()) - { - sdfPaths.push_back(path); - } + addUniquePaths(sdfPaths, gzPaths); + addUniquePaths(commonPaths, gzPaths); + - if (std::find(commonPaths.begin(), - commonPaths.end(), path) == commonPaths.end()) - { - commonPaths.push_back(path); - } - } // Update the vars std::string sdfPathsStr; diff --git a/src/cmd/runGui_main.cc b/src/cmd/runGui_main.cc new file mode 100644 index 0000000000..0b8aa66d4d --- /dev/null +++ b/src/cmd/runGui_main.cc @@ -0,0 +1,23 @@ +/* + * Copyright (C) 2023 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. + * +*/ + +#include "gz/sim/gui/Gui.hh" + +int main(int argc, char* argv[]) +{ + return gz::sim::gui::runGui(argc, argv, nullptr); +} diff --git a/src/gz.cc b/src/gz.cc index bead69e5dd..3c7772e627 100644 --- a/src/gz.cc +++ b/src/gz.cc @@ -470,8 +470,3 @@ extern "C" int runGui(const char *_guiConfig, const char *_file, int _waitGui, return gz::sim::gui::runGui(argc, argv, _guiConfig, _file, _waitGui, _renderEngine, _renderEngineGuiApiBackend); } - -int main(int argc, char* argv[]) -{ - return sim::gui::runGui(argc, argv, nullptr); -} diff --git a/src/gz_TEST.cc b/src/gz_TEST.cc index f01173aba8..18b30d73d0 100644 --- a/src/gz_TEST.cc +++ b/src/gz_TEST.cc @@ -211,12 +211,18 @@ TEST(CmdLine, GZ_UTILS_TEST_DISABLED_ON_WIN32(ResourcePath)) // Correct path auto path = std::string("GZ_SIM_RESOURCE_PATH=") + - PROJECT_SOURCE_PATH + "/test/worlds "; + gz::common::joinPaths(PROJECT_SOURCE_PATH, "test", "worlds "); output = customExecStr(path + cmd); EXPECT_EQ(output.find("Unable to find file plugins.sdf"), std::string::npos) << output; + path = std::string("GZ_SIM_RESOURCE_PATH=") + + gz::common::joinPaths(PROJECT_SOURCE_PATH, "test", "worlds "); + output = customExecStr(path + cmd); + EXPECT_EQ(output.find("Unable to find file plugins.sdf"), std::string::npos) + << output; + // Several paths path = std::string("GZ_SIM_RESOURCE_PATH=banana:") + PROJECT_SOURCE_PATH + "/test/worlds:orange "; @@ -224,6 +230,22 @@ TEST(CmdLine, GZ_UTILS_TEST_DISABLED_ON_WIN32(ResourcePath)) output = customExecStr(path + cmd); EXPECT_EQ(output.find("Unable to find file plugins.sdf"), std::string::npos) << output; + + // Test nested models + // Use a direct path to the input file. Using a file name that has to be + // resolved interacts with how resource environment variables are processed + // and so will have different behavior than when a direct path is provided. + cmd = kGzCommand + " -s -r -v 4 --iterations 1 " + + gz::common::joinPaths(PROJECT_SOURCE_PATH, "test", "worlds", + "include_nested_models.sdf"); + output = customExecStr(cmd); + EXPECT_NE(output.find("Unable to find"), std::string::npos) << output; + + std::string pathValue = + gz::common::joinPaths(PROJECT_SOURCE_PATH, "test", "worlds", "models"); + + output = customExecStr("GZ_SIM_RESOURCE_PATH=" + pathValue + " " + cmd); + EXPECT_EQ(output.find("Unable to find"), std::string::npos) << output; } ////////////////////////////////////////////////// diff --git a/src/systems/environment_preload/EnvironmentPreload.cc b/src/systems/environment_preload/EnvironmentPreload.cc index 5192fa46df..3aa0ca4e7a 100644 --- a/src/systems/environment_preload/EnvironmentPreload.cc +++ b/src/systems/environment_preload/EnvironmentPreload.cc @@ -71,7 +71,7 @@ class gz::sim::systems::EnvironmentPreloadPrivate public: bool visualize{false}; /// \brief Sample resolutions - public: math::Vector3d samples; + public: math::Vector3 samples; /// \brief Is the file loaded public: bool fileLoaded{false}; @@ -106,8 +106,13 @@ class gz::sim::systems::EnvironmentPreloadPrivate // Only visualize if a file exists return; } - auto converted = msgs::Convert(_resChanged); - if (this->samples == converted) + math::Vector3 converted{ + static_cast(ceil(_resChanged.x())), + static_cast(ceil(_resChanged.y())), + static_cast(ceil(_resChanged.z()))}; + if (this->samples.X() == converted.X() && + this->samples.Y() == converted.Y() && + this->samples.Z() == converted.Z()) { // If the sample has not changed return. // This is because resampling is expensive. @@ -184,7 +189,7 @@ class gz::sim::systems::EnvironmentPreloadPrivate } else if (unitName != "radians") { - ignerr << "Unrecognized unit " << unitName << "\n"; + gzerr << "Unrecognized unit " << unitName << "\n"; } } } diff --git a/src/systems/environment_preload/VisualizationTool.cc b/src/systems/environment_preload/VisualizationTool.cc index 75b37b6557..338b92aede 100644 --- a/src/systems/environment_preload/VisualizationTool.cc +++ b/src/systems/environment_preload/VisualizationTool.cc @@ -61,7 +61,7 @@ void EnvironmentVisualizationTool::Step( const UpdateInfo &_info, const EntityComponentManager &_ecm, const std::shared_ptr &_data, - double _xSamples, double _ySamples, double _zSamples) + unsigned int _xSamples, unsigned int _ySamples, unsigned int _zSamples) { if (this->finishedTime) { @@ -114,7 +114,7 @@ void EnvironmentVisualizationTool::Step( ///////////////////////////////////////////////// void EnvironmentVisualizationTool::Visualize( const std::shared_ptr &_data, - double _xSamples, double _ySamples, double _zSamples) + unsigned int _xSamples, unsigned int _ySamples, unsigned int _zSamples) { for (auto key : _data->frame.Keys()) { @@ -126,13 +126,13 @@ void EnvironmentVisualizationTool::Visualize( auto dy = step.Y() / _ySamples; auto dz = step.Z() / _zSamples; std::size_t idx = 0; - for (std::size_t x_steps = 0; x_steps < ceil(_xSamples); x_steps++) + for (std::size_t x_steps = 0; x_steps < _xSamples; x_steps++) { auto x = lower_bound.X() + x_steps * dx; - for (std::size_t y_steps = 0; y_steps < ceil(_ySamples); y_steps++) + for (std::size_t y_steps = 0; y_steps < _ySamples; y_steps++) { auto y = lower_bound.Y() + y_steps * dy; - for (std::size_t z_steps = 0; z_steps < ceil(_zSamples); z_steps++) + for (std::size_t z_steps = 0; z_steps < _zSamples; z_steps++) { auto z = lower_bound.Z() + z_steps * dz; auto res = frame.LookUp(session, math::Vector3d(x, y, z)); diff --git a/src/systems/environment_preload/VisualizationTool.hh b/src/systems/environment_preload/VisualizationTool.hh index bb2e2fce1b..06505c042b 100644 --- a/src/systems/environment_preload/VisualizationTool.hh +++ b/src/systems/environment_preload/VisualizationTool.hh @@ -69,7 +69,7 @@ class EnvironmentVisualizationTool /// \brief Create publisher structures whenever a new environment is made /// available. - /// \param[in] _data Data to be visuallized + /// \param[in] _data Data to be visualized /// \param[in] _info simulation info for current time step private: void CreatePointCloudTopics( const std::shared_ptr &_data, @@ -81,7 +81,7 @@ class EnvironmentVisualizationTool /// \brief Step the visualizations /// \param[in] _info The simulation info including timestep /// \param[in] _ecm The Entity-Component-Manager - /// \param[in] _data The data to be visuallized + /// \param[in] _data The data to be visualized /// \param[in] _xSample Samples along x /// \param[in] _ySample Samples along y /// \param[in] _zSample Samples along z @@ -89,16 +89,16 @@ class EnvironmentVisualizationTool const UpdateInfo &_info, const EntityComponentManager &_ecm, const std::shared_ptr &_data, - double _xSamples, double _ySamples, double _zSamples); + unsigned int _xSamples, unsigned int _ySamples, unsigned int _zSamples); /// \brief Publishes a sample of the data - /// \param[in] _data The data to be visuallized + /// \param[in] _data The data to be visualized /// \param[in] _xSample Samples along x /// \param[in] _ySample Samples along y /// \param[in] _zSample Samples along z private: void Visualize( const std::shared_ptr &_data, - double _xSamples, double _ySamples, double _zSamples); + unsigned int _xSamples, unsigned int _ySamples, unsigned int _zSamples); /// \brief Get the point cloud data. private: void Publish(); @@ -106,7 +106,7 @@ class EnvironmentVisualizationTool /// \brief Resize the point cloud structure (used to reallocate /// memory when resolution changes) /// \param[in] _ecm The Entity-Component-Manager - /// \param[in] _data The data to be visuallized + /// \param[in] _data The data to be visualized /// \param[in] _xSample Samples along x /// \param[in] _ySample Samples along y /// \param[in] _zSample Samples along z diff --git a/src/systems/lift_drag/LiftDrag.cc b/src/systems/lift_drag/LiftDrag.cc index 504e7c6334..c83837c5ff 100644 --- a/src/systems/lift_drag/LiftDrag.cc +++ b/src/systems/lift_drag/LiftDrag.cc @@ -74,7 +74,7 @@ class gz::sim::systems::LiftDragPrivate /// \brief Coefficient of Moment / alpha slope. /// Moment = C_M * q * S /// where q (dynamic pressure) = 0.5 * rho * v^2 - public: double cma = 0.01; + public: double cma = 0.0; /// \brief angle of attach when airfoil stalls public: double alphaStall = GZ_PI_2; @@ -331,8 +331,10 @@ void LiftDragPrivate::Update(EntityComponentManager &_ecm) double sinSweepAngle = math::clamp( spanwiseI.Dot(velI), minRatio, maxRatio); - // get cos from trig identity - double cosSweepAngle = sqrt(1.0 - sinSweepAngle * sinSweepAngle); + // The sweep adjustment depends on the velocity component normal to + // the wing leading edge which appears quadratically in the + // dynamic pressure, so scale by cos^2 . + double cos2SweepAngle = 1.0 - sinSweepAngle * sinSweepAngle; double sweep = std::asin(sinSweepAngle); // truncate sweep to within +/-90 deg @@ -394,7 +396,7 @@ void LiftDragPrivate::Update(EntityComponentManager &_ecm) { cl = (this->cla * this->alphaStall + this->claStall * (alpha - this->alphaStall)) * - cosSweepAngle; + cos2SweepAngle; // make sure cl is still great than 0 cl = std::max(0.0, cl); } @@ -402,12 +404,12 @@ void LiftDragPrivate::Update(EntityComponentManager &_ecm) { cl = (-this->cla * this->alphaStall + this->claStall * (alpha + this->alphaStall)) - * cosSweepAngle; + * cos2SweepAngle; // make sure cl is still less than 0 cl = std::min(0.0, cl); } else - cl = this->cla * alpha * cosSweepAngle; + cl = this->cla * alpha * cos2SweepAngle; // modify cl per control joint value if (controlJointPosition && !controlJointPosition->Data().empty()) @@ -425,16 +427,16 @@ void LiftDragPrivate::Update(EntityComponentManager &_ecm) { cd = (this->cda * this->alphaStall + this->cdaStall * (alpha - this->alphaStall)) - * cosSweepAngle; + * cos2SweepAngle; } else if (alpha < -this->alphaStall) { cd = (-this->cda * this->alphaStall + this->cdaStall * (alpha + this->alphaStall)) - * cosSweepAngle; + * cos2SweepAngle; } else - cd = (this->cda * alpha) * cosSweepAngle; + cd = (this->cda * alpha) * cos2SweepAngle; // make sure drag is positive cd = std::fabs(cd); @@ -448,7 +450,7 @@ void LiftDragPrivate::Update(EntityComponentManager &_ecm) { cm = (this->cma * this->alphaStall + this->cmaStall * (alpha - this->alphaStall)) - * cosSweepAngle; + * cos2SweepAngle; // make sure cm is still great than 0 cm = std::max(0.0, cm); } @@ -456,12 +458,12 @@ void LiftDragPrivate::Update(EntityComponentManager &_ecm) { cm = (-this->cma * this->alphaStall + this->cmaStall * (alpha + this->alphaStall)) - * cosSweepAngle; + * cos2SweepAngle; // make sure cm is still less than 0 cm = std::min(0.0, cm); } else - cm = this->cma * alpha * cosSweepAngle; + cm = this->cma * alpha * cos2SweepAngle; // Take into account the effect of control surface deflection angle to cm if (controlJointPosition && !controlJointPosition->Data().empty()) diff --git a/test/helpers/Util.hh b/test/helpers/Util.hh new file mode 100644 index 0000000000..0584403210 --- /dev/null +++ b/test/helpers/Util.hh @@ -0,0 +1,49 @@ +/* + * Copyright (C) 2024 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. + * + */ + +#include +#include +#include +#include + +#include + +namespace gz::sim::test +{ +/// \brief Wait until a service becomes available. +/// See https://github.com/gazebosim/gz-transport/issues/468 for why this might +/// be necessary before make a service request. +/// \param[in] _node Transport Node to use +/// \param[in] _service Name of service to wait for +/// \param[in] _timeoutS Time out in seconds +bool waitForService(const transport::Node &_node, const std::string &_service, + int _timeoutS = 5) +{ + int curSleep = 0; + while (curSleep < _timeoutS) + { + std::vector publishers; + if (_node.ServiceInfo(_service, publishers)) + { + return true; + } + std::this_thread::sleep_for(std::chrono::seconds(1)); + ++curSleep; + } + return false; +} +} // namespace gz::sim::test diff --git a/tutorials/files/joint_controllers/JoinControllerForceMode.gif b/tutorials/files/joint_controllers/JoinControllerForceMode.gif new file mode 100644 index 0000000000..0754b1eb07 Binary files /dev/null and b/tutorials/files/joint_controllers/JoinControllerForceMode.gif differ diff --git a/tutorials/files/joint_controllers/JointController.png b/tutorials/files/joint_controllers/JointController.png new file mode 100644 index 0000000000..b88711e4cf Binary files /dev/null and b/tutorials/files/joint_controllers/JointController.png differ diff --git a/tutorials/files/joint_controllers/JointControllerVelMode1.gif b/tutorials/files/joint_controllers/JointControllerVelMode1.gif new file mode 100644 index 0000000000..1f7578027d Binary files /dev/null and b/tutorials/files/joint_controllers/JointControllerVelMode1.gif differ diff --git a/tutorials/files/joint_controllers/JointControllerVelMode2.gif b/tutorials/files/joint_controllers/JointControllerVelMode2.gif new file mode 100644 index 0000000000..26d5280212 Binary files /dev/null and b/tutorials/files/joint_controllers/JointControllerVelMode2.gif differ diff --git a/tutorials/files/joint_controllers/JointPositionController.gif b/tutorials/files/joint_controllers/JointPositionController.gif new file mode 100644 index 0000000000..a5ae36a176 Binary files /dev/null and b/tutorials/files/joint_controllers/JointPositionController.gif differ diff --git a/tutorials/files/joint_controllers/JointTrajectoryController.gif b/tutorials/files/joint_controllers/JointTrajectoryController.gif new file mode 100644 index 0000000000..c1419854a0 Binary files /dev/null and b/tutorials/files/joint_controllers/JointTrajectoryController.gif differ diff --git a/tutorials/files/joint_controllers/JointTrajectoryController.png b/tutorials/files/joint_controllers/JointTrajectoryController.png new file mode 100644 index 0000000000..f1445fac29 Binary files /dev/null and b/tutorials/files/joint_controllers/JointTrajectoryController.png differ diff --git a/tutorials/joint_controller.md b/tutorials/joint_controller.md new file mode 100644 index 0000000000..12b77666e2 --- /dev/null +++ b/tutorials/joint_controller.md @@ -0,0 +1,675 @@ +\page jointcontrollers Joint Controllers + +Gazebo provides three joint controller plugins which are `JointController`, `JointPositionController`, and `JointTrajectoryController`. + +Let's see a detailed description of each of them and an example usage to help users select the right joint controller for their usage. + +## 1) JointController + +- Joint controller which can be attached to a model with a reference to a single joint. +- Currently, only the first axis of a joint can be actuated. + +### Modes of JointController + +1) Velocity mode: +This mode lets the user control the desired joint velocity directly. + +2) Force mode: +A user who wants to control joint velocity using a PID controller can use this mode. + +**Note**: This force mode is for the user who is looking to manually tune PID gains for velocity control according to a specific use case (e.g. Custom models). For general testing purposes, velocity mode will give the best results. + +All the parameters related to this controller can be found \ref gz::sim::systems::JointController "here". + +The commanded velocity(cmd_vel) can be published or subscribed at the topic: `/model//joint//cmd_vel` by default. + +Message data type: `Double` + +### Example usage + +Let's see an example for both modes using a simple model having only one joint. + +For controlling joints one would require adding the Gazebo's joint controller plugin to the existing `.sdf` file. + +1) Save the SDF file in the desired directory or create one + +e.g. + +```bash +mkdir gz_tutorial +cd gz_turtorial +``` + +2) In this tutorial we will be using the following SDF file (this is just a slight modification of the original `joint_controller.sdf` [example](https://github.com/gazebosim/gz-sim/blob/gz-sim7/examples/worlds/joint_controller.sdf)). + +After changing the directory, name the SDF file as `example.sdf` + +- SDF file: + +```xml + + + + + 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 + + + 0 0 0 0 0 0 + + 0.0 0.0 0.0 0 0 0 + + + 2.501 + 0 + 0 + 2.501 + 0 + 5 + + 120.0 + + + 0.0 0.0 0.0 0 0 0 + + + 0.5 0.5 0.01 + + + + + 0.0 0.0 0.0 0 0 0 + + + 0.5 0.5 0.01 + + + + + + 0.0 0.0 0.1 0 0 0 + + + 0.032 + 0 + 0 + 0.032 + 0 + 0.00012 + + 0.6 + + + + + 0.25 0.1 0.05 + + + + 0.2 0.8 0.2 1 + 0.8 0 0 1 + + + + + + 0.25 0.1 0.05 + + + + + + + world + base_link + + + + 0 0 -0.5 0 0 0 + base_link + rotor + + 0 0 1 + + + + + +``` + +3) Run the following command to launch the gazebo simulation: + +```bash +gz sim -v 4 -r example.sdf +``` + +This is how the model will look: + +
+ \image html files/joint_controllers/JointController.png width=50% +
+ +4) Now let's add the Gazebo JointController plugin to the SDF file. Add the following line to your file just before the tag ``. + +**Note**: All the plugins discussed here should be between `` and `` tags. Ideally just before the `` tag for better readability. + +- Velocity mode + +```xml + + j1 + 1.0 + +``` + +The initial velocity is set to 1.0 rad/s. + +
+ \image html files/joint_controllers/JointControllerVelMode1.gif width=50% +
+ +One can change the joint velocity by publishing a message on the topic `/model/joint_controller_demo/joint/j1/cmd_vel` or can change the topic name within the plugin. + +To change the topic name add the following line before `` tag in the SDF file. + +```xml +topic_name +``` + +- Sending velocity commands + +```bash +gz topic -t "/topic_name" -m gz.msgs.Double -p "data: 10.0" +``` + +
+ \image html files/joint_controllers/JointControllerVelMode2.gif width=50% +
+ +- Force mode + +Replace the velocity mode plugin mentioned above with the following lines in the SDF file for force mode. + +```xml + + j1 + true + 0.2 + 0.01 + +``` + +This would look almost the same as velocity mode if PID gains are tuned properly. + +5) Checking Joint states. +Here the state of the joint is obtained using the Gazebo’s JointStatepublisher plugin. Please visit \ref gz::sim::systems::JointStatePublisher for more information. +- Add the following lines to the SDF file before `` tag: +```xml + + j1 + +``` +- To check joint state. + +```bash +gz topic -e -t /world/default/model/joint_controller_demo/joint_state +``` + +```bash +joint { + name: "j1" + id: 12 + parent: "base_link" + child: "rotor" + pose { + position { + z: -0.5 + } + orientation { + w: 1 + } + } + axis1 { + xyz { + z: 1 + } + limit_lower: -inf + limit_upper: inf + position: 35.115896338490096 + velocity: 1.0000051832309742 + } +} +``` + +An example where p_gain was set to 2.0 and the joint controller failed to reach the desired velocity and behaved absurdly due to improper gains is shown below. + +
+ \image html files/joint_controllers/JoinControllerForceMode.gif width=50% +
+ +```bash +joint { + name: "j1" + id: 12 + parent: "base_link" + child: "rotor" + pose { + position { + z: -0.5 + } + orientation { + w: 1 + } + } + axis1 { + xyz { + z: 1 + } + limit_lower: -inf + limit_upper: inf + position: 44282.754868627489 + velocity: -2891.1685359866523 + } +} +``` + +## 2) JointPositionController + +- Joint position controller which can be attached to a model with a reference to a single joint. +- One can mention the axis of the joint they want to control. + +JointPositionController uses a PID controller to reach a desired joint position. + +All the parameters related to this controller can be found \ref gz::sim::systems::JointPositionController "here". + +Commanded position(cmd_pos) can be published or subscribed at the topic: `/model//joint///cmd_pos` by default. + +Message data type: `Double`. + +### Example usage: + +For this let's use the previously discussed SDF file. + +1) Replace the JointController plugin with the JointPositionController plugin in SDF file. + +```xml + + j1 + topic_name + 1 + 0.1 + 0.01 + 1 + -1 + 1000 + -1000 + +``` + +2) Sending joint position command. + +```bash +gz topic -t "/topic_name" -m gz.msgs.Double -p "data: -1.0" +``` + +
+ \image html files/joint_controllers/JointPositionController.gif width=50% +
+ + +3) Checking joint state. + +```bash +gz topic -e -t /world/default/model/joint_controller_demo/joint_state +``` + +```bash +joint { + name: "j1" + id: 12 + parent: "base_link" + child: "rotor" + pose { + position { + z: -0.5 + } + orientation { + w: 1 + } + } + axis1 { + xyz { + z: 1 + } + limit_lower: -inf + limit_upper: inf + position: 0.99999991907580654 + velocity: 8.1005154347602952e-09 + } +} +``` + +## 3) JointTrajectoryController. + +- Joint trajectory controller, which can be attached to a model with reference to one or more 1-axis joints to follow a trajectory. + +JointTrajectoryController lets’s user specify the required position, velocity, and effort with respect to time. For velocity and position, this controller uses a PID controller. + +A detailed description and related parameter of JointTrajectoryController can be found \ref gz::sim::systems::JointTrajectoryController "here". + +The trajectory message can be published or subscribed at `/model/${MODEL_NAME}/joint_trajectory` by default. + +Message type: [`JointTrajectory`](https://gazebosim.org/api/msgs/7.2/classignition_1_1msgs_1_1JointTrajectory.html) + +### Example usage: + +Let’s set up a new model for this example. A two-linked manipulator arm which has a total of two joints to control ([`joint_trajectory_controller.sdf`](https://github.com/gazebosim/gz-sim/blob/gz-sim7/examples/worlds/joint_trajectory_controller.sdf) is the original example). Name it as `example2.sdf`. + +- SDF file: + +```xml + + + + 0.4 0.4 0.4 + false + + + + + + false + 5 5 5 0 0 0 + 0.8 0.8 0.8 1 + 0.2 0.2 0.2 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -1 -1 -1 + + + + + + + -0.1 0 0 0 1.5708 0 + true + + + + + 0 0 1 + 5 5 + + + + 0.5 0.5 0.5 1 + 0.5 0.5 0.5 1 + + + + + + + + 0 0 0 0 -3.14159 0 + + + world + RR_position_control_link0 + + + + + + + 0.025 + + + + + + + 0.025 + + + + 0 0.5 0.5 1 + 0 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + 0 0 0.1 0 0 0 + + + + 0.01 + 0.2 + + + + + + + 0.0125 + + + + + + + 0.01 + 0.2 + + + + 0.5 0 0 1 + 0.8 0 0 1 + 0.8 0 0 1 + + + + 0 0 0.2 0 0 0 + + + 0.0125 + + + + 0.5 0.5 0.5 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + 0.1 + + 0.0003358 + 0.0003358 + 0.000005 + + + + + 0 0 0.1 0 0 0 + + + + 0.01 + 0.2 + + + + + + + 0.01 + 0.2 + + + + 0.5 0 0 1 + 0.8 0 0 1 + 0.8 0 0 1 + + + + 0.1 + + 0.0003358 + 0.0003358 + 0.000005 + + + + + + 0 0 0 0 0 0 + RR_position_control_link0 + RR_position_control_link1 + + 1 0 0 + + 0.5 + + + + + 0 0 0.1 0 0 0 + RR_position_control_link1 + RR_position_control_link2 + + 1 0 0 + + 0.25 + + + + + + + +``` + +1) Launching gazebo simulation. + +```bash +gz sim -v 4 -r example2.sdf +``` + +This is how the model will look: + +
+ \image html files/joint_controllers/JointTrajectoryController.png width=50% +
+ +2) Adding the JointTrajectoryController plugin and let’s do position control for both joints. + +Append the following lines before `` tag in the SDF file. + +```xml + + RR_position_control_joint1 + 0.7854 + 20 + 0.4 + 1.0 + -1 + 1 + -20 + 20 + + RR_position_control_joint2 + -1.5708 + 10 + 0.2 + 0.5 + -1 + 1 + -10 + 10 + +``` + +3) Sending trajectory message (Can also change the topic name). + +```bash +gz topic -t "topic_name" -m gz.msgs.JointTrajectory -p ' + joint_names: "RR_position_control_joint1" + joint_names: "RR_position_control_joint2" + points { + positions: -0.7854 + positions: 1.5708 + time_from_start { + sec: 0 + nsec: 250000000 + } + } + points { + positions: -1.5708 + positions: 0 + time_from_start { + sec: 0 + nsec: 500000000 + } + } + points { + positions: -1.5708 + positions: -1.5708 + time_from_start { + sec: 0 + nsec: 750000000 + } + } + points { + positions: 0 + positions: 0 + time_from_start { + sec: 1 + nsec: 0 + } + } +``` + +
+ \image html files/joint_controllers/JointTrajectoryController.gif width=50% +
+ +**Note**: by default velocity and position control are disabled if one wants to use this mode, they must specify the PID gains value according to usage. + +In case, PID gains are not specified then by default force mode will work. + +4) Checking the progress of the commanded trajectory. + +```bash +gz topic -e -t "/model/RR_position_control/joint_trajectory_progress" +``` + +This returns the progress of the commanded trajectory which is a value between (0,1]. + +Finally, JointTrajectoryController can be used for hybrid control (e.g. In manipulator robots) where value from position PID, velocity PID, and commanded force value are summed up and applied.