From ca051a46db3694e863958c481f2488c4896442da Mon Sep 17 00:00:00 2001 From: Balduin Date: Mon, 3 Mar 2025 14:18:07 +0100 Subject: [PATCH] Moving platform: random rocking motion More realistic than plain constant movement. Implemented with white noise, low pass filter & feedback term ensuring we stay close enough to upright. Tuning knobs maybe not very intuitive at the moment. There are: - update rates alpha_v, alpha_w (bigger = faster movement) - amplitudes ampl_v, ampl_w (bigger = larger movement) - feedback gains pos_gains, attitude_gain (bigger = smaller movement) maybe we could remove the redundance between the latter two? Also: - overload setPlatformVelocity for different inputs - don't set aircraft pose at all (make sure with environment variable that it is on platform) - platform height = 2m defined in sdf file - rename pose -> navsat to be more precise - gz submodule update. heavier platform & initial pose. --- Tools/simulation/gz | 2 +- src/modules/simulation/gz_bridge/GZBridge.cpp | 115 +++++++++++++++--- src/modules/simulation/gz_bridge/GZBridge.hpp | 16 ++- 3 files changed, 114 insertions(+), 19 deletions(-) diff --git a/Tools/simulation/gz b/Tools/simulation/gz index f9204722ea..40deb719e3 160000 --- a/Tools/simulation/gz +++ b/Tools/simulation/gz @@ -1 +1 @@ -Subproject commit f9204722eaaa5b95dbe99e32723653731fe46434 +Subproject commit 40deb719e3429098d808589dfa600b82d03439ea diff --git a/src/modules/simulation/gz_bridge/GZBridge.cpp b/src/modules/simulation/gz_bridge/GZBridge.cpp index 13a1729839..d8b99e3c2d 100644 --- a/src/modules/simulation/gz_bridge/GZBridge.cpp +++ b/src/modules/simulation/gz_bridge/GZBridge.cpp @@ -105,12 +105,6 @@ int GZBridge::init() position->set_y(model_pose_v[1]); position->set_z(model_pose_v[2]); - if (_has_platform) { - // why does this only look good if way bigger than actual platform height? - float platform_height = 0.5; - position->set_z(fmaxf(platform_height, model_pose_v[2])); - } - gz::math::Quaterniond q(model_pose_v[3], model_pose_v[4], model_pose_v[5]); q.Normalize(); @@ -183,7 +177,6 @@ int GZBridge::init() } if (_has_platform) { - // Initialise publisher for setPlatformVelocity // TODO less hardcoding of these topic strings std::string cmd_vel_topic = "/model/flat_platform/link/platform_link/cmd_vel"; @@ -266,11 +259,11 @@ int GZBridge::init() } if (_has_platform) { - std::string platform_pose_topic = "/world/" + _world_name + - "/model/flat_platform/link/platform_link/sensor/navsat_sensor/navsat"; + std::string platform_navsat_topic = "/world/" + _world_name + + "/model/flat_platform/link/platform_link/sensor/navsat_sensor/navsat"; - if (!_node.Subscribe(platform_pose_topic, &GZBridge::platformNavsatCallback, this)) { - PX4_ERR("failed to subscribe to %s", platform_pose_topic.c_str()); + if (!_node.Subscribe(platform_navsat_topic, &GZBridge::platformNavsatCallback, this)) { + PX4_ERR("failed to subscribe to %s", platform_navsat_topic.c_str()); return PX4_ERROR; } } @@ -299,13 +292,80 @@ int GZBridge::init() return OK; } +void GZBridge::updatePlatformVelocity(const gz::math::Vector3d& mean_velocity) +{ -void GZBridge::setPlatformVelocity(float vx, float vy, float vz) + // Velocity and angular velocity = low pass filtered white noise + feedback term. + + gz::math::Vector3d noise_v = gz::math::Vector3d( + gz::math::Rand::DblNormal(), + gz::math::Rand::DblNormal(), + gz::math::Rand::DblNormal() + ); + + gz::math::Vector3d noise_w = gz::math::Vector3d( + gz::math::Rand::DblNormal(), + gz::math::Rand::DblNormal(), + gz::math::Rand::DblNormal() + ); + + // Update rates for the filtered white noise. + // larger number here = faster movement + // write these in terms of time constants? + // https://ethz.ch/content/dam/ethz/special-interest/mavt/dynamic-systems-n-control/idsc-dam/Lectures/Signals-and-Systems/Lectures/Lecture%20Notes%209.pdf + const double alpha_v = 0.01; + const double alpha_w = 0.01; + + // Noise amplitude. + // larger number here = bigger movement + // we scale it by the update rates to keep total energy constant + const double ampl_v = 0.01 / alpha_v; + const double ampl_w = 0.01 / alpha_w; + + // For ultra realism we might have axis specific versions of these + // constants -- Real ships roll more quickly than they pitch and yaw. + // But probably overkill for now. + + _noise_v_lowpass = (1-alpha_v) * _noise_v_lowpass + alpha_v * noise_v; + _noise_w_lowpass = (1-alpha_w) * _noise_w_lowpass + alpha_w * noise_w; + + _platform_v = ampl_v * _noise_v_lowpass + mean_velocity; + _platform_w = ampl_w * _noise_w_lowpass; + + const bool feedback = true; + + // feedback terms to ensure the random walk (= integral of noise) + // stays within a realistic region. + if (feedback) { + + // small feedback to maintain height. no attempt to stabilise x and y. + const double platform_height = 2.; + const gz::math::Vector3d pos_gains(0., 0., 1.); // [m/s / m] + _platform_v += -pos_gains * (_platform_position - gz::math::Vector3d(0., platform_height, 0.)); + + // eq. 23 from Nonlinear Quadrocopter Attitude Control (Brescianini, Hehn, D'Andrea) + // https://www.research-collection.ethz.ch/handle/20.500.11850/154099 + const double sgn = _platform_orientation.W() > 0 ? 1. : -1.; + + const double attitude_gain = 1.; + + gz::math::Vector3d q_imag = gz::math::Vector3d( + _platform_orientation.X(), + _platform_orientation.Y(), + _platform_orientation.Z() + ); + + _platform_w += -attitude_gain * sgn * q_imag; + } + +} + +void GZBridge::setPlatformVelocity(const gz::math::Vector3d& vs, const gz::math::Vector3d& ws) { gz::msgs::Twist twist; - gz::msgs::Set(twist.mutable_linear(), - gz::math::Vector3d(vx, vy, vz)); + gz::msgs::Set(twist.mutable_linear(), vs); + gz::msgs::Set(twist.mutable_angular(), ws); if (_platform_twist_pub) { _platform_twist_pub.Publish(twist); @@ -317,6 +377,18 @@ void GZBridge::setPlatformVelocity(float vx, float vy, float vz) // TODO unified error handling? PX4_ERR at the lowest level or return success bool? } +void GZBridge::setPlatformVelocity(double vx, double vy, double vz, double wx, double wy, double wz) +{ + const gz::math::Vector3d vs(vx, vy, vz); + const gz::math::Vector3d ws(wx, wy, wz); + setPlatformVelocity(vs, ws); +} + +void GZBridge::setPlatformVelocity(double vx, double vy, double vz) +{ + setPlatformVelocity(vx, vy, vz, 0, 0, 0); +} + bool GZBridge::setPlatformPose(gz::msgs::Pose &pose) { @@ -734,8 +806,15 @@ void GZBridge::poseInfoCallback(const gz::msgs::Pose_V &pose) local_position_groundtruth.timestamp = hrt_absolute_time(); _lpos_ground_truth_pub.publish(local_position_groundtruth); - pthread_mutex_unlock(&_node_mutex); - return; + } else if (_has_platform && pose.pose(p).name() == "flat_platform") { + + // We need this for realistic boat rocking motion + // Keep it all in gazebo's ENU for these purposes + // TODO consider renaming flat_platform to something less generic to avoid possible name collisions + + _platform_position = gz::msgs::Convert(pose.pose(p).position()); + _platform_orientation = gz::msgs::Convert(pose.pose(p).orientation()); + } } @@ -1122,7 +1201,9 @@ void GZBridge::Run() } if (_has_platform) { - setPlatformVelocity(1.0, 0.0, 0.0); + const gz::math::Vector3d mean_vel(2.0, 0.0, 0.0); + updatePlatformVelocity(mean_vel); + setPlatformVelocity(_platform_v, _platform_w); } ScheduleDelayed(10_ms); diff --git a/src/modules/simulation/gz_bridge/GZBridge.hpp b/src/modules/simulation/gz_bridge/GZBridge.hpp index 2a7d62307f..061dea8df0 100644 --- a/src/modules/simulation/gz_bridge/GZBridge.hpp +++ b/src/modules/simulation/gz_bridge/GZBridge.hpp @@ -64,7 +64,9 @@ #include #include +#include #include +#include #include #include @@ -120,7 +122,10 @@ private: bool setPlatformPose(gz::msgs::Pose &pose); - void setPlatformVelocity(float vx, float vy, float vz); + void updatePlatformVelocity(const gz::math::Vector3d& mean_velocity); + void setPlatformVelocity(const gz::math::Vector3d& vs, const gz::math::Vector3d& ws); + void setPlatformVelocity(double vx, double vy, double vz, double wx, double wy, double wz); + void setPlatformVelocity(double vx, double vy, double vz); bool createMovingPlatform(); @@ -217,4 +222,13 @@ private: gz::transport::Node _node; gz::transport::Node::Publisher _platform_twist_pub; + + gz::math::Vector3d _noise_v_lowpass{0., 0., 0.}; + gz::math::Vector3d _noise_w_lowpass{0., 0., 0.}; + + gz::math::Vector3d _platform_v{0., 0., 0.}; + gz::math::Vector3d _platform_w{0., 0., 0.}; + + gz::math::Vector3d _platform_position; + gz::math::Quaterniond _platform_orientation{1., 0., 0., 0.}; };