From 435cc35563faf3f4af4dcba2ea99c8dec36bb4d0 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Fri, 25 Jul 2025 23:40:20 +0000 Subject: [PATCH 01/10] Add lookup wheel slip system Signed-off-by: Ian Chen --- .../trisphere_cycle_lookup_wheel_slip.sdf | 883 ++++++++++++++++++ src/ServerPrivate.cc | 10 +- src/ServerPrivate.hh | 5 +- src/SimulationRunner.hh | 4 - src/systems/CMakeLists.txt | 1 + src/systems/lookup_wheel_slip/CMakeLists.txt | 6 + .../lookup_wheel_slip/LookupWheelSlip.cc | 591 ++++++++++++ .../lookup_wheel_slip/LookupWheelSlip.hh | 66 ++ src/systems/wheel_slip/WheelSlip.cc | 306 +++++- src/systems/wheel_slip/WheelSlip.hh | 6 + 10 files changed, 1850 insertions(+), 28 deletions(-) create mode 100644 examples/worlds/trisphere_cycle_lookup_wheel_slip.sdf create mode 100644 src/systems/lookup_wheel_slip/CMakeLists.txt create mode 100644 src/systems/lookup_wheel_slip/LookupWheelSlip.cc create mode 100644 src/systems/lookup_wheel_slip/LookupWheelSlip.hh diff --git a/examples/worlds/trisphere_cycle_lookup_wheel_slip.sdf b/examples/worlds/trisphere_cycle_lookup_wheel_slip.sdf new file mode 100644 index 0000000000..318e462996 --- /dev/null +++ b/examples/worlds/trisphere_cycle_lookup_wheel_slip.sdf @@ -0,0 +1,883 @@ + + + + + + -2 0 -9.8 + + + + + + + + + + true + + + + + 0 0 1 + 100 100 + + + + + + + 0 0 1 + 100 100 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + + 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.40855911616047164 0 0.38502293110800634 0 -0.522020852957719 0 + + 0.0 0 0 0 0 0 + 10 + + 0.22799999999999998 + 0.7435210984814149 + 0.9655210984814149 + 0 + 0 + 0 + + + + -0.4713346258704366 0 0 1.5707963267948966 0 0 + + + 1.0392304845413263 + 0.03 + + + + + -0.4713346258704366 0 0 1.5707963267948966 0 0 + + 0.8 0.3 0.3 1 + 0.8 0.3 0.3 1 + 0.8 0.3 0.3 1 + + + + 1.0392304845413263 + 0.03 + + + + + 0 0.17155177419583564 0 0 1.5707963267948966 -0.3490658503988659 + + + 1.0031676644991372 + 0.03 + + + + + 0 0.17155177419583564 0 0 1.5707963267948966 -0.3490658503988659 + + 0.8 0.3 0.3 1 + 0.8 0.3 0.3 1 + 0.8 0.3 0.3 1 + + + + 1.0031676644991372 + 0.03 + + + + + 0 -0.17155177419583564 0 0 1.5707963267948966 0.3490658503988659 + + + 1.0031676644991372 + 0.03 + + + + + 0 -0.17155177419583564 0 0 1.5707963267948966 0.3490658503988659 + + 0.8 0.3 0.3 1 + 0.8 0.3 0.3 1 + 0.8 0.3 0.3 1 + + + + 1.0031676644991372 + 0.03 + + + + + + 0.04144088383952833 0 0.38502293110800634 0 -0.17453292519943295 0 + + 3 + + 0.15820312499999997 + 0.058359374999999984 + 0.10265624999999999 + 0 + 0 + 0 + + + + 0 0 0.397747564417433 1.5707963267948966 0 0 + + + 0.6363961030678927 + 0.0375 + + + + + 0 0 0.397747564417433 1.5707963267948966 0 0 + + 0.8 0.3 0.3 1 + 0.8 0.3 0.3 1 + 0.8 0.3 0.3 1 + + + + 0.6363961030678927 + 0.0375 + + + + + 0 0 0.2386485386504598 0 0 0 + + + 0.31819805153394637 + 0.0375 + + + + + 0 0 0.2386485386504598 0 0 0 + + 0.8 0.3 0.3 1 + 0.8 0.3 0.3 1 + 0.8 0.3 0.3 1 + + + + 0.31819805153394637 + 0.0375 + + + + + 0 0 -0.23864853865045976 1.5707963267948966 0 0 + + + 0.6363961030678927 + 0.015 + + + + + 0 0 -0.23864853865045976 1.5707963267948966 0 0 + + 0.8 0.3 0.3 1 + 0.8 0.3 0.3 1 + 0.8 0.3 0.3 1 + + + + 0.6363961030678927 + 0.015 + + + + + 0 0.15909902576697318 -0.07954951288348658 0.7853981633974483 0 0 + + + 0.45 + 0.0375 + + + + + 0 0.15909902576697318 -0.07954951288348658 0.7853981633974483 0 0 + + 0.8 0.3 0.3 1 + 0.8 0.3 0.3 1 + 0.8 0.3 0.3 1 + + + + 0.45 + 0.0375 + + + + + 0 -0.15909902576697318 -0.07954951288348658 -0.7853981633974483 0 0 + + + 0.45 + 0.0375 + + + + + 0 -0.15909902576697318 -0.07954951288348658 -0.7853981633974483 0 0 + + 0.8 0.3 0.3 1 + 0.8 0.3 0.3 1 + 0.8 0.3 0.3 1 + + + + 0.45 + 0.0375 + + + + + + 0.08288176767905665 0 0.15 0 0 0 + + 0.8 0.3 0.3 1 + 0.8 0.3 0.3 1 + 0.8 0.3 0.3 1 + + + 0.5 + + 0.0045 + 0.0045 + 0.0045 + 0 + 0 + 0 + + + + + + 0.15 + + + + + + 100000000.0 + 1 + 0.0005 + + + + + + + 0.8 0.3 0.3 1 + 0.8 0.3 0.3 1 + 0.8 0.3 0.3 1 + + + + 0.15 + + + + + + frame + fork + + 0 0 1 + + -0.9599310885968813 + 0.9599310885968813 + + + + + fork + wheel_front + + 0 1 0 + + + + -0.8171182323209433 0.5196152422706631 0.15 0 0 0 + + 0.5 + + 0.0045 + 0.0045 + 0.0045 + 0 + 0 + 0 + + + + + + 0.15 + + + + + + 100000000.0 + 1 + 0.0005 + + + + + + + 0.8 0.3 0.3 1 + 0.8 0.3 0.3 1 + 0.8 0.3 0.3 1 + + + + 0.15 + + + + + + frame + wheel_rear_left + + 0 1 0 + + + + -0.8171182323209433 -0.5196152422706631 0.15 0 0 0 + + 0.5 + + 0.0045 + 0.0045 + 0.0045 + 0 + 0 + 0 + + + + + + 0.15 + + + + + + 100000000.0 + 1 + 0.0005 + + + + + + + 0.8 0.3 0.3 1 + 0.8 0.3 0.3 1 + 0.8 0.3 0.3 1 + + + + 0.15 + + + + + + frame + wheel_rear_right + + 0 1 0 + + + + + + 0.15 + 0 + 0 + 77 + + + 0.15 + 0 + 0 + 32 + + + 0.15 + 0 + 0 + 32 + + + + + 0 2 0 0 0 0 + + -0.40855911616047164 0 0.38502293110800634 0 -0.522020852957719 0 + + 0.0 0 0 0 0 0 + 10 + + 0.22799999999999998 + 0.7435210984814149 + 0.9655210984814149 + 0 + 0 + 0 + + + + -0.4713346258704366 0 0 1.5707963267948966 0 0 + + + 1.0392304845413263 + 0.03 + + + + + -0.4713346258704366 0 0 1.5707963267948966 0 0 + + 0.3 0.3 0.8 1 + 0.3 0.3 0.8 1 + 0.3 0.3 0.8 1 + + + + 1.0392304845413263 + 0.03 + + + + + 0 0.17155177419583564 0 0 1.5707963267948966 -0.3490658503988659 + + + 1.0031676644991372 + 0.03 + + + + + + 0.3 0.3 0.8 1 + 0.3 0.3 0.8 1 + 0.3 0.3 0.8 1 + + 0 0.17155177419583564 0 0 1.5707963267948966 -0.3490658503988659 + + + 1.0031676644991372 + 0.03 + + + + + 0 -0.17155177419583564 0 0 1.5707963267948966 0.3490658503988659 + + + 1.0031676644991372 + 0.03 + + + + + 0 -0.17155177419583564 0 0 1.5707963267948966 0.3490658503988659 + + 0.3 0.3 0.8 1 + 0.3 0.3 0.8 1 + 0.3 0.3 0.8 1 + + + + 1.0031676644991372 + 0.03 + + + + + + 0.04144088383952833 0 0.38502293110800634 0 -0.17453292519943295 0 + + 3 + + 0.15820312499999997 + 0.058359374999999984 + 0.10265624999999999 + 0 + 0 + 0 + + + + 0 0 0.397747564417433 1.5707963267948966 0 0 + + + 0.6363961030678927 + 0.0375 + + + + + 0 0 0.397747564417433 1.5707963267948966 0 0 + + 0.3 0.3 0.8 1 + 0.3 0.3 0.8 1 + 0.3 0.3 0.8 1 + + + + 0.6363961030678927 + 0.0375 + + + + + 0 0 0.2386485386504598 0 0 0 + + + 0.31819805153394637 + 0.0375 + + + + + 0 0 0.2386485386504598 0 0 0 + + 0.3 0.3 0.8 1 + 0.3 0.3 0.8 1 + 0.3 0.3 0.8 1 + + + + 0.31819805153394637 + 0.0375 + + + + + 0 0 -0.23864853865045976 1.5707963267948966 0 0 + + + 0.6363961030678927 + 0.015 + + + + + 0 0 -0.23864853865045976 1.5707963267948966 0 0 + + 0.3 0.3 0.8 1 + 0.3 0.3 0.8 1 + 0.3 0.3 0.8 1 + + + + 0.6363961030678927 + 0.015 + + + + + 0 0.15909902576697318 -0.07954951288348658 0.7853981633974483 0 0 + + + 0.45 + 0.0375 + + + + + 0 0.15909902576697318 -0.07954951288348658 0.7853981633974483 0 0 + + 0.3 0.3 0.8 1 + 0.3 0.3 0.8 1 + 0.3 0.3 0.8 1 + + + + 0.45 + 0.0375 + + + + + 0 -0.15909902576697318 -0.07954951288348658 -0.7853981633974483 0 0 + + + 0.45 + 0.0375 + + + + + 0 -0.15909902576697318 -0.07954951288348658 -0.7853981633974483 0 0 + + 0.3 0.3 0.8 1 + 0.3 0.3 0.8 1 + 0.3 0.3 0.8 1 + + + + 0.45 + 0.0375 + + + + + + 0.08288176767905665 0 0.15 0 0 0 + + 0.5 + + 0.0045 + 0.0045 + 0.0045 + 0 + 0 + 0 + + + + + + 0.15 + + + + + + 100000000.0 + 1 + 0.0005 + + + + + + + 0.3 0.3 0.8 1 + 0.3 0.3 0.8 1 + 0.3 0.3 0.8 1 + + + + 0.15 + + + + + + frame + fork + + 0 0 1 + + -0.9599310885968813 + 0.9599310885968813 + + + + + fork + wheel_front + + 0 1 0 + + + + -0.8171182323209433 0.5196152422706631 0.15 0 0 0 + + 0.5 + + 0.0045 + 0.0045 + 0.0045 + 0 + 0 + 0 + + + + + + 0.15 + + + + + + 100000000.0 + 1 + 0.0005 + + + + + + + 0.3 0.3 0.8 1 + 0.3 0.3 0.8 1 + 0.3 0.3 0.8 1 + + + + 0.15 + + + + + + frame + wheel_rear_left + + 0 1 0 + + + + -0.8171182323209433 -0.5196152422706631 0.15 0 0 0 + + 0.5 + + 0.0045 + 0.0045 + 0.0045 + 0 + 0 + 0 + + + + + + 0.15 + + + + + + 100000000.0 + 1 + 0.0005 + + + + + + + 0.3 0.3 0.8 1 + 0.3 0.3 0.8 1 + 0.3 0.3 0.8 1 + + + + 0.15 + + + + + + frame + wheel_rear_right + + 0 1 0 + + + + + + 1 + 1 + 77 + 0.15 + + + 1 + 1 + 32 + 0.15 + + + 1 + 1 + 32 + 0.15 + + + + lookup_slip_map.png + 10 + 10 + 0.04 + 0.04 + 0.4 + wheel_front + wheel_rear_left + wheel_rear_right + + + + + + diff --git a/src/ServerPrivate.cc b/src/ServerPrivate.cc index bf5c1161b0..6d2682038a 100644 --- a/src/ServerPrivate.cc +++ b/src/ServerPrivate.cc @@ -198,16 +198,22 @@ bool ServerPrivate::Run(const uint64_t _iterations, } else { + if (!this->workerPool.has_value()) + { + // Initialize the workerpool if we do have multiple simulation runners and + // it hasn't been initialized before + this->workerPool.emplace(2); + } for (std::unique_ptr &runner : this->simRunners) { - this->workerPool.AddWork([&runner, &_iterations] () + this->workerPool->AddWork([&runner, &_iterations] () { runner->Run(_iterations); }); } // Wait for the runner to complete. - result = this->workerPool.WaitForResults(); + result = this->workerPool->WaitForResults(); } // See comments ServerPrivate::Stop() for why we lock this mutex here. diff --git a/src/ServerPrivate.hh b/src/ServerPrivate.hh index 6fe36f8b1f..8dd9930166 100644 --- a/src/ServerPrivate.hh +++ b/src/ServerPrivate.hh @@ -154,7 +154,10 @@ namespace gz const gz::msgs::ServerControl &_req, msgs::Boolean &_res); /// \brief A pool of worker threads. - public: common::WorkerPool workerPool{2}; + /// \note We use optional here since most of the time, there will be a + /// single simulation runner and a workerpool is not needed. We will + /// initialize the workerpool as necessary later on. + public: std::optional workerPool; /// \brief All the simulation runners. public: std::vector> simRunners; diff --git a/src/SimulationRunner.hh b/src/SimulationRunner.hh index 44dd1c7ff0..f97a40b7cd 100644 --- a/src/SimulationRunner.hh +++ b/src/SimulationRunner.hh @@ -41,7 +41,6 @@ #include #include -#include #include #include @@ -433,9 +432,6 @@ namespace gz /// \brief Manager of distributing/receiving network work. private: std::unique_ptr networkMgr{nullptr}; - /// \brief A pool of worker threads. - private: common::WorkerPool workerPool{2}; - /// \brief Wall time of the previous update. private: std::chrono::steady_clock::time_point prevUpdateRealTime; diff --git a/src/systems/CMakeLists.txt b/src/systems/CMakeLists.txt index 16eae8326b..e6b902c36f 100644 --- a/src/systems/CMakeLists.txt +++ b/src/systems/CMakeLists.txt @@ -118,6 +118,7 @@ add_subdirectory(log) add_subdirectory(log_video_recorder) add_subdirectory(logical_audio_sensor_plugin) add_subdirectory(logical_camera) +add_subdirectory(lookup_wheel_slip) add_subdirectory(magnetometer) add_subdirectory(model_photo_shoot) add_subdirectory(mecanum_drive) diff --git a/src/systems/lookup_wheel_slip/CMakeLists.txt b/src/systems/lookup_wheel_slip/CMakeLists.txt new file mode 100644 index 0000000000..365b7b5f1c --- /dev/null +++ b/src/systems/lookup_wheel_slip/CMakeLists.txt @@ -0,0 +1,6 @@ +gz_add_system(lookup-wheel-slip + SOURCES + LookupWheelSlip.cc + PUBLIC_LINK_LIBS + gz-common::gz-common +) diff --git a/src/systems/lookup_wheel_slip/LookupWheelSlip.cc b/src/systems/lookup_wheel_slip/LookupWheelSlip.cc new file mode 100644 index 0000000000..77d68dd86a --- /dev/null +++ b/src/systems/lookup_wheel_slip/LookupWheelSlip.cc @@ -0,0 +1,591 @@ +/* + * Copyright (C) 2025 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 "LookupWheelSlip.hh" + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include "gz/sim/components/Geometry.hh" +#include "gz/sim/components/World.hh" +#include "gz/sim/Link.hh" +#include "gz/sim/Model.hh" +#include "gz/sim/Util.hh" + +using namespace gz; +using namespace sim; +using namespace systems; + +class gz::sim::systems::LookupWheelSlipPrivate +{ + /// \brief Can the nominal (original) surface param values from the parameter + /// registry. + /// \param[in] _ecm Immutable reference to the EntityComponentManager + public: bool GetNominalSurfaceParams(const EntityComponentManager &_ecm); + + /// \brief Get the scoped param name for an entity + /// \param[in] _entity Entity id. + /// \param[in] _paramName Name of the parameter. + /// \param[in] _ecm Immutable reference to the EntityComponentManager + /// \return Scoped name of the parameter. + public: std::string ScopedParamName(const Entity &_entity, + const std::string_view &_paramName, + const EntityComponentManager &_ecm) const; + + /// \brief Update the params in the parameter registry + /// \param[in] _ecm Immutable reference to the EntityComponentManager + public: void UpdateParams(const EntityComponentManager &_ecm); + + /// \brief Model interface + public: Model model{kNullEntity}; + + /// \brief /Filename of the slip map texture + public: std::string slipMapFilename; + + /// \brief The loaded slip map image + public: common::Image slipMapImg; + + /// \brief RGB data of the slip map img; + public: std::vector slipMapRgb; + + /// \brief X dimension of the collision geometry in meters. + public: double sizeX = 0.0; + + /// \brief Y dimension of the collision geometry in meters. + public: double sizeY = 0.0; + + /// \brief The lateral slip delta to apply. + public: double lateralSlipDelta = 0.05; + + /// \brief The lateral slip delta to apply. + public: double longitudinalSlipDelta = 0.005; + + /// \brief The friction delta to apply. + public: double frictionDelta = 0.5; + + /// \brief A transfrom from world to img space. + public: math::Matrix4d worldToImgTransform; + + /// \brief True if the system is initialized, false otherwise + public: bool initialized = false; + + /// \brief A unique set of link entities + public: std::unordered_set linkEntities; + + /// \brief The nominal surface param values. + /// These are the original, non-modified param values. + /// The elements are + public: std::unordered_map nominalParamValues; + + /// \brief The new surface param values computed by this plugin. The params + /// on the paramater registry will be updated by these values. + /// The elements are + public: std::unordered_map newParamValues; + + /// \brief Transport parameter client + public: transport::parameters::ParametersClient client; + + /// \brief Lateral slip compliance parameter name in the parameter registry. + /// The name needs to match the one in WheelSlip system. + public: static constexpr std::string_view kSlipComplianceLateralParamName = + "slip_compliance_lateral"; + + /// \brief Longitudinal slip compliance parameter name in the parameter + /// registry. + /// The name needs to match the one in WheelSlip system. + public: static constexpr std::string_view + kSlipComplianceLongitudinalParamName = "slip_compliance_longitudinal"; + + /// \brief Primary friction coefficient parameter name in the parameter + /// registry. + /// The name needs to match the one in WheelSlip system. + public: static constexpr std::string_view + kFrictionCoefficientPrimaryParamName = "friction_coefficient_primary"; + + /// \brief Secondary friction coefficient parameter name in the parameter + /// registry. + /// The name needs to match the one in WheelSlip system. + public: static constexpr std::string_view + kFrictionCoefficientSecondaryParamName = "friction_coefficient_secondary"; + + /// \brief Lateral slip color channel representation (red channel) + public: static constexpr unsigned int kLateralColorChannel = 0; + + /// \brief Longitudinal slip color channel representation (green channel) + public: static constexpr unsigned int kLongitudinalColorChannel = 1; + + /// \brief Friction color channel representation (blue channel) + public: static constexpr unsigned int kFrictionColorChannel = 2; + + /// \brief Nominal color value - half of 8 bit + public: static constexpr unsigned int kNominalColor = 1 << 7; +}; + +////////////////////////////////////////////////// +LookupWheelSlip::LookupWheelSlip() + : dataPtr(std::make_unique()) +{ +} + +////////////////////////////////////////////////// +void LookupWheelSlip::Configure(const Entity &_entity, + const std::shared_ptr &_sdf, + EntityComponentManager &_ecm, + EventManager &/*_eventMgr*/) +{ + this->dataPtr->model = Model(_entity); + + if (!this->dataPtr->model.Valid(_ecm)) + { + gzerr << "LookupWheelSlip plugin should be attached to a model entity. " + << "Failed to initialize." << std::endl; + return; + } + + auto slipMapElem = _sdf->FindElement("slip_map"); + if (!slipMapElem) + { + gzerr << "No 'slip_map' provided. Will not dynamically update " + << "wheel slip values. " << std::endl; + return; + } + this->dataPtr->slipMapFilename = slipMapElem->Get(); + if (this->dataPtr->slipMapFilename.empty()) + { + gzerr << "No value for 'slip_path' provided. Will not dynamically update " + << "wheel slip values. " << std::endl; + return; + } + + // \todo(iche033) Auto determine size from region collision geometry? + math::Vector3d colSize; + /* + for (const auto &linkEntity : regionModel.Links(_ecm)) + { + sim::Link link(linkEntity); + for (const auto &collisionEntity : link.Collisions(_ecm)) + { + auto geomComp = _ecm.Component(collisionEntity); + if (geomComp && geomComp->Data().Type() == sdf::GeometryType::HEIGHTMAP) + { + const sdf::Heightmap *shape = geomComp->Data().HeightmapShape(); + colSize = shape->Size(); + break; + } + } + } + */ + + auto sizeXElem = _sdf->FindElement("size_x"); + if (sizeXElem) + { + this->dataPtr->sizeX = sizeXElem->Get(); + } + else if (colSize.X() > 0.0) + { + this->dataPtr->sizeX = colSize.X(); + } + else + { + gzerr << "Unable to determine size x from collision and missing " + << "'size_x' param. Will not dynamically update wheel slip values." + << std::endl; + return; + } + auto sizeYElem = _sdf->FindElement("size_y"); + if (sizeYElem) + { + this->dataPtr->sizeY = sizeYElem->Get(); + } + else if (colSize.Y() > 0.0) + { + this->dataPtr->sizeY = colSize.Y(); + } + else + { + gzerr << "Unable to determine size y from collision and missing " + << "'size_y' param. Will not dynamically update wheel slip values." + << std::endl; + return; + } + + // \todo(iche033) Swap R and B channels? + + // transformation matrix from world to image coordinates + std::string filePath; + if (common::isFile(this->dataPtr->slipMapFilename)) + { + filePath = this->dataPtr->slipMapFilename; + } + else if (common::isRelativePath(this->dataPtr->slipMapFilename)) + { + auto *component = + _ecm.Component(worldEntity(_ecm)); + const std::string rootPath = + common::parentPath(component->Data().Element()->FilePath()); + std::string path = common::joinPaths(rootPath, + this->dataPtr->slipMapFilename); + if (common::isFile(path)) + filePath = path; + } + if (filePath.empty()) + { + gzerr << "Unable to find slip_map file: " << this->dataPtr->slipMapFilename + << std::endl; + return; + } + gzdbg << "Using slip_map: " << filePath << std::endl; + this->dataPtr->slipMapImg.Load(filePath); + this->dataPtr->slipMapRgb = this->dataPtr->slipMapImg.RGBData(); + this->dataPtr->worldToImgTransform(0, 0) = + this->dataPtr->sizeX / + static_cast(this->dataPtr->slipMapImg.Width()); + this->dataPtr->worldToImgTransform(0, 3) = -this->dataPtr->sizeX / 2.0; + this->dataPtr->worldToImgTransform(1, 1) = + (this->dataPtr->sizeY / + static_cast(this->dataPtr->slipMapImg.Height())) * cos(GZ_PI); + this->dataPtr->worldToImgTransform(1, 2) = -sin(GZ_PI); + this->dataPtr->worldToImgTransform(1, 3) = this->dataPtr->sizeY / 2.0; + this->dataPtr->worldToImgTransform(2, 1) = sin(GZ_PI); + this->dataPtr->worldToImgTransform(2, 2) = cos(GZ_PI); + this->dataPtr->worldToImgTransform(3, 3) = 1.0; + this->dataPtr->worldToImgTransform = + this->dataPtr->worldToImgTransform.Inverse(); + + std::unordered_set wheelLinkNames; + auto wheelLinkElem = _sdf->FindElement("wheel_link_name"); + while (wheelLinkElem) + { + wheelLinkNames.insert(wheelLinkElem->Get()); + wheelLinkElem = wheelLinkElem->GetNextElement("wheel_link_name"); + } + if (wheelLinkNames.empty()) + { + gzerr << "Error loading 'wheel_slip_name' element. Unable to load plugin." + << std::endl; + return; + } + + for (const auto &linkName : wheelLinkNames) + { + auto link = Link(this->dataPtr->model.LinkByName(_ecm, linkName)); + if (!link.Valid(_ecm)) + { + gzerr << "Could not find link named [" << linkName + << "] in model [" << this->dataPtr->model.Name(_ecm) << "]" + << std::endl; + continue; + } + this->dataPtr->linkEntities.insert(link.Entity()); + } + + auto latSlipDeltaElem = _sdf->FindElement("slip_compliance_lateral_delta"); + if (latSlipDeltaElem) + { + this->dataPtr->lateralSlipDelta = latSlipDeltaElem->Get(); + } + auto lonSlipDeltaElem = _sdf->FindElement( + "slip_compliance_longitudinal_delta"); + if (lonSlipDeltaElem) + { + this->dataPtr->longitudinalSlipDelta = lonSlipDeltaElem->Get(); + } + auto frictionDeltaElem = _sdf->FindElement("friction_delta"); + if (frictionDeltaElem) + { + this->dataPtr->frictionDelta = frictionDeltaElem->Get(); + } + + std::string ns = sim::scopedName(worldEntity(_ecm), _ecm, "/", true); + this->dataPtr->client = transport::parameters::ParametersClient(ns); + this->dataPtr->initialized = true; + + gzdbg << "LookupWheelSlip plugin params: \n"; + gzdbg << "slip_map: " << this->dataPtr->slipMapFilename<< "\n"; + gzdbg << "size_x: " << this->dataPtr->sizeX << "\n"; + gzdbg << "size_y: " << this->dataPtr->sizeY << "\n"; + gzdbg << "slip_compliance_lateral_delta: " + << this->dataPtr->lateralSlipDelta<< "\n"; + gzdbg << "slip_compliance_longitudinal_delta: " + << this->dataPtr->longitudinalSlipDelta<< "\n"; + gzdbg << "friction_delta: " << this->dataPtr->frictionDelta << "\n"; + gzdbg << "wheel link name(s): " << "\n"; + for (const auto &linkName : wheelLinkNames) + gzdbg << " " << linkName << "\n"; +} + +////////////////////////////////////////////////// +void LookupWheelSlip::PreUpdate(const UpdateInfo &, + EntityComponentManager &_ecm) +{ + if (!this->dataPtr->initialized) + return; + + if (this->dataPtr->nominalParamValues.empty() && + !this->dataPtr->GetNominalSurfaceParams(_ecm)) + { + return; + } + + this->dataPtr->UpdateParams(_ecm); +} + +////////////////////////////////////////////////// +std::string LookupWheelSlipPrivate::ScopedParamName( + const Entity &_entity, + const std::string_view &_paramName, + const EntityComponentManager &_ecm) const +{ + std::string prefix = std::string("WheelSlip.") + + sim::scopedName(_entity, _ecm, ".", false); + return prefix + "." + std::string(_paramName); +} + +////////////////////////////////////////////////// +bool LookupWheelSlipPrivate::GetNominalSurfaceParams( + const EntityComponentManager &_ecm) +{ + bool receivedAllParams = true; + for (const auto &linkEnt : this->linkEntities) + { + // Slip compliance lateral + std::string paramName = + this->ScopedParamName(linkEnt, kSlipComplianceLateralParamName, _ecm); + msgs::Double msg; + transport::parameters::ParameterResult result = + this->client.Parameter(paramName, msg); + if (!result) + { + receivedAllParams = false; + gzerr << "Unable to get parameter " << paramName << std::endl; + } + else + { + this->nominalParamValues[paramName] = msg.data(); + } + + // Slip compliance longitudinal + paramName = + this->ScopedParamName( + linkEnt, kSlipComplianceLongitudinalParamName, _ecm); + result = this->client.Parameter(paramName, msg); + if (!result) + { + receivedAllParams = false; + gzerr << "Unable to get parameter " << paramName << std::endl; + } + else + { + this->nominalParamValues[paramName] = msg.data(); + } + + // Primary friction coeff + paramName = + this->ScopedParamName( + linkEnt, kFrictionCoefficientPrimaryParamName, _ecm); + result = this->client.Parameter(paramName, msg); + if (!result) + { + receivedAllParams = false; + gzerr << "Unable to get parameter " << paramName << std::endl; + } + else + { + this->nominalParamValues[paramName] = msg.data(); + } + + // Secondary friction coeff + paramName = + this->ScopedParamName( + linkEnt, kFrictionCoefficientSecondaryParamName, _ecm); + result = this->client.Parameter(paramName, msg); + if (!result) + { + receivedAllParams = false; + gzerr << "Unable to get parameter " << paramName << std::endl; + } + else + { + this->nominalParamValues[paramName] = msg.data(); + } + } + + if (!receivedAllParams) + this->nominalParamValues.clear(); + + for (const auto &coeffs : this->nominalParamValues) + { + gzdbg << "Received nominal param: " << coeffs.first + << ": " << coeffs.second << std::endl; + } + return receivedAllParams; +} + +////////////////////////////////////////////////// +void LookupWheelSlipPrivate::UpdateParams( + const EntityComponentManager &_ecm) +{ + for (const auto &linkEnt : this->linkEntities) + { + sim::Link link(linkEnt); + // math::Vector3d linkWorldPos = link.WorldPose(_ecm).value().Pos(); + math::Vector3d linkWorldPos(1, 2, 0.5); + math::Vector3d imgPos = this->worldToImgTransform * linkWorldPos; + + int u = static_cast(std::round(imgPos.X())); + int v = static_cast(std::round(imgPos.Y())); + + // check wheel isn't outside drivable bounds (ie. outside of slipMapImg) + if (u < 0 || static_cast(u) >= this->slipMapImg.Width() || + v < 0 || static_cast(v) >= this->slipMapImg.Height()) + continue; + + std::string latParamName = + this->ScopedParamName(linkEnt, kSlipComplianceLateralParamName, _ecm); + std::string lonParamName = + this->ScopedParamName( + linkEnt, kSlipComplianceLongitudinalParamName, _ecm); + std::string mu1ParamName = + this->ScopedParamName( + linkEnt, kFrictionCoefficientPrimaryParamName, _ecm); + std::string mu2ParamName = + this->ScopedParamName( + linkEnt, kFrictionCoefficientSecondaryParamName, _ecm); + + const unsigned int channels = 3u; + unsigned int idx = (v * this->slipMapImg.Height() + u) * channels; + + int latDeltaScale = static_cast( + this->slipMapRgb[idx + kLateralColorChannel] - kNominalColor); + double latCoeff = this->nominalParamValues[latParamName] + + (latDeltaScale * this->lateralSlipDelta); + if (latCoeff < 0.0) + { + latCoeff = 0.0; + } + int lonDeltaScale = static_cast( + this->slipMapRgb[idx + kLongitudinalColorChannel] - kNominalColor); + double lonCoeff = this->nominalParamValues[lonParamName] + + (lonDeltaScale * this->longitudinalSlipDelta); + if (lonCoeff < 0.0) + { + lonCoeff = 0.0; + } + int muDeltaScale = static_cast( + this->slipMapRgb[idx + kFrictionColorChannel] - kNominalColor); + double mu1Coeff = this->nominalParamValues[mu1ParamName] + + (muDeltaScale * this->frictionDelta); + if (mu1Coeff < 0.0) + { + mu1Coeff = 0.0; + } + double mu2Coeff = this->nominalParamValues[mu2ParamName] + + (muDeltaScale * this->frictionDelta); + if (mu2Coeff < 0.0) + { + mu2Coeff = 0.0; + } + + // gzmsg << "[u, v]: " << u << ", " << v << std::endl; + // gzmsg << "color: " + // << static_cast(slipMapRgb[idx + kLateralColorChannel]) + // << ", " + // << static_cast(slipMapRgb[idx + kLongitudinalColorChannel]) + // << ", " + // << static_cast(slipMapRgb[idx + kFrictionColorChannel]) + // << std::endl; + // gzmsg << "[lat|lon|mu|mu2] nominal: " + // << this->nominalParamValues[latParamName] << ", " + // << this->nominalParamValues[lonParamName] << ", " + // << this->nominalParamValues[mu1ParamName] << ", " + // << this->nominalParamValues[mu2ParamName] << std::endl; + // gzmsg << "[lat|lon|friction] delta: " + // << latDeltaScale << ", " + // << lonDeltaScale << ", " + // << muDeltaScale << std::endl; + // gzmsg << "[lat|lon|mu|mu2] coeff: " + // << latCoeff << ", " + // << lonCoeff << ", " + // << mu1Coeff << ", " + // << mu2Coeff << ", " << std::endl; + + // Update surface params + if (!math::equal(this->newParamValues[latParamName], latCoeff, 1e-6)) + { + this->newParamValues[latParamName] = latCoeff; + msgs::Double msg; + auto result = this->client.SetParameter(latParamName, msg); + if (!result) + { + gzerr << "Error setting param " << latParamName << std::endl; + } + } + if (!math::equal(this->newParamValues[lonParamName], lonCoeff, 1e-6)) + { + this->newParamValues[lonParamName] = lonCoeff; + msgs::Double msg; + auto result = this->client.SetParameter(lonParamName, msg); + if (!result) + { + gzerr << "Error setting param " << lonParamName << std::endl; + } + } + if (!math::equal(this->newParamValues[mu1ParamName], mu1Coeff, 1e-6)) + { + this->newParamValues[mu1ParamName] = mu1Coeff; + msgs::Double msg; + auto result = this->client.SetParameter(mu1ParamName, msg); + if (!result) + { + gzerr << "Error setting param " << mu1ParamName << std::endl; + } + } + if (!math::equal(this->newParamValues[mu2ParamName], mu2Coeff, 1e-6)) + { + this->newParamValues[mu2ParamName] = mu2Coeff; + msgs::Double msg; + auto result = this->client.SetParameter(mu2ParamName, msg); + if (!result) + { + gzerr << "Error setting param " << mu2ParamName << std::endl; + } + } + } +} + +GZ_ADD_PLUGIN(LookupWheelSlip, + System, + LookupWheelSlip::ISystemConfigure, + LookupWheelSlip::ISystemPreUpdate) + +GZ_ADD_PLUGIN_ALIAS(LookupWheelSlip, + "gz::sim::systems::LookupWheelSlip") diff --git a/src/systems/lookup_wheel_slip/LookupWheelSlip.hh b/src/systems/lookup_wheel_slip/LookupWheelSlip.hh new file mode 100644 index 0000000000..e8c7b039ae --- /dev/null +++ b/src/systems/lookup_wheel_slip/LookupWheelSlip.hh @@ -0,0 +1,66 @@ +/* + * Copyright (C) 2025 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. + * + */ + +#ifndef GZ_SIM_SYSTEMS_LOOKUPWHEELSLIP_HH_ +#define GZ_SIM_SYSTEMS_LOOKUPWHEELSLIP_HH_ + +#include +#include + +namespace gz +{ +namespace sim +{ +// Inline bracket to help doxygen filtering. +inline namespace GZ_SIM_VERSION_NAMESPACE { +namespace systems +{ + // Forward declaration + class LookupWheelSlipPrivate; + + /// \brief Lookup Wheel Slip system. + class LookupWheelSlip + : public System, + public ISystemConfigure, + public ISystemPreUpdate + { + /// \brief Constructor + public: LookupWheelSlip(); + + /// \brief Destructor + public: ~LookupWheelSlip() override = default; + + // Documentation inherited + public: void Configure(const Entity &_entity, + const std::shared_ptr &_sdf, + EntityComponentManager &_ecm, + EventManager &_eventMgr) override; + + // Documentation inherited + public: void PreUpdate( + const gz::sim::UpdateInfo &_info, + gz::sim::EntityComponentManager &_ecm) override; + + /// \brief Private data pointer + private: std::unique_ptr dataPtr; + }; + } +} +} +} + +#endif diff --git a/src/systems/wheel_slip/WheelSlip.cc b/src/systems/wheel_slip/WheelSlip.cc index 31771e096f..5646116eb6 100644 --- a/src/systems/wheel_slip/WheelSlip.cc +++ b/src/systems/wheel_slip/WheelSlip.cc @@ -18,15 +18,22 @@ #include "WheelSlip.hh" #include +#include +#include #include +#include +#include +#include #include +#include #include +#include #include -#include #include "gz/sim/Link.hh" #include "gz/sim/Model.hh" +#include "gz/sim/Util.hh" #include "gz/sim/components/AngularVelocity.hh" #include "gz/sim/components/ChildLinkName.hh" #include "gz/sim/components/Collision.hh" @@ -34,6 +41,7 @@ #include "gz/sim/components/JointVelocity.hh" #include "gz/sim/components/SlipComplianceCmd.hh" #include "gz/sim/components/WheelSlipCmd.hh" +#include "gz/sim/physics/Events.hh" using namespace gz; using namespace sim; @@ -44,26 +52,27 @@ using namespace systems; class gz::sim::systems::WheelSlipPrivate { /// \brief Initialize the plugin - public: bool Load(const EntityComponentManager &_ecm, sdf::ElementPtr _sdf); + public: bool Load(EntityComponentManager &_ecm, sdf::ElementPtr _sdf); /// \brief Update wheel slip plugin data based on physics data - /// \param[in] _ecm Immutable reference to the EntityComponentManager + /// \param[in] _ecm Mutable reference to the EntityComponentManager public: void Update(EntityComponentManager &_ecm); - /// \brief Gazebo communication node - public: transport::Node node; - - /// \brief Joint Entity - public: Entity jointEntity; - - /// \brief Joint name - public: std::string jointName; - - /// \brief Commanded joint force - public: double jointForceCmd; - - /// \brief mutex to protect jointForceCmd - public: std::mutex jointForceCmdMutex; + /// \brief Update susrface parameters from the transport parameter registry. + public: void UpdateParams(); + + public: using P = physics::FeaturePolicy3d; + public: using F = physics::SetContactPropertiesCallbackFeature; + /// \brief The callback for CollectContactSurfaceProperties. + /// This is where we set surface properties. + /// \param[in] _collision1 The first colliding body. + /// \param[in] _collision2 The second colliding body. + /// \param[in,out] _params The contact surface parameters to be set by this + /// system. + public: void SetSurfaceProperties( + const Entity &_collision1, + const Entity &_collision2, + F::ContactSurfaceParams

&_params); /// \brief Model interface public: Model model{kNullEntity}; @@ -95,6 +104,12 @@ class gz::sim::systems::WheelSlipPrivate /// \brief Wheel radius extracted from collision shape if not /// specified as xml parameter. public: double wheelRadius = 0; + + /// \brief Wheel collision primary friction coefficient. + public: double frictionCoeffPrimary = 1.0; + + /// \brief Wheel collision secondary friction coefficient. + public: double frictionCoeffSecondary = 1.0; }; /// \brief The map relating links to their respective surface parameters. @@ -122,10 +137,45 @@ class gz::sim::systems::WheelSlipPrivate public: bool validConfig{false}; public: bool initialized{false}; + + /// \brief Transport paramerter registry. A number of surface params will + /// be exposed in the registry for user configuration. + public: transport::parameters::ParametersRegistry *registry{nullptr}; + + /// \brief The map relating links to their parameters in the transport + /// parameter registry. The elements are: + /// >. + public: std::unordered_map> mapLinkParamNames; + + /// \brief Lateral slip compliance parameter name in the parameter registry. + public: static constexpr std::string_view kSlipComplianceLateralParamName = + "slip_compliance_lateral"; + + /// \brief Longitudinal slip compliance parameter name in the parameter + /// registry. + public: static constexpr std::string_view + kSlipComplianceLongitudinalParamName = "slip_compliance_longitudinal"; + + /// \brief Primary friction coefficient parameter name in the parameter + /// registry. + public: static constexpr std::string_view + kFrictionCoefficientPrimaryParamName = "friction_coefficient_primary"; + + /// \brief Secondary friction coefficient parameter name in the parameter + /// registry. + public: static constexpr std::string_view + kFrictionCoefficientSecondaryParamName = "friction_coefficient_secondary"; + + /// \brief Event manager. + public: EventManager* eventManager{nullptr}; + + /// \brief Connection to CollectContactSurfaceProperties event. + public: common::ConnectionPtr eventConnection; }; ///////////////////////////////////////////////// -bool WheelSlipPrivate::Load(const EntityComponentManager &_ecm, +bool WheelSlipPrivate::Load(EntityComponentManager &_ecm, sdf::ElementPtr _sdf) { const std::string modelName = this->model.Name(_ecm); @@ -190,10 +240,10 @@ bool WheelSlipPrivate::Load(const EntityComponentManager &_ecm, continue; } - auto collision = collisions.front(); - params.collision = collision; + _ecm.SetComponentData( + collision, true); auto joints = _ecm.ChildrenByComponents(model.Entity(), components::Joint(), @@ -242,9 +292,110 @@ bool WheelSlipPrivate::Load(const EntityComponentManager &_ecm, return true; } + +///////////////////////////////////////////////// +void WheelSlipPrivate::UpdateParams() +{ + if (!this->registry) + return; + + for (auto &linkSurface : this->mapLinkSurfaceParams) + { + // Slip compliance lateral + std::string paramName = this->mapLinkParamNames[ + linkSurface.first][std::string(kSlipComplianceLateralParamName)]; + auto value = std::make_unique(); + auto result = this->registry->Parameter(paramName, *value); + if (result) + { + if (!math::equal(linkSurface.second.slipComplianceLateral, + value->data(), 1e-6)) + { + gzdbg << "Parameter " << paramName << " updated from " + << linkSurface.second.slipComplianceLateral << " to " + << value->data() << std::endl; + linkSurface.second.slipComplianceLateral = value->data(); + } + } + else + { + gzerr << "Failed to get parameter [" << paramName << "] :" + << result << std::endl; + } + + // Slip compliance longitudinal + paramName = this->mapLinkParamNames[ + linkSurface.first][std::string(kSlipComplianceLongitudinalParamName)]; + value = std::make_unique(); + result = this->registry->Parameter(paramName, *value); + if (result) + { + if (!math::equal(linkSurface.second.slipComplianceLongitudinal, + value->data(), 1e-6)) + { + gzdbg << "Parameter " << paramName << " updated from " + << linkSurface.second.slipComplianceLongitudinal << " to " + << value->data() << std::endl; + linkSurface.second.slipComplianceLongitudinal = value->data(); + } + } + else + { + gzerr << "Failed to get parameter [" << paramName << "] :" + << result << std::endl; + } + + // Primary friction coeff + paramName = this->mapLinkParamNames[ + linkSurface.first][std::string(kFrictionCoefficientPrimaryParamName)]; + value = std::make_unique(); + result = this->registry->Parameter(paramName, *value); + if (result) + { + if (!math::equal(linkSurface.second.frictionCoeffPrimary, + value->data(), 1e-6)) + { + gzdbg << "Parameter " << paramName << " updated from " + << linkSurface.second.frictionCoeffPrimary << " to " + << value->data() << std::endl; + linkSurface.second.frictionCoeffPrimary = value->data(); + } + } + else + { + gzerr << "Failed to get parameter [" << paramName << "] :" + << result << std::endl; + } + + // Secondary friction coeff + paramName = this->mapLinkParamNames[ + linkSurface.first][std::string(kFrictionCoefficientSecondaryParamName)]; + value = std::make_unique(); + result = this->registry->Parameter(paramName, *value); + if (result) + { + if (!math::equal(linkSurface.second.frictionCoeffSecondary, + value->data(), 1e-6)) + { + gzdbg << "Parameter " << paramName << " updated from " + << linkSurface.second.frictionCoeffSecondary << " to " + << value->data() << std::endl; + linkSurface.second.frictionCoeffSecondary = value->data(); + } + } + else + { + gzerr << "Failed to get parameter [" << paramName << "] :" + << result << std::endl; + } + } +} + ///////////////////////////////////////////////// void WheelSlipPrivate::Update(EntityComponentManager &_ecm) { + this->UpdateParams(); + for (auto &linkSurface : this->mapLinkSurfaceParams) { auto ¶ms = linkSurface.second; @@ -321,6 +472,7 @@ void WheelSlipPrivate::Update(EntityComponentManager &_ecm) } } } + ////////////////////////////////////////////////// WheelSlip::WheelSlip() : dataPtr(std::make_unique()) @@ -331,7 +483,7 @@ WheelSlip::WheelSlip() void WheelSlip::Configure(const Entity &_entity, const std::shared_ptr &_sdf, EntityComponentManager &_ecm, - EventManager &/*_eventMgr*/) + EventManager &_eventMgr) { this->dataPtr->model = Model(_entity); @@ -344,6 +496,95 @@ void WheelSlip::Configure(const Entity &_entity, auto sdfClone = _sdf->Clone(); this->dataPtr->validConfig = this->dataPtr->Load(_ecm, sdfClone); + + using P = physics::FeaturePolicy3d; + using F = physics::SetContactPropertiesCallbackFeature; + this->dataPtr->eventManager = &_eventMgr; + this->dataPtr->eventConnection = this->dataPtr->eventManager-> + Connect( + [this]( + const Entity &_collision1, + const Entity &_collision2, + const math::Vector3d &/*_point*/, + const std::optional /* _force */, + const std::optional /*_normal*/, + const std::optional /*_depth */, + const size_t /*_numContactsOnCollision*/, + F::ContactSurfaceParams

& _params) + { + this->dataPtr->SetSurfaceProperties(_collision1, _collision2, _params); + } + ); +} + +////////////////////////////////////////////////// +void WheelSlip::ConfigureParameters( + gz::transport::parameters::ParametersRegistry &_registry, + gz::sim::EntityComponentManager &_ecm) +{ + this->dataPtr->registry = &_registry; + + for (const auto &linkSurface : this->dataPtr->mapLinkSurfaceParams) + { + auto linkEnt = linkSurface.first; + std::string prefix = std::string("WheelSlip.") + + sim::scopedName(linkEnt, _ecm, ".", false); + std::string paramName = prefix + "." + + std::string(WheelSlipPrivate::kSlipComplianceLateralParamName); + auto value = std::make_unique(); + value->set_data(linkSurface.second.slipComplianceLateral); + auto result = this->dataPtr->registry->DeclareParameter( + paramName, std::move(value)); + if (!result) + { + gzerr << "Unable to declare parameter " << paramName << std::endl; + } + this->dataPtr->mapLinkParamNames[linkEnt][ + std::string(WheelSlipPrivate::kSlipComplianceLateralParamName)] = + paramName; + + paramName = prefix + "." + + std::string(WheelSlipPrivate::kSlipComplianceLongitudinalParamName); + value = std::make_unique(); + value->set_data(linkSurface.second.slipComplianceLongitudinal); + result = this->dataPtr->registry->DeclareParameter( + paramName, std::move(value)); + if (!result) + { + gzerr << "Unable to declare parameter " << paramName << std::endl; + } + this->dataPtr->mapLinkParamNames[linkEnt][ + std::string(WheelSlipPrivate::kSlipComplianceLongitudinalParamName)] = + paramName; + + paramName = prefix + "." + + std::string(WheelSlipPrivate::kFrictionCoefficientPrimaryParamName); + value = std::make_unique(); + value->set_data(linkSurface.second.frictionCoeffPrimary); + result = this->dataPtr->registry->DeclareParameter( + paramName, std::move(value)); + if (!result) + { + gzerr << "Unable to declare parameter " << paramName << std::endl; + } + this->dataPtr->mapLinkParamNames[linkEnt][ + std::string(WheelSlipPrivate::kFrictionCoefficientPrimaryParamName)] = + paramName; + + paramName = prefix + "." + + std::string(WheelSlipPrivate::kFrictionCoefficientSecondaryParamName); + value = std::make_unique(); + value->set_data(linkSurface.second.frictionCoeffSecondary); + result = this->dataPtr->registry->DeclareParameter( + paramName, std::move(value)); + if (!result) + { + gzerr << "Unable to declare parameter " << paramName << std::endl; + } + this->dataPtr->mapLinkParamNames[linkEnt][ + std::string(WheelSlipPrivate::kFrictionCoefficientSecondaryParamName)] = + paramName; + } } ////////////////////////////////////////////////// @@ -385,9 +626,32 @@ void WheelSlip::PreUpdate(const UpdateInfo &_info, EntityComponentManager &_ecm) } } +////////////////////////////////////////////////// +void WheelSlipPrivate::SetSurfaceProperties( + const Entity &_collision1, + const Entity &_collision2, + F::ContactSurfaceParams

&_params) +{ + // Sets surface friction properties + // \todo(iche033) Consider setting slip compliance here in this + // callback function instead of using SlipComplianceCmd components. + for (const auto &linkSurface : this->mapLinkSurfaceParams) + { + if (linkSurface.second.collision == _collision1 || + linkSurface.second.collision == _collision2) + { + _params.frictionCoeff = linkSurface.second.frictionCoeffPrimary; + _params.secondaryFrictionCoeff = + linkSurface.second.frictionCoeffSecondary; + break; + } + } +} + GZ_ADD_PLUGIN(WheelSlip, System, WheelSlip::ISystemConfigure, + WheelSlip::ISystemConfigureParameters, WheelSlip::ISystemPreUpdate) GZ_ADD_PLUGIN_ALIAS(WheelSlip, diff --git a/src/systems/wheel_slip/WheelSlip.hh b/src/systems/wheel_slip/WheelSlip.hh index fa34ff119f..40ce8dcbd7 100644 --- a/src/systems/wheel_slip/WheelSlip.hh +++ b/src/systems/wheel_slip/WheelSlip.hh @@ -118,6 +118,7 @@ namespace systems class WheelSlip : public System, public ISystemConfigure, + public ISystemConfigureParameters, public ISystemPreUpdate { /// \brief Constructor @@ -137,6 +138,11 @@ namespace systems const gz::sim::UpdateInfo &_info, gz::sim::EntityComponentManager &_ecm) override; + // Documentation inherited + public: void ConfigureParameters( + gz::transport::parameters::ParametersRegistry &_registry, + gz::sim::EntityComponentManager &_ecm) override; + /// \brief Private data pointer private: std::unique_ptr dataPtr; }; From 565caf025af66d470b83186f8b102f9ac7475329 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Fri, 1 Aug 2025 20:16:16 +0000 Subject: [PATCH 02/10] lint Signed-off-by: Ian Chen --- src/systems/lookup_wheel_slip/LookupWheelSlip.cc | 5 +++-- src/systems/wheel_slip/WheelSlip.cc | 2 +- 2 files changed, 4 insertions(+), 3 deletions(-) diff --git a/src/systems/lookup_wheel_slip/LookupWheelSlip.cc b/src/systems/lookup_wheel_slip/LookupWheelSlip.cc index 77d68dd86a..2c9503c07f 100644 --- a/src/systems/lookup_wheel_slip/LookupWheelSlip.cc +++ b/src/systems/lookup_wheel_slip/LookupWheelSlip.cc @@ -23,6 +23,7 @@ #include #include #include +#include #include #include @@ -92,7 +93,7 @@ class gz::sim::systems::LookupWheelSlipPrivate /// \brief The friction delta to apply. public: double frictionDelta = 0.5; - /// \brief A transfrom from world to img space. + /// \brief A transform from world to img space. public: math::Matrix4d worldToImgTransform; /// \brief True if the system is initialized, false otherwise @@ -107,7 +108,7 @@ class gz::sim::systems::LookupWheelSlipPrivate public: std::unordered_map nominalParamValues; /// \brief The new surface param values computed by this plugin. The params - /// on the paramater registry will be updated by these values. + /// on the parameter registry will be updated by these values. /// The elements are public: std::unordered_map newParamValues; diff --git a/src/systems/wheel_slip/WheelSlip.cc b/src/systems/wheel_slip/WheelSlip.cc index 5646116eb6..725fc63fb2 100644 --- a/src/systems/wheel_slip/WheelSlip.cc +++ b/src/systems/wheel_slip/WheelSlip.cc @@ -138,7 +138,7 @@ class gz::sim::systems::WheelSlipPrivate public: bool validConfig{false}; public: bool initialized{false}; - /// \brief Transport paramerter registry. A number of surface params will + /// \brief Transport parameter registry. A number of surface params will /// be exposed in the registry for user configuration. public: transport::parameters::ParametersRegistry *registry{nullptr}; From b9ac3a03e705268e816a1dbf31035eef05191ae8 Mon Sep 17 00:00:00 2001 From: Jenn Nguyen <8602001+jennuine@users.noreply.github.com> Date: Thu, 14 Aug 2025 15:29:28 -0700 Subject: [PATCH 03/10] Add test world for lookup wheel slip system (#3018) * Add test world for lookup wheel slip system Signed-off-by: Jenn Nguyen <8602001+jennuine@users.noreply.github.com> Signed-off-by: Ian Chen Co-authored-by: Ian Chen --- examples/worlds/lookup_slip_heightmap.png | Bin 0 -> 1354 bytes .../worlds/lookup_slip_heightmap_normal.png | Bin 0 -> 5765 bytes examples/worlds/lookup_slip_map.png | Bin 0 -> 4759 bytes examples/worlds/lookup_wheel_slip.sdf | 623 ++++++++++++ .../trisphere_cycle_lookup_wheel_slip.sdf | 883 ------------------ .../lookup_wheel_slip/LookupWheelSlip.cc | 22 +- test/integration/CMakeLists.txt | 1 + 7 files changed, 642 insertions(+), 887 deletions(-) create mode 100644 examples/worlds/lookup_slip_heightmap.png create mode 100644 examples/worlds/lookup_slip_heightmap_normal.png create mode 100644 examples/worlds/lookup_slip_map.png create mode 100644 examples/worlds/lookup_wheel_slip.sdf delete mode 100644 examples/worlds/trisphere_cycle_lookup_wheel_slip.sdf diff --git a/examples/worlds/lookup_slip_heightmap.png b/examples/worlds/lookup_slip_heightmap.png new file mode 100644 index 0000000000000000000000000000000000000000..8680f53045bf0430fadb24ecb46ddd30aa88692a GIT binary patch literal 1354 zcmeHG%}*0i5TCbON?Qs+I9QRx%NmiGv|G>+y4g0=R%wY_YsI8`>O!}$p>RLsEP65O^EtlTUtQzFYs+0D$gjB%C0+O2uX+@AcY;I8m0lpcDjHz1_7bcaS}vjU*(1%R>OhRe&GFWqbly z^a6ZI0SNa2_7_)PoE1snv675tBngfZ+Xfw|4_IO$@&FnD%0?`hLI+!R0u47Y48RQ) zEKSTsqUnkdp*an0xqBTt$?gD=J2lj1uQ9F6YK?8%1P8%$p=gxoP)U(>C5_9fK947( zQXC$dF^&Z6#mEH>psSw>g5?z#0OMDulX_A*EzA~j9ywi@Q9PAgk@A695r~;nbQxE2 z*}NuH{9GL&5SzMr4%Z?2f}cxDaU3d?6g=kfdc0h~fiV_K>5PyFpK7L)ou8Z6^`gM@ z<#O3mKISQu=J*kx&&PX5`O#4~LAbR`d0no!^V*>X$revo(Pm3(QCACjOnK#*!lLfy zI9jN+8hz?&rd=toHIGFO$kPZv;_>o-OH(T99%(evNUNW#QBI^}!kLn)kcMenff2D@ zV27{Wo=91Q^O{l$6>>R}w2g}})`u0$?(%L0vML>-kd^auey-w9D;aq)t8;-rE?ea3 zaI)$y^162?yU6d=P=jOJz#F$&=OEV^aEN^C(+D`Ot!K!8gI-3%!RZS6`mVaq#7=hg z`NlV=xPZsZN809-@2{>1)nA`KZfr`x^jaAkPNlE`j5WrLA#1Oy=RHF*k;OUBSXWr50_RZ>H#9 AI{*Lx literal 0 HcmV?d00001 diff --git a/examples/worlds/lookup_slip_heightmap_normal.png b/examples/worlds/lookup_slip_heightmap_normal.png new file mode 100644 index 0000000000000000000000000000000000000000..c6590fb9329edf47334d4e583a03a45e8f14281e GIT binary patch literal 5765 zcmeAS@N?(olHy`uVBq!ia0y~yU;#2&7&zE~RK2WrGmv5|4sv&5Sa(k5C6L3G|H(?D8gCb5n0T@z%2~Ij105pNB{-dOFVsD+3#@62&q`lF1rxNz#zuz>Eakt zaqI03LqP@u4u*|E&%Xw2)k$DrjZu1dy#M=VKA^S@x26I~1|AS`07MXGGJ{kZfCvUc z%mZLm#2HSUiAYw`%5dVGi)1?~E+@rsLZOQk77!+l3`g=RkxUX?PNa!ouaOeaV70U| zoRokj+2y1dPC`JBCK0TGH5ifzvjYOY5$4^DWbIv;-vuPgg&ebxsLQ0Q+t&6951J literal 0 HcmV?d00001 diff --git a/examples/worlds/lookup_slip_map.png b/examples/worlds/lookup_slip_map.png new file mode 100644 index 0000000000000000000000000000000000000000..43a55834cd357e0697d0db6395c95ec72cf3428a GIT binary patch literal 4759 zcmeHJT}TvB6#hon-BnxK#X?CR1`Ho6Nc%I=Y7|X3@~1_@5@JKL6dxAMFkigX_7G$d zWr*xUK}J0k`YJ^d5)?=T_Txe%ME__7L0M9rGo&!=IL-xym0UQ?-gD2r=ljmz%$e%S zGEd5?6aXH7c}W0p&|e4K3H12Vac6`cN=u-uSQ&k|u7kIZ6jc-f{pVAs4kgiAtuGH$ z0PQ(|2m?P!CEf$AxxmLDu&oUvBIGbxqc!&&Zo^B(q0j_8s$R$YNUHd7;mvIKw0; z4RWoML5;A;6Cq1l@4y8v4p~eZy5tGVP)UpL^|N=S5O0C`(Tp_1s{num7+y_5M61(~F>+3|xH8ypDl z$!7bus57{BheF|xL2rGVU<`9msT+1<&8Nw_>o=8}24-R-%j>r$XNc~RtM|{PIcvK- zbSP`ciJ*V`iM#m|7T zuSV;FjWYG47KeNy w{uPwXCQ`|KW + + + + + + -1 0 -9.8 + + + + bullet + + + + + + + + + + + + true + + + + + 0 0 1 + 100 100 + + + + + + + 0 0 1 + 100 100 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + + 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.3105 -2 0.325 0 -0 0 + + + -0.151427 -0 0.175 0 -0 0 + + 1.14395 + + 0.126164 + 0 + 0 + 0.416519 + 0 + 0.481014 + + + + + + 2.01142 1 0.568726 + + + + 0.5 1.0 0.5 1 + 0.5 1.0 0.5 1 + 0.0 1.0 0.0 1 + + + + + + 2.01142 1 0.568726 + + + + + + + 0.554283 0.625029 -0.025 -1.5707 0 0 + + 2 + + 0.145833 + 0 + 0 + 0.145833 + 0 + 0.125 + + + + + + 0.3 + + + + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + + + + + + 0.3 + + + + + + 1 + 1 + 0.035 + 0 + 0 0 1 + + + 1 + 1 + 0.1 + + + + + + + + 0.554282 -0.625029 -0.025 -1.5707 0 0 + + 2 + + 0.145833 + 0 + 0 + 0.145833 + 0 + 0.125 + + + + + + 0.3 + + + + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + + + + + + 0.3 + + + + + + 1 + 1 + 0.035 + 0 + 0 0 1 + + + 1 + 1 + 0.1 + + + + + + + + -0.957138 -0 -0.125 0 -0 0 + + 1 + + 0.1 + 0 + 0 + 0.1 + 0 + 0.1 + + + + + + 0.2 + + + + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + + + + + + 0.2 + + + + + + + chassis + left_wheel + + 0 0 1 + + -1.79769e+308 + 1.79769e+308 + + + + + + chassis + right_wheel + + 0 0 1 + + -1.79769e+308 + 1.79769e+308 + + + + + + chassis + caster + + + + left_wheel_joint + right_wheel_joint + 1.25 + 0.3 + 1 + -1 + 2 + -2 + 0.5 + -0.5 + 1 + -1 + + + + + 1 + 1 + 77 + 0.15 + + + 1 + 1 + 32 + 0.15 + + + 1 + 1 + 32 + 0.15 + + + + + + -5.3313 3.9740 0.325 0 -0 0 + + + -0.151427 -0 0.175 0 -0 0 + + 1.14395 + + 0.126164 + 0 + 0 + 0.416519 + 0 + 0.481014 + + + + + + 2.01142 1 0.568726 + + + + 0.5 0.5 1.0 1 + 0.5 0.5 1.0 1 + 0.0 0.0 1.0 1 + + + + + + 2.01142 1 0.568726 + + + + + + + 0.554283 0.625029 -0.025 -1.5707 0 0 + + 2 + + 0.145833 + 0 + 0 + 0.145833 + 0 + 0.125 + + + + + + 0.3 + + + + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + + + + + + 0.3 + + + + + + 1 + 1 + 0.035 + 0 + 0 0 1 + + + 1 + 1 + 0.1 + + + + + + + + 0.554282 -0.625029 -0.025 -1.5707 0 0 + + 2 + + 0.145833 + 0 + 0 + 0.145833 + 0 + 0.125 + + + + + + 0.3 + + + + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + + + + + + 0.3 + + + + + + 1 + 1 + 0.035 + 0 + 0 0 1 + + + 1 + 1 + 0.1 + + + + + + + + -0.957138 -0 -0.125 0 -0 0 + + 1 + + 0.1 + 0 + 0 + 0.1 + 0 + 0.1 + + + + + + 0.2 + + + + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + + + + + + 0.2 + + + + + + + chassis + left_wheel + + 0 0 1 + + -1.79769e+308 + 1.79769e+308 + + + + + + chassis + right_wheel + + 0 0 1 + + -1.79769e+308 + 1.79769e+308 + + + + + + chassis + caster + + + + left_wheel_joint + right_wheel_joint + 1.25 + 0.3 + 1 + 1 + -1 + 2 + -2 + 0.5 + -0.5 + 1 + -1 + + + + + 0.0 + 0.0 + 1 + 0.3 + + + 0.0 + 0.0 + 1 + 0.3 + + + + lookup_slip_map.png + 129 + 129 + 0.05 + 0.001 + 0.5 + left_wheel + right_wheel + + + + + true + + + + + lookup_slip_heightmap.png + 129 129 0.001 + 0 0 0 + + + + + + + false + + lookup_slip_map.png + lookup_slip_heightmap_normal.png + 129 + + + 2 + 5 + + + 4 + 5 + + lookup_slip_map.png + 129 129 0.001 + 0 0 0 + + + + + + + + diff --git a/examples/worlds/trisphere_cycle_lookup_wheel_slip.sdf b/examples/worlds/trisphere_cycle_lookup_wheel_slip.sdf deleted file mode 100644 index 318e462996..0000000000 --- a/examples/worlds/trisphere_cycle_lookup_wheel_slip.sdf +++ /dev/null @@ -1,883 +0,0 @@ - - - - - - -2 0 -9.8 - - - - - - - - - - true - - - - - 0 0 1 - 100 100 - - - - - - - 0 0 1 - 100 100 - - - - 0.8 0.8 0.8 1 - 0.8 0.8 0.8 1 - 0.8 0.8 0.8 1 - - - - - - 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.40855911616047164 0 0.38502293110800634 0 -0.522020852957719 0 - - 0.0 0 0 0 0 0 - 10 - - 0.22799999999999998 - 0.7435210984814149 - 0.9655210984814149 - 0 - 0 - 0 - - - - -0.4713346258704366 0 0 1.5707963267948966 0 0 - - - 1.0392304845413263 - 0.03 - - - - - -0.4713346258704366 0 0 1.5707963267948966 0 0 - - 0.8 0.3 0.3 1 - 0.8 0.3 0.3 1 - 0.8 0.3 0.3 1 - - - - 1.0392304845413263 - 0.03 - - - - - 0 0.17155177419583564 0 0 1.5707963267948966 -0.3490658503988659 - - - 1.0031676644991372 - 0.03 - - - - - 0 0.17155177419583564 0 0 1.5707963267948966 -0.3490658503988659 - - 0.8 0.3 0.3 1 - 0.8 0.3 0.3 1 - 0.8 0.3 0.3 1 - - - - 1.0031676644991372 - 0.03 - - - - - 0 -0.17155177419583564 0 0 1.5707963267948966 0.3490658503988659 - - - 1.0031676644991372 - 0.03 - - - - - 0 -0.17155177419583564 0 0 1.5707963267948966 0.3490658503988659 - - 0.8 0.3 0.3 1 - 0.8 0.3 0.3 1 - 0.8 0.3 0.3 1 - - - - 1.0031676644991372 - 0.03 - - - - - - 0.04144088383952833 0 0.38502293110800634 0 -0.17453292519943295 0 - - 3 - - 0.15820312499999997 - 0.058359374999999984 - 0.10265624999999999 - 0 - 0 - 0 - - - - 0 0 0.397747564417433 1.5707963267948966 0 0 - - - 0.6363961030678927 - 0.0375 - - - - - 0 0 0.397747564417433 1.5707963267948966 0 0 - - 0.8 0.3 0.3 1 - 0.8 0.3 0.3 1 - 0.8 0.3 0.3 1 - - - - 0.6363961030678927 - 0.0375 - - - - - 0 0 0.2386485386504598 0 0 0 - - - 0.31819805153394637 - 0.0375 - - - - - 0 0 0.2386485386504598 0 0 0 - - 0.8 0.3 0.3 1 - 0.8 0.3 0.3 1 - 0.8 0.3 0.3 1 - - - - 0.31819805153394637 - 0.0375 - - - - - 0 0 -0.23864853865045976 1.5707963267948966 0 0 - - - 0.6363961030678927 - 0.015 - - - - - 0 0 -0.23864853865045976 1.5707963267948966 0 0 - - 0.8 0.3 0.3 1 - 0.8 0.3 0.3 1 - 0.8 0.3 0.3 1 - - - - 0.6363961030678927 - 0.015 - - - - - 0 0.15909902576697318 -0.07954951288348658 0.7853981633974483 0 0 - - - 0.45 - 0.0375 - - - - - 0 0.15909902576697318 -0.07954951288348658 0.7853981633974483 0 0 - - 0.8 0.3 0.3 1 - 0.8 0.3 0.3 1 - 0.8 0.3 0.3 1 - - - - 0.45 - 0.0375 - - - - - 0 -0.15909902576697318 -0.07954951288348658 -0.7853981633974483 0 0 - - - 0.45 - 0.0375 - - - - - 0 -0.15909902576697318 -0.07954951288348658 -0.7853981633974483 0 0 - - 0.8 0.3 0.3 1 - 0.8 0.3 0.3 1 - 0.8 0.3 0.3 1 - - - - 0.45 - 0.0375 - - - - - - 0.08288176767905665 0 0.15 0 0 0 - - 0.8 0.3 0.3 1 - 0.8 0.3 0.3 1 - 0.8 0.3 0.3 1 - - - 0.5 - - 0.0045 - 0.0045 - 0.0045 - 0 - 0 - 0 - - - - - - 0.15 - - - - - - 100000000.0 - 1 - 0.0005 - - - - - - - 0.8 0.3 0.3 1 - 0.8 0.3 0.3 1 - 0.8 0.3 0.3 1 - - - - 0.15 - - - - - - frame - fork - - 0 0 1 - - -0.9599310885968813 - 0.9599310885968813 - - - - - fork - wheel_front - - 0 1 0 - - - - -0.8171182323209433 0.5196152422706631 0.15 0 0 0 - - 0.5 - - 0.0045 - 0.0045 - 0.0045 - 0 - 0 - 0 - - - - - - 0.15 - - - - - - 100000000.0 - 1 - 0.0005 - - - - - - - 0.8 0.3 0.3 1 - 0.8 0.3 0.3 1 - 0.8 0.3 0.3 1 - - - - 0.15 - - - - - - frame - wheel_rear_left - - 0 1 0 - - - - -0.8171182323209433 -0.5196152422706631 0.15 0 0 0 - - 0.5 - - 0.0045 - 0.0045 - 0.0045 - 0 - 0 - 0 - - - - - - 0.15 - - - - - - 100000000.0 - 1 - 0.0005 - - - - - - - 0.8 0.3 0.3 1 - 0.8 0.3 0.3 1 - 0.8 0.3 0.3 1 - - - - 0.15 - - - - - - frame - wheel_rear_right - - 0 1 0 - - - - - - 0.15 - 0 - 0 - 77 - - - 0.15 - 0 - 0 - 32 - - - 0.15 - 0 - 0 - 32 - - - - - 0 2 0 0 0 0 - - -0.40855911616047164 0 0.38502293110800634 0 -0.522020852957719 0 - - 0.0 0 0 0 0 0 - 10 - - 0.22799999999999998 - 0.7435210984814149 - 0.9655210984814149 - 0 - 0 - 0 - - - - -0.4713346258704366 0 0 1.5707963267948966 0 0 - - - 1.0392304845413263 - 0.03 - - - - - -0.4713346258704366 0 0 1.5707963267948966 0 0 - - 0.3 0.3 0.8 1 - 0.3 0.3 0.8 1 - 0.3 0.3 0.8 1 - - - - 1.0392304845413263 - 0.03 - - - - - 0 0.17155177419583564 0 0 1.5707963267948966 -0.3490658503988659 - - - 1.0031676644991372 - 0.03 - - - - - - 0.3 0.3 0.8 1 - 0.3 0.3 0.8 1 - 0.3 0.3 0.8 1 - - 0 0.17155177419583564 0 0 1.5707963267948966 -0.3490658503988659 - - - 1.0031676644991372 - 0.03 - - - - - 0 -0.17155177419583564 0 0 1.5707963267948966 0.3490658503988659 - - - 1.0031676644991372 - 0.03 - - - - - 0 -0.17155177419583564 0 0 1.5707963267948966 0.3490658503988659 - - 0.3 0.3 0.8 1 - 0.3 0.3 0.8 1 - 0.3 0.3 0.8 1 - - - - 1.0031676644991372 - 0.03 - - - - - - 0.04144088383952833 0 0.38502293110800634 0 -0.17453292519943295 0 - - 3 - - 0.15820312499999997 - 0.058359374999999984 - 0.10265624999999999 - 0 - 0 - 0 - - - - 0 0 0.397747564417433 1.5707963267948966 0 0 - - - 0.6363961030678927 - 0.0375 - - - - - 0 0 0.397747564417433 1.5707963267948966 0 0 - - 0.3 0.3 0.8 1 - 0.3 0.3 0.8 1 - 0.3 0.3 0.8 1 - - - - 0.6363961030678927 - 0.0375 - - - - - 0 0 0.2386485386504598 0 0 0 - - - 0.31819805153394637 - 0.0375 - - - - - 0 0 0.2386485386504598 0 0 0 - - 0.3 0.3 0.8 1 - 0.3 0.3 0.8 1 - 0.3 0.3 0.8 1 - - - - 0.31819805153394637 - 0.0375 - - - - - 0 0 -0.23864853865045976 1.5707963267948966 0 0 - - - 0.6363961030678927 - 0.015 - - - - - 0 0 -0.23864853865045976 1.5707963267948966 0 0 - - 0.3 0.3 0.8 1 - 0.3 0.3 0.8 1 - 0.3 0.3 0.8 1 - - - - 0.6363961030678927 - 0.015 - - - - - 0 0.15909902576697318 -0.07954951288348658 0.7853981633974483 0 0 - - - 0.45 - 0.0375 - - - - - 0 0.15909902576697318 -0.07954951288348658 0.7853981633974483 0 0 - - 0.3 0.3 0.8 1 - 0.3 0.3 0.8 1 - 0.3 0.3 0.8 1 - - - - 0.45 - 0.0375 - - - - - 0 -0.15909902576697318 -0.07954951288348658 -0.7853981633974483 0 0 - - - 0.45 - 0.0375 - - - - - 0 -0.15909902576697318 -0.07954951288348658 -0.7853981633974483 0 0 - - 0.3 0.3 0.8 1 - 0.3 0.3 0.8 1 - 0.3 0.3 0.8 1 - - - - 0.45 - 0.0375 - - - - - - 0.08288176767905665 0 0.15 0 0 0 - - 0.5 - - 0.0045 - 0.0045 - 0.0045 - 0 - 0 - 0 - - - - - - 0.15 - - - - - - 100000000.0 - 1 - 0.0005 - - - - - - - 0.3 0.3 0.8 1 - 0.3 0.3 0.8 1 - 0.3 0.3 0.8 1 - - - - 0.15 - - - - - - frame - fork - - 0 0 1 - - -0.9599310885968813 - 0.9599310885968813 - - - - - fork - wheel_front - - 0 1 0 - - - - -0.8171182323209433 0.5196152422706631 0.15 0 0 0 - - 0.5 - - 0.0045 - 0.0045 - 0.0045 - 0 - 0 - 0 - - - - - - 0.15 - - - - - - 100000000.0 - 1 - 0.0005 - - - - - - - 0.3 0.3 0.8 1 - 0.3 0.3 0.8 1 - 0.3 0.3 0.8 1 - - - - 0.15 - - - - - - frame - wheel_rear_left - - 0 1 0 - - - - -0.8171182323209433 -0.5196152422706631 0.15 0 0 0 - - 0.5 - - 0.0045 - 0.0045 - 0.0045 - 0 - 0 - 0 - - - - - - 0.15 - - - - - - 100000000.0 - 1 - 0.0005 - - - - - - - 0.3 0.3 0.8 1 - 0.3 0.3 0.8 1 - 0.3 0.3 0.8 1 - - - - 0.15 - - - - - - frame - wheel_rear_right - - 0 1 0 - - - - - - 1 - 1 - 77 - 0.15 - - - 1 - 1 - 32 - 0.15 - - - 1 - 1 - 32 - 0.15 - - - - lookup_slip_map.png - 10 - 10 - 0.04 - 0.04 - 0.4 - wheel_front - wheel_rear_left - wheel_rear_right - - - - - - diff --git a/src/systems/lookup_wheel_slip/LookupWheelSlip.cc b/src/systems/lookup_wheel_slip/LookupWheelSlip.cc index 2c9503c07f..4e4bbbc46c 100644 --- a/src/systems/lookup_wheel_slip/LookupWheelSlip.cc +++ b/src/systems/lookup_wheel_slip/LookupWheelSlip.cc @@ -455,11 +455,12 @@ bool LookupWheelSlipPrivate::GetNominalSurfaceParams( void LookupWheelSlipPrivate::UpdateParams( const EntityComponentManager &_ecm) { + // std::cout << "UpdateParams" << std::endl; + for (const auto &linkEnt : this->linkEntities) { sim::Link link(linkEnt); - // math::Vector3d linkWorldPos = link.WorldPose(_ecm).value().Pos(); - math::Vector3d linkWorldPos(1, 2, 0.5); + math::Vector3d linkWorldPos = link.WorldPose(_ecm).value().Pos(); math::Vector3d imgPos = this->worldToImgTransform * linkWorldPos; int u = static_cast(std::round(imgPos.X())); @@ -503,6 +504,7 @@ void LookupWheelSlipPrivate::UpdateParams( } int muDeltaScale = static_cast( this->slipMapRgb[idx + kFrictionColorChannel] - kNominalColor); + double mu1Coeff = this->nominalParamValues[mu1ParamName] + (muDeltaScale * this->frictionDelta); if (mu1Coeff < 0.0) @@ -517,7 +519,8 @@ void LookupWheelSlipPrivate::UpdateParams( } // gzmsg << "[u, v]: " << u << ", " << v << std::endl; - // gzmsg << "color: " + // std::cout << "-------------------------------------" << std::endl; + // std::cout << "color: " // << static_cast(slipMapRgb[idx + kLateralColorChannel]) // << ", " // << static_cast(slipMapRgb[idx + kLongitudinalColorChannel]) @@ -537,13 +540,14 @@ void LookupWheelSlipPrivate::UpdateParams( // << latCoeff << ", " // << lonCoeff << ", " // << mu1Coeff << ", " - // << mu2Coeff << ", " << std::endl; + // << mu2Coeff << std::endl; // Update surface params if (!math::equal(this->newParamValues[latParamName], latCoeff, 1e-6)) { this->newParamValues[latParamName] = latCoeff; msgs::Double msg; + msg.set_data(latCoeff); auto result = this->client.SetParameter(latParamName, msg); if (!result) { @@ -554,6 +558,7 @@ void LookupWheelSlipPrivate::UpdateParams( { this->newParamValues[lonParamName] = lonCoeff; msgs::Double msg; + msg.set_data(lonCoeff); auto result = this->client.SetParameter(lonParamName, msg); if (!result) { @@ -562,8 +567,16 @@ void LookupWheelSlipPrivate::UpdateParams( } if (!math::equal(this->newParamValues[mu1ParamName], mu1Coeff, 1e-6)) { + // std::cout << "color: " + // << static_cast(slipMapRgb[idx + kLateralColorChannel]) + // << ", " + // << static_cast(slipMapRgb[idx + kLongitudinalColorChannel]) + // << ", " + // << static_cast(slipMapRgb[idx + kFrictionColorChannel]) + // << std::endl; this->newParamValues[mu1ParamName] = mu1Coeff; msgs::Double msg; + msg.set_data(mu1Coeff); auto result = this->client.SetParameter(mu1ParamName, msg); if (!result) { @@ -574,6 +587,7 @@ void LookupWheelSlipPrivate::UpdateParams( { this->newParamValues[mu2ParamName] = mu2Coeff; msgs::Double msg; + msg.set_data(mu2Coeff); auto result = this->client.SetParameter(mu2ParamName, msg); if (!result) { diff --git a/test/integration/CMakeLists.txt b/test/integration/CMakeLists.txt index 66191cc292..b598ae2ca6 100644 --- a/test/integration/CMakeLists.txt +++ b/test/integration/CMakeLists.txt @@ -50,6 +50,7 @@ set(tests load_system_static_registry.cc logical_camera_system.cc logical_audio_sensor_plugin.cc + # lookup_wheel_slip_system.cc magnetometer_system.cc material.cc mecanum_drive_system.cc From c2cd28fc5608e4ccebea52e61fff540ecb1e1001 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Thu, 14 Aug 2025 22:28:15 +0000 Subject: [PATCH 04/10] add test Signed-off-by: Ian Chen --- examples/worlds/lookup_wheel_slip.sdf | 24 ++-- test/integration/CMakeLists.txt | 2 +- test/integration/lookup_wheel_slip_system.cc | 144 +++++++++++++++++++ 3 files changed, 154 insertions(+), 16 deletions(-) create mode 100644 test/integration/lookup_wheel_slip_system.cc diff --git a/examples/worlds/lookup_wheel_slip.sdf b/examples/worlds/lookup_wheel_slip.sdf index f2851f0bff..79be1ab9d0 100644 --- a/examples/worlds/lookup_wheel_slip.sdf +++ b/examples/worlds/lookup_wheel_slip.sdf @@ -4,7 +4,7 @@ The world consists of two vehicles, blue and green, on a flat heightmap. The blue vehicle has a lookup wheel slip plugin while the green only has - a normal wheel slip plugin with fixed slip compliance values. + a regular wheel slip plugin with fixed slip compliance values. The slip map used in this demo contains several regions with increased longitudinal slip compliance values. The same slip map is overlaid on top @@ -313,22 +313,16 @@ filename="gz-sim-wheel-slip-system" name="gz::sim::systems::WheelSlip"> - 1 - 1 - 77 - 0.15 + 0.1 + 0.1 + 1 + 0.3 - 1 - 1 - 32 - 0.15 - - - 1 - 1 - 32 - 0.15 + 0.1 + 0.1 + 1 + 0.3 diff --git a/test/integration/CMakeLists.txt b/test/integration/CMakeLists.txt index b598ae2ca6..e8e1900c7c 100644 --- a/test/integration/CMakeLists.txt +++ b/test/integration/CMakeLists.txt @@ -50,7 +50,7 @@ set(tests load_system_static_registry.cc logical_camera_system.cc logical_audio_sensor_plugin.cc - # lookup_wheel_slip_system.cc + lookup_wheel_slip_system.cc magnetometer_system.cc material.cc mecanum_drive_system.cc diff --git a/test/integration/lookup_wheel_slip_system.cc b/test/integration/lookup_wheel_slip_system.cc new file mode 100644 index 0000000000..0d5394807c --- /dev/null +++ b/test/integration/lookup_wheel_slip_system.cc @@ -0,0 +1,144 @@ +/* + * Copyright (C) 2025 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 +#include +#include + +#include "gz/sim/components/Name.hh" +#include "gz/sim/components/Model.hh" +#include "gz/sim/components/Pose.hh" +#include "gz/sim/Server.hh" +#include "gz/sim/SystemLoader.hh" +#include "test_config.hh" + +#include "../helpers/Relay.hh" +#include "../helpers/EnvTestFixture.hh" + +using namespace gz; +using namespace sim; + +/// \brief Test LookupWheelSlip system +class LookupWheelSlipTest : public InternalFixture<::testing::Test> +{ +}; + +///////////////////////////////////////////////// +TEST_F(LookupWheelSlipTest, DriveForwardSlip) +{ + // Verify lookup wheelslip system is able to dynamically update the + // vehicle's wheel slip compliance parameters as the vehicle traverses + // over regions of increased slip values. + // The test sends a command to move the vehicle forward and the vehicle's + // right wheel should come into contact with the slip region after + // some distance, at which point it should start slipping which causes + // the vehicle to rotate and move to the right. + + // Start server + ServerConfig serverConfig; + const auto sdfFile = common::joinPaths(std::string(PROJECT_SOURCE_PATH), + "examples", "worlds", "lookup_wheel_slip.sdf"); + serverConfig.SetSdfFile(sdfFile); + + Server server(serverConfig); + EXPECT_FALSE(server.Running()); + EXPECT_FALSE(*server.Running(0)); + + // Create a system that records the vehicle poses + test::Relay testSystem; + + std::vector poses; + testSystem.OnPostUpdate([&poses](const UpdateInfo &, + const EntityComponentManager &_ecm) + { + auto id = _ecm.EntityByComponents( + components::Model(), + components::Name("vehicle_blue")); + EXPECT_NE(kNullEntity, id); + + auto poseComp = _ecm.Component(id); + ASSERT_NE(nullptr, poseComp); + + poses.push_back(poseComp->Data()); + }); + server.AddSystem(testSystem.systemPtr); + + // Run server and check that vehicle didn't move + server.Run(true, 1000, false); + EXPECT_EQ(1000u, poses.size()); + for (const auto &pose : poses) + { + EXPECT_EQ(poses[0], pose); + } + poses.clear(); + + // Send a twist message to move the vehicle forward + const std::string topic = "/model/vehicle_blue/cmd_vel"; + transport::Node node; + auto pub = node.Advertise(topic); + + msgs::Twist msg; + double desiredLinVel = 1.0; + double desiredAngVel = 0.0; + msgs::Set(msg.mutable_linear(), + math::Vector3d(desiredLinVel, 0, 0)); + msgs::Set(msg.mutable_angular(), + math::Vector3d(0.0, 0, desiredAngVel)); + pub.Publish(msg); + server.Run(true, 3000, false); + ASSERT_EQ(3000u, poses.size()); + + // Look through the vehicle poses to verify that the vehicle did slip + constexpr double kVehiclePoseInSlipRegion = -4.76; + math::Pose3d prevPose; + for (const auto &pose : poses) + { + if (prevPose == math::Pose3d::Zero) + { + prevPose = pose; + continue; + } + + // Verify that the vehicle moved forward in +x direction + math::Vector3d dir = pose.Pos() - prevPose.Pos(); + prevPose = pose; + EXPECT_LT(0, dir.X()) << pose; + EXPECT_NEAR(0, dir.Z(), 1e-4); + + // Vehicle should have no roll and pitch + EXPECT_NEAR(0.0, pose.Rot().Euler().X(), 1e-4); + EXPECT_NEAR(0.0, pose.Rot().Euler().Y(), 1e-4); + + if (pose.Pos().X() > kVehiclePoseInSlipRegion) + { + // Vehicle should have rotate to the right as its right wheel entered + // the slip region + EXPECT_GE(0.0, pose.Rot().Euler().Z()) << pose; + } + else + { + // Vehicle should move straight before entering the slip region + EXPECT_NEAR(0.0, pose.Rot().Euler().Z(), 1e-4) << pose; + } + } +} From 3cd2def8a54257764dacee9578964b947ef0908f Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Thu, 14 Aug 2025 22:47:41 +0000 Subject: [PATCH 05/10] update debug statements Signed-off-by: Ian Chen --- src/systems/lookup_wheel_slip/LookupWheelSlip.cc | 13 ++----------- 1 file changed, 2 insertions(+), 11 deletions(-) diff --git a/src/systems/lookup_wheel_slip/LookupWheelSlip.cc b/src/systems/lookup_wheel_slip/LookupWheelSlip.cc index 4e4bbbc46c..a9b9d837e6 100644 --- a/src/systems/lookup_wheel_slip/LookupWheelSlip.cc +++ b/src/systems/lookup_wheel_slip/LookupWheelSlip.cc @@ -455,8 +455,6 @@ bool LookupWheelSlipPrivate::GetNominalSurfaceParams( void LookupWheelSlipPrivate::UpdateParams( const EntityComponentManager &_ecm) { - // std::cout << "UpdateParams" << std::endl; - for (const auto &linkEnt : this->linkEntities) { sim::Link link(linkEnt); @@ -518,9 +516,9 @@ void LookupWheelSlipPrivate::UpdateParams( mu2Coeff = 0.0; } + // Uncomment the following for debugging slip and friction params // gzmsg << "[u, v]: " << u << ", " << v << std::endl; - // std::cout << "-------------------------------------" << std::endl; - // std::cout << "color: " + // gzmsg << "color: " // << static_cast(slipMapRgb[idx + kLateralColorChannel]) // << ", " // << static_cast(slipMapRgb[idx + kLongitudinalColorChannel]) @@ -567,13 +565,6 @@ void LookupWheelSlipPrivate::UpdateParams( } if (!math::equal(this->newParamValues[mu1ParamName], mu1Coeff, 1e-6)) { - // std::cout << "color: " - // << static_cast(slipMapRgb[idx + kLateralColorChannel]) - // << ", " - // << static_cast(slipMapRgb[idx + kLongitudinalColorChannel]) - // << ", " - // << static_cast(slipMapRgb[idx + kFrictionColorChannel]) - // << std::endl; this->newParamValues[mu1ParamName] = mu1Coeff; msgs::Double msg; msg.set_data(mu1Coeff); From c376626ab3756193b05e7400239c5cb91a36e8b3 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Thu, 14 Aug 2025 23:25:12 +0000 Subject: [PATCH 06/10] cleanup Signed-off-by: Ian Chen --- examples/worlds/lookup_wheel_slip.sdf | 14 ++++--- .../lookup_wheel_slip/LookupWheelSlip.cc | 39 ++----------------- .../lookup_wheel_slip/LookupWheelSlip.hh | 34 ++++++++++++++++ 3 files changed, 46 insertions(+), 41 deletions(-) diff --git a/examples/worlds/lookup_wheel_slip.sdf b/examples/worlds/lookup_wheel_slip.sdf index 79be1ab9d0..bc2d3b2421 100644 --- a/examples/worlds/lookup_wheel_slip.sdf +++ b/examples/worlds/lookup_wheel_slip.sdf @@ -3,14 +3,16 @@ Gazebo lookup wheel slip demo The world consists of two vehicles, blue and green, on a flat heightmap. - The blue vehicle has a lookup wheel slip plugin while the green only has - a regular wheel slip plugin with fixed slip compliance values. + The blue vehicle has a lookup wheel slip system + a regular wheel slip + system while the green only has a regular wheel slip plugin with fixed + slip compliance values. The slip map used in this demo contains several regions with increased - longitudinal slip compliance values. The same slip map is overlaid on top - of the heightmap to help visualize where the slippage regions are. When the - blue vehicle drives over these green regions, its wheels' longitudinal slip - compliance values should be dynamically updated (increased). + longitudinal slip compliance values (encoded in the green channel of the + slip map). The same slip map is overlaid on top of the heightmap to help + visualize where the slippage regions are. When the blue vehicle drives over + these green regions, its wheels' longitudinal slip compliance values should + be dynamically updated (increased). Note: wheel slippage takes effect when a vehicle accelerates, e.g. climbing uphill. So the gravity is set to a small -x direction to simulate diff --git a/src/systems/lookup_wheel_slip/LookupWheelSlip.cc b/src/systems/lookup_wheel_slip/LookupWheelSlip.cc index a9b9d837e6..923e9668cf 100644 --- a/src/systems/lookup_wheel_slip/LookupWheelSlip.cc +++ b/src/systems/lookup_wheel_slip/LookupWheelSlip.cc @@ -187,39 +187,15 @@ void LookupWheelSlip::Configure(const Entity &_entity, return; } - // \todo(iche033) Auto determine size from region collision geometry? - math::Vector3d colSize; - /* - for (const auto &linkEntity : regionModel.Links(_ecm)) - { - sim::Link link(linkEntity); - for (const auto &collisionEntity : link.Collisions(_ecm)) - { - auto geomComp = _ecm.Component(collisionEntity); - if (geomComp && geomComp->Data().Type() == sdf::GeometryType::HEIGHTMAP) - { - const sdf::Heightmap *shape = geomComp->Data().HeightmapShape(); - colSize = shape->Size(); - break; - } - } - } - */ - auto sizeXElem = _sdf->FindElement("size_x"); if (sizeXElem) { this->dataPtr->sizeX = sizeXElem->Get(); } - else if (colSize.X() > 0.0) - { - this->dataPtr->sizeX = colSize.X(); - } else { - gzerr << "Unable to determine size x from collision and missing " - << "'size_x' param. Will not dynamically update wheel slip values." - << std::endl; + gzerr << "Missing param. Will not dynamically update wheel + << "slip values." << std::endl; return; } auto sizeYElem = _sdf->FindElement("size_y"); @@ -227,20 +203,13 @@ void LookupWheelSlip::Configure(const Entity &_entity, { this->dataPtr->sizeY = sizeYElem->Get(); } - else if (colSize.Y() > 0.0) - { - this->dataPtr->sizeY = colSize.Y(); - } else { - gzerr << "Unable to determine size y from collision and missing " - << "'size_y' param. Will not dynamically update wheel slip values." - << std::endl; + gzerr << "Missing param. Will not dynamically update wheel + << "slip values." << std::endl; return; } - // \todo(iche033) Swap R and B channels? - // transformation matrix from world to image coordinates std::string filePath; if (common::isFile(this->dataPtr->slipMapFilename)) diff --git a/src/systems/lookup_wheel_slip/LookupWheelSlip.hh b/src/systems/lookup_wheel_slip/LookupWheelSlip.hh index e8c7b039ae..5801f99c83 100644 --- a/src/systems/lookup_wheel_slip/LookupWheelSlip.hh +++ b/src/systems/lookup_wheel_slip/LookupWheelSlip.hh @@ -33,6 +33,40 @@ namespace systems class LookupWheelSlipPrivate; /// \brief Lookup Wheel Slip system. + /// This plugin uses a lookup map to dynamically adjust the + /// wheel slip and friction parameters based on the wheel's position on the + /// region covered by the lookup map. + /// This plugin needs to be used together with the WheelSlip system. + /// LookupWheelSlip system params: + /// * : (Required) Lookup slip map filename. This needs to be an + /// 8 bit RGB image. + /// * Channels: + /// * Red - updates lateral slip + /// * Green - updates longitudinal slip + /// * Blue - updates friction + /// * Pixel values: + /// * A pixel value of 128 represents nominal slip / friction, i.e. + /// Apply the original slip / friction values from the WheelSlip system. + /// * A pixel value of >128 tells the WheelSlip system to apply an increased + /// amount of slip / friction values. + /// * A pixel value of <128 tells the WheelSlip system to apply a reduced + /// amount of slip / friction values. + /// * The change in slip / friction to apply to the wheels is computed as: + /// (pixel_value - 128) * delta. See the different delta parameters + /// below. + /// To visualize the lookup map, set the model's visual diffuse texture to + /// this file, e.g. set this texture to a plane or heightmap. + /// * : (Required) x size of lookup slip map in meters. + /// * : (Required) y size of lookup slip map in meters. + /// * : (Required) The wheel link name from the + /// WheelSlip system. Specify one per wheel link. + /// * `: (Optional) The increase / decrease + /// step to be applied to the lateral slip. Default is 0.05 + /// * : (Optional) The increase / decrease + /// step to be applied to the longitudinal slip. Default is 0.005 + /// * : (Optional) The increase/decrease step to be applied + /// to the friction coefficients in the primary and secondary directions. + /// Default is 0.5 class LookupWheelSlip : public System, public ISystemConfigure, From eb81cdf5f8bde5072644ddc7e8fc1dd404ad4321 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Thu, 14 Aug 2025 23:34:56 +0000 Subject: [PATCH 07/10] fix lint / build Signed-off-by: Ian Chen --- src/systems/lookup_wheel_slip/LookupWheelSlip.cc | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/systems/lookup_wheel_slip/LookupWheelSlip.cc b/src/systems/lookup_wheel_slip/LookupWheelSlip.cc index 923e9668cf..adff0b784d 100644 --- a/src/systems/lookup_wheel_slip/LookupWheelSlip.cc +++ b/src/systems/lookup_wheel_slip/LookupWheelSlip.cc @@ -175,14 +175,14 @@ void LookupWheelSlip::Configure(const Entity &_entity, auto slipMapElem = _sdf->FindElement("slip_map"); if (!slipMapElem) { - gzerr << "No 'slip_map' provided. Will not dynamically update " + gzerr << "No provided. Will not dynamically update " << "wheel slip values. " << std::endl; return; } this->dataPtr->slipMapFilename = slipMapElem->Get(); if (this->dataPtr->slipMapFilename.empty()) { - gzerr << "No value for 'slip_path' provided. Will not dynamically update " + gzerr << "No value for provided. Will not dynamically update " << "wheel slip values. " << std::endl; return; } @@ -194,7 +194,7 @@ void LookupWheelSlip::Configure(const Entity &_entity, } else { - gzerr << "Missing param. Will not dynamically update wheel + gzerr << "Missing param. Will not dynamically update wheel " << "slip values." << std::endl; return; } @@ -205,7 +205,7 @@ void LookupWheelSlip::Configure(const Entity &_entity, } else { - gzerr << "Missing param. Will not dynamically update wheel + gzerr << "Missing param. Will not dynamically update wheel " << "slip values." << std::endl; return; } @@ -260,7 +260,7 @@ void LookupWheelSlip::Configure(const Entity &_entity, } if (wheelLinkNames.empty()) { - gzerr << "Error loading 'wheel_slip_name' element. Unable to load plugin." + gzerr << "Error loading element. Unable to load plugin." << std::endl; return; } From bbea019484906ba38f1b604cc1a9eb87995994a8 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Thu, 14 Aug 2025 23:46:12 +0000 Subject: [PATCH 08/10] lint Signed-off-by: Ian Chen --- src/systems/lookup_wheel_slip/LookupWheelSlip.hh | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/systems/lookup_wheel_slip/LookupWheelSlip.hh b/src/systems/lookup_wheel_slip/LookupWheelSlip.hh index 5801f99c83..278ca8018d 100644 --- a/src/systems/lookup_wheel_slip/LookupWheelSlip.hh +++ b/src/systems/lookup_wheel_slip/LookupWheelSlip.hh @@ -47,8 +47,8 @@ namespace systems /// * Pixel values: /// * A pixel value of 128 represents nominal slip / friction, i.e. /// Apply the original slip / friction values from the WheelSlip system. - /// * A pixel value of >128 tells the WheelSlip system to apply an increased - /// amount of slip / friction values. + /// * A pixel value of >128 tells the WheelSlip system to apply an + /// increased amount of slip / friction values. /// * A pixel value of <128 tells the WheelSlip system to apply a reduced /// amount of slip / friction values. /// * The change in slip / friction to apply to the wheels is computed as: From b9458c15764fc7fdf6e75a884319e851b842fbcc Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Tue, 19 Aug 2025 22:44:59 +0000 Subject: [PATCH 09/10] fix doc Signed-off-by: Ian Chen --- src/systems/lookup_wheel_slip/LookupWheelSlip.cc | 11 +++++++---- src/systems/lookup_wheel_slip/LookupWheelSlip.hh | 7 ++++--- src/systems/wheel_slip/WheelSlip.cc | 2 +- 3 files changed, 12 insertions(+), 8 deletions(-) diff --git a/src/systems/lookup_wheel_slip/LookupWheelSlip.cc b/src/systems/lookup_wheel_slip/LookupWheelSlip.cc index adff0b784d..b63b7e59e3 100644 --- a/src/systems/lookup_wheel_slip/LookupWheelSlip.cc +++ b/src/systems/lookup_wheel_slip/LookupWheelSlip.cc @@ -48,9 +48,11 @@ using namespace systems; class gz::sim::systems::LookupWheelSlipPrivate { - /// \brief Can the nominal (original) surface param values from the parameter - /// registry. + /// \brief Get and store the nominal (original) surface param values from the + /// parameter registry. /// \param[in] _ecm Immutable reference to the EntityComponentManager + /// \return True if all params were retrieved and stored successfully, false + /// otherwise. public: bool GetNominalSurfaceParams(const EntityComponentManager &_ecm); /// \brief Get the scoped param name for an entity @@ -69,7 +71,7 @@ class gz::sim::systems::LookupWheelSlipPrivate /// \brief Model interface public: Model model{kNullEntity}; - /// \brief /Filename of the slip map texture + /// \brief Filename of the slip map public: std::string slipMapFilename; /// \brief The loaded slip map image @@ -182,7 +184,7 @@ void LookupWheelSlip::Configure(const Entity &_entity, this->dataPtr->slipMapFilename = slipMapElem->Get(); if (this->dataPtr->slipMapFilename.empty()) { - gzerr << "No value for provided. Will not dynamically update " + gzerr << "No value for provided. Will not dynamically update " << "wheel slip values. " << std::endl; return; } @@ -230,6 +232,7 @@ void LookupWheelSlip::Configure(const Entity &_entity, if (filePath.empty()) { gzerr << "Unable to find slip_map file: " << this->dataPtr->slipMapFilename + << "\nWill not dynamically update wheel slip values." << std::endl; return; } diff --git a/src/systems/lookup_wheel_slip/LookupWheelSlip.hh b/src/systems/lookup_wheel_slip/LookupWheelSlip.hh index 278ca8018d..2004eee47d 100644 --- a/src/systems/lookup_wheel_slip/LookupWheelSlip.hh +++ b/src/systems/lookup_wheel_slip/LookupWheelSlip.hh @@ -33,9 +33,10 @@ namespace systems class LookupWheelSlipPrivate; /// \brief Lookup Wheel Slip system. - /// This plugin uses a lookup map to dynamically adjust the - /// wheel slip and friction parameters based on the wheel's position on the - /// region covered by the lookup map. + /// This plugin dynamically adjusts the wheel slip and friction parameters + /// based on the wheel's position on the region covered by the specified + /// lookup image map (slip map) where slip and friction values are encoded in + /// the image's RGB channels. /// This plugin needs to be used together with the WheelSlip system. /// LookupWheelSlip system params: /// * : (Required) Lookup slip map filename. This needs to be an diff --git a/src/systems/wheel_slip/WheelSlip.cc b/src/systems/wheel_slip/WheelSlip.cc index 725fc63fb2..6c5b90aa8d 100644 --- a/src/systems/wheel_slip/WheelSlip.cc +++ b/src/systems/wheel_slip/WheelSlip.cc @@ -58,7 +58,7 @@ class gz::sim::systems::WheelSlipPrivate /// \param[in] _ecm Mutable reference to the EntityComponentManager public: void Update(EntityComponentManager &_ecm); - /// \brief Update susrface parameters from the transport parameter registry. + /// \brief Update surface parameters from the transport parameter registry. public: void UpdateParams(); public: using P = physics::FeaturePolicy3d; From 037fb6e3b409b8d4eaf8482c12e411c10e913e52 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Thu, 21 Aug 2025 02:52:33 +0000 Subject: [PATCH 10/10] update bazel build file Signed-off-by: Ian Chen --- BUILD.bazel | 22 ++++++++++++++++++++++ 1 file changed, 22 insertions(+) diff --git a/BUILD.bazel b/BUILD.bazel index 5fbd1714df..9d6d2d9bb9 100644 --- a/BUILD.bazel +++ b/BUILD.bazel @@ -1131,6 +1131,28 @@ gz_sim_system_libraries( ], ) +gz_sim_system_libraries( + srcs = glob( + [ + "src/systems/lookup_wheel_slip/**/*.cc", + "src/systems/lookup_wheel_slip/**/*.hh", + ], + ), + so_lib_name = "gz-sim-lookup-wheel-slip-system.so", + static_lib_name = "gz-sim-lookup-wheel-slip-system-static", + visibility = ["//visibility:public"], + deps = [ + ":gz-sim", + "@gz-common", + "@gz-common//graphics", + "@gz-common//profiler", + "@gz-plugin//:register", + "@gz-math", + "@gz-transport", + "@sdformat", + ], +) + gz_sim_system_libraries( srcs = glob( [