mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-11-22 18:43:19 +08:00
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:
co-authored by
Daniel Agar
parent
09fe34af0e
commit
e14eea5615
@@ -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
|
||||
Reference in New Issue
Block a user