feat: BlueROV2 Heavy updated control (attitude and position) and model (#25052)

* rft: clean merge to PX4

* fix: formatting

* fix: extra line

* fix: moved submarine out of "is_ground_vehicle", added proper check for center-throttle

* feat: updated gazebo models to include bluerov update

* fix: use 'is_uuv_vehicle', remove FW_MM/LLC from uuv build

* fix: added saturation to thrust and torque messages via param

* doc: updated parameters documentation for uuv

* fix: formatting

* feat: matching hardware reference

* fix: thrusters kg

* rft: removed commented lines

* fix: update gz reference given hw setup

* fix: hardware references

* fix: recommendations

* fix: updated settings to match hardware

* rft: check only for fixed and rotary wing for high throttle

Co-authored-by: Daniel Agar <daniel@agar.ca>

* fix: commit oupsie

* fix: format

* rft: remove is_uuv

* fix: hw parameters, uuv build target for v6x

* feat: added support for D-pad attitude changes in stabilized position control

* fix: position setpoint update and parametrized trajectory age and att change

* fix: format

* fix: removed duplicated call to check_validity_setpoint

* fix: setpoint update on arming logic

* fix: setpoint initialization for stabilized mode

---------

Co-authored-by: Daniel Agar <daniel@agar.ca>
This commit is contained in:
Pedro Roque
2025-07-23 10:29:22 -07:00
committed by GitHub
co-authored by Daniel Agar
parent 09fe34af0e
commit e14eea5615
23 changed files with 2891 additions and 201 deletions
@@ -15,6 +15,8 @@
</plugin>
<plugin entity_name="*" entity_type="world" filename="libOpticalFlowSystem.so" name="custom::OpticalFlowSystem"/>
<plugin entity_name="*" entity_type="world" filename="libGstCameraSystem.so" name="custom::GstCameraSystem"/>
<plugin entity_name="*" entity_type="world" filename="libGenericMotorModelPlugin.so" name="gz::sim::systems::GenericMotorModel"/>
<plugin entity_name="*" entity_type="world" filename="libBuoyancySystemPlugin.so" name="gz::sim::systems::BuoyancySystem"/>
<!-- <plugin entity_name="*" entity_type="world" filename="libTemplatePlugin.so" name="custom::TemplateSystem"/> -->
</plugins>
</server_config>
@@ -56,12 +56,14 @@ if (gz-transport_FOUND)
add_subdirectory(template_plugin)
add_subdirectory(gstreamer)
add_subdirectory(moving_platform_controller)
add_subdirectory(generic_motor)
add_subdirectory(buoyancy)
add_subdirectory(spacecraft_thruster)
# Add an alias target for each plugin
if (TARGET GstCameraSystem)
add_custom_target(px4_gz_plugins ALL DEPENDS OpticalFlowSystem MovingPlatformController SpacecraftThrusterModelPlugin TemplatePlugin GstCameraSystem)
add_custom_target(px4_gz_plugins ALL DEPENDS OpticalFlowSystem MovingPlatformController TemplatePlugin GenericMotorModelPlugin BuoyancySystemPlugin GstCameraSystem SpacecraftThrusterModelPlugin)
else()
add_custom_target(px4_gz_plugins ALL DEPENDS OpticalFlowSystem MovingPlatformController SpacecraftThrusterModelPlugin TemplatePlugin)
add_custom_target(px4_gz_plugins ALL DEPENDS OpticalFlowSystem MovingPlatformController TemplatePlugin GenericMotorModelPlugin BuoyancySystemPlugin SpacecraftThrusterModelPlugin)
endif()
endif()
@@ -0,0 +1,699 @@
/*
* Copyright (C) 2020 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/msgs/wrench.pb.h>
#include <map>
#include <mutex>
#include <string>
#include <unordered_map>
#include <unordered_set>
#include <utility>
#include <vector>
#include <gz/common/Mesh.hh>
#include <gz/common/MeshManager.hh>
#include <gz/common/Profiler.hh>
#include <gz/plugin/Register.hh>
#include <gz/math/Helpers.hh>
#include <gz/math/Pose3.hh>
#include <gz/math/Vector3.hh>
#include <gz/math/Rand.hh>
#include <gz/msgs/Utility.hh>
#include <sdf/sdf.hh>
#include "gz/sim/components/CenterOfVolume.hh"
#include "gz/sim/components/Collision.hh"
#include "gz/sim/components/Gravity.hh"
#include "gz/sim/components/Inertial.hh"
#include "gz/sim/components/Link.hh"
#include "gz/sim/components/ParentEntity.hh"
#include "gz/sim/components/Pose.hh"
#include "gz/sim/components/Volume.hh"
#include "gz/sim/components/World.hh"
#include "gz/sim/Link.hh"
#include "gz/sim/Model.hh"
#include "gz/sim/Util.hh"
#include "BuoyancySystem.hpp"
using namespace gz;
using namespace sim;
using namespace systems;
class gz::sim::systems::BuoyancyPrivate
{
public: enum BuoyancyType {
/// \brief Applies same buoyancy to whole world.
UNIFORM_BUOYANCY,
/// \brief Uses z-axis to determine buoyancy of the world
/// This is useful for worlds where we want to simulate the ocean interface.
/// Or for instance if we want to simulate different levels of buoyancies
/// at different depths.
GRADED_BUOYANCY
};
public: BuoyancyType buoyancyType{BuoyancyType::UNIFORM_BUOYANCY};
/// \brief Get the fluid density based on a pose.
/// \param[in] _pose The pose to use when computing the fluid density. The
/// pose frame is left undefined because this function currently returns
/// a constant value, see the todo in the function implementation.
/// \return The fluid density at the givein pose.
public: double UniformFluidDensity(const math::Pose3d &_pose) const;
/// \brief Get the resultant buoyant force on a shape.
/// \param[in] _pose World pose of the shape's origin.
/// \param[in] _shape The collision mesh of a shape. Currently must
/// be box or sphere.
/// \param[in] _gravity Gravity acceleration in the world frame.
/// Updates this->buoyancyForces containing {force, center_of_volume} to be
/// applied on the link.
public:
template<typename T>
void GradedFluidDensity(
const math::Pose3d &_pose, const T &_shape, const math::Vector3d &_gravity);
/// \brief Check for new links to apply buoyancy forces to. Calculates the
/// volume and center of volume for every new link and stages them to be
/// committed when `CommitNewEntities` is called.
/// \param[in] _ecm The Entity Component Manager.
public: void CheckForNewEntities(const EntityComponentManager &_ecm);
/// \brief Commits the new entities to the ECM.
/// \param[in] _ecm The Entity Component Manager.
public: void CommitNewEntities(EntityComponentManager &_ecm);
/// \brief Check if an entity is enabled or not.
/// \param[in] _entity Target entity
/// \param[in] _ecm Entity component manager
/// \return True if buoyancy should be applied.
public: bool IsEnabled(Entity _entity,
const EntityComponentManager &_ecm) const;
/// \brief Model interface
public: Entity world{kNullEntity};
/// \brief The density of the fluid in which the object is submerged in
/// kg/m^3. Defaults to 1000, the fluid density of water.
public: double fluidDensity{1000};
/// \brief When using GradedBuoyancy, we provide a different buoyancy for
/// each layer. The key on this map is height in meters and the value is fluid
/// density. I.E all the fluid between $key$m and $next_key$m has the density
/// $value$kg/m^3. Everything below the first key is considered as having
/// fluidDensity.
public: std::map<double, double> layers;
/// \brief Holds information about forces contributed by a single collision
/// shape.
public: struct BuoyancyActionPoint {
/// \brief The force to be applied, expressed in the world frame.
math::Vector3d force;
/// \brief The point from which the force will be applied, expressed in
/// the collision's frame.
math::Vector3d point;
/// \brief The world pose of the collision.
math::Pose3d pose;
};
/// \brief List of points from where the forces act.
/// This holds values refent to the current link being processed and must be
/// cleared between links.
/// \TODO(chapulina) It's dangerous to keep link-specific values in a member
/// variable. We should consider reducing the scope of this variable and pass
/// it across functions as needed.
public: std::vector<BuoyancyActionPoint> buoyancyForces;
/// \brief Resolve all forces as if they act as a Wrench from the give pose.
/// \param[in] _linkInWorld The point from which all poses are to be resolved.
/// This is the link's origin in the world frame.
/// \return A pair of {force, torque} describing the wrench to be applied
/// at _pose, expressed in the world frame.
public: std::pair<math::Vector3d, math::Vector3d> ResolveForces(
const math::Pose3d &_linkInWorld);
/// \brief Scoped names of entities that buoyancy should apply to. If empty,
/// all links will receive buoyancy.
public: std::unordered_set<std::string> enabled;
/// \brief Basic buoyancy enabler flag. This is used to set the buoyancy
/// equal to the gravity compensation of the base_link mass, with added noise
/// to simulate the effect of buoyancy. Useful for testing purposes.
public: bool basicBuoyancy{false};
/// \brief Center of volumes to be added on the next Pre-update
public: std::unordered_map<Entity, math::Vector3d> centerOfVolumes;
/// \brief Volumes to be added on the next.
public: std::unordered_map<Entity, double> volumes;
};
//////////////////////////////////////////////////
double BuoyancyPrivate::UniformFluidDensity(const math::Pose3d &/*_pose*/) const
{
return this->fluidDensity;
}
//////////////////////////////////////////////////
template<typename T>
void BuoyancyPrivate::GradedFluidDensity(
const math::Pose3d &_pose, const T &_shape, const math::Vector3d &_gravity)
{
auto prevLayerFluidDensity = this->fluidDensity;
auto prevLayerVol = 0.0;
auto centerOfBuoyancy = math::Vector3d{0, 0, 0};
for (const auto &[height, currFluidDensity] : this->layers) {
// TODO(arjo): Transform plane and slice the shape
math::Planed plane{math::Vector3d{0, 0, 1}, height - _pose.Pos().Z()};
auto vol = _shape.VolumeBelow(plane);
// Short circuit.
if (vol <= 0) {
prevLayerFluidDensity = currFluidDensity;
continue;
}
// Calculate point from which force is applied
auto cov = _shape.CenterOfVolumeBelow(plane);
if (!cov.has_value()) {
prevLayerFluidDensity = currFluidDensity;
continue;
}
// Archimedes principle for this layer
auto forceMag = - (vol - prevLayerVol) * _gravity * prevLayerFluidDensity;
// Accumulate layers.
prevLayerFluidDensity = currFluidDensity;
auto cob = (cov.value() * vol - centerOfBuoyancy * prevLayerVol)
/ (vol - prevLayerVol);
centerOfBuoyancy = cov.value();
auto buoyancyAction = BuoyancyActionPoint {
forceMag,
cob,
_pose
};
this->buoyancyForces.push_back(buoyancyAction);
prevLayerVol = vol;
}
// For the rest of the layers.
auto vol = _shape.Volume();
// No force contributed by this layer.
if (std::abs(vol - prevLayerVol) < 1e-10) {
return;
}
// Archimedes principle for this layer
auto forceMag = - (vol - prevLayerVol) * _gravity * prevLayerFluidDensity;
// Calculate centre of buoyancy
auto cov = math::Vector3d{0, 0, 0};
auto cob =
(cov * vol - centerOfBuoyancy * prevLayerVol) / (vol - prevLayerVol);
centerOfBuoyancy = cov;
auto buoyancyAction = BuoyancyActionPoint {
forceMag,
cob,
_pose
};
this->buoyancyForces.push_back(buoyancyAction);
}
//////////////////////////////////////////////////
std::pair<math::Vector3d, math::Vector3d> BuoyancyPrivate::ResolveForces(
const math::Pose3d &_linkInWorld)
{
auto force = math::Vector3d{0, 0, 0};
auto torque = math::Vector3d{0, 0, 0};
for (const auto &b : this->buoyancyForces) {
force += b.force;
// Pose offset from application point (COV) to collision origin, expressed
// in the collision frame
math::Pose3d pointInCol{b.point, math::Quaterniond::Identity};
// Application point in the world frame
auto pointInWorld = b.pose * pointInCol;
// Offset between the link origin and the force application point
auto offset = _linkInWorld.Pos() - pointInWorld.Pos();
torque += b.force.Cross(offset);
}
return {force, torque};
}
//////////////////////////////////////////////////
void BuoyancyPrivate::CheckForNewEntities(const EntityComponentManager &_ecm)
{
// Compute the volume and center of volume for each new link
_ecm.EachNew<components::Link, components::Inertial>(
[&](const Entity & _entity,
const components::Link *,
const components::Inertial *) -> bool {
if (this->basicBuoyancy == true) return true;
// Skip if the entity already has a volume and center of volume
if (_ecm.EntityHasComponentType(_entity,
components::CenterOfVolume().TypeId()) &&
_ecm.EntityHasComponentType(_entity,
components::Volume().TypeId()))
{
return true;
}
if (!this->IsEnabled(_entity, _ecm))
{
return true;
}
Link link(_entity);
std::vector<Entity> collisions = _ecm.ChildrenByComponents(
_entity, components::Collision());
double volumeSum = 0;
gz::math::Vector3d weightedPosInLinkSum =
gz::math::Vector3d::Zero;
// Compute the volume of the link by iterating over all the collision
// elements and storing each geometry's volume.
for (const Entity &collision : collisions)
{
double volume = 0;
const components::CollisionElement *coll =
_ecm.Component<components::CollisionElement>(collision);
if (!coll) {
gzerr << "Invalid collision pointer. This shouldn't happen\n";
continue;
}
switch (coll->Data().Geom()->Type()) {
case sdf::GeometryType::BOX:
volume = coll->Data().Geom()->BoxShape()->Shape().Volume();
break;
case sdf::GeometryType::SPHERE:
volume = coll->Data().Geom()->SphereShape()->Shape().Volume();
break;
case sdf::GeometryType::CYLINDER:
volume = coll->Data().Geom()->CylinderShape()->Shape().Volume();
break;
case sdf::GeometryType::PLANE:
// Ignore plane shapes. They have no volume and are not expected
// to be buoyant.
break;
case sdf::GeometryType::MESH: {
std::string file = asFullPath(
coll->Data().Geom()->MeshShape()->Uri(),
coll->Data().Geom()->MeshShape()->FilePath());
if (common::MeshManager::Instance()->IsValidFilename(file)) {
const common::Mesh *mesh =
common::MeshManager::Instance()->Load(file);
if (mesh) {
volume = mesh->Volume();
} else {
gzerr << "Unable to load mesh[" << file << "]\n";
}
} else {
gzerr << "Invalid mesh filename[" << file << "]\n";
}
break;
}
default:
gzerr << "Unsupported collision geometry["
<< static_cast<int>(coll->Data().Geom()->Type()) << "]\n";
break;
}
volumeSum += volume;
auto poseInLink = _ecm.Component<components::Pose>(collision)->Data();
weightedPosInLinkSum += volume * poseInLink.Pos();
}
if (volumeSum > 0)
{
// Stage calculation results for future commit. We do this because
// during PostUpdate the ECM is const, so we can't modify it,
this->centerOfVolumes[_entity] = weightedPosInLinkSum / volumeSum;
this->volumes[_entity] = volumeSum;
}
return true;
});
}
//////////////////////////////////////////////////
void BuoyancyPrivate::CommitNewEntities(EntityComponentManager &_ecm)
{
for (const auto [_entity, _cov] : this->centerOfVolumes) {
if (_ecm.HasEntity(_entity)) {
_ecm.CreateComponent(_entity, components::CenterOfVolume(_cov));
}
}
for (const auto [_entity, _vol] : this->volumes) {
if (_ecm.HasEntity(_entity)) {
_ecm.CreateComponent(_entity, components::Volume(_vol));
}
}
this->centerOfVolumes.clear();
this->volumes.clear();
}
//////////////////////////////////////////////////
bool BuoyancyPrivate::IsEnabled(Entity _entity,
const EntityComponentManager &_ecm) const
{
// If there's nothing enabled, all entities are enabled
if (this->enabled.empty()) {
return true;
}
auto entity = _entity;
while (entity != kNullEntity) {
// Fully scoped name
auto name = scopedName(entity, _ecm, "::", false);
// Remove world name
name = removeParentScope(name, "::");
if (this->enabled.find(name) != this->enabled.end()) {
return true;
}
// Check parent
auto parentComp = _ecm.Component<components::ParentEntity>(entity);
if (nullptr == parentComp) {
return false;
}
entity = parentComp->Data();
}
return false;
}
//////////////////////////////////////////////////
Buoyancy::Buoyancy()
: dataPtr(std::make_unique<BuoyancyPrivate>())
{
}
//////////////////////////////////////////////////
void Buoyancy::Configure(const Entity &_entity,
const std::shared_ptr<const sdf::Element> &_sdf,
EntityComponentManager &_ecm,
EventManager &/*_eventMgr*/)
{
// Store the world.
this->dataPtr->world = _entity;
// Get the gravity (defined in world frame)
const components::Gravity *gravity = _ecm.Component<components::Gravity>(
this->dataPtr->world);
if (!gravity) {
gzerr << "Unable to get the gravity vector. Make sure this plugin is "
<< "attached to a <world>, not a <model>." << std::endl;
return;
}
if (_sdf->HasElement("uniform_fluid_density")) {
this->dataPtr->fluidDensity = _sdf->Get<double>("uniform_fluid_density");
} else if (_sdf->HasElement("graded_buoyancy")) {
this->dataPtr->buoyancyType =
BuoyancyPrivate::BuoyancyType::GRADED_BUOYANCY;
auto gradedElement = _sdf->GetFirstElement();
if (gradedElement == nullptr) {
gzerr << "Unable to get element description" << std::endl;
return;
}
auto argument = gradedElement->GetFirstElement();
while (argument != nullptr) {
if (argument->GetName() == "default_density") {
argument->GetValue()->Get<double>(this->dataPtr->fluidDensity);
gzdbg << "Default density set to "
<< this->dataPtr->fluidDensity << std::endl;
}
if (argument->GetName() == "density_change") {
auto depth = argument->Get<double>("above_depth", 0.0);
auto density = argument->Get<double>("density", 0.0);
if (!depth.second) {
gzwarn << "No <above_depth> tag was found as a "
<< "child of <density_change>" << std::endl;
}
if (!density.second) {
gzwarn << "No <density> tag was found as a "
<< "child of <density_change>" << std::endl;
}
this->dataPtr->layers[depth.first] = density.first;
gzdbg << "Added layer at " << depth.first << ", "
<< density.first << std::endl;
}
argument = argument->GetNextElement();
}
} else if (_sdf->HasElement("basic_buoyancy")) {
// This is a legacy tag, which is equivalent to
this->dataPtr->basicBuoyancy = _sdf->Get<bool>("basic_buoyancy", false).first;
} else {
gzwarn <<
"Neither <graded_buoyancy>, <uniform_fluid_density>, or <basic_buyancy> were specified"
<< std::endl
<< "\tDefaulting to <basic_buyancy>"
<< std::endl;
this->dataPtr->basicBuoyancy = true;
}
if (_sdf->HasElement("enable")) {
for (auto enableElem = _sdf->FindElement("enable");
enableElem != nullptr;
enableElem = enableElem->GetNextElement("enable")) {
this->dataPtr->enabled.insert(enableElem->Get<std::string>());
}
}
}
//////////////////////////////////////////////////
void Buoyancy::PreUpdate(const UpdateInfo &_info,
EntityComponentManager &_ecm)
{
GZ_PROFILE("Buoyancy::PreUpdate");
this->dataPtr->CheckForNewEntities(_ecm);
this->dataPtr->CommitNewEntities(_ecm);
// Only update if not paused.
if (_info.paused) {
return;
}
const components::Gravity *gravity = _ecm.Component<components::Gravity>(
this->dataPtr->world);
if (!gravity) {
gzerr << "Unable to get the gravity vector. Has gravity been defined?"
<< std::endl;
return;
}
if (!this->dataPtr->basicBuoyancy) {
// Iterate over all links and apply buoyancy forces.
_ecm.Each<components::Link,
components::Volume,
components::CenterOfVolume>(
[&](const Entity & _entity,
const components::Link *,
const components::Volume * _volume,
const components::CenterOfVolume * _centerOfVolume) -> bool {
// World pose of the link.
math::Pose3d linkWorldPose = worldPose(_entity, _ecm);
Link link(_entity);
math::Vector3d buoyancy;
// By Archimedes' principle,
// buoyancy = -(mass*gravity)*fluid_density/object_density
// object_density = mass/volume, so the mass term cancels.
if (this->dataPtr->buoyancyType
== BuoyancyPrivate::BuoyancyType::UNIFORM_BUOYANCY)
{
buoyancy =
-this->dataPtr->UniformFluidDensity(linkWorldPose) *
_volume->Data() * gravity->Data();
// Convert the center of volume to the world frame
math::Vector3d offsetWorld = linkWorldPose.Rot().RotateVector(
_centerOfVolume->Data());
// Compute the torque that should be applied due to buoyancy and
// the center of volume.
math::Vector3d torque = offsetWorld.Cross(buoyancy);
// Apply the wrench to the link. This wrench is applied in the
// Physics System.
link.AddWorldWrench(_ecm, buoyancy, torque);
} else if (this->dataPtr->buoyancyType
== BuoyancyPrivate::BuoyancyType::GRADED_BUOYANCY)
{
std::vector<Entity> collisions = _ecm.ChildrenByComponents(
_entity, components::Collision());
this->dataPtr->buoyancyForces.clear();
for (auto e : collisions) {
const components::CollisionElement *coll =
_ecm.Component<components::CollisionElement>(e);
auto pose = worldPose(e, _ecm);
if (!coll) {
gzerr << "Invalid collision pointer. This shouldn't happen\n";
continue;
}
switch (coll->Data().Geom()->Type()) {
case sdf::GeometryType::BOX:
this->dataPtr->GradedFluidDensity<math::Boxd>(
pose,
coll->Data().Geom()->BoxShape()->Shape(),
gravity->Data());
break;
case sdf::GeometryType::SPHERE:
this->dataPtr->GradedFluidDensity<math::Sphered>(
pose,
coll->Data().Geom()->SphereShape()->Shape(),
gravity->Data());
break;
default: {
static bool warned{false};
if (!warned) {
gzwarn << "Only <box> and <sphere> collisions are supported "
<< "by the graded buoyancy option." << std::endl;
warned = true;
}
break;
}
}
}
auto [force, torque] = this->dataPtr->ResolveForces(linkWorldPose);
// Apply the wrench to the link. This wrench is applied in the
// Physics System.
link.AddWorldWrench(_ecm, force, torque);
}
return true;
});
} else {
// Apply simple buyancy to baselink with some white noise.
// Collect all masses in the model
double mass = 0.0;
_ecm.Each<components::Link,
components::Inertial>(
[&](const Entity & _entity,
const components::Link *,
const components::Inertial * _inertial) -> bool {
// Get all masses and add them up
Link link(_entity);
if (_ecm.EntityHasComponentType(_entity,
components::Inertial().TypeId()))
{
const auto *inertialComp =
_ecm.Component<components::Inertial>(_entity);
if (inertialComp) {
mass = inertialComp->Data().MassMatrix().Mass();
// Apply the buoyancy force
math::Vector3d buoyancy = -gravity->Data() * mass;
math::Vector3d randVec(0.0, 0.0, -gravity->Data().Z() * mass * math::Rand::DblUniform(-0.01, 0.01));
math::Vector3d force = buoyancy + randVec;
link.AddWorldWrench(_ecm, force, math::Vector3d{0, 0, 0});
}
}
return true;
});
}
}
//////////////////////////////////////////////////
void Buoyancy::PostUpdate(
const UpdateInfo &/*_info*/,
const EntityComponentManager &_ecm)
{
this->dataPtr->CheckForNewEntities(_ecm);
}
//////////////////////////////////////////////////
bool Buoyancy::IsEnabled(Entity _entity,
const EntityComponentManager &_ecm) const
{
return this->dataPtr->IsEnabled(_entity, _ecm);
}
GZ_ADD_PLUGIN(Buoyancy,
System,
Buoyancy::ISystemConfigure,
Buoyancy::ISystemPreUpdate,
Buoyancy::ISystemPostUpdate)
GZ_ADD_PLUGIN_ALIAS(Buoyancy,
"gz::sim::systems::BuoyancySystem")
@@ -0,0 +1,158 @@
/*
* Copyright (C) 2020 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_BUOYANCY_SYSTEM_HPP_
#define GZ_SIM_SYSTEMS_BUOYANCY_SYSTEM_HPP_
#include <gz/sim/System.hh>
#include <memory>
namespace gz
{
namespace sim
{
// Inline bracket to help doxygen filtering.
inline namespace GZ_SIM_VERSION_NAMESPACE
{
namespace systems
{
// Forward declaration
class BuoyancyPrivate;
/// \brief A system that simulates buoyancy of objects immersed in fluid.
/// All SDF parameters are optional. This system must be attached to the
/// world and this system will apply buoyancy to all links that have collision
/// shapes.
///
/// The volume and center of volume will be computed for each link, and
/// stored as components. During each iteration, Archimedes' principle is
/// applied to each link with a volume and center of volume component.
///
/// Plane shapes are not handled by this plugin, and will not be affected
/// by buoyancy.
///
/// ## System Parameters
///
/// * `<uniform_fluid_density>` sets the density of the fluid that surrounds
/// the buoyant object. [Units: kgm^-3]
/// * `<graded_buoyancy>` allows you to define a world where the buoyancy
/// changes along the Z axis. An example of such a world could be if we are
/// simulating an open ocean with its surface and under water behaviour. This
/// mode slices the volume of each collision mesh according to where the water
/// line is set. When defining a `<graded_buoyancy>` tag, one must also define
/// `<default_density>` and `<density_change>` tags.
/// * `<default_density>` is the default fluid which the world should be
/// filled with. [Units: kgm^-3]
/// * `<density_change>` allows you to define a new layer.
/// * `<above_depth>` a child property of `<density_change>`. This determines
/// the height at which the next fluid layer should start. [Units: m]
/// * `<density>` the density of the fluid in this layer. [Units: kgm^-3]
/// * `<enable>` used to indicate which models will have buoyancy.
/// Add one enable element per model or link. This element accepts names
/// scoped from the top level model (i.e. `<model>::<nested_model>::<link>`).
/// If there are no enabled entities, all models in simulation will be
/// affected by buoyancy.
///
/// ## Examples
///
/// ### uniform_fluid_density world
///
/// The `buoyancy.sdf` SDF file contains three submarines. The first
/// submarine is neutrally buoyant, the second sinks, and the third
/// floats. To run:
///
/// ```
/// gz sim -v 4 buoyancy.sdf
/// ```
///
/// ### graded_buoyancy world
///
/// Often when simulating a maritime environment one may need to simulate both
/// surface and underwater vessels. This means the buoyancy plugin needs to
/// take into account two different fluids. One being water with a density of
/// 1000kgm^-3 and another being air with a very light density of say 1kgm^-3.
/// An example for such a configuration may be found in the
/// `graded_buoyancy.sdf` world.
///
/// ```
/// gz sim -v 4 graded_buoyancy.sdf
/// ```
///
/// You should be able to see a sphere bobbing up and down undergoing simple
/// harmonic motion on the surface of the fluid (this is expected behaviour
/// as the SHM is usually damped by the hydrodynamic forces. See the hydro-
/// dynamics plugin for an example of how to use it). The key part of this is
///
/// ```
/// <graded_buoyancy>
/// <default_density>1000</default_density>
/// <density_change>
/// <above_depth>0</above_depth>
/// <density>1</density>
/// </density_change>
/// </graded_buoyancy>
/// ```
/// The default density tag says that by default the world has a fluid density
/// of 1000kgm^-3. This essentially states that by default the world is filled
/// with dihydrogen monoxide (aka water). The `<density_change>` tag
/// essentially establishes the fact that there is a nother fluid. The
/// `<above_depth>` tag says that above z=0 there is another fluid with a
/// different density. The density of that fluid is defined by the `<density>`
/// tag. We will be simulating air with a fluid density of 1kgm^-3.
class Buoyancy
: public System,
public ISystemConfigure,
public ISystemPreUpdate,
public ISystemPostUpdate
{
/// \brief Constructor
public: Buoyancy();
/// \brief Destructor
public: ~Buoyancy() override = default;
// Documentation inherited
public: void Configure(const Entity &_entity,
const std::shared_ptr<const sdf::Element> &_sdf,
EntityComponentManager &_ecm,
EventManager &_eventMgr) override;
// Documentation inherited
public: void PreUpdate(
const UpdateInfo &_info,
EntityComponentManager &_ecm) override;
// Documentation inherited
public: void PostUpdate(
const UpdateInfo &_info,
const EntityComponentManager &_ecm) override;
/// \brief Check if an entity is enabled or not.
/// \param[in] _entity Target entity
/// \param[in] _ecm Entity component manager
/// \return True if buoyancy should be applied.
public: bool IsEnabled(Entity _entity,
const EntityComponentManager &_ecm) const;
/// \brief Private data pointer
private: std::unique_ptr<BuoyancyPrivate> dataPtr;
};
}
}
}
}
#endif
@@ -0,0 +1,68 @@
############################################################################
#
# Copyright (c) 2025 PX4 Development Team. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
# 1. Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
# 2. Redistributions in binary form must reproduce the above copyright
# notice, this list of conditions and the following disclaimer in
# the documentation and/or other materials provided with the
# distribution.
# 3. Neither the name PX4 nor the names of its contributors may be
# used to endorse or promote products derived from this software
# without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
############################################################################
# Template for a new plugin project
# Replace TemplatePlugin with your plugin name
project(BuoyancySystemPlugin)
# Add external dependencies if needed
# include(${CMAKE_CURRENT_SOURCE_DIR}/dependency.cmake)
# Find required packages
# find_package(PackageName REQUIRED)
add_library(${PROJECT_NAME} SHARED
# Add your source files here
BuoyancySystem.cpp
)
target_link_libraries(${PROJECT_NAME}
PUBLIC px4_gz_msgs
PUBLIC gz-sensors${gz-sensors_VERSION_MAJOR}::gz-sensors${gz-sensors_VERSION_MAJOR}
PUBLIC gz-plugin${gz-plugin_VERSION_MAJOR}::gz-plugin${gz-plugin_VERSION_MAJOR}
PUBLIC gz-sim${gz-sim_VERSION_MAJOR}::gz-sim${gz-sim_VERSION_MAJOR}
PUBLIC gz-transport${gz-transport_VERSION_MAJOR}::gz-transport${gz-transport_VERSION_MAJOR}
# Add other dependencies as needed
# PUBLIC ${OtherLib_LIBS}
)
target_include_directories(${PROJECT_NAME}
PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}
PUBLIC ${CMAKE_CURRENT_BINARY_DIR}
PUBLIC px4_gz_msgs
# Add other include directories as needed
# PUBLIC ${OtherLib_INCLUDE_DIRS}
)
# Add dependencies if needed
# add_dependencies(${PROJECT_NAME} ExternalDependency)
@@ -0,0 +1,68 @@
############################################################################
#
# Copyright (c) 2025 PX4 Development Team. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
# 1. Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
# 2. Redistributions in binary form must reproduce the above copyright
# notice, this list of conditions and the following disclaimer in
# the documentation and/or other materials provided with the
# distribution.
# 3. Neither the name PX4 nor the names of its contributors may be
# used to endorse or promote products derived from this software
# without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
############################################################################
# Template for a new plugin project
# Replace TemplatePlugin with your plugin name
project(GenericMotorModelPlugin)
# Add external dependencies if needed
# include(${CMAKE_CURRENT_SOURCE_DIR}/dependency.cmake)
# Find required packages
# find_package(PackageName REQUIRED)
add_library(${PROJECT_NAME} SHARED
# Add your source files here
GenericMotorModel.cpp
)
target_link_libraries(${PROJECT_NAME}
PUBLIC px4_gz_msgs
PUBLIC gz-sensors${gz-sensors_VERSION_MAJOR}::gz-sensors${gz-sensors_VERSION_MAJOR}
PUBLIC gz-plugin${gz-plugin_VERSION_MAJOR}::gz-plugin${gz-plugin_VERSION_MAJOR}
PUBLIC gz-sim${gz-sim_VERSION_MAJOR}::gz-sim${gz-sim_VERSION_MAJOR}
PUBLIC gz-transport${gz-transport_VERSION_MAJOR}::gz-transport${gz-transport_VERSION_MAJOR}
# Add other dependencies as needed
# PUBLIC ${OtherLib_LIBS}
)
target_include_directories(${PROJECT_NAME}
PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}
PUBLIC ${CMAKE_CURRENT_BINARY_DIR}
PUBLIC px4_gz_msgs
# Add other include directories as needed
# PUBLIC ${OtherLib_INCLUDE_DIRS}
)
# Add dependencies if needed
# add_dependencies(${PROJECT_NAME} ExternalDependency)
@@ -0,0 +1,931 @@
/*
* Copyright 2015 Fadri Furrer, ASL, ETH Zurich, Switzerland
* Copyright 2015 Michael Burri, ASL, ETH Zurich, Switzerland
* Copyright 2015 Mina Kamel, ASL, ETH Zurich, Switzerland
* Copyright 2015 Janosch Nikolic, ASL, ETH Zurich, Switzerland
* Copyright 2015 Markus Achtelik, ASL, ETH Zurich, Switzerland
* Copyright 2016 Geoffrey Hunter <gbmhunter@gmail.com>
* Copyright (C) 2019 Open Source Robotics Foundation
* Copyright (C) 2022 Benjamin Perseghetti, Rudis Laboratories
*
* 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 "GenericMotorModel.hpp"
#include <mutex>
#include <gz/msgs/actuators.pb.h>
#include <gz/common/Profiler.hh>
#include <gz/plugin/Register.hh>
#include <gz/transport/Node.hh>
#include <gz/math/Helpers.hh>
#include <gz/math/Pose3.hh>
#include <gz/math/Vector3.hh>
#include <gz/msgs/Utility.hh>
#include <sdf/sdf.hh>
#include "gz/sim/components/Actuators.hh"
#include "gz/sim/components/ExternalWorldWrenchCmd.hh"
#include "gz/sim/components/JointAxis.hh"
#include "gz/sim/components/JointVelocity.hh"
#include "gz/sim/components/JointVelocityCmd.hh"
#include "gz/sim/components/LinearVelocity.hh"
#include "gz/sim/components/ParentLinkName.hh"
#include "gz/sim/components/Pose.hh"
#include "gz/sim/components/Wind.hh"
#include "gz/sim/Link.hh"
#include "gz/sim/Model.hh"
#include "gz/sim/Util.hh"
// from rotors_gazebo_plugins/include/rotors_gazebo_plugins/common.h
/// \brief This class can be used to apply a first order filter on a signal.
/// It allows different acceleration and deceleration time constants.
/// \details
/// Short reveiw of discrete time implementation of first order system
/// Laplace:
/// X(s)/U(s) = 1/(tau*s + 1)
/// continous time system:
/// dx(t) = (-1/tau)*x(t) + (1/tau)*u(t)
/// discretized system (ZoH):
/// x(k+1) = exp(samplingTime*(-1/tau))*x(k) + (1 - exp(samplingTime*(-1/tau))) * u(k) // NOLINT
template <typename T>
class FirstOrderFilter
{
public:
FirstOrderFilter(double _timeConstantUp, double _timeConstantDown, T _initialState): // NOLINT
timeConstantUp(_timeConstantUp),
timeConstantDown(_timeConstantDown),
previousState(_initialState) {}
/// \brief This method will apply a first order filter on the _inputState.
T UpdateFilter(T _inputState, double _samplingTime)
{
T outputState;
if (_inputState > previousState) {
// Calcuate the outputState if accelerating.
double alphaUp = exp(-_samplingTime / timeConstantUp);
// x(k+1) = Ad*x(k) + Bd*u(k)
outputState = alphaUp * previousState + (1 - alphaUp) * _inputState;
} else {
// Calculate the outputState if decelerating.
double alphaDown = exp(-_samplingTime / timeConstantDown);
outputState = alphaDown * previousState + (1 - alphaDown) * _inputState;
}
previousState = outputState;
return outputState;
}
~FirstOrderFilter() = default;
protected:
double timeConstantUp;
double timeConstantDown;
T previousState;
};
using namespace gz;
using namespace sim;
using namespace systems;
/// \brief Constants for specifying clockwise (kCw) and counter-clockwise (kCcw)
/// directions of rotation.
namespace turning_direction
{
static const int kCcw = 1;
static const int kCw = -1;
} // namespace turning_direction
/// \brief Type of input command to motor.
enum class MotorType {
kVelocity,
kPosition,
kForce,
kForcePolynomial
};
/// \brief Type of control methology
enum class ControlMethod {
kRPM,
kDutyCycle
};
class gz::sim::systems::GenericMotorModelPrivate
{
/// \brief Callback for actuator commands.
public: void OnActuatorMsg(const msgs::Actuators &_msg);
/// \brief Apply link forces and moments based on propeller state.
public: void UpdateForcesAndMoments(EntityComponentManager &_ecm);
/// \brief Joint Entity
public: Entity jointEntity;
/// \brief Joint name
public: std::string jointName;
/// \brief Link Entity
public: Entity linkEntity;
/// \brief Link name
public: std::string linkName;
/// \brief Parent link Entity
public: Entity parentLinkEntity;
/// \brief Parent link name
public: std::string parentLinkName;
/// \brief Model interface
public: Model model{kNullEntity};
/// \brief Topic for actuator commands.
public: std::string commandSubTopic;
/// \brief Topic namespace.
public: std::string robotNamespace;
/// \brief Sampling time (from motor_model.hpp).
public: double samplingTime = 0.01;
/// \brief Index of motor in Actuators msg on multirotor_base.
public: int actuatorNumber = 0;
/// \brief Turning direction of the motor.
public: int turningDirection = turning_direction::kCw;
/// \brief Type of input command to motor.
public: MotorType motorType = MotorType::kVelocity;
/// \brief Type of input command to motor.
public: ControlMethod controlMethod = ControlMethod::kRPM;
/// \brief Maximum rotational velocity command with units of rad/s.
/// The default value is taken from gazebo_motor_model.h
/// and is approximately 8000 revolutions / minute (rpm).
public: double maxRotVelocity = 838.0;
/// \brief Minimum rotor velocity, below which the rotor is considered
/// to be stopped. Simulates minimum velocity achieved by the motor
public: double minCommand = 0.0;
/// \brief Maximum duty cycle value
public: double maxDutyCycle = 0.0;
/// \brief Minimum duty cycle value
public: double minDutyCycle = 0.0;
/// \brief Bidirectional-capable motor. Set to true if it mimics an ESC
/// with bidirectional control. Zero input will be middle of max and min
/// duty cycles.
public: int bidirectionalMotor = 0;
/// \brief Moment constant for computing drag torque based on thrust
/// with units of length (m).
/// The default value is taken from gazebo_motor_model.h
public: double momentConstant = 0.016;
/// \brief Thrust coefficient for propeller with units of N / (rad/s)^2.
/// The default value is taken from gazebo_motor_model.h
public: double motorConstant = 8.54858e-06;
/// \brief Reference input to motor. For MotorType kVelocity, this
/// is the reference angular velocity in rad/s.
public: double refMotorInput = 0.0;
/// \brief Rolling moment coefficient with units of N*m / (m/s^2).
/// The default value is taken from gazebo_motor_model.h
public: double rollingMomentCoefficient = 1.0e-6;
/// \brief Rotor drag coefficient for propeller with units of N / (m/s^2).
/// The default value is taken from gazebo_motor_model.h
public: double rotorDragCoefficient = 1.0e-4;
/// \brief Large joint velocities can cause problems with aliasing,
/// so the joint velocity used by the physics engine is reduced
/// this factor, while the larger value is used for computing
/// propeller thrust.
/// The default value is taken from gazebo_motor_model.h
public: double rotorVelocitySlowdownSim = 10.0;
/// \brief Time constant for rotor deceleration.
/// The default value is taken from gazebo_motor_model.h
public: double timeConstantDown = 1.0 / 40.0;
/// \brief Time constant for rotor acceleration.
/// The default value is taken from gazebo_motor_model.h
public: double timeConstantUp = 1.0 / 80.0;
/// \brief Filter on rotor velocity that has different time constants
/// for increasing and decreasing values.
public: std::unique_ptr<FirstOrderFilter<double>> rotorVelocityFilter;
/// \brief Received Actuators message. This is nullopt if no message has been
/// received.
public: std::optional<msgs::Actuators> recvdActuatorsMsg;
/// \brief Mutex to protect recvdActuatorsMsg.
public: std::mutex recvdActuatorsMsgMutex;
/// \brief Polynomial for RPM to Thrust conversion - admits 5 degrees with asymmetric behavior
public: std::vector<double> positiveTorquePolynomial = {};
public: std::vector<double> positiveThrustPolynomial = {};
public: std::vector<double> negativeTorquePolynomial = {};
public: std::vector<double> negativeThrustPolynomial = {};
/// \brief Gazebo communication node.
public: transport::Node node;
};
//////////////////////////////////////////////////
GenericMotorModel::GenericMotorModel()
: dataPtr(std::make_unique<GenericMotorModelPrivate>())
{
}
//////////////////////////////////////////////////
void GenericMotorModel::Configure(const Entity &_entity,
const std::shared_ptr<const sdf::Element> &_sdf,
EntityComponentManager &_ecm,
EventManager &/*_eventMgr*/)
{
this->dataPtr->model = Model(_entity);
if (!this->dataPtr->model.Valid(_ecm)) {
gzerr << "GenericMotorModel plugin should be attached to a model "
<< "entity. Failed to initialize." << std::endl;
return;
}
auto sdfClone = _sdf->Clone();
this->dataPtr->robotNamespace.clear();
if (sdfClone->HasElement("robotNamespace")) {
this->dataPtr->robotNamespace =
sdfClone->Get<std::string>("robotNamespace");
} else {
gzwarn << "No robotNamespace set using entity name.\n";
this->dataPtr->robotNamespace = this->dataPtr->model.Name(_ecm);
}
// Get params from SDF
if (sdfClone->HasElement("jointName")) {
this->dataPtr->jointName = sdfClone->Get<std::string>("jointName");
}
if (this->dataPtr->jointName.empty()) {
gzerr << "GenericMotorModel found an empty jointName parameter. "
<< "Failed to initialize.";
return;
}
if (sdfClone->HasElement("linkName")) {
this->dataPtr->linkName = sdfClone->Get<std::string>("linkName");
}
if (this->dataPtr->linkName.empty()) {
gzerr << "GenericMotorModel found an empty linkName parameter. "
<< "Failed to initialize.";
return;
}
if (sdfClone->HasElement("actuator_number")) {
this->dataPtr->actuatorNumber =
sdfClone->GetElement("actuator_number")->Get<int>();
} else if (sdfClone->HasElement("motorNumber")) {
this->dataPtr->actuatorNumber =
sdfClone->GetElement("motorNumber")->Get<int>();
gzwarn << "<motorNumber> is depricated, "
<< "please use <actuator_number>.\n";
} else {
gzerr << "Please specify a actuator_number.\n";
}
if (sdfClone->HasElement("turningDirection")) {
auto turningDirection =
sdfClone->GetElement("turningDirection")->Get<std::string>();
if (turningDirection == "cw") {
this->dataPtr->turningDirection = turning_direction::kCw;
} else if (turningDirection == "ccw") {
this->dataPtr->turningDirection = turning_direction::kCcw;
} else {
gzerr << "Please only use 'cw' or 'ccw' as turningDirection.\n";
}
} else {
gzerr << "Please specify a turning direction ('cw' or 'ccw').\n";
}
if (sdfClone->HasElement("controlMethod")) {
auto controlMethod =
sdfClone->GetElement("controlMethod")->Get<std::string>();
if (controlMethod == "rpm") {
this->dataPtr->controlMethod = ControlMethod::kRPM;
} else if (controlMethod == "duty_cycle") {
this->dataPtr->controlMethod = ControlMethod::kDutyCycle;
} else {
gzerr << "Please only use 'rpm' or 'duty_cycle' as controlMethod.\n";
}
} else {
this->dataPtr->controlMethod = ControlMethod::kRPM;
}
// Check required parameters for each control method type
if (this->dataPtr->controlMethod == ControlMethod::kDutyCycle) {
if (sdfClone->HasElement("minCommand")) {
this->dataPtr->minCommand =
sdfClone->GetElement("minCommand")->Get<double>();
} else {
gzerr << "Please specify a minCommand.\n";
}
if (sdfClone->HasElement("maxDutyCycle")) {
this->dataPtr->maxDutyCycle =
sdfClone->GetElement("maxDutyCycle")->Get<double>();
} else {
gzerr << "Please specify a maxDutyCycle.\n";
}
if (sdfClone->HasElement("minDutyCycle")) {
this->dataPtr->minDutyCycle =
sdfClone->GetElement("minDutyCycle")->Get<double>();
} else {
gzerr << "Please specify a minDutyCycle.\n";
}
}
if (sdfClone->HasElement("motorType")) {
auto motorType = sdfClone->GetElement("motorType")->Get<std::string>();
if (motorType == "velocity") {
this->dataPtr->motorType = MotorType::kVelocity;
} else if (motorType == "position") {
this->dataPtr->motorType = MotorType::kPosition;
gzerr << "motorType 'position' not implemented" << std::endl;
} else if (motorType == "force") {
this->dataPtr->motorType = MotorType::kForce;
gzerr << "motorType 'force' not implemented" << std::endl;
} else if (motorType == "force_polynomial") {
this->dataPtr->motorType = MotorType::kForcePolynomial;
// Check validity of required parameters
if (sdfClone->HasElement("positiveThrustPolynomial")) {
std::string raw_coeffs = sdfClone->GetElement("positiveThrustPolynomial")->Get<std::string>();
std::vector<double> coeffs = this->ParsePolynomial(raw_coeffs);
if (coeffs.size() < 4) {
gzerr << "positiveThrustPolynomial must have at least 4 elements, but got "
<< coeffs.size() << std::endl;
}
for (const auto &coeff : coeffs) {
this->dataPtr->positiveThrustPolynomial.push_back(coeff);
}
} else {
gzerr << "Please specify a positiveThrustPolynomial.\n";
}
if (sdfClone->HasElement("negativeThrustPolynomial")) {
std::string raw_coeffs = sdfClone->GetElement("negativeThrustPolynomial")->Get<std::string>();
std::vector<double> coeffs = this->ParsePolynomial(raw_coeffs);
if (coeffs.size() < 4) {
gzerr << "negativeThrustPolynomial must have at least 4 elements, but got "
<< coeffs.size() << std::endl;
}
for (const auto &coeff : coeffs) {
this->dataPtr->negativeThrustPolynomial.push_back(coeff);
}
} else {
this->dataPtr->negativeThrustPolynomial = this->dataPtr->positiveThrustPolynomial;
gzwarn << "Using positive thrust polynomial as negative thrust polynomial (symmetric performance)." << std::endl;
}
if (sdfClone->HasElement("positiveTorquePolynomial")) {
std::string raw_coeffs = sdfClone->GetElement("positiveTorquePolynomial")->Get<std::string>();
std::vector<double> coeffs = this->ParsePolynomial(raw_coeffs);
if (coeffs.size() < 4 || coeffs.size() > 6) {
gzerr << "positiveTorquePolynomial must have between 4 and 6 elements, but got "
<< coeffs.size() << std::endl;
}
for (const auto &coeff : coeffs) {
this->dataPtr->positiveTorquePolynomial.push_back(coeff);
}
} else {
gzdbg << "No positive torque polynomial. Using zero.\n" << std::endl;
this->dataPtr->positiveTorquePolynomial = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
}
if (sdfClone->HasElement("negativeTorquePolynomial")) {
std::string raw_coeffs = sdfClone->GetElement("negativeTorquePolynomial")->Get<std::string>();
std::vector<double> coeffs = this->ParsePolynomial(raw_coeffs);
if (coeffs.size() < 4 || coeffs.size() > 6) {
gzerr << "negativeTorquePolynomial must have between 4 and 6 elements, but got "
<< coeffs.size() << std::endl;
}
for (const auto &coeff : coeffs) {
this->dataPtr->negativeTorquePolynomial.push_back(coeff);
}
} else {
this->dataPtr->negativeTorquePolynomial = this->dataPtr->positiveTorquePolynomial;
gzwarn << "Using positive torque polynomial as negative torque polynomial (symmetric performance)." << std::endl;
}
// print final polynomial values
gzdbg << "Added motor " << this->dataPtr->actuatorNumber << ". Polynomials:\n - "
<< "Positive thrust polynomial: ";
for (const auto &val : this->dataPtr->positiveThrustPolynomial) {
gzdbg << val << " ";
}
gzdbg << "\n - Negative thrust polynomial: ";
for (const auto &val : this->dataPtr->negativeThrustPolynomial) {
gzdbg << val << " ";
}
gzdbg << "\n - Positive torque polynomial: ";
for (const auto &val : this->dataPtr->positiveTorquePolynomial) {
gzdbg << val << " ";
}
gzdbg << "\n - Negative torque polynomial: ";
for (const auto &val : this->dataPtr->negativeTorquePolynomial) {
gzdbg << val << " ";
}
gzdbg << std::endl;
} else {
gzerr << "Please only use 'velocity', 'position', "
"'force' or 'force_polynomial' as motorType.\n"
<< std::endl;
}
} else {
gzwarn << "motorType not specified, using velocity.\n";
this->dataPtr->motorType = MotorType::kVelocity;
}
sdfClone->Get<std::string>("commandSubTopic",
this->dataPtr->commandSubTopic, this->dataPtr->commandSubTopic);
sdfClone->Get<double>("rotorDragCoefficient",
this->dataPtr->rotorDragCoefficient,
this->dataPtr->rotorDragCoefficient);
sdfClone->Get<double>("rollingMomentCoefficient",
this->dataPtr->rollingMomentCoefficient,
this->dataPtr->rollingMomentCoefficient);
sdfClone->Get<double>("maxRotVelocity",
this->dataPtr->maxRotVelocity, this->dataPtr->maxRotVelocity);
sdfClone->Get<double>("motorConstant",
this->dataPtr->motorConstant, this->dataPtr->motorConstant);
sdfClone->Get<double>("momentConstant",
this->dataPtr->momentConstant, this->dataPtr->momentConstant);
sdfClone->Get<int>("bidirectionalMotor",
this->dataPtr->bidirectionalMotor, this->dataPtr->bidirectionalMotor);
sdfClone->Get<double>("timeConstantUp",
this->dataPtr->timeConstantUp, this->dataPtr->timeConstantUp);
sdfClone->Get<double>("timeConstantDown",
this->dataPtr->timeConstantDown, this->dataPtr->timeConstantDown);
sdfClone->Get<double>("rotorVelocitySlowdownSim",
this->dataPtr->rotorVelocitySlowdownSim, 10);
// Create the first order filter.
this->dataPtr->rotorVelocityFilter =
std::make_unique<FirstOrderFilter<double>>(
this->dataPtr->timeConstantUp, this->dataPtr->timeConstantDown,
this->dataPtr->refMotorInput);
// Subscribe to actuator command messages
std::string topic = transport::TopicUtils::AsValidTopic(
this->dataPtr->robotNamespace + "/" + this->dataPtr->commandSubTopic);
if (topic.empty()) {
gzerr << "Failed to create topic for [" << this->dataPtr->robotNamespace
<< "]" << std::endl;
return;
} else {
gzdbg << "Listening to topic: " << topic << std::endl;
}
this->dataPtr->node.Subscribe(topic,
&GenericMotorModelPrivate::OnActuatorMsg, this->dataPtr.get());
}
//////////////////////////////////////////////////
void GenericMotorModel::PreUpdate(const UpdateInfo &_info,
EntityComponentManager &_ecm)
{
GZ_PROFILE("GenericMotorModel::PreUpdate");
// \TODO(anyone) Support rewind
if (_info.dt < std::chrono::steady_clock::duration::zero()) {
gzwarn << "Detected jump back in time ["
<< std::chrono::duration<double>(_info.dt).count()
<< "s]. System may not work properly." << std::endl;
}
// If the joint or links haven't been identified yet, look for them
if (this->dataPtr->jointEntity == kNullEntity) {
this->dataPtr->jointEntity =
this->dataPtr->model.JointByName(_ecm, this->dataPtr->jointName);
const auto parentLinkName = _ecm.Component<components::ParentLinkName>(
this->dataPtr->jointEntity);
if (parentLinkName) {
this->dataPtr->parentLinkName = parentLinkName->Data();
}
}
if (this->dataPtr->linkEntity == kNullEntity) {
this->dataPtr->linkEntity =
this->dataPtr->model.LinkByName(_ecm, this->dataPtr->linkName);
}
if (this->dataPtr->parentLinkEntity == kNullEntity) {
this->dataPtr->parentLinkEntity =
this->dataPtr->model.LinkByName(_ecm, this->dataPtr->parentLinkName);
}
if (this->dataPtr->jointEntity == kNullEntity ||
this->dataPtr->linkEntity == kNullEntity ||
this->dataPtr->parentLinkEntity == kNullEntity) {
return;
}
// skip UpdateForcesAndMoments if needed components are missing
bool doUpdateForcesAndMoments = true;
const auto jointVelocity = _ecm.Component<components::JointVelocity>(
this->dataPtr->jointEntity);
if (!jointVelocity) {
_ecm.CreateComponent(this->dataPtr->jointEntity,
components::JointVelocity());
doUpdateForcesAndMoments = false;
} else if (jointVelocity->Data().empty()) {
doUpdateForcesAndMoments = false;
}
if (!_ecm.Component<components::JointVelocityCmd>(
this->dataPtr->jointEntity)) {
_ecm.CreateComponent(this->dataPtr->jointEntity,
components::JointVelocityCmd({0}));
doUpdateForcesAndMoments = false;
}
if (!_ecm.Component<components::WorldPose>(this->dataPtr->linkEntity)) {
_ecm.CreateComponent(this->dataPtr->linkEntity, components::WorldPose());
doUpdateForcesAndMoments = false;
}
if (!_ecm.Component<components::WorldLinearVelocity>(
this->dataPtr->linkEntity)) {
_ecm.CreateComponent(this->dataPtr->linkEntity,
components::WorldLinearVelocity());
doUpdateForcesAndMoments = false;
}
if (!_ecm.Component<components::WorldPose>(this->dataPtr->parentLinkEntity)) {
_ecm.CreateComponent(this->dataPtr->parentLinkEntity,
components::WorldPose());
doUpdateForcesAndMoments = false;
}
// Nothing left to do if paused.
if (_info.paused) {
return;
}
this->dataPtr->samplingTime =
std::chrono::duration<double>(_info.dt).count();
if (doUpdateForcesAndMoments) {
this->dataPtr->UpdateForcesAndMoments(_ecm);
}
}
//////////////////////////////////////////////////
void GenericMotorModelPrivate::OnActuatorMsg(
const msgs::Actuators &_msg)
{
std::lock_guard<std::mutex> lock(this->recvdActuatorsMsgMutex);
this->recvdActuatorsMsg = _msg;
}
//////////////////////////////////////////////////
void GenericMotorModelPrivate::UpdateForcesAndMoments(
EntityComponentManager &_ecm)
{
GZ_PROFILE("GenericMotorModelPrivate::UpdateForcesAndMoments");
std::optional<msgs::Actuators> msg;
auto actuatorMsgComp =
_ecm.Component<components::Actuators>(this->model.Entity());
// Actuators messages can come in from transport or via a component. If a
// component is available, it takes precedence.
if (actuatorMsgComp) {
msg = actuatorMsgComp->Data();
} else {
std::lock_guard<std::mutex> lock(this->recvdActuatorsMsgMutex);
if (this->recvdActuatorsMsg.has_value()) {
msg = *this->recvdActuatorsMsg;
this->recvdActuatorsMsg.reset();
}
}
if (msg.has_value()) {
if (this->actuatorNumber > msg->velocity_size() - 1) {
gzerr << "You tried to access index " << this->actuatorNumber
<< " of the Actuator array which is of size "
<< std::max(msg->velocity_size(), msg->normalized_size()) << std::endl;
return;
}
if (this->motorType == MotorType::kVelocity) {
this->refMotorInput = std::min(
static_cast<double>(msg->velocity(this->actuatorNumber)),
static_cast<double>(this->maxRotVelocity));
} else if (this->motorType == MotorType::kForcePolynomial &&
this->controlMethod == ControlMethod::kRPM) {
// Uses RPM as reference
this->refMotorInput = std::min(
static_cast<double>(msg->velocity(this->actuatorNumber)),
static_cast<double>(this->maxRotVelocity));
} else if (this->motorType == MotorType::kForcePolynomial &&
this->controlMethod == ControlMethod::kDutyCycle &&
msg->velocity(this->actuatorNumber) >= this->minDutyCycle) {
// Create normalized reference based on the duty cycle
this->refMotorInput = ((msg->velocity(this->actuatorNumber) - (this->minDutyCycle +
(this->maxDutyCycle - this->minDutyCycle) * this->bidirectionalMotor / 2.0)) / ((this->maxDutyCycle -
this->minDutyCycle) - (this->maxDutyCycle - this->minDutyCycle) * this->bidirectionalMotor / 2.0));
} else {
this->refMotorInput = msg->velocity(this->actuatorNumber);
}
}
switch (this->motorType) {
case (MotorType::kPosition): {
// TODO: Integrate
// double err = joint_->GetAngle(0).Radian() - this->refMotorInput;
// double force = pids_.Update(err, this->samplingTime);
// joint_->SetForce(0, force);
break;
}
case (MotorType::kForce): {
// TODO: Integrate
// joint_->SetForce(0, this->refMotorInput);
break;
}
case (MotorType::kForcePolynomial): {
sim::Link link(this->linkEntity);
const auto worldPose = link.WorldPose(_ecm);
using Vector3 = math::Vector3d;
// Compute thrust
double thrust = 0.0;
if (this->refMotorInput >= this->minCommand) {
for (unsigned int i = 0; i < this->positiveThrustPolynomial.size(); i++) {
thrust += this->positiveThrustPolynomial[i] * std::pow(this->refMotorInput, i);
}
} else if (this->refMotorInput <= -this->minCommand) {
for (unsigned int i = 0; i < this->negativeThrustPolynomial.size(); i++) {
thrust -= this->negativeThrustPolynomial[i] * std::pow(std::abs(this->refMotorInput), i);
}
} else {
thrust = 0.0;
}
link.AddWorldForce(_ecm, worldPose->Rot().RotateVector(Vector3(0, 0, thrust)));
// Compute torques
double torque = 0.0;
if (this->refMotorInput >= this->minCommand) {
for (unsigned int i = 0; i < this->positiveTorquePolynomial.size(); i++) {
torque += this->positiveTorquePolynomial[i] * std::pow(this->refMotorInput, i);
}
} else if (this->refMotorInput <= -this->minCommand) {
for (unsigned int i = 0; i < this->negativeTorquePolynomial.size(); i++) {
torque -= this->negativeTorquePolynomial[i] * std::pow(std::abs(this->refMotorInput), i);
}
} else {
torque = 0.0;
}
link.AddWorldForce(_ecm, worldPose->Rot().RotateVector(Vector3(0, 0, torque)));
// Set joint velocities
const auto jointVelCmd = _ecm.Component<components::JointVelocityCmd>(
this->jointEntity);
*jointVelCmd = components::JointVelocityCmd({this->refMotorInput / this->rotorVelocitySlowdownSim});
break;
}
default: { // MotorType::kVelocity
const auto jointVelocity = _ecm.Component<components::JointVelocity>(
this->jointEntity);
double motorRotVel = jointVelocity->Data()[0];
if (motorRotVel / (2 * GZ_PI) > 1 / (2 * this->samplingTime)) {
gzerr << "Aliasing on motor [" << this->actuatorNumber
<< "] might occur. Consider making smaller simulation time "
"steps or raising the rotorVelocitySlowdownSim param.\n";
}
double realMotorVelocity =
motorRotVel * this->rotorVelocitySlowdownSim;
// Get the direction of the rotor rotation.
int realMotorVelocitySign =
(realMotorVelocity > 0) - (realMotorVelocity < 0);
// Assuming symmetric propellers (or rotors) for the thrust calculation.
double thrust = this->turningDirection * realMotorVelocitySign *
realMotorVelocity * realMotorVelocity *
this->motorConstant;
using Pose = math::Pose3d;
using Vector3 = math::Vector3d;
Link link(this->linkEntity);
const auto worldPose = link.WorldPose(_ecm);
// Apply a force to the link.
link.AddWorldForce(_ecm,
worldPose->Rot().RotateVector(Vector3(0, 0, thrust)));
const auto jointPose = _ecm.Component<components::Pose>(
this->jointEntity);
if (!jointPose) {
gzerr << "joint " << this->jointName << " has no Pose"
<< "component" << std::endl;
return;
}
// computer joint world pose by multiplying child link WorldPose
// with joint Pose
Pose jointWorldPose = *worldPose * jointPose->Data();
const auto jointAxisComp = _ecm.Component<components::JointAxis>(
this->jointEntity);
if (!jointAxisComp) {
gzerr << "joint " << this->jointName << " has no JointAxis"
<< "component" << std::endl;
return;
}
const auto worldLinearVel = link.WorldLinearVelocity(_ecm);
Entity windEntity = _ecm.EntityByComponents(components::Wind());
auto windLinearVel =
_ecm.Component<components::WorldLinearVelocity>(windEntity);
Vector3 windSpeedWorld = windLinearVel->Data();
// Forces from Philppe Martin's and Erwan Salaun's
// 2010 IEEE Conference on Robotics and Automation paper
// The True Role of Accelerometer Feedback in Quadrotor Control
// - \omega * \lambda_1 * V_A^{\perp}
Vector3 jointAxis =
jointWorldPose.Rot().RotateVector(jointAxisComp->Data().Xyz());
Vector3 bodyVelocityWorld = *worldLinearVel;
Vector3 relativeWindVelocityWorld = bodyVelocityWorld - windSpeedWorld;
Vector3 bodyVelocityPerpendicular =
relativeWindVelocityWorld -
(relativeWindVelocityWorld.Dot(jointAxis) * jointAxis);
Vector3 airDrag = -std::abs(realMotorVelocity) *
this->rotorDragCoefficient *
bodyVelocityPerpendicular;
// Apply air drag to link.
link.AddWorldForce(_ecm, airDrag);
// Moments get the parent link, such that the resulting torques can be
// applied.
Vector3 parentWorldTorque;
auto parentWrenchComp =
_ecm.Component<components::ExternalWorldWrenchCmd>(
this->parentLinkEntity);
// gazebo_motor_model.cpp subtracts the GetWorldCoGPose() of the
// child link from the parent but only uses the rotation component.
// Since GetWorldCoGPose() uses the link frame orientation, it
// is equivalent to use WorldPose().Rot().
Link parentLink(this->parentLinkEntity);
const auto parentWorldPose = parentLink.WorldPose(_ecm);
// The transformation from the parent_link to the link_.
// Pose poseDifference =
// parent_links.at(0)->GetWorldCoGPose().Inverse()
// * link_->GetWorldCoGPose()
Pose poseDifference = (*parentWorldPose).Inverse() * (*worldPose);
Vector3 dragTorque(
0, 0, -this->turningDirection * thrust * this->momentConstant);
// Transforming the drag torque into the parent frame to handle
// arbitrary rotor orientations.
Vector3 dragTorqueParentFrame =
poseDifference.Rot().RotateVector(dragTorque);
parentWorldTorque =
parentWorldPose->Rot().RotateVector(dragTorqueParentFrame);
Vector3 rollingMoment;
// - \omega * \mu_1 * V_A^{\perp}
rollingMoment = -std::abs(realMotorVelocity) *
this->rollingMomentCoefficient *
bodyVelocityPerpendicular;
parentWorldTorque += rollingMoment;
if (!parentWrenchComp) {
components::ExternalWorldWrenchCmd wrench;
msgs::Set(wrench.Data().mutable_torque(), parentWorldTorque);
_ecm.CreateComponent(this->parentLinkEntity, wrench);
} else {
msgs::Set(parentWrenchComp->Data().mutable_torque(),
msgs::Convert(parentWrenchComp->Data().torque()) + parentWorldTorque);
}
// Apply the filter on the motor's velocity.
double refMotorRotVel;
refMotorRotVel = this->rotorVelocityFilter->UpdateFilter(
this->refMotorInput, this->samplingTime);
const auto jointVelCmd = _ecm.Component<components::JointVelocityCmd>(
this->jointEntity);
*jointVelCmd = components::JointVelocityCmd({
this->turningDirection *refMotorRotVel
/ this->rotorVelocitySlowdownSim});
}
}
}
GZ_ADD_PLUGIN(GenericMotorModel,
System,
GenericMotorModel::ISystemConfigure,
GenericMotorModel::ISystemPreUpdate)
GZ_ADD_PLUGIN_ALIAS(GenericMotorModel,
"gz::sim::systems::GenericMotorModel")
@@ -0,0 +1,94 @@
/*
* Copyright (C) 2019 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_GENERICMOTORMODEL_HPP_
#define GZ_SIM_SYSTEMS_GENERICMOTORMODEL_HPP_
#include <gz/sim/System.hh>
#include <memory>
#include <string>
#include <vector>
namespace gz
{
namespace sim
{
// Inline bracket to help doxygen filtering.
inline namespace GZ_SIM_VERSION_NAMESPACE
{
namespace systems
{
// Forward declaration
class GenericMotorModelPrivate;
/// \brief This system applies a thrust force to models with spinning
/// propellers. See examples/worlds/quadcopter.sdf for a demonstration.
class GenericMotorModel
: public System,
public ISystemConfigure,
public ISystemPreUpdate
{
/// \brief Constructor
public: GenericMotorModel();
/// \brief Destructor
public: ~GenericMotorModel() override = default;
// Documentation inherited
public: void Configure(const Entity &_entity,
const std::shared_ptr<const sdf::Element> &_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<GenericMotorModelPrivate> dataPtr;
private:
std::vector<double> ParsePolynomial(const std::string &input)
{
std::vector<double> result;
std::string trimmed = input;
// Optional: remove brackets
trimmed.erase(std::remove(trimmed.begin(), trimmed.end(), '['), trimmed.end());
trimmed.erase(std::remove(trimmed.begin(), trimmed.end(), ']'), trimmed.end());
std::stringstream ss(trimmed);
std::string token;
while (std::getline(ss, token, ',')) {
try {
result.push_back(std::stod(token));
} catch (const std::invalid_argument &e) {
gzerr << "[YourPlugin] Invalid number: " << token << std::endl;
}
}
return result;
}
};
}
}
}
}
#endif