Repository: PX4/PX4-Autopilot
Pull request #24471: Gazebo: Moving platform
Author: login: mbjd
Created: 2025-03-07T13:22:17Z | Merged: 2025-04-06T18:57:45Z

Gazebo plugin that controls a moving platform, producing random wobbling motion to simulate takeoff/landing on moving vehicles. 

Questions for reviewers: 
 - Only way to configure random motion frequency & amplitude is by changing the hardcoded constants. What would be a nicer way to do this (if needed)?
   - velocity now configurable, rest hardcoded still. 
 - Gazebo plugin depends on specific name of gazebo model, GZBridge callback depends on specific name of world. Is this acceptable? 
   - first one fixed, second one circumvented by not including PX4 communication in this PR. 

Launch using: 
```
PX4_GZ_MODEL_POSE=0,0,2.2,0,0,0 PX4_GZ_WORLD=moving_platform make px4_sitl gz_standard_vtol
```
with the desired model. Pose has to be adjusted so the vehicle is above the platform, which is at a height of 2m. By default the platform moves at 1 m/s -- this velocity can be changed with the environment variable `PX4_GZ_PLATFORM_VEL`:
```
PX4_GZ_PLATFORM_VEL=5 PX4_GZ_MODEL_POSE=0,0,2.2,0,0,0 PX4_GZ_WORLD=moving_platform make px4_sitl gz_standard_vtol
```


corresponding gazebo models merged [here](https://github.com/PX4/PX4-gazebo-models/pull/83).


Diff:
diff --git a/Tools/simulation/gz b/Tools/simulation/gz
index 6caa6d54fa9..e05f4312d3f 160000
--- a/Tools/simulation/gz
+++ b/Tools/simulation/gz
@@ -1 +1 @@
-Subproject commit 6caa6d54fa95b0141c9974bbd836b0f6fe0f26ee
+Subproject commit e05f4312d3f28aa621157610584a4870406cb6d3
diff --git a/src/modules/simulation/gz_plugins/CMakeLists.txt b/src/modules/simulation/gz_plugins/CMakeLists.txt
index 328d45139a9..cd4b7e7c70c 100644
--- a/src/modules/simulation/gz_plugins/CMakeLists.txt
+++ b/src/modules/simulation/gz_plugins/CMakeLists.txt
@@ -55,11 +55,12 @@ if (gz-transport_FOUND)
     add_subdirectory(optical_flow)
     add_subdirectory(template_plugin)
     add_subdirectory(gstreamer)
+    add_subdirectory(moving_platform_controller)
 
     # Add an alias target for each plugin
     if (TARGET GstCameraSystem)
-        add_custom_target(px4_gz_plugins ALL DEPENDS OpticalFlowSystem TemplatePlugin GstCameraSystem)
+        add_custom_target(px4_gz_plugins ALL DEPENDS OpticalFlowSystem MovingPlatformController TemplatePlugin GstCameraSystem)
     else()
-        add_custom_target(px4_gz_plugins ALL DEPENDS OpticalFlowSystem TemplatePlugin)
+        add_custom_target(px4_gz_plugins ALL DEPENDS OpticalFlowSystem MovingPlatformController TemplatePlugin)
     endif()
 endif()
diff --git a/src/modules/simulation/gz_plugins/moving_platform_controller/CMakeLists.txt b/src/modules/simulation/gz_plugins/moving_platform_controller/CMakeLists.txt
new file mode 100644
index 00000000000..f0ddf3322f0
--- /dev/null
+++ b/src/modules/simulation/gz_plugins/moving_platform_controller/CMakeLists.txt
@@ -0,0 +1,52 @@
+############################################################################
+#
+#   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.
+#
+############################################################################
+
+project(MovingPlatformController)
+
+add_library(${PROJECT_NAME} SHARED
+    MovingPlatformController.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}
+)
+
+target_include_directories(${PROJECT_NAME}
+    PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}
+    PUBLIC ${CMAKE_CURRENT_BINARY_DIR}
+    PUBLIC px4_gz_msgs
+)
diff --git a/src/modules/simulation/gz_plugins/moving_platform_controller/MovingPlatformController.cpp b/src/modules/simulation/gz_plugins/moving_platform_controller/MovingPlatformController.cpp
new file mode 100644
index 00000000000..f8f1ab26e24
--- /dev/null
+++ b/src/modules/simulation/gz_plugins/moving_platform_controller/MovingPlatformController.cpp
@@ -0,0 +1,395 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+#include "MovingPlatformController.hpp"
+
+using namespace custom;
+
+// Register the plugin
+GZ_ADD_PLUGIN(
+	MovingPlatformController,
+	gz::sim::System,
+	gz::sim::ISystemPreUpdate,
+	gz::sim::ISystemConfigure
+)
+
+void MovingPlatformController::Configure(const gz::sim::Entity &entity,
+		const std::shared_ptr<const sdf::Element> &sdf,
+		gz::sim::EntityComponentManager &ecm,
+		gz::sim::EventManager &eventMgr)
+{
+	_entity = entity;
+	_model = gz::sim::Model(entity);
+
+	const std::string link_name = sdf->Get<std::string>("link_name");
+	_link_entity = _model.LinkByName(ecm, link_name);
+
+	if (!_link_entity) {
+		throw std::runtime_error("MovingPlatformController::Configure: Link \"" + link_name + "\" was not found. "
+					 "Please ensure that your model contains the corresponding link.");
+	}
+
+	_link = gz::sim::Link(_link_entity);
+
+	// Needed to report linear & angular velocity
+	_link.EnableVelocityChecks(ecm, true);
+
+	_world_entity = gz::sim::worldEntity(ecm);
+	_world = gz::sim::World(_world_entity);
+
+	getVehicleModelName();
+
+	_startup_timer = gz::common::Timer();
+	_startup_timer.Start();
+
+	// Get velocity and orientation setpoints from env vars
+	{
+		const double vel_forward = readEnvVar("PX4_GZ_PLATFORM_VEL", 1.0);
+		const double heading_deg = readEnvVar("PX4_GZ_PLATFORM_HEADING_DEG", 0.0);
+		const double heading = GZ_DTOR(heading_deg);
+
+		_orientation_sp = gz::math::Quaterniond(0., 0., heading);
+
+		// Set initial orientation to match the heading
+		const auto optional_original_pose = _link.WorldPose(ecm);
+
+		if (optional_original_pose.has_value()) {
+			const gz::math::Pose3d original_pose = optional_original_pose.value();
+			const gz::math::Pose3d new_pose(original_pose.Pos(), _orientation_sp);
+			_model.SetWorldPoseCmd(ecm, new_pose);
+
+		} else {
+			gzwarn << "Unable to get initial platform pose. Heading will not be set" << std::endl;
+		}
+
+		// Velocity setpoint in world frame.
+		const gz::math::Vector3d _body_velocity_sp(vel_forward, 0., 0.);
+		_velocity_sp = _orientation_sp * _body_velocity_sp;
+	}
+
+	// Get gravity, model mass, platform height.
+	{
+		const auto gravity = _world.Gravity(ecm);
+
+		if (gravity.has_value()) {
+			_gravity = gravity.value().Z();
+
+		} else {
+			gzwarn << "Unable to get gazebo world gravity. Keeping default of " << _gravity << std::endl;
+		}
+
+		auto inertial_component = ecm.Component<gz::sim::components::Inertial>(_link_entity);
+
+		if (inertial_component) {
+			_platform_mass = inertial_component->Data().MassMatrix().Mass();
+			_platform_diag_moments = inertial_component->Data().MassMatrix().DiagonalMoments();
+
+		} else {
+			gzwarn << "Unable to get inertial component for link " << link_name << ". Keeping default mass of " << _platform_mass <<
+			       std::endl;
+		}
+
+		auto pose_component = ecm.Component<gz::sim::components::Pose>(_link_entity);
+
+		if (pose_component) {
+			_platform_height_setpoint = pose_component->Data().Z();
+
+		} else {
+			gzwarn << "Unable to get pose component for link " << link_name << ". Keeping default platform height setpoint of " <<
+			       _platform_height_setpoint << std::endl;
+		}
+	}
+
+}
+
+void MovingPlatformController::PreUpdate(const gz::sim::UpdateInfo &_info, gz::sim::EntityComponentManager &ecm)
+{
+	getPlatformState(ecm);
+
+	const double dt_sec = std::chrono::duration<double>(_info.dt).count();
+	updateNoise(dt_sec);
+
+	// Keep stationary if model is not yet spawned (and model name valid).
+	// If model name invalid, then we don't know what to wait for and move it immediately.
+	const bool vehicle_has_spawned = 0 != _world.ModelByName(ecm, _vehicle_model_name);
+	const bool keep_stationary = _wait_for_vehicle_spawned && !vehicle_has_spawned;
+
+	updateWrenchCommand(_velocity_sp, _orientation_sp, keep_stationary);
+
+	sendWrenchCommand(ecm);
+}
+
+void MovingPlatformController::updateNoise(const double dt)
+{
+	gz::math::Vector3d noise_force = gz::math::Vector3d(
+			gz::math::Rand::DblNormal(),
+			gz::math::Rand::DblNormal(),
+			gz::math::Rand::DblNormal()
+					 );
+
+	gz::math::Vector3d noise_torque = gz::math::Vector3d(
+			gz::math::Rand::DblNormal(),
+			gz::math::Rand::DblNormal(),
+			gz::math::Rand::DblNormal()
+					  );
+
+	// Cutoff frequencies for low-pass filtering the white noise.
+	// larger number = faster movement
+	// Should be between 0 and sample_freq / 2, with sample_freq = 1/dt
+	const double cutoff_freq_force  = 5.;  // 1/s
+	const double cutoff_freq_torque = 5.;  // 1/s
+
+	const double max_cutoff_freq = 0.5 / dt;
+
+	if (cutoff_freq_force < 0 || cutoff_freq_force > max_cutoff_freq ||
+	    cutoff_freq_torque < 0 || cutoff_freq_torque > max_cutoff_freq) {
+		throw std::runtime_error("MovingPlatformController::updateWrenchCommand: invalid cutoff frequency");
+	}
+
+	// Cutoff frequency -> filter coefficient calculation from PX4's AlphaFilter.hpp
+	// from AlphaFilter::setCutoffFreq
+	const double time_constant_force = 1. / (2 * GZ_PI * cutoff_freq_force);
+	const double time_constant_torque = 1. / (2 * GZ_PI * cutoff_freq_torque);
+
+	// from AlphaFilter::setParameters
+	const double filter_coef_force = dt / (dt + time_constant_force);
+	const double filter_coef_torque = dt / (dt + time_constant_torque);
+
+	// This noise is dimensionless, units only come into play in updateWrenchCommand
+	_noise_lowpass_force = (1. - filter_coef_force) * _noise_lowpass_force + filter_coef_force * noise_force;
+	_noise_lowpass_torque = (1. - filter_coef_torque) * _noise_lowpass_torque + filter_coef_torque * noise_torque;
+}
+
+void MovingPlatformController::updateWrenchCommand(
+	const gz::math::Vector3d &velocity_setpoint,
+	const gz::math::Quaterniond &orientation_setpoint,
+	const bool keep_stationary)
+{
+	// (force, torque) =
+	//    constant normal force / buoyancy term offsetting gravity
+	//  + low-pass filtered white noise representing waves, road noise, etc.
+	//  + PD feedback term representing stable vehicle design
+
+	// If keep_stationary, override noise amplitude and velocity setpoint to zero.
+	// This is to wait for the model to spawn initially.
+
+	// Noise amplitude >= 0
+	// larger number = bigger movement
+	const double noise_ampl_force_scaling = keep_stationary ? 0. : 1.;          // [N / kg]
+	const double noise_ampl_force = noise_ampl_force_scaling * _platform_mass;  // [N]
+
+	const double noise_ampl_torque_scaling = keep_stationary ? 0. : 1.;         // [N m / (kg m^2) = 1/s^2]
+	const gz::math::Vector3d noise_ampl_torque = noise_ampl_torque_scaling * _platform_diag_moments; // [N m]
+
+	const gz::math::Vector3d normal_force(0., 0., -_gravity * _platform_mass);  // [N]
+
+	_force = noise_ampl_force * _noise_lowpass_force + normal_force;            // [N]
+	_torque = noise_ampl_torque * _noise_lowpass_torque;                        // [N m]
+
+	// Feedback terms to ensure stability of the platform.
+	{
+
+		// Position - simple PD controller, but with zero position gains in xy direction.
+		// With the Vector3d on the RHS having units of 1/s^2 and 1/s, respectively
+		const gz::math::Vector3d pos_gains = _platform_mass * gz::math::Vector3d(0., 0., 1.); // [N / m]
+		const gz::math::Vector3d vel_gains = _platform_mass * gz::math::Vector3d(1., 1., 1.); // [N / (m/s)]
+
+		const gz::math::Vector3d platform_position_setpoint(0., 0., _platform_height_setpoint);
+		const gz::math::Vector3d current_velocity_setpoint = keep_stationary ? gz::math::Vector3d::Zero : velocity_setpoint;
+
+		const gz::math::Vector3d platform_pos_error = (_platform_position - platform_position_setpoint);
+		const gz::math::Vector3d platform_vel_error = (_platform_velocity - current_velocity_setpoint);
+
+		// * is element-wise
+		const gz::math::Vector3d feedback_force = -pos_gains * platform_pos_error - vel_gains * platform_vel_error;
+
+		// Clip horizontal force to avoid large accelerations, which might cause the drone to slip off the platform.
+		const float max_accel = 2.; // [m/s^2]
+		const gz::math::Vector2d _force_xy = gz::math::Vector2d(feedback_force.X(), feedback_force.Y());
+		const float accel_xy = _force_xy.Length() / _platform_mass;
+
+		if (accel_xy > max_accel) {
+			const float scaling = max_accel / accel_xy;
+			_force += feedback_force * gz::math::Vector3d(scaling, scaling, 1.);
+
+		} else {
+			_force += feedback_force;
+		}
+
+		// Attitude - similar but accounting for quaternion weirdness.
+		// Combining ideas from:
+		//  - Eq. 23 in Nonlinear Quadrocopter Attitude Control (Brescianini, Hehn, D'Andrea)
+		//    https://www.research-collection.ethz.ch/handle/20.500.11850/154099
+		//  - Eq. 20 in Full Quaternion Based Attitude Control for a Quadrotor (Fresk, Nikolakopoulos)
+		//    https://www.diva-portal.org/smash/get/diva2:1010947/FULLTEXT01.pdf
+
+		const gz::math::Quaterniond attitude_err = _platform_orientation * orientation_setpoint.Inverse();
+
+		// With the factors of 1. having units of 1 / (m rad) and s / (m rad), respectively
+		const gz::math::Vector3d attitude_p_gain = 1. * _platform_diag_moments; // [N m / rad]
+		const gz::math::Vector3d attitude_d_gain = 1. * _platform_diag_moments; // [N m / (rad/s)]
+
+		const double sgn = attitude_err.W() > 0. ? 1. : -1.;
+		gz::math::Vector3d attitude_err_imag = sgn * gz::math::Vector3d(attitude_err.X(), attitude_err.Y(), attitude_err.Z());
+
+		// Factor of 2 to convert quaternion error to rad
+		_torque += -2. * attitude_p_gain * attitude_err_imag - attitude_d_gain * _platform_angular_velocity;
+	}
+}
+
+
+void MovingPlatformController::getPlatformState(const gz::sim::EntityComponentManager &ecm)
+{
+
+	const auto optional_pose = _link.WorldPose(ecm);
+
+	if (optional_pose.has_value()) {
+		_platform_position = optional_pose.value().Pos();
+		_platform_orientation = optional_pose.value().Rot();
+
+	} else {
+		gzerr << "Unable to get pose" << std::endl;
+	}
+
+
+	const auto optional_vel = _link.WorldLinearVelocity(ecm);
+
+	if (optional_vel.has_value()) {
+		_platform_velocity = optional_vel.value();
+
+	} else {
+		gzerr << "Unable to get linear velocity" << std::endl;
+	}
+
+
+	const auto optional_angular_vel = _link.WorldAngularVelocity(ecm);
+
+	if (optional_angular_vel.has_value()) {
+		_platform_angular_velocity = optional_angular_vel.value();
+
+	} else {
+		gzerr << "Unable to get angular velocity" << std::endl;
+	}
+
+}
+
+void MovingPlatformController::sendWrenchCommand(gz::sim::EntityComponentManager &ecm)
+{
+	_link.AddWorldWrench(ecm, _force, _torque);
+}
+
+
+double MovingPlatformController::readEnvVar(const char *env_var_name, double default_value)
+{
+
+	const char *env_var_value = std::getenv(env_var_name);
+
+	if (env_var_value) {
+		try {
+			double value = std::stod(env_var_value);
+			return value;
+
+		} catch (const std::invalid_argument &e) {
+			// These warnings will only be visible with sufficient verbosity level...
+			gzwarn << "Invalid argument: " << env_var_value << " is not a valid double for " << env_var_name << std::endl;
+			gzwarn << "Keeping default value of " << default_value << " m/s." << std::endl;
+
+		} catch (const std::out_of_range &e) {
+			gzwarn << "Out of range: " << env_var_value << " is out of range for " << env_var_name << std::endl;
+			gzwarn << "Keeping default value of " << default_value << " m/s." << std::endl;
+		}
+	}
+
+	return default_value;
+}
+
+void MovingPlatformController::getVehicleModelName()
+{
+	// Find the name of the gazebo vehicle model.
+	// The name is constructed in px4-rc.gzsim as:
+	//     MODEL_NAME="${PX4_SIM_MODEL#*gz_}"
+	//     MODEL_NAME_INSTANCE="${MODEL_NAME}_${px4_instance}"
+	// So here we replicate that.
+
+	const char *px4_sim_model_cstr = std::getenv("PX4_SIM_MODEL");
+	std::string px4_sim_model = "";
+
+	const char *px4_gz_model_name_cstr = std::getenv("PX4_GZ_MODEL_NAME");
+
+	if (px4_sim_model_cstr != nullptr) {
+
+		px4_sim_model = px4_sim_model_cstr;
+
+	} else if (px4_gz_model_name_cstr != nullptr) {
+
+		// This happens if we attach to an existing model. In this case,
+		// do not wait for any vehicle to spawn.
+
+		gzwarn << "PX4_SIM_MODEL not set. Proceeding without vehicle name and moving platform immediately." << std::endl;
+		_wait_for_vehicle_spawned = false;
+
+	} else {
+
+		// If neither are set, the px4-rc.gzsim script should have
+		// exited 1. We could land here if these environment variables
+		// change -- if so, update this function accordingly.
+
+		gzerr << "Neither PX4_MODEL nor PX4_GZ_MODEL_NAME are set. One needed to proceed." << std::endl;
+		_wait_for_vehicle_spawned = false;
+	}
+
+	// Remove leading "gz_"
+	const std::string prefix_to_remove = "gz_";
+	size_t pos = px4_sim_model.find(prefix_to_remove);
+
+	if (pos != std::string::npos) {
+		px4_sim_model = px4_sim_model.substr(pos + prefix_to_remove.length());
+
+	} else {
+		gzwarn << "Error: \"gz_\" not found in PX4_SIM_MODEL. Using the entire string as MODEL_NAME." << std::endl;
+	}
+
+	// Get the px4_instance environment variable
+	const char *px4_instance_cstr = std::getenv("px4_instance");
+	std::string px4_instance = "";
+
+	if (px4_instance_cstr != nullptr) {
+		px4_instance = px4_instance_cstr;
+
+	} else {
+		px4_instance = "0";
+	}
+
+	_vehicle_model_name = px4_sim_model + "_" + px4_instance;
+	_wait_for_vehicle_spawned = true;
+}
diff --git a/src/modules/simulation/gz_plugins/moving_platform_controller/MovingPlatformController.hpp b/src/modules/simulation/gz_plugins/moving_platform_controller/MovingPlatformController.hpp
new file mode 100644
index 00000000000..aba113fdfce
--- /dev/null
+++ b/src/modules/simulation/gz_plugins/moving_platform_controller/MovingPlatformController.hpp
@@ -0,0 +1,125 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#include <gz/sim/Util.hh>
+#include <gz/sim/Link.hh>
+#include <gz/sim/Model.hh>
+#include <gz/sim/World.hh>
+#include <gz/sim/System.hh>
+#include "gz/sim/components/Pose.hh"
+#include <gz/sim/components/Model.hh>
+#include "gz/sim/components/Inertial.hh"
+#include "gz/sim/components/LinearVelocity.hh"
+#include "gz/sim/components/AngularVelocity.hh"
+#include <gz/sim/EntityComponentManager.hh>
+
+#include <gz/common/Timer.hh>
+
+#include <gz/plugin/Register.hh>
+
+#include <gz/msgs/Utility.hh>
+#include <gz/msgs/twist.pb.h>
+
+#include <gz/math.hh>
+#include <gz/math/Rand.hh>
+#include <gz/math/Pose3.hh>
+
+using namespace std::chrono_literals;
+
+namespace custom
+{
+class MovingPlatformController:
+	public gz::sim::System,
+	public gz::sim::ISystemPreUpdate,
+	public gz::sim::ISystemConfigure
+{
+public:
+	void PreUpdate(const gz::sim::UpdateInfo &_info,
+		       gz::sim::EntityComponentManager &_ecm) final;
+
+	void Configure(const gz::sim::Entity &entity,
+		       const std::shared_ptr<const sdf::Element> &sdf,
+		       gz::sim::EntityComponentManager &ecm,
+		       gz::sim::EventManager &eventMgr) override;
+
+private:
+
+	void getPlatformState(const gz::sim::EntityComponentManager &ecm);
+	void updateNoise(const double dt);
+	void updateWrenchCommand(const gz::math::Vector3d &velocity_setpoint,
+				 const gz::math::Quaterniond &orientation_setpoint,
+				 const bool keep_stationary);
+	void sendWrenchCommand(gz::sim::EntityComponentManager &ecm);
+	double readEnvVar(const char *env_var_name, double default_value);
+	void getVehicleModelName();
+
+	gz::sim::Entity _entity;
+	gz::sim::Model _model{gz::sim::kNullEntity};
+	gz::sim::Entity _link_entity;
+	gz::sim::Link _link;
+
+	gz::sim::Entity _world_entity;
+	gz::sim::World _world;
+
+	// Low-pass filtered white noise for driving boat motion.
+	gz::math::Vector3d _noise_lowpass_force{0., 0., 0.};
+	gz::math::Vector3d _noise_lowpass_torque{0., 0., 0.};
+
+	// Platform linear & angular velocity.
+	gz::math::Vector3d _force{0., 0., 0.};
+	gz::math::Vector3d _torque{0., 0., 0.};
+
+	// Platform position & orientation for feedback.
+	gz::math::Vector3d _platform_position{0., 0., 0.};
+	gz::math::Quaterniond _platform_orientation{1., 0., 0., 0.};
+	gz::math::Vector3d _platform_velocity{0., 0., 0.};
+	gz::math::Vector3d _platform_angular_velocity{0., 0., 0.};
+
+	// Platform velocity setpoint [m/s].
+	gz::math::Vector3d _velocity_sp{1., 0., 0.};
+	// Orientation setpoint.
+	gz::math::Quaterniond _orientation_sp{1., 0., 0., 0.};
+	// Height setpoint [m]
+	double _platform_height_setpoint{2.};
+
+	double _gravity{-9.8};
+	double _platform_mass{10000.};
+	gz::math::Vector3d _platform_diag_moments;
+
+	gz::common::Timer _startup_timer;
+	std::string _vehicle_model_name;
+	bool _wait_for_vehicle_spawned;
+};
+} // end namespace custom
diff --git a/src/modules/simulation/gz_plugins/moving_platform_controller/README.md b/src/modules/simulation/gz_plugins/moving_platform_controller/README.md
new file mode 100644
index 00000000000..440cb4dd80a
--- /dev/null
+++ b/src/modules/simulation/gz_plugins/moving_platform_controller/README.md
@@ -0,0 +1,56 @@
+# Moving Platform Controller
+
+This plugin controls a moving platform that emulates ships/trucks/etc. to
+takeoff and land on. The platform moves at a constant mean velocity, with added
+random fluctuations in velocity and angular velocity.
+
+## Dependencies
+
+This depends on the [moving platform world](https://github.com/PX4/PX4-gazebo-models/blob/moving_platform_world/worlds/moving_platform.sdf) in the `PX4-gazebo-models` repo, so ensure that the `Tools/simulation/gz` submodule is recent enough. That world contains the corresponding [moving platform model](https://github.com/PX4/PX4-gazebo-models/blob/moving_platform_world/models/moving_platform/model.sdf). Within that, we include this plugin.
+
+
+## Usage & Configuration
+
+Start by selecting the moving_platform world, which loads the plugin. We need to set the pose so the aircraft is above the platform, which is at a height of 2m.
+
+```
+PX4_GZ_MODEL_POSE=0,0,2.2 PX4_GZ_WORLD=moving_platform make px4_sitl gz_standard_vtol
+```
+
+The velocity (in m/s) can be set with the `PX4_GZ_PLATFORM_VEL` and `PX4_GZ_PLATFORM_HEADING_DEG` environment variables. By default it is 1 m/s in east direction. The heading is such that 0 represents east, 90 north, 180 west, and 270 south.
+
+```
+PX4_GZ_PLATFORM_VEL=5 PX4_GZ_PLATFORM_HEADING_DEG=135 PX4_GZ_MODEL_POSE=0,0,2.2,0,0,0 PX4_GZ_WORLD=moving_platform make px4_sitl gz_standard_vtol
+```
+
+To use the plugin with a *different* world or model, add the following to the model.sdf:
+
+```
+<?xml version="1.0" encoding="UTF-8"?>
+<sdf version="1.9">
+  <model name="flat_platform">
+
+    <!-- the rest of your model -->
+
+    <link name="platform_link">
+      <!-- define the link representing the platform -->
+    </link>
+
+    <plugin
+      filename="libMovingPlatformController.so"
+      name="custom::MovingPlatformController">
+      <link_name>platform_link</link_name>
+    </plugin>
+
+  </model>
+</sdf>
+```
+
+## Limitations & Future Ideas
+
+ - Apart from the velocity and heading, nothing is configurable: Noise amplitude and frequency spectrum, initial acceleration, feedback gains.
+    - Feel free to change these ad-hoc in code, or open an issue / propose a PR for better config options.
+ - This plugin does not communicate the state of the platform with PX4. If that is needed, there are a couple options:
+    - Read the pose of the platform in `GZBridge::poseInfoCallback`
+    - Add an IMU sensor to the platform link, listen to it and the existing NavSat sensor from `GZBridge`.
+    - Add a custom gazebo transport message containing all needed info about the platform, populate and publish it from the plugin here, listen to that in `GZBridge`.


Reviews:
user: login: dakejahl
body: you checked in gazebo-classic changes but otherwise LGTM
state: COMMENTED
author_association: CONTRIBUTOR
submitted_at: 2025-03-13T07:02:48Z
user: login: Jaeyoung-Lim
body: - The commit history needs to be cleaned up before being able to merge
state: CHANGES_REQUESTED
author_association: MEMBER
submitted_at: 2025-03-13T09:08:44Z
user: login: mbjd
state: COMMENTED
author_association: CONTRIBUTOR
submitted_at: 2025-03-13T09:25:49Z
user: login: mbjd
state: COMMENTED
author_association: CONTRIBUTOR
submitted_at: 2025-03-13T09:32:12Z
user: login: RomanBapst
state: COMMENTED
author_association: CONTRIBUTOR
submitted_at: 2025-03-13T09:34:40Z
user: login: RomanBapst
state: COMMENTED
author_association: CONTRIBUTOR
submitted_at: 2025-03-13T09:35:43Z
user: login: RomanBapst
state: COMMENTED
author_association: CONTRIBUTOR
submitted_at: 2025-03-13T09:36:00Z
user: login: dakejahl
state: DISMISSED
author_association: CONTRIBUTOR
submitted_at: 2025-03-13T23:07:56Z
user: login: mbjd
state: COMMENTED
author_association: CONTRIBUTOR
submitted_at: 2025-03-14T07:53:55Z
user: login: Jaeyoung-Lim
state: COMMENTED
author_association: MEMBER
submitted_at: 2025-03-17T12:33:27Z
user: login: mbjd
state: COMMENTED
author_association: CONTRIBUTOR
submitted_at: 2025-03-18T08:15:45Z
user: login: Jaeyoung-Lim
state: COMMENTED
author_association: MEMBER
submitted_at: 2025-03-18T08:25:27Z
user: login: mbjd
state: COMMENTED
author_association: CONTRIBUTOR
submitted_at: 2025-03-18T09:50:45Z
user: login: dakejahl
state: DISMISSED
author_association: CONTRIBUTOR
submitted_at: 2025-04-01T21:53:52Z
user: login: perrrre
state: COMMENTED
author_association: CONTRIBUTOR
submitted_at: 2025-04-02T05:58:01Z
user: login: perrrre
state: COMMENTED
author_association: CONTRIBUTOR
submitted_at: 2025-04-02T06:25:49Z
user: login: mbjd
state: COMMENTED
author_association: CONTRIBUTOR
submitted_at: 2025-04-02T06:54:51Z
user: login: mbjd
state: COMMENTED
author_association: CONTRIBUTOR
submitted_at: 2025-04-02T07:00:01Z
user: login: perrrre
state: COMMENTED
author_association: CONTRIBUTOR
submitted_at: 2025-04-02T07:31:13Z
user: login: perrrre
state: COMMENTED
author_association: CONTRIBUTOR
submitted_at: 2025-04-02T07:56:23Z
user: login: mbjd
state: COMMENTED
author_association: CONTRIBUTOR
submitted_at: 2025-04-02T11:57:06Z
user: login: mbjd
state: COMMENTED
author_association: CONTRIBUTOR
submitted_at: 2025-04-02T12:58:03Z
user: login: KonradRudin
body: Just a question, otherwise LGTM
state: COMMENTED
author_association: CONTRIBUTOR
submitted_at: 2025-04-04T08:21:56Z
user: login: mbjd
state: COMMENTED
author_association: CONTRIBUTOR
submitted_at: 2025-04-04T08:33:12Z
user: login: Jaeyoung-Lim
state: COMMENTED
author_association: MEMBER
submitted_at: 2025-04-04T08:38:15Z
user: login: mbjd
state: COMMENTED
author_association: CONTRIBUTOR
submitted_at: 2025-04-04T08:48:17Z
user: login: Jaeyoung-Lim
state: COMMENTED
author_association: MEMBER
submitted_at: 2025-04-04T09:02:12Z
user: login: mbjd
state: COMMENTED
author_association: CONTRIBUTOR
submitted_at: 2025-04-04T12:20:10Z
user: login: mbjd
state: COMMENTED
author_association: CONTRIBUTOR
submitted_at: 2025-04-04T12:44:38Z
user: login: Jaeyoung-Lim
state: COMMENTED
author_association: MEMBER
submitted_at: 2025-04-06T18:14:01Z
user: login: Jaeyoung-Lim
state: DISMISSED
author_association: MEMBER
submitted_at: 2025-04-06T18:14:10Z
user: login: dakejahl
state: APPROVED
author_association: CONTRIBUTOR
submitted_at: 2025-04-06T18:57:36Z

Review comments:
diff_hunk: @@ -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
path: src/modules/simulation/gz_plugins/moving_platform_controller/CMakeLists.txt
user: login: dakejahl
body: delete old template comments
created_at: 2025-03-13T06:58:25Z
author_association: CONTRIBUTOR
diff_hunk: @@ -0,0 +1,181 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+#include "MovingPlatformController.hpp"
+
+using namespace custom;
+
+// Register the plugin
+GZ_ADD_PLUGIN(
+	MovingPlatformController,
+	gz::sim::System,
+	gz::sim::ISystemPreUpdate,
+	gz::sim::ISystemConfigure
+)
+
+void MovingPlatformController::Configure(const gz::sim::Entity &entity,
+		const std::shared_ptr<const sdf::Element> &sdf,
+		gz::sim::EntityComponentManager &ecm,
+		gz::sim::EventManager &eventMgr)
+{
+	_entity = entity;
+	_model = gz::sim::Model(entity);
+
+	double default_mean_vel = 1.0f;
+	double v_x = default_mean_vel;
+
+	// Read v_x from environment variable.
+	{
+		const char *env_var_name = "PX4_GZ_PLATFORM_VEL";
path: src/modules/simulation/gz_plugins/moving_platform_controller/MovingPlatformController.cpp
user: login: dakejahl
body: document this somewhere
created_at: 2025-03-13T06:59:44Z
author_association: CONTRIBUTOR
diff_hunk: @@ -0,0 +1,123 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#include <gz/sim/Util.hh>
+#include <gz/sim/Model.hh>
+#include <gz/sim/System.hh>
+#include "gz/sim/components/Pose.hh"
+#include <gz/sim/components/Model.hh>
+#include <gz/sim/EntityComponentManager.hh>
+
+#include <gz/transport/Node.hh>
+#include <gz/plugin/Register.hh>
+#include <gz/msgs/Utility.hh>
+#include <gz/msgs/twist.pb.h>
+
+#include <gz/math.hh>
+#include <gz/math/Rand.hh>
+#include <gz/math/Pose3.hh>
+
+#include <iostream>
+
+
+/*
+ *  To use this plugin, add the following to your model.sdf:
+ *
+ *      <?xml version="1.0" encoding="UTF-8"?>
+ *      <sdf version="1.9">
+ *        <model name="flat_platform">
+ *
+ *          <!-- the rest of your model -->
+ *
+ *          <plugin
+ *            filename="gz-sim-velocity-control-system"
+ *            name="gz::sim::systems::VelocityControl">
+ *          </plugin>
+ *
+ *          <plugin
+ *            filename="libMovingPlatformController.so"
+ *            name="custom::MovingPlatformController">
+ *          </plugin>
+ *
+ *        </model>
+ *      </sdf>
+ *
+ */
path: src/modules/simulation/gz_plugins/moving_platform_controller/MovingPlatformController.hpp
user: login: dakejahl
body: maybe this comment and the mention of the env var can go in a README.md in this plugin dir? I don't know... might be helpful to have info consolidated
created_at: 2025-03-13T07:01:51Z
author_association: CONTRIBUTOR
diff_hunk: @@ -0,0 +1,197 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+#include "MovingPlatformController.hpp"
+
+using namespace custom;
+
+// Register the plugin
+GZ_ADD_PLUGIN(
+	MovingPlatformController,
+	gz::sim::System,
+	gz::sim::ISystemPreUpdate,
+	gz::sim::ISystemConfigure
+)
+
+void MovingPlatformController::Configure(const gz::sim::Entity &entity,
+		const std::shared_ptr<const sdf::Element> &sdf,
+		gz::sim::EntityComponentManager &ecm,
+		gz::sim::EventManager &eventMgr)
+{
+	_entity = entity;
+	_model = gz::sim::Model(entity);
+
+	double default_mean_vel = 1.0f;
+	double v_x = default_mean_vel;
+
+	// Read v_x from environment variable.
+	{
+		const char *env_var_name = "PX4_GZ_PLATFORM_VEL";
+		const char *env_var_value = std::getenv(env_var_name);
+
+		if (env_var_value) {
+			try {
+				v_x = std::stof(env_var_value);
+
+			} catch (const std::invalid_argument &e) {
+				// These warnings will only be visible with sufficient verbosity level...
+				gzwarn << "Invalid argument: " << env_var_value << " is not a valid double for " << env_var_name << std::endl;
+				gzwarn << "Keeping default value of " << v_x << " m/s." << std::endl;
+
+			} catch (const std::out_of_range &e) {
+				gzwarn << "Out of range: " << env_var_value << " is out of range for " << env_var_name << std::endl;
+				gzwarn << "Keeping default value of " << v_x << " m/s." << std::endl;
+			}
+		}
+	}
+
+	_target_vel_sp = gz::math::Vector3d(v_x, 0., 0.);
+
+	// Advertise topic to communicate with VelocityControl plugin
+	std::string model_name = _model.Name(ecm);
+	std::string cmd_vel_topic = "/model/" + model_name + "/cmd_vel";
+	_platform_twist_pub = _node.Advertise<gz::msgs::Twist>(cmd_vel_topic);
+}
+
+void MovingPlatformController::PreUpdate(const gz::sim::UpdateInfo &_info, gz::sim::EntityComponentManager &ecm)
+{
+	updatePose(ecm);
+
+	// Initially, ramp up the velocity with constant acceleration. Otherwise
+	// we can't really simulate fast velocities as the drone just slips off
+	// the platform.
+
+	double max_accel_mag = 2.; // m/s^2
+	double dt = std::chrono::duration<double>(_info.dt).count(); // s
+
+	const gz::math::Vector3d vel_sp_err = _target_vel_sp - _current_vel_sp;
+
+	if (vel_sp_err.Length() < max_accel_mag * dt) {
+		// we are already almost there. set directly to avoid division by 0.
+		_current_vel_sp = _target_vel_sp;
+	} else {
+		// acclerate towards target velocity.
+		const gz::math::Vector3d accel = vel_sp_err.Normalized() * max_accel_mag;
+		_current_vel_sp = _current_vel_sp + accel * dt;
+	}
+
+	updateVelocityCommands(_current_vel_sp);
+
+	sendVelocityCommands();
+}
+
+void MovingPlatformController::updateVelocityCommands(const gz::math::Vector3d &velocity_setpoint)
+{
+	// 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;
path: src/modules/simulation/gz_plugins/moving_platform_controller/MovingPlatformController.cpp
user: login: Jaeyoung-Lim
body: what is alpha? Could you briefly add in the comments what this means?
created_at: 2025-03-13T09:06:16Z
author_association: MEMBER
diff_hunk: @@ -0,0 +1,197 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+#include "MovingPlatformController.hpp"
+
+using namespace custom;
+
+// Register the plugin
+GZ_ADD_PLUGIN(
+	MovingPlatformController,
+	gz::sim::System,
+	gz::sim::ISystemPreUpdate,
+	gz::sim::ISystemConfigure
+)
+
+void MovingPlatformController::Configure(const gz::sim::Entity &entity,
+		const std::shared_ptr<const sdf::Element> &sdf,
+		gz::sim::EntityComponentManager &ecm,
+		gz::sim::EventManager &eventMgr)
+{
+	_entity = entity;
+	_model = gz::sim::Model(entity);
+
+	double default_mean_vel = 1.0f;
+	double v_x = default_mean_vel;
+
+	// Read v_x from environment variable.
+	{
+		const char *env_var_name = "PX4_GZ_PLATFORM_VEL";
+		const char *env_var_value = std::getenv(env_var_name);
+
+		if (env_var_value) {
+			try {
+				v_x = std::stof(env_var_value);
+
+			} catch (const std::invalid_argument &e) {
+				// These warnings will only be visible with sufficient verbosity level...
+				gzwarn << "Invalid argument: " << env_var_value << " is not a valid double for " << env_var_name << std::endl;
+				gzwarn << "Keeping default value of " << v_x << " m/s." << std::endl;
+
+			} catch (const std::out_of_range &e) {
+				gzwarn << "Out of range: " << env_var_value << " is out of range for " << env_var_name << std::endl;
+				gzwarn << "Keeping default value of " << v_x << " m/s." << std::endl;
+			}
+		}
+	}
+
+	_target_vel_sp = gz::math::Vector3d(v_x, 0., 0.);
+
+	// Advertise topic to communicate with VelocityControl plugin
+	std::string model_name = _model.Name(ecm);
+	std::string cmd_vel_topic = "/model/" + model_name + "/cmd_vel";
+	_platform_twist_pub = _node.Advertise<gz::msgs::Twist>(cmd_vel_topic);
+}
+
+void MovingPlatformController::PreUpdate(const gz::sim::UpdateInfo &_info, gz::sim::EntityComponentManager &ecm)
+{
+	updatePose(ecm);
+
+	// Initially, ramp up the velocity with constant acceleration. Otherwise
+	// we can't really simulate fast velocities as the drone just slips off
+	// the platform.
+
+	double max_accel_mag = 2.; // m/s^2
+	double dt = std::chrono::duration<double>(_info.dt).count(); // s
+
+	const gz::math::Vector3d vel_sp_err = _target_vel_sp - _current_vel_sp;
+
+	if (vel_sp_err.Length() < max_accel_mag * dt) {
+		// we are already almost there. set directly to avoid division by 0.
+		_current_vel_sp = _target_vel_sp;
+	} else {
+		// acclerate towards target velocity.
+		const gz::math::Vector3d accel = vel_sp_err.Normalized() * max_accel_mag;
+		_current_vel_sp = _current_vel_sp + accel * dt;
+	}
+
+	updateVelocityCommands(_current_vel_sp);
+
+	sendVelocityCommands();
+}
+
+void MovingPlatformController::updateVelocityCommands(const gz::math::Vector3d &velocity_setpoint)
+{
+	// 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 + velocity_setpoint;
+	_platform_w = ampl_w * _noise_w_lowpass;
+
+	// feedback terms to ensure the random walk (= integral of noise)
+	// stays within a realistic region.
+	{
+		// 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;
path: src/modules/simulation/gz_plugins/moving_platform_controller/MovingPlatformController.cpp
user: login: Jaeyoung-Lim
body: Why are we controlling the platform movement with angular velocity? Can't you just constrain the movement?
created_at: 2025-03-13T09:08:18Z
author_association: MEMBER
diff_hunk: @@ -0,0 +1,197 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+#include "MovingPlatformController.hpp"
+
+using namespace custom;
+
+// Register the plugin
+GZ_ADD_PLUGIN(
+	MovingPlatformController,
+	gz::sim::System,
+	gz::sim::ISystemPreUpdate,
+	gz::sim::ISystemConfigure
+)
+
+void MovingPlatformController::Configure(const gz::sim::Entity &entity,
+		const std::shared_ptr<const sdf::Element> &sdf,
+		gz::sim::EntityComponentManager &ecm,
+		gz::sim::EventManager &eventMgr)
+{
+	_entity = entity;
+	_model = gz::sim::Model(entity);
+
+	double default_mean_vel = 1.0f;
+	double v_x = default_mean_vel;
+
+	// Read v_x from environment variable.
+	{
+		const char *env_var_name = "PX4_GZ_PLATFORM_VEL";
+		const char *env_var_value = std::getenv(env_var_name);
+
+		if (env_var_value) {
+			try {
+				v_x = std::stof(env_var_value);
+
+			} catch (const std::invalid_argument &e) {
+				// These warnings will only be visible with sufficient verbosity level...
+				gzwarn << "Invalid argument: " << env_var_value << " is not a valid double for " << env_var_name << std::endl;
+				gzwarn << "Keeping default value of " << v_x << " m/s." << std::endl;
+
+			} catch (const std::out_of_range &e) {
+				gzwarn << "Out of range: " << env_var_value << " is out of range for " << env_var_name << std::endl;
+				gzwarn << "Keeping default value of " << v_x << " m/s." << std::endl;
+			}
+		}
+	}
+
+	_target_vel_sp = gz::math::Vector3d(v_x, 0., 0.);
+
+	// Advertise topic to communicate with VelocityControl plugin
+	std::string model_name = _model.Name(ecm);
+	std::string cmd_vel_topic = "/model/" + model_name + "/cmd_vel";
+	_platform_twist_pub = _node.Advertise<gz::msgs::Twist>(cmd_vel_topic);
+}
+
+void MovingPlatformController::PreUpdate(const gz::sim::UpdateInfo &_info, gz::sim::EntityComponentManager &ecm)
+{
+	updatePose(ecm);
+
+	// Initially, ramp up the velocity with constant acceleration. Otherwise
+	// we can't really simulate fast velocities as the drone just slips off
+	// the platform.
+
+	double max_accel_mag = 2.; // m/s^2
+	double dt = std::chrono::duration<double>(_info.dt).count(); // s
+
+	const gz::math::Vector3d vel_sp_err = _target_vel_sp - _current_vel_sp;
+
+	if (vel_sp_err.Length() < max_accel_mag * dt) {
+		// we are already almost there. set directly to avoid division by 0.
+		_current_vel_sp = _target_vel_sp;
+	} else {
+		// acclerate towards target velocity.
+		const gz::math::Vector3d accel = vel_sp_err.Normalized() * max_accel_mag;
+		_current_vel_sp = _current_vel_sp + accel * dt;
+	}
+
+	updateVelocityCommands(_current_vel_sp);
+
+	sendVelocityCommands();
+}
+
+void MovingPlatformController::updateVelocityCommands(const gz::math::Vector3d &velocity_setpoint)
+{
+	// 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;
path: src/modules/simulation/gz_plugins/moving_platform_controller/MovingPlatformController.cpp
user: login: mbjd
body: Coefficients for the low-pass filters. I renamed them `filter_coef_*` and changed the comment right above to use that terminology as well to hopefully make it more obvious. 
created_at: 2025-03-13T09:25:49Z
author_association: CONTRIBUTOR
diff_hunk: @@ -0,0 +1,197 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+#include "MovingPlatformController.hpp"
+
+using namespace custom;
+
+// Register the plugin
+GZ_ADD_PLUGIN(
+	MovingPlatformController,
+	gz::sim::System,
+	gz::sim::ISystemPreUpdate,
+	gz::sim::ISystemConfigure
+)
+
+void MovingPlatformController::Configure(const gz::sim::Entity &entity,
+		const std::shared_ptr<const sdf::Element> &sdf,
+		gz::sim::EntityComponentManager &ecm,
+		gz::sim::EventManager &eventMgr)
+{
+	_entity = entity;
+	_model = gz::sim::Model(entity);
+
+	double default_mean_vel = 1.0f;
+	double v_x = default_mean_vel;
+
+	// Read v_x from environment variable.
+	{
+		const char *env_var_name = "PX4_GZ_PLATFORM_VEL";
+		const char *env_var_value = std::getenv(env_var_name);
+
+		if (env_var_value) {
+			try {
+				v_x = std::stof(env_var_value);
+
+			} catch (const std::invalid_argument &e) {
+				// These warnings will only be visible with sufficient verbosity level...
+				gzwarn << "Invalid argument: " << env_var_value << " is not a valid double for " << env_var_name << std::endl;
+				gzwarn << "Keeping default value of " << v_x << " m/s." << std::endl;
+
+			} catch (const std::out_of_range &e) {
+				gzwarn << "Out of range: " << env_var_value << " is out of range for " << env_var_name << std::endl;
+				gzwarn << "Keeping default value of " << v_x << " m/s." << std::endl;
+			}
+		}
+	}
+
+	_target_vel_sp = gz::math::Vector3d(v_x, 0., 0.);
+
+	// Advertise topic to communicate with VelocityControl plugin
+	std::string model_name = _model.Name(ecm);
+	std::string cmd_vel_topic = "/model/" + model_name + "/cmd_vel";
+	_platform_twist_pub = _node.Advertise<gz::msgs::Twist>(cmd_vel_topic);
+}
+
+void MovingPlatformController::PreUpdate(const gz::sim::UpdateInfo &_info, gz::sim::EntityComponentManager &ecm)
+{
+	updatePose(ecm);
+
+	// Initially, ramp up the velocity with constant acceleration. Otherwise
+	// we can't really simulate fast velocities as the drone just slips off
+	// the platform.
+
+	double max_accel_mag = 2.; // m/s^2
+	double dt = std::chrono::duration<double>(_info.dt).count(); // s
+
+	const gz::math::Vector3d vel_sp_err = _target_vel_sp - _current_vel_sp;
+
+	if (vel_sp_err.Length() < max_accel_mag * dt) {
+		// we are already almost there. set directly to avoid division by 0.
+		_current_vel_sp = _target_vel_sp;
+	} else {
+		// acclerate towards target velocity.
+		const gz::math::Vector3d accel = vel_sp_err.Normalized() * max_accel_mag;
+		_current_vel_sp = _current_vel_sp + accel * dt;
+	}
+
+	updateVelocityCommands(_current_vel_sp);
+
+	sendVelocityCommands();
+}
+
+void MovingPlatformController::updateVelocityCommands(const gz::math::Vector3d &velocity_setpoint)
+{
+	// 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 + velocity_setpoint;
+	_platform_w = ampl_w * _noise_w_lowpass;
+
+	// feedback terms to ensure the random walk (= integral of noise)
+	// stays within a realistic region.
+	{
+		// 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;
path: src/modules/simulation/gz_plugins/moving_platform_controller/MovingPlatformController.cpp
user: login: mbjd
body: What do you mean precisely by constraining the movement? 

My initial idea was to directly set the pose, which would remove the need for a feedback controller. However, this results in discontinous movement and incorrect contact forces (the platform would slowly move but the aircraft sitting on top of it would stay still...), so to simulate correctly we can only use velocity-level inputs (unless I'm missing something...) 

and within that constraint, the paradigm of low-pass filtered white noise + feedback for stabilisation seemed the most natural to me. the feedback makes sure we stay in some reasonable stationary distribution, and we mimic actual ship physics (driven by random waves, staying close to upright because the ship is stable).
created_at: 2025-03-13T09:32:12Z
author_association: CONTRIBUTOR
diff_hunk: @@ -0,0 +1,198 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+#include "MovingPlatformController.hpp"
+
+using namespace custom;
+
+// Register the plugin
+GZ_ADD_PLUGIN(
+	MovingPlatformController,
+	gz::sim::System,
+	gz::sim::ISystemPreUpdate,
+	gz::sim::ISystemConfigure
+)
+
+void MovingPlatformController::Configure(const gz::sim::Entity &entity,
+		const std::shared_ptr<const sdf::Element> &sdf,
+		gz::sim::EntityComponentManager &ecm,
+		gz::sim::EventManager &eventMgr)
+{
+	_entity = entity;
+	_model = gz::sim::Model(entity);
+
+	double default_mean_vel = 1.0f;
path: src/modules/simulation/gz_plugins/moving_platform_controller/MovingPlatformController.cpp
user: login: RomanBapst
body: @mbjd Just remove this and set the other variable directly
created_at: 2025-03-13T09:34:40Z
author_association: CONTRIBUTOR
diff_hunk: @@ -0,0 +1,198 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+#include "MovingPlatformController.hpp"
+
+using namespace custom;
+
+// Register the plugin
+GZ_ADD_PLUGIN(
+	MovingPlatformController,
+	gz::sim::System,
+	gz::sim::ISystemPreUpdate,
+	gz::sim::ISystemConfigure
+)
+
+void MovingPlatformController::Configure(const gz::sim::Entity &entity,
+		const std::shared_ptr<const sdf::Element> &sdf,
+		gz::sim::EntityComponentManager &ecm,
+		gz::sim::EventManager &eventMgr)
+{
+	_entity = entity;
+	_model = gz::sim::Model(entity);
+
+	double default_mean_vel = 1.0f;
+	double v_x = default_mean_vel;
+
+	// Read v_x from environment variable.
+	{
+		const char *env_var_name = "PX4_GZ_PLATFORM_VEL";
+		const char *env_var_value = std::getenv(env_var_name);
+
+		if (env_var_value) {
+			try {
+				v_x = std::stof(env_var_value);
+
+			} catch (const std::invalid_argument &e) {
+				// These warnings will only be visible with sufficient verbosity level...
+				gzwarn << "Invalid argument: " << env_var_value << " is not a valid double for " << env_var_name << std::endl;
+				gzwarn << "Keeping default value of " << v_x << " m/s." << std::endl;
+
+			} catch (const std::out_of_range &e) {
+				gzwarn << "Out of range: " << env_var_value << " is out of range for " << env_var_name << std::endl;
+				gzwarn << "Keeping default value of " << v_x << " m/s." << std::endl;
+			}
+		}
+	}
+
+	_target_vel_sp = gz::math::Vector3d(v_x, 0., 0.);
+
+	// Advertise topic to communicate with VelocityControl plugin
+	std::string model_name = _model.Name(ecm);
+	std::string cmd_vel_topic = "/model/" + model_name + "/cmd_vel";
+	_platform_twist_pub = _node.Advertise<gz::msgs::Twist>(cmd_vel_topic);
+}
+
+void MovingPlatformController::PreUpdate(const gz::sim::UpdateInfo &_info, gz::sim::EntityComponentManager &ecm)
+{
+	updatePose(ecm);
+
+	// Initially, ramp up the velocity with constant acceleration. Otherwise
+	// we can't really simulate fast velocities as the drone just slips off
+	// the platform.
+
+	double max_accel_mag = 2.; // m/s^2
path: src/modules/simulation/gz_plugins/moving_platform_controller/MovingPlatformController.cpp
user: login: RomanBapst
body: ```suggestion
	const double max_accel_mag = 2.; // m/s^2
```
created_at: 2025-03-13T09:35:43Z
author_association: CONTRIBUTOR
diff_hunk: @@ -0,0 +1,198 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+#include "MovingPlatformController.hpp"
+
+using namespace custom;
+
+// Register the plugin
+GZ_ADD_PLUGIN(
+	MovingPlatformController,
+	gz::sim::System,
+	gz::sim::ISystemPreUpdate,
+	gz::sim::ISystemConfigure
+)
+
+void MovingPlatformController::Configure(const gz::sim::Entity &entity,
+		const std::shared_ptr<const sdf::Element> &sdf,
+		gz::sim::EntityComponentManager &ecm,
+		gz::sim::EventManager &eventMgr)
+{
+	_entity = entity;
+	_model = gz::sim::Model(entity);
+
+	double default_mean_vel = 1.0f;
+	double v_x = default_mean_vel;
+
+	// Read v_x from environment variable.
+	{
+		const char *env_var_name = "PX4_GZ_PLATFORM_VEL";
+		const char *env_var_value = std::getenv(env_var_name);
+
+		if (env_var_value) {
+			try {
+				v_x = std::stof(env_var_value);
+
+			} catch (const std::invalid_argument &e) {
+				// These warnings will only be visible with sufficient verbosity level...
+				gzwarn << "Invalid argument: " << env_var_value << " is not a valid double for " << env_var_name << std::endl;
+				gzwarn << "Keeping default value of " << v_x << " m/s." << std::endl;
+
+			} catch (const std::out_of_range &e) {
+				gzwarn << "Out of range: " << env_var_value << " is out of range for " << env_var_name << std::endl;
+				gzwarn << "Keeping default value of " << v_x << " m/s." << std::endl;
+			}
+		}
+	}
+
+	_target_vel_sp = gz::math::Vector3d(v_x, 0., 0.);
+
+	// Advertise topic to communicate with VelocityControl plugin
+	std::string model_name = _model.Name(ecm);
+	std::string cmd_vel_topic = "/model/" + model_name + "/cmd_vel";
+	_platform_twist_pub = _node.Advertise<gz::msgs::Twist>(cmd_vel_topic);
+}
+
+void MovingPlatformController::PreUpdate(const gz::sim::UpdateInfo &_info, gz::sim::EntityComponentManager &ecm)
+{
+	updatePose(ecm);
+
+	// Initially, ramp up the velocity with constant acceleration. Otherwise
+	// we can't really simulate fast velocities as the drone just slips off
+	// the platform.
+
+	double max_accel_mag = 2.; // m/s^2
+	double dt = std::chrono::duration<double>(_info.dt).count(); // s
path: src/modules/simulation/gz_plugins/moving_platform_controller/MovingPlatformController.cpp
user: login: RomanBapst
body: ```suggestion
	const double dt = std::chrono::duration<double>(_info.dt).count(); // s
```
created_at: 2025-03-13T09:36:00Z
author_association: CONTRIBUTOR
diff_hunk: @@ -0,0 +1,197 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+#include "MovingPlatformController.hpp"
+
+using namespace custom;
+
+// Register the plugin
+GZ_ADD_PLUGIN(
+	MovingPlatformController,
+	gz::sim::System,
+	gz::sim::ISystemPreUpdate,
+	gz::sim::ISystemConfigure
+)
+
+void MovingPlatformController::Configure(const gz::sim::Entity &entity,
+		const std::shared_ptr<const sdf::Element> &sdf,
+		gz::sim::EntityComponentManager &ecm,
+		gz::sim::EventManager &eventMgr)
+{
+	_entity = entity;
+	_model = gz::sim::Model(entity);
+
+	double default_mean_vel = 1.0f;
+	double v_x = default_mean_vel;
+
+	// Read v_x from environment variable.
+	{
+		const char *env_var_name = "PX4_GZ_PLATFORM_VEL";
+		const char *env_var_value = std::getenv(env_var_name);
+
+		if (env_var_value) {
+			try {
+				v_x = std::stof(env_var_value);
+
+			} catch (const std::invalid_argument &e) {
+				// These warnings will only be visible with sufficient verbosity level...
+				gzwarn << "Invalid argument: " << env_var_value << " is not a valid double for " << env_var_name << std::endl;
+				gzwarn << "Keeping default value of " << v_x << " m/s." << std::endl;
+
+			} catch (const std::out_of_range &e) {
+				gzwarn << "Out of range: " << env_var_value << " is out of range for " << env_var_name << std::endl;
+				gzwarn << "Keeping default value of " << v_x << " m/s." << std::endl;
+			}
+		}
+	}
+
+	_target_vel_sp = gz::math::Vector3d(v_x, 0., 0.);
+
+	// Advertise topic to communicate with VelocityControl plugin
+	std::string model_name = _model.Name(ecm);
+	std::string cmd_vel_topic = "/model/" + model_name + "/cmd_vel";
+	_platform_twist_pub = _node.Advertise<gz::msgs::Twist>(cmd_vel_topic);
+}
+
+void MovingPlatformController::PreUpdate(const gz::sim::UpdateInfo &_info, gz::sim::EntityComponentManager &ecm)
+{
+	updatePose(ecm);
+
+	// Initially, ramp up the velocity with constant acceleration. Otherwise
+	// we can't really simulate fast velocities as the drone just slips off
+	// the platform.
+
+	double max_accel_mag = 2.; // m/s^2
+	double dt = std::chrono::duration<double>(_info.dt).count(); // s
+
+	const gz::math::Vector3d vel_sp_err = _target_vel_sp - _current_vel_sp;
+
+	if (vel_sp_err.Length() < max_accel_mag * dt) {
+		// we are already almost there. set directly to avoid division by 0.
+		_current_vel_sp = _target_vel_sp;
+	} else {
+		// acclerate towards target velocity.
+		const gz::math::Vector3d accel = vel_sp_err.Normalized() * max_accel_mag;
+		_current_vel_sp = _current_vel_sp + accel * dt;
+	}
+
+	updateVelocityCommands(_current_vel_sp);
+
+	sendVelocityCommands();
+}
+
+void MovingPlatformController::updateVelocityCommands(const gz::math::Vector3d &velocity_setpoint)
+{
+	// 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 + velocity_setpoint;
+	_platform_w = ampl_w * _noise_w_lowpass;
+
+	// feedback terms to ensure the random walk (= integral of noise)
+	// stays within a realistic region.
+	{
+		// 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;
path: src/modules/simulation/gz_plugins/moving_platform_controller/MovingPlatformController.cpp
user: login: mbjd
body: Any further thoughts on this or should I mark this as resolved? 
created_at: 2025-03-14T07:53:54Z
author_association: CONTRIBUTOR
diff_hunk: @@ -0,0 +1,197 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+#include "MovingPlatformController.hpp"
+
+using namespace custom;
+
+// Register the plugin
+GZ_ADD_PLUGIN(
+	MovingPlatformController,
+	gz::sim::System,
+	gz::sim::ISystemPreUpdate,
+	gz::sim::ISystemConfigure
+)
+
+void MovingPlatformController::Configure(const gz::sim::Entity &entity,
+		const std::shared_ptr<const sdf::Element> &sdf,
+		gz::sim::EntityComponentManager &ecm,
+		gz::sim::EventManager &eventMgr)
+{
+	_entity = entity;
+	_model = gz::sim::Model(entity);
+
+	double default_mean_vel = 1.0f;
+	double v_x = default_mean_vel;
+
+	// Read v_x from environment variable.
+	{
+		const char *env_var_name = "PX4_GZ_PLATFORM_VEL";
+		const char *env_var_value = std::getenv(env_var_name);
+
+		if (env_var_value) {
+			try {
+				v_x = std::stof(env_var_value);
+
+			} catch (const std::invalid_argument &e) {
+				// These warnings will only be visible with sufficient verbosity level...
+				gzwarn << "Invalid argument: " << env_var_value << " is not a valid double for " << env_var_name << std::endl;
+				gzwarn << "Keeping default value of " << v_x << " m/s." << std::endl;
+
+			} catch (const std::out_of_range &e) {
+				gzwarn << "Out of range: " << env_var_value << " is out of range for " << env_var_name << std::endl;
+				gzwarn << "Keeping default value of " << v_x << " m/s." << std::endl;
+			}
+		}
+	}
+
+	_target_vel_sp = gz::math::Vector3d(v_x, 0., 0.);
+
+	// Advertise topic to communicate with VelocityControl plugin
+	std::string model_name = _model.Name(ecm);
+	std::string cmd_vel_topic = "/model/" + model_name + "/cmd_vel";
+	_platform_twist_pub = _node.Advertise<gz::msgs::Twist>(cmd_vel_topic);
+}
+
+void MovingPlatformController::PreUpdate(const gz::sim::UpdateInfo &_info, gz::sim::EntityComponentManager &ecm)
+{
+	updatePose(ecm);
+
+	// Initially, ramp up the velocity with constant acceleration. Otherwise
+	// we can't really simulate fast velocities as the drone just slips off
+	// the platform.
+
+	double max_accel_mag = 2.; // m/s^2
+	double dt = std::chrono::duration<double>(_info.dt).count(); // s
+
+	const gz::math::Vector3d vel_sp_err = _target_vel_sp - _current_vel_sp;
+
+	if (vel_sp_err.Length() < max_accel_mag * dt) {
+		// we are already almost there. set directly to avoid division by 0.
+		_current_vel_sp = _target_vel_sp;
+	} else {
+		// acclerate towards target velocity.
+		const gz::math::Vector3d accel = vel_sp_err.Normalized() * max_accel_mag;
+		_current_vel_sp = _current_vel_sp + accel * dt;
+	}
+
+	updateVelocityCommands(_current_vel_sp);
+
+	sendVelocityCommands();
+}
+
+void MovingPlatformController::updateVelocityCommands(const gz::math::Vector3d &velocity_setpoint)
+{
+	// 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 + velocity_setpoint;
+	_platform_w = ampl_w * _noise_w_lowpass;
+
+	// feedback terms to ensure the random walk (= integral of noise)
+	// stays within a realistic region.
+	{
+		// 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;
path: src/modules/simulation/gz_plugins/moving_platform_controller/MovingPlatformController.cpp
user: login: Jaeyoung-Lim
body: I see now what problem you were trying to solve. However, if you are concerned about the contact dynamics, using body rate commands would still introduce the same problems as you set exact pose, as this imposes a kinematics constraint on the simulation. Therefore, using wrench commands would have been the correct way to solve this.
created_at: 2025-03-17T12:33:26Z
author_association: MEMBER
diff_hunk: @@ -0,0 +1,197 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+#include "MovingPlatformController.hpp"
+
+using namespace custom;
+
+// Register the plugin
+GZ_ADD_PLUGIN(
+	MovingPlatformController,
+	gz::sim::System,
+	gz::sim::ISystemPreUpdate,
+	gz::sim::ISystemConfigure
+)
+
+void MovingPlatformController::Configure(const gz::sim::Entity &entity,
+		const std::shared_ptr<const sdf::Element> &sdf,
+		gz::sim::EntityComponentManager &ecm,
+		gz::sim::EventManager &eventMgr)
+{
+	_entity = entity;
+	_model = gz::sim::Model(entity);
+
+	double default_mean_vel = 1.0f;
+	double v_x = default_mean_vel;
+
+	// Read v_x from environment variable.
+	{
+		const char *env_var_name = "PX4_GZ_PLATFORM_VEL";
+		const char *env_var_value = std::getenv(env_var_name);
+
+		if (env_var_value) {
+			try {
+				v_x = std::stof(env_var_value);
+
+			} catch (const std::invalid_argument &e) {
+				// These warnings will only be visible with sufficient verbosity level...
+				gzwarn << "Invalid argument: " << env_var_value << " is not a valid double for " << env_var_name << std::endl;
+				gzwarn << "Keeping default value of " << v_x << " m/s." << std::endl;
+
+			} catch (const std::out_of_range &e) {
+				gzwarn << "Out of range: " << env_var_value << " is out of range for " << env_var_name << std::endl;
+				gzwarn << "Keeping default value of " << v_x << " m/s." << std::endl;
+			}
+		}
+	}
+
+	_target_vel_sp = gz::math::Vector3d(v_x, 0., 0.);
+
+	// Advertise topic to communicate with VelocityControl plugin
+	std::string model_name = _model.Name(ecm);
+	std::string cmd_vel_topic = "/model/" + model_name + "/cmd_vel";
+	_platform_twist_pub = _node.Advertise<gz::msgs::Twist>(cmd_vel_topic);
+}
+
+void MovingPlatformController::PreUpdate(const gz::sim::UpdateInfo &_info, gz::sim::EntityComponentManager &ecm)
+{
+	updatePose(ecm);
+
+	// Initially, ramp up the velocity with constant acceleration. Otherwise
+	// we can't really simulate fast velocities as the drone just slips off
+	// the platform.
+
+	double max_accel_mag = 2.; // m/s^2
+	double dt = std::chrono::duration<double>(_info.dt).count(); // s
+
+	const gz::math::Vector3d vel_sp_err = _target_vel_sp - _current_vel_sp;
+
+	if (vel_sp_err.Length() < max_accel_mag * dt) {
+		// we are already almost there. set directly to avoid division by 0.
+		_current_vel_sp = _target_vel_sp;
+	} else {
+		// acclerate towards target velocity.
+		const gz::math::Vector3d accel = vel_sp_err.Normalized() * max_accel_mag;
+		_current_vel_sp = _current_vel_sp + accel * dt;
+	}
+
+	updateVelocityCommands(_current_vel_sp);
+
+	sendVelocityCommands();
+}
+
+void MovingPlatformController::updateVelocityCommands(const gz::math::Vector3d &velocity_setpoint)
+{
+	// 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 + velocity_setpoint;
+	_platform_w = ampl_w * _noise_w_lowpass;
+
+	// feedback terms to ensure the random walk (= integral of noise)
+	// stays within a realistic region.
+	{
+		// 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;
path: src/modules/simulation/gz_plugins/moving_platform_controller/MovingPlatformController.cpp
user: login: mbjd
body: Yes, you're right that technically the same problem with the kinematic constraint persists. However, practically it is now so small as to be negligible. All of our use cases have platform mass >> drone mass, making the kinematic constraint a close approximation of reality. 

Do you consider a second-order, wrench-based implementation a must-have for this PR? (either to account for use cases where drone and platform mass are on similar scales, or as a matter of principle)? Or is your comment more of a suggestion for future improvement?

I'm open to changing it, but from our side it's not a requirement. 

created_at: 2025-03-18T08:15:45Z
author_association: CONTRIBUTOR
diff_hunk: @@ -0,0 +1,197 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+#include "MovingPlatformController.hpp"
+
+using namespace custom;
+
+// Register the plugin
+GZ_ADD_PLUGIN(
+	MovingPlatformController,
+	gz::sim::System,
+	gz::sim::ISystemPreUpdate,
+	gz::sim::ISystemConfigure
+)
+
+void MovingPlatformController::Configure(const gz::sim::Entity &entity,
+		const std::shared_ptr<const sdf::Element> &sdf,
+		gz::sim::EntityComponentManager &ecm,
+		gz::sim::EventManager &eventMgr)
+{
+	_entity = entity;
+	_model = gz::sim::Model(entity);
+
+	double default_mean_vel = 1.0f;
+	double v_x = default_mean_vel;
+
+	// Read v_x from environment variable.
+	{
+		const char *env_var_name = "PX4_GZ_PLATFORM_VEL";
+		const char *env_var_value = std::getenv(env_var_name);
+
+		if (env_var_value) {
+			try {
+				v_x = std::stof(env_var_value);
+
+			} catch (const std::invalid_argument &e) {
+				// These warnings will only be visible with sufficient verbosity level...
+				gzwarn << "Invalid argument: " << env_var_value << " is not a valid double for " << env_var_name << std::endl;
+				gzwarn << "Keeping default value of " << v_x << " m/s." << std::endl;
+
+			} catch (const std::out_of_range &e) {
+				gzwarn << "Out of range: " << env_var_value << " is out of range for " << env_var_name << std::endl;
+				gzwarn << "Keeping default value of " << v_x << " m/s." << std::endl;
+			}
+		}
+	}
+
+	_target_vel_sp = gz::math::Vector3d(v_x, 0., 0.);
+
+	// Advertise topic to communicate with VelocityControl plugin
+	std::string model_name = _model.Name(ecm);
+	std::string cmd_vel_topic = "/model/" + model_name + "/cmd_vel";
+	_platform_twist_pub = _node.Advertise<gz::msgs::Twist>(cmd_vel_topic);
+}
+
+void MovingPlatformController::PreUpdate(const gz::sim::UpdateInfo &_info, gz::sim::EntityComponentManager &ecm)
+{
+	updatePose(ecm);
+
+	// Initially, ramp up the velocity with constant acceleration. Otherwise
+	// we can't really simulate fast velocities as the drone just slips off
+	// the platform.
+
+	double max_accel_mag = 2.; // m/s^2
+	double dt = std::chrono::duration<double>(_info.dt).count(); // s
+
+	const gz::math::Vector3d vel_sp_err = _target_vel_sp - _current_vel_sp;
+
+	if (vel_sp_err.Length() < max_accel_mag * dt) {
+		// we are already almost there. set directly to avoid division by 0.
+		_current_vel_sp = _target_vel_sp;
+	} else {
+		// acclerate towards target velocity.
+		const gz::math::Vector3d accel = vel_sp_err.Normalized() * max_accel_mag;
+		_current_vel_sp = _current_vel_sp + accel * dt;
+	}
+
+	updateVelocityCommands(_current_vel_sp);
+
+	sendVelocityCommands();
+}
+
+void MovingPlatformController::updateVelocityCommands(const gz::math::Vector3d &velocity_setpoint)
+{
+	// 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 + velocity_setpoint;
+	_platform_w = ampl_w * _noise_w_lowpass;
+
+	// feedback terms to ensure the random walk (= integral of noise)
+	// stays within a realistic region.
+	{
+		// 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;
path: src/modules/simulation/gz_plugins/moving_platform_controller/MovingPlatformController.cpp
user: login: Jaeyoung-Lim
body: @mbjd I would prefer if we change it OR if this is a blocker, merge it as is now and change it as a follow up PR
created_at: 2025-03-18T08:25:27Z
author_association: MEMBER
diff_hunk: @@ -0,0 +1,197 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+#include "MovingPlatformController.hpp"
+
+using namespace custom;
+
+// Register the plugin
+GZ_ADD_PLUGIN(
+	MovingPlatformController,
+	gz::sim::System,
+	gz::sim::ISystemPreUpdate,
+	gz::sim::ISystemConfigure
+)
+
+void MovingPlatformController::Configure(const gz::sim::Entity &entity,
+		const std::shared_ptr<const sdf::Element> &sdf,
+		gz::sim::EntityComponentManager &ecm,
+		gz::sim::EventManager &eventMgr)
+{
+	_entity = entity;
+	_model = gz::sim::Model(entity);
+
+	double default_mean_vel = 1.0f;
+	double v_x = default_mean_vel;
+
+	// Read v_x from environment variable.
+	{
+		const char *env_var_name = "PX4_GZ_PLATFORM_VEL";
+		const char *env_var_value = std::getenv(env_var_name);
+
+		if (env_var_value) {
+			try {
+				v_x = std::stof(env_var_value);
+
+			} catch (const std::invalid_argument &e) {
+				// These warnings will only be visible with sufficient verbosity level...
+				gzwarn << "Invalid argument: " << env_var_value << " is not a valid double for " << env_var_name << std::endl;
+				gzwarn << "Keeping default value of " << v_x << " m/s." << std::endl;
+
+			} catch (const std::out_of_range &e) {
+				gzwarn << "Out of range: " << env_var_value << " is out of range for " << env_var_name << std::endl;
+				gzwarn << "Keeping default value of " << v_x << " m/s." << std::endl;
+			}
+		}
+	}
+
+	_target_vel_sp = gz::math::Vector3d(v_x, 0., 0.);
+
+	// Advertise topic to communicate with VelocityControl plugin
+	std::string model_name = _model.Name(ecm);
+	std::string cmd_vel_topic = "/model/" + model_name + "/cmd_vel";
+	_platform_twist_pub = _node.Advertise<gz::msgs::Twist>(cmd_vel_topic);
+}
+
+void MovingPlatformController::PreUpdate(const gz::sim::UpdateInfo &_info, gz::sim::EntityComponentManager &ecm)
+{
+	updatePose(ecm);
+
+	// Initially, ramp up the velocity with constant acceleration. Otherwise
+	// we can't really simulate fast velocities as the drone just slips off
+	// the platform.
+
+	double max_accel_mag = 2.; // m/s^2
+	double dt = std::chrono::duration<double>(_info.dt).count(); // s
+
+	const gz::math::Vector3d vel_sp_err = _target_vel_sp - _current_vel_sp;
+
+	if (vel_sp_err.Length() < max_accel_mag * dt) {
+		// we are already almost there. set directly to avoid division by 0.
+		_current_vel_sp = _target_vel_sp;
+	} else {
+		// acclerate towards target velocity.
+		const gz::math::Vector3d accel = vel_sp_err.Normalized() * max_accel_mag;
+		_current_vel_sp = _current_vel_sp + accel * dt;
+	}
+
+	updateVelocityCommands(_current_vel_sp);
+
+	sendVelocityCommands();
+}
+
+void MovingPlatformController::updateVelocityCommands(const gz::math::Vector3d &velocity_setpoint)
+{
+	// 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 + velocity_setpoint;
+	_platform_w = ampl_w * _noise_w_lowpass;
+
+	// feedback terms to ensure the random walk (= integral of noise)
+	// stays within a realistic region.
+	{
+		// 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;
path: src/modules/simulation/gz_plugins/moving_platform_controller/MovingPlatformController.cpp
user: login: mbjd
body: Okay, makes sense. I will change accordingly early next week, leaving this PR open until then. 

ATM we're not blocked, for testing we just merge this branch with others as necessary :)

created_at: 2025-03-18T09:50:45Z
author_association: CONTRIBUTOR
diff_hunk: @@ -0,0 +1,276 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+#include "MovingPlatformController.hpp"
+
+using namespace custom;
+
+// Register the plugin
+GZ_ADD_PLUGIN(
+	MovingPlatformController,
+	gz::sim::System,
+	gz::sim::ISystemPreUpdate,
+	gz::sim::ISystemConfigure
+)
+
+void MovingPlatformController::Configure(const gz::sim::Entity &entity,
+		const std::shared_ptr<const sdf::Element> &sdf,
+		gz::sim::EntityComponentManager &ecm,
+		gz::sim::EventManager &eventMgr)
+{
+	_entity = entity;
+	_model = gz::sim::Model(entity);
+
+	const std::string link_name = sdf->Get<std::string>("link_name");
+	_link_entity = _model.LinkByName(ecm, link_name);
+
+	if (!_link_entity) {
+		throw std::runtime_error("MovingPlatformController::Configure: Link \"" + link_name + "\" was not found. "
+			"Please ensure that your model contains the corresponding link.");
+	}
+
+	_link = gz::sim::Link(_link_entity);
+
+	// Needed to report linear & angular velocity
+	_link.EnableVelocityChecks(ecm, true);
+
+	// Get velocity and orientation setpoints from env vars
+	{
+		const double vel_forward = ReadEnvVar("PX4_GZ_PLATFORM_VEL", 1.0);
+		const double heading_deg = ReadEnvVar("PX4_GZ_PLATFORM_HEADING_DEG", 0.0);
+		const double heading = GZ_DTOR(heading_deg);
+
+		_orientation_sp = gz::math::Quaterniond(0., 0., heading);
+
+		// Set initial orientation to match the heading
+		const auto optional_original_pose = _link.WorldPose(ecm);
+		if (optional_original_pose.has_value()) {
+			const gz::math::Pose3d original_pose = optional_original_pose.value();
+			const gz::math::Pose3d new_pose(original_pose.Pos(), _orientation_sp);
+			_model.SetWorldPoseCmd(ecm, new_pose);
+
+		} else {
+			gzwarn << "Unable to get initial platform pose. Heading will not be set" << std::endl;
+		}
+
+		// Velocity setpoint in world frame.
+		const gz::math::Vector3d _body_velocity_sp(vel_forward, 0., 0.);
+		_velocity_sp = _orientation_sp * _body_velocity_sp;
+	}
+
+	// Get gravity, model mass, platform height.
+	{
+		const auto world_entity = gz::sim::worldEntity(ecm);
+		const auto world = gz::sim::World(world_entity);
+		const auto gravity = world.Gravity(ecm);
+
+		if (gravity.has_value()) {
+			_gravity = world.Gravity(ecm).value().Z();
+		} else {
+			gzwarn << "Unable to get gazebo world gravity. Keeping default of " << _gravity << std::endl;
+		}
+
+		auto inertial_component = ecm.Component<gz::sim::components::Inertial>(_link_entity);
+
+		if (inertial_component) {
+			_platform_mass = inertial_component->Data().MassMatrix().Mass();
+		} else {
+			gzwarn << "Unable to get inertial component for link " << link_name << ". Keeping default mass of " << _platform_mass << std::endl;
+		}
+
+		auto pose_component = ecm.Component<gz::sim::components::Pose>(_link_entity);
+
+		if (pose_component) {
+			_platform_height_setpoint = pose_component->Data().Z();
+		} else {
+			gzwarn << "Unable to get inertial component for link " << link_name << ". Keeping default mass of " << _platform_mass << std::endl;
path: src/modules/simulation/gz_plugins/moving_platform_controller/MovingPlatformController.cpp
user: login: perrrre
body: ```suggestion
			gzwarn << "Unable to get pose component for link " << link_name << ". Keeping default height set point of " << _platform_height_setpoint << std::endl;
```
created_at: 2025-04-02T05:58:01Z
author_association: CONTRIBUTOR
diff_hunk: @@ -0,0 +1,276 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+#include "MovingPlatformController.hpp"
+
+using namespace custom;
+
+// Register the plugin
+GZ_ADD_PLUGIN(
+	MovingPlatformController,
+	gz::sim::System,
+	gz::sim::ISystemPreUpdate,
+	gz::sim::ISystemConfigure
+)
+
+void MovingPlatformController::Configure(const gz::sim::Entity &entity,
+		const std::shared_ptr<const sdf::Element> &sdf,
+		gz::sim::EntityComponentManager &ecm,
+		gz::sim::EventManager &eventMgr)
+{
+	_entity = entity;
+	_model = gz::sim::Model(entity);
+
+	const std::string link_name = sdf->Get<std::string>("link_name");
+	_link_entity = _model.LinkByName(ecm, link_name);
+
+	if (!_link_entity) {
+		throw std::runtime_error("MovingPlatformController::Configure: Link \"" + link_name + "\" was not found. "
+			"Please ensure that your model contains the corresponding link.");
+	}
+
+	_link = gz::sim::Link(_link_entity);
+
+	// Needed to report linear & angular velocity
+	_link.EnableVelocityChecks(ecm, true);
+
+	// Get velocity and orientation setpoints from env vars
+	{
+		const double vel_forward = ReadEnvVar("PX4_GZ_PLATFORM_VEL", 1.0);
+		const double heading_deg = ReadEnvVar("PX4_GZ_PLATFORM_HEADING_DEG", 0.0);
+		const double heading = GZ_DTOR(heading_deg);
+
+		_orientation_sp = gz::math::Quaterniond(0., 0., heading);
+
+		// Set initial orientation to match the heading
+		const auto optional_original_pose = _link.WorldPose(ecm);
+		if (optional_original_pose.has_value()) {
+			const gz::math::Pose3d original_pose = optional_original_pose.value();
+			const gz::math::Pose3d new_pose(original_pose.Pos(), _orientation_sp);
+			_model.SetWorldPoseCmd(ecm, new_pose);
+
+		} else {
+			gzwarn << "Unable to get initial platform pose. Heading will not be set" << std::endl;
+		}
+
+		// Velocity setpoint in world frame.
+		const gz::math::Vector3d _body_velocity_sp(vel_forward, 0., 0.);
+		_velocity_sp = _orientation_sp * _body_velocity_sp;
+	}
+
+	// Get gravity, model mass, platform height.
+	{
+		const auto world_entity = gz::sim::worldEntity(ecm);
+		const auto world = gz::sim::World(world_entity);
+		const auto gravity = world.Gravity(ecm);
+
+		if (gravity.has_value()) {
+			_gravity = world.Gravity(ecm).value().Z();
+		} else {
+			gzwarn << "Unable to get gazebo world gravity. Keeping default of " << _gravity << std::endl;
+		}
+
+		auto inertial_component = ecm.Component<gz::sim::components::Inertial>(_link_entity);
+
+		if (inertial_component) {
+			_platform_mass = inertial_component->Data().MassMatrix().Mass();
+		} else {
+			gzwarn << "Unable to get inertial component for link " << link_name << ". Keeping default mass of " << _platform_mass << std::endl;
+		}
+
+		auto pose_component = ecm.Component<gz::sim::components::Pose>(_link_entity);
+
+		if (pose_component) {
+			_platform_height_setpoint = pose_component->Data().Z();
+		} else {
+			gzwarn << "Unable to get inertial component for link " << link_name << ". Keeping default mass of " << _platform_mass << std::endl;
+		}
+	}
+
+}
+
+void MovingPlatformController::PreUpdate(const gz::sim::UpdateInfo &_info, gz::sim::EntityComponentManager &ecm)
+{
+	updatePlatformState(ecm);
+	updateWrenchCommand(_velocity_sp, _orientation_sp);
+	sendWrenchCommand(ecm);
+}
+
+void MovingPlatformController::updateWrenchCommand(
+	const gz::math::Vector3d &velocity_setpoint,
+	const gz::math::Quaterniond &orientation_setpoint)
+{
+	// (force, torque) =
+	//    constant normal force / buoyancy term offsetting gravity
+	//  + low-pass filtered white noise representing waves, road noise, etc.
+	//  + PD feedback term representing stable vehicle design
+
+	gz::math::Vector3d noise_force = gz::math::Vector3d(
+			gz::math::Rand::DblNormal(),
+			gz::math::Rand::DblNormal(),
+			gz::math::Rand::DblNormal()
+					  );
+
+	gz::math::Vector3d noise_torque = gz::math::Vector3d(
+			gz::math::Rand::DblNormal(),
+			gz::math::Rand::DblNormal(),
+			gz::math::Rand::DblNormal()
+					  );
+
+	// Filter coefficients for low-pass filtering the white noise.
+	// larger number = faster movement
+	// should be between 0 and 1
+	const double filter_coef_force = 0.1;
+	const double filter_coef_torque = 0.1;
+
+	// Noise amplitude.
+	// larger number = bigger movement
+	// should be >= 0
+	const double noise_ampl_force = 1. * _platform_mass;
+	const double noise_ampl_torque = 1. * _platform_mass;
+
+	_noise_lowpass_force = (1 - filter_coef_force) * _noise_lowpass_force + filter_coef_force * noise_force;
path: src/modules/simulation/gz_plugins/moving_platform_controller/MovingPlatformController.cpp
user: login: perrrre
body: you could use this: PX4-Autopilot/src/lib/mathlib/math/filter/AlphaFilter.hpp as its done [here](https://github.com/PX4/PX4-Autopilot/blob/main/src/modules/mc_pos_control/MulticopterPositionControl.hpp#L197)
created_at: 2025-04-02T06:25:49Z
author_association: CONTRIBUTOR
diff_hunk: @@ -0,0 +1,276 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+#include "MovingPlatformController.hpp"
+
+using namespace custom;
+
+// Register the plugin
+GZ_ADD_PLUGIN(
+	MovingPlatformController,
+	gz::sim::System,
+	gz::sim::ISystemPreUpdate,
+	gz::sim::ISystemConfigure
+)
+
+void MovingPlatformController::Configure(const gz::sim::Entity &entity,
+		const std::shared_ptr<const sdf::Element> &sdf,
+		gz::sim::EntityComponentManager &ecm,
+		gz::sim::EventManager &eventMgr)
+{
+	_entity = entity;
+	_model = gz::sim::Model(entity);
+
+	const std::string link_name = sdf->Get<std::string>("link_name");
+	_link_entity = _model.LinkByName(ecm, link_name);
+
+	if (!_link_entity) {
+		throw std::runtime_error("MovingPlatformController::Configure: Link \"" + link_name + "\" was not found. "
+			"Please ensure that your model contains the corresponding link.");
+	}
+
+	_link = gz::sim::Link(_link_entity);
+
+	// Needed to report linear & angular velocity
+	_link.EnableVelocityChecks(ecm, true);
+
+	// Get velocity and orientation setpoints from env vars
+	{
+		const double vel_forward = ReadEnvVar("PX4_GZ_PLATFORM_VEL", 1.0);
+		const double heading_deg = ReadEnvVar("PX4_GZ_PLATFORM_HEADING_DEG", 0.0);
+		const double heading = GZ_DTOR(heading_deg);
+
+		_orientation_sp = gz::math::Quaterniond(0., 0., heading);
+
+		// Set initial orientation to match the heading
+		const auto optional_original_pose = _link.WorldPose(ecm);
+		if (optional_original_pose.has_value()) {
+			const gz::math::Pose3d original_pose = optional_original_pose.value();
+			const gz::math::Pose3d new_pose(original_pose.Pos(), _orientation_sp);
+			_model.SetWorldPoseCmd(ecm, new_pose);
+
+		} else {
+			gzwarn << "Unable to get initial platform pose. Heading will not be set" << std::endl;
+		}
+
+		// Velocity setpoint in world frame.
+		const gz::math::Vector3d _body_velocity_sp(vel_forward, 0., 0.);
+		_velocity_sp = _orientation_sp * _body_velocity_sp;
+	}
+
+	// Get gravity, model mass, platform height.
+	{
+		const auto world_entity = gz::sim::worldEntity(ecm);
+		const auto world = gz::sim::World(world_entity);
+		const auto gravity = world.Gravity(ecm);
+
+		if (gravity.has_value()) {
+			_gravity = world.Gravity(ecm).value().Z();
+		} else {
+			gzwarn << "Unable to get gazebo world gravity. Keeping default of " << _gravity << std::endl;
+		}
+
+		auto inertial_component = ecm.Component<gz::sim::components::Inertial>(_link_entity);
+
+		if (inertial_component) {
+			_platform_mass = inertial_component->Data().MassMatrix().Mass();
+		} else {
+			gzwarn << "Unable to get inertial component for link " << link_name << ". Keeping default mass of " << _platform_mass << std::endl;
+		}
+
+		auto pose_component = ecm.Component<gz::sim::components::Pose>(_link_entity);
+
+		if (pose_component) {
+			_platform_height_setpoint = pose_component->Data().Z();
+		} else {
+			gzwarn << "Unable to get inertial component for link " << link_name << ". Keeping default mass of " << _platform_mass << std::endl;
path: src/modules/simulation/gz_plugins/moving_platform_controller/MovingPlatformController.cpp
user: login: mbjd
body: oops, thanks for pointing that out, fixed! 
created_at: 2025-04-02T06:54:51Z
author_association: CONTRIBUTOR
diff_hunk: @@ -0,0 +1,276 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+#include "MovingPlatformController.hpp"
+
+using namespace custom;
+
+// Register the plugin
+GZ_ADD_PLUGIN(
+	MovingPlatformController,
+	gz::sim::System,
+	gz::sim::ISystemPreUpdate,
+	gz::sim::ISystemConfigure
+)
+
+void MovingPlatformController::Configure(const gz::sim::Entity &entity,
+		const std::shared_ptr<const sdf::Element> &sdf,
+		gz::sim::EntityComponentManager &ecm,
+		gz::sim::EventManager &eventMgr)
+{
+	_entity = entity;
+	_model = gz::sim::Model(entity);
+
+	const std::string link_name = sdf->Get<std::string>("link_name");
+	_link_entity = _model.LinkByName(ecm, link_name);
+
+	if (!_link_entity) {
+		throw std::runtime_error("MovingPlatformController::Configure: Link \"" + link_name + "\" was not found. "
+			"Please ensure that your model contains the corresponding link.");
+	}
+
+	_link = gz::sim::Link(_link_entity);
+
+	// Needed to report linear & angular velocity
+	_link.EnableVelocityChecks(ecm, true);
+
+	// Get velocity and orientation setpoints from env vars
+	{
+		const double vel_forward = ReadEnvVar("PX4_GZ_PLATFORM_VEL", 1.0);
+		const double heading_deg = ReadEnvVar("PX4_GZ_PLATFORM_HEADING_DEG", 0.0);
+		const double heading = GZ_DTOR(heading_deg);
+
+		_orientation_sp = gz::math::Quaterniond(0., 0., heading);
+
+		// Set initial orientation to match the heading
+		const auto optional_original_pose = _link.WorldPose(ecm);
+		if (optional_original_pose.has_value()) {
+			const gz::math::Pose3d original_pose = optional_original_pose.value();
+			const gz::math::Pose3d new_pose(original_pose.Pos(), _orientation_sp);
+			_model.SetWorldPoseCmd(ecm, new_pose);
+
+		} else {
+			gzwarn << "Unable to get initial platform pose. Heading will not be set" << std::endl;
+		}
+
+		// Velocity setpoint in world frame.
+		const gz::math::Vector3d _body_velocity_sp(vel_forward, 0., 0.);
+		_velocity_sp = _orientation_sp * _body_velocity_sp;
+	}
+
+	// Get gravity, model mass, platform height.
+	{
+		const auto world_entity = gz::sim::worldEntity(ecm);
+		const auto world = gz::sim::World(world_entity);
+		const auto gravity = world.Gravity(ecm);
+
+		if (gravity.has_value()) {
+			_gravity = world.Gravity(ecm).value().Z();
+		} else {
+			gzwarn << "Unable to get gazebo world gravity. Keeping default of " << _gravity << std::endl;
+		}
+
+		auto inertial_component = ecm.Component<gz::sim::components::Inertial>(_link_entity);
+
+		if (inertial_component) {
+			_platform_mass = inertial_component->Data().MassMatrix().Mass();
+		} else {
+			gzwarn << "Unable to get inertial component for link " << link_name << ". Keeping default mass of " << _platform_mass << std::endl;
+		}
+
+		auto pose_component = ecm.Component<gz::sim::components::Pose>(_link_entity);
+
+		if (pose_component) {
+			_platform_height_setpoint = pose_component->Data().Z();
+		} else {
+			gzwarn << "Unable to get inertial component for link " << link_name << ". Keeping default mass of " << _platform_mass << std::endl;
+		}
+	}
+
+}
+
+void MovingPlatformController::PreUpdate(const gz::sim::UpdateInfo &_info, gz::sim::EntityComponentManager &ecm)
+{
+	updatePlatformState(ecm);
+	updateWrenchCommand(_velocity_sp, _orientation_sp);
+	sendWrenchCommand(ecm);
+}
+
+void MovingPlatformController::updateWrenchCommand(
+	const gz::math::Vector3d &velocity_setpoint,
+	const gz::math::Quaterniond &orientation_setpoint)
+{
+	// (force, torque) =
+	//    constant normal force / buoyancy term offsetting gravity
+	//  + low-pass filtered white noise representing waves, road noise, etc.
+	//  + PD feedback term representing stable vehicle design
+
+	gz::math::Vector3d noise_force = gz::math::Vector3d(
+			gz::math::Rand::DblNormal(),
+			gz::math::Rand::DblNormal(),
+			gz::math::Rand::DblNormal()
+					  );
+
+	gz::math::Vector3d noise_torque = gz::math::Vector3d(
+			gz::math::Rand::DblNormal(),
+			gz::math::Rand::DblNormal(),
+			gz::math::Rand::DblNormal()
+					  );
+
+	// Filter coefficients for low-pass filtering the white noise.
+	// larger number = faster movement
+	// should be between 0 and 1
+	const double filter_coef_force = 0.1;
+	const double filter_coef_torque = 0.1;
+
+	// Noise amplitude.
+	// larger number = bigger movement
+	// should be >= 0
+	const double noise_ampl_force = 1. * _platform_mass;
+	const double noise_ampl_torque = 1. * _platform_mass;
+
+	_noise_lowpass_force = (1 - filter_coef_force) * _noise_lowpass_force + filter_coef_force * noise_force;
path: src/modules/simulation/gz_plugins/moving_platform_controller/MovingPlatformController.cpp
user: login: mbjd
body: Yes, thought about that briefly too. I chose not to because I remember some discussion about potentially moving the gazebo plugins into the PX4-gazebo-models repo, in which case I'm not sure if we can and should have it rely on PX4 stuff. 
created_at: 2025-04-02T07:00:01Z
author_association: CONTRIBUTOR
diff_hunk: @@ -0,0 +1,276 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+#include "MovingPlatformController.hpp"
+
+using namespace custom;
+
+// Register the plugin
+GZ_ADD_PLUGIN(
+	MovingPlatformController,
+	gz::sim::System,
+	gz::sim::ISystemPreUpdate,
+	gz::sim::ISystemConfigure
+)
+
+void MovingPlatformController::Configure(const gz::sim::Entity &entity,
+		const std::shared_ptr<const sdf::Element> &sdf,
+		gz::sim::EntityComponentManager &ecm,
+		gz::sim::EventManager &eventMgr)
+{
+	_entity = entity;
+	_model = gz::sim::Model(entity);
+
+	const std::string link_name = sdf->Get<std::string>("link_name");
+	_link_entity = _model.LinkByName(ecm, link_name);
+
+	if (!_link_entity) {
+		throw std::runtime_error("MovingPlatformController::Configure: Link \"" + link_name + "\" was not found. "
+			"Please ensure that your model contains the corresponding link.");
+	}
+
+	_link = gz::sim::Link(_link_entity);
+
+	// Needed to report linear & angular velocity
+	_link.EnableVelocityChecks(ecm, true);
+
+	// Get velocity and orientation setpoints from env vars
+	{
+		const double vel_forward = ReadEnvVar("PX4_GZ_PLATFORM_VEL", 1.0);
+		const double heading_deg = ReadEnvVar("PX4_GZ_PLATFORM_HEADING_DEG", 0.0);
+		const double heading = GZ_DTOR(heading_deg);
+
+		_orientation_sp = gz::math::Quaterniond(0., 0., heading);
+
+		// Set initial orientation to match the heading
+		const auto optional_original_pose = _link.WorldPose(ecm);
+		if (optional_original_pose.has_value()) {
+			const gz::math::Pose3d original_pose = optional_original_pose.value();
+			const gz::math::Pose3d new_pose(original_pose.Pos(), _orientation_sp);
+			_model.SetWorldPoseCmd(ecm, new_pose);
+
+		} else {
+			gzwarn << "Unable to get initial platform pose. Heading will not be set" << std::endl;
+		}
+
+		// Velocity setpoint in world frame.
+		const gz::math::Vector3d _body_velocity_sp(vel_forward, 0., 0.);
+		_velocity_sp = _orientation_sp * _body_velocity_sp;
+	}
+
+	// Get gravity, model mass, platform height.
+	{
+		const auto world_entity = gz::sim::worldEntity(ecm);
+		const auto world = gz::sim::World(world_entity);
+		const auto gravity = world.Gravity(ecm);
+
+		if (gravity.has_value()) {
+			_gravity = world.Gravity(ecm).value().Z();
+		} else {
+			gzwarn << "Unable to get gazebo world gravity. Keeping default of " << _gravity << std::endl;
+		}
+
+		auto inertial_component = ecm.Component<gz::sim::components::Inertial>(_link_entity);
+
+		if (inertial_component) {
+			_platform_mass = inertial_component->Data().MassMatrix().Mass();
+		} else {
+			gzwarn << "Unable to get inertial component for link " << link_name << ". Keeping default mass of " << _platform_mass << std::endl;
+		}
+
+		auto pose_component = ecm.Component<gz::sim::components::Pose>(_link_entity);
+
+		if (pose_component) {
+			_platform_height_setpoint = pose_component->Data().Z();
+		} else {
+			gzwarn << "Unable to get inertial component for link " << link_name << ". Keeping default mass of " << _platform_mass << std::endl;
+		}
+	}
+
+}
+
+void MovingPlatformController::PreUpdate(const gz::sim::UpdateInfo &_info, gz::sim::EntityComponentManager &ecm)
+{
+	updatePlatformState(ecm);
+	updateWrenchCommand(_velocity_sp, _orientation_sp);
+	sendWrenchCommand(ecm);
+}
+
+void MovingPlatformController::updateWrenchCommand(
+	const gz::math::Vector3d &velocity_setpoint,
+	const gz::math::Quaterniond &orientation_setpoint)
+{
+	// (force, torque) =
+	//    constant normal force / buoyancy term offsetting gravity
+	//  + low-pass filtered white noise representing waves, road noise, etc.
+	//  + PD feedback term representing stable vehicle design
+
+	gz::math::Vector3d noise_force = gz::math::Vector3d(
+			gz::math::Rand::DblNormal(),
+			gz::math::Rand::DblNormal(),
+			gz::math::Rand::DblNormal()
+					  );
+
+	gz::math::Vector3d noise_torque = gz::math::Vector3d(
+			gz::math::Rand::DblNormal(),
+			gz::math::Rand::DblNormal(),
+			gz::math::Rand::DblNormal()
+					  );
+
+	// Filter coefficients for low-pass filtering the white noise.
+	// larger number = faster movement
+	// should be between 0 and 1
+	const double filter_coef_force = 0.1;
+	const double filter_coef_torque = 0.1;
+
+	// Noise amplitude.
+	// larger number = bigger movement
+	// should be >= 0
+	const double noise_ampl_force = 1. * _platform_mass;
+	const double noise_ampl_torque = 1. * _platform_mass;
+
+	_noise_lowpass_force = (1 - filter_coef_force) * _noise_lowpass_force + filter_coef_force * noise_force;
path: src/modules/simulation/gz_plugins/moving_platform_controller/MovingPlatformController.cpp
user: login: perrrre
body: make sense!
created_at: 2025-04-02T07:31:13Z
author_association: CONTRIBUTOR
diff_hunk: @@ -0,0 +1,276 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+#include "MovingPlatformController.hpp"
+
+using namespace custom;
+
+// Register the plugin
+GZ_ADD_PLUGIN(
+	MovingPlatformController,
+	gz::sim::System,
+	gz::sim::ISystemPreUpdate,
+	gz::sim::ISystemConfigure
+)
+
+void MovingPlatformController::Configure(const gz::sim::Entity &entity,
+		const std::shared_ptr<const sdf::Element> &sdf,
+		gz::sim::EntityComponentManager &ecm,
+		gz::sim::EventManager &eventMgr)
+{
+	_entity = entity;
+	_model = gz::sim::Model(entity);
+
+	const std::string link_name = sdf->Get<std::string>("link_name");
+	_link_entity = _model.LinkByName(ecm, link_name);
+
+	if (!_link_entity) {
+		throw std::runtime_error("MovingPlatformController::Configure: Link \"" + link_name + "\" was not found. "
+			"Please ensure that your model contains the corresponding link.");
+	}
+
+	_link = gz::sim::Link(_link_entity);
+
+	// Needed to report linear & angular velocity
+	_link.EnableVelocityChecks(ecm, true);
+
+	// Get velocity and orientation setpoints from env vars
+	{
+		const double vel_forward = ReadEnvVar("PX4_GZ_PLATFORM_VEL", 1.0);
+		const double heading_deg = ReadEnvVar("PX4_GZ_PLATFORM_HEADING_DEG", 0.0);
+		const double heading = GZ_DTOR(heading_deg);
+
+		_orientation_sp = gz::math::Quaterniond(0., 0., heading);
+
+		// Set initial orientation to match the heading
+		const auto optional_original_pose = _link.WorldPose(ecm);
+		if (optional_original_pose.has_value()) {
+			const gz::math::Pose3d original_pose = optional_original_pose.value();
+			const gz::math::Pose3d new_pose(original_pose.Pos(), _orientation_sp);
+			_model.SetWorldPoseCmd(ecm, new_pose);
+
+		} else {
+			gzwarn << "Unable to get initial platform pose. Heading will not be set" << std::endl;
+		}
+
+		// Velocity setpoint in world frame.
+		const gz::math::Vector3d _body_velocity_sp(vel_forward, 0., 0.);
+		_velocity_sp = _orientation_sp * _body_velocity_sp;
+	}
+
+	// Get gravity, model mass, platform height.
+	{
+		const auto world_entity = gz::sim::worldEntity(ecm);
+		const auto world = gz::sim::World(world_entity);
+		const auto gravity = world.Gravity(ecm);
+
+		if (gravity.has_value()) {
+			_gravity = world.Gravity(ecm).value().Z();
+		} else {
+			gzwarn << "Unable to get gazebo world gravity. Keeping default of " << _gravity << std::endl;
+		}
+
+		auto inertial_component = ecm.Component<gz::sim::components::Inertial>(_link_entity);
+
+		if (inertial_component) {
+			_platform_mass = inertial_component->Data().MassMatrix().Mass();
+		} else {
+			gzwarn << "Unable to get inertial component for link " << link_name << ". Keeping default mass of " << _platform_mass << std::endl;
+		}
+
+		auto pose_component = ecm.Component<gz::sim::components::Pose>(_link_entity);
+
+		if (pose_component) {
+			_platform_height_setpoint = pose_component->Data().Z();
+		} else {
+			gzwarn << "Unable to get pose component for link " << link_name << ". Keeping default platform height setpoint of " << _platform_height_setpoint << std::endl;
+		}
+	}
+
+}
+
+void MovingPlatformController::PreUpdate(const gz::sim::UpdateInfo &_info, gz::sim::EntityComponentManager &ecm)
+{
+	updatePlatformState(ecm);
+	updateWrenchCommand(_velocity_sp, _orientation_sp);
+	sendWrenchCommand(ecm);
+}
+
+void MovingPlatformController::updateWrenchCommand(
+	const gz::math::Vector3d &velocity_setpoint,
+	const gz::math::Quaterniond &orientation_setpoint)
+{
+	// (force, torque) =
+	//    constant normal force / buoyancy term offsetting gravity
+	//  + low-pass filtered white noise representing waves, road noise, etc.
+	//  + PD feedback term representing stable vehicle design
+
+	gz::math::Vector3d noise_force = gz::math::Vector3d(
+			gz::math::Rand::DblNormal(),
+			gz::math::Rand::DblNormal(),
+			gz::math::Rand::DblNormal()
+					  );
+
+	gz::math::Vector3d noise_torque = gz::math::Vector3d(
+			gz::math::Rand::DblNormal(),
+			gz::math::Rand::DblNormal(),
+			gz::math::Rand::DblNormal()
+					  );
+
+	// Filter coefficients for low-pass filtering the white noise.
+	// larger number = faster movement
+	// should be between 0 and 1
+	const double filter_coef_force = 0.1;
+	const double filter_coef_torque = 0.1;
+
+	// Noise amplitude.
+	// larger number = bigger movement
+	// should be >= 0
+	const double noise_ampl_force = 1. * _platform_mass;
+	const double noise_ampl_torque = 1. * _platform_mass;
+
+	_noise_lowpass_force = (1 - filter_coef_force) * _noise_lowpass_force + filter_coef_force * noise_force;
+	_noise_lowpass_torque = (1 - filter_coef_torque) * _noise_lowpass_torque + filter_coef_torque * noise_torque;
+
+	const gz::math::Vector3d normal_force(0., 0., -_gravity * _platform_mass);
+
+	_force = noise_ampl_force * _noise_lowpass_force + normal_force;
+	_torque = noise_ampl_torque * _noise_lowpass_torque;
+
+	// Feedback terms to ensure stability of the platform.
+	{
+
+		const gz::math::Vector3d pos_gains = _platform_mass * gz::math::Vector3d(0., 0., 1.);
path: src/modules/simulation/gz_plugins/moving_platform_controller/MovingPlatformController.cpp
user: login: perrrre
body: would be nice with units to simplify the reading 
created_at: 2025-04-02T07:56:23Z
author_association: CONTRIBUTOR
diff_hunk: @@ -0,0 +1,276 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+#include "MovingPlatformController.hpp"
+
+using namespace custom;
+
+// Register the plugin
+GZ_ADD_PLUGIN(
+	MovingPlatformController,
+	gz::sim::System,
+	gz::sim::ISystemPreUpdate,
+	gz::sim::ISystemConfigure
+)
+
+void MovingPlatformController::Configure(const gz::sim::Entity &entity,
+		const std::shared_ptr<const sdf::Element> &sdf,
+		gz::sim::EntityComponentManager &ecm,
+		gz::sim::EventManager &eventMgr)
+{
+	_entity = entity;
+	_model = gz::sim::Model(entity);
+
+	const std::string link_name = sdf->Get<std::string>("link_name");
+	_link_entity = _model.LinkByName(ecm, link_name);
+
+	if (!_link_entity) {
+		throw std::runtime_error("MovingPlatformController::Configure: Link \"" + link_name + "\" was not found. "
+			"Please ensure that your model contains the corresponding link.");
+	}
+
+	_link = gz::sim::Link(_link_entity);
+
+	// Needed to report linear & angular velocity
+	_link.EnableVelocityChecks(ecm, true);
+
+	// Get velocity and orientation setpoints from env vars
+	{
+		const double vel_forward = ReadEnvVar("PX4_GZ_PLATFORM_VEL", 1.0);
+		const double heading_deg = ReadEnvVar("PX4_GZ_PLATFORM_HEADING_DEG", 0.0);
+		const double heading = GZ_DTOR(heading_deg);
+
+		_orientation_sp = gz::math::Quaterniond(0., 0., heading);
+
+		// Set initial orientation to match the heading
+		const auto optional_original_pose = _link.WorldPose(ecm);
+		if (optional_original_pose.has_value()) {
+			const gz::math::Pose3d original_pose = optional_original_pose.value();
+			const gz::math::Pose3d new_pose(original_pose.Pos(), _orientation_sp);
+			_model.SetWorldPoseCmd(ecm, new_pose);
+
+		} else {
+			gzwarn << "Unable to get initial platform pose. Heading will not be set" << std::endl;
+		}
+
+		// Velocity setpoint in world frame.
+		const gz::math::Vector3d _body_velocity_sp(vel_forward, 0., 0.);
+		_velocity_sp = _orientation_sp * _body_velocity_sp;
+	}
+
+	// Get gravity, model mass, platform height.
+	{
+		const auto world_entity = gz::sim::worldEntity(ecm);
+		const auto world = gz::sim::World(world_entity);
+		const auto gravity = world.Gravity(ecm);
+
+		if (gravity.has_value()) {
+			_gravity = world.Gravity(ecm).value().Z();
+		} else {
+			gzwarn << "Unable to get gazebo world gravity. Keeping default of " << _gravity << std::endl;
+		}
+
+		auto inertial_component = ecm.Component<gz::sim::components::Inertial>(_link_entity);
+
+		if (inertial_component) {
+			_platform_mass = inertial_component->Data().MassMatrix().Mass();
+		} else {
+			gzwarn << "Unable to get inertial component for link " << link_name << ". Keeping default mass of " << _platform_mass << std::endl;
+		}
+
+		auto pose_component = ecm.Component<gz::sim::components::Pose>(_link_entity);
+
+		if (pose_component) {
+			_platform_height_setpoint = pose_component->Data().Z();
+		} else {
+			gzwarn << "Unable to get pose component for link " << link_name << ". Keeping default platform height setpoint of " << _platform_height_setpoint << std::endl;
+		}
+	}
+
+}
+
+void MovingPlatformController::PreUpdate(const gz::sim::UpdateInfo &_info, gz::sim::EntityComponentManager &ecm)
+{
+	updatePlatformState(ecm);
+	updateWrenchCommand(_velocity_sp, _orientation_sp);
+	sendWrenchCommand(ecm);
+}
+
+void MovingPlatformController::updateWrenchCommand(
+	const gz::math::Vector3d &velocity_setpoint,
+	const gz::math::Quaterniond &orientation_setpoint)
+{
+	// (force, torque) =
+	//    constant normal force / buoyancy term offsetting gravity
+	//  + low-pass filtered white noise representing waves, road noise, etc.
+	//  + PD feedback term representing stable vehicle design
+
+	gz::math::Vector3d noise_force = gz::math::Vector3d(
+			gz::math::Rand::DblNormal(),
+			gz::math::Rand::DblNormal(),
+			gz::math::Rand::DblNormal()
+					  );
+
+	gz::math::Vector3d noise_torque = gz::math::Vector3d(
+			gz::math::Rand::DblNormal(),
+			gz::math::Rand::DblNormal(),
+			gz::math::Rand::DblNormal()
+					  );
+
+	// Filter coefficients for low-pass filtering the white noise.
+	// larger number = faster movement
+	// should be between 0 and 1
+	const double filter_coef_force = 0.1;
+	const double filter_coef_torque = 0.1;
+
+	// Noise amplitude.
+	// larger number = bigger movement
+	// should be >= 0
+	const double noise_ampl_force = 1. * _platform_mass;
+	const double noise_ampl_torque = 1. * _platform_mass;
+
+	_noise_lowpass_force = (1 - filter_coef_force) * _noise_lowpass_force + filter_coef_force * noise_force;
+	_noise_lowpass_torque = (1 - filter_coef_torque) * _noise_lowpass_torque + filter_coef_torque * noise_torque;
+
+	const gz::math::Vector3d normal_force(0., 0., -_gravity * _platform_mass);
+
+	_force = noise_ampl_force * _noise_lowpass_force + normal_force;
+	_torque = noise_ampl_torque * _noise_lowpass_torque;
+
+	// Feedback terms to ensure stability of the platform.
+	{
+
+		const gz::math::Vector3d pos_gains = _platform_mass * gz::math::Vector3d(0., 0., 1.);
path: src/modules/simulation/gz_plugins/moving_platform_controller/MovingPlatformController.cpp
user: login: mbjd
body: addressed in latest commit :) 
created_at: 2025-04-02T11:57:05Z
author_association: CONTRIBUTOR
diff_hunk: @@ -0,0 +1,197 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+#include "MovingPlatformController.hpp"
+
+using namespace custom;
+
+// Register the plugin
+GZ_ADD_PLUGIN(
+	MovingPlatformController,
+	gz::sim::System,
+	gz::sim::ISystemPreUpdate,
+	gz::sim::ISystemConfigure
+)
+
+void MovingPlatformController::Configure(const gz::sim::Entity &entity,
+		const std::shared_ptr<const sdf::Element> &sdf,
+		gz::sim::EntityComponentManager &ecm,
+		gz::sim::EventManager &eventMgr)
+{
+	_entity = entity;
+	_model = gz::sim::Model(entity);
+
+	double default_mean_vel = 1.0f;
+	double v_x = default_mean_vel;
+
+	// Read v_x from environment variable.
+	{
+		const char *env_var_name = "PX4_GZ_PLATFORM_VEL";
+		const char *env_var_value = std::getenv(env_var_name);
+
+		if (env_var_value) {
+			try {
+				v_x = std::stof(env_var_value);
+
+			} catch (const std::invalid_argument &e) {
+				// These warnings will only be visible with sufficient verbosity level...
+				gzwarn << "Invalid argument: " << env_var_value << " is not a valid double for " << env_var_name << std::endl;
+				gzwarn << "Keeping default value of " << v_x << " m/s." << std::endl;
+
+			} catch (const std::out_of_range &e) {
+				gzwarn << "Out of range: " << env_var_value << " is out of range for " << env_var_name << std::endl;
+				gzwarn << "Keeping default value of " << v_x << " m/s." << std::endl;
+			}
+		}
+	}
+
+	_target_vel_sp = gz::math::Vector3d(v_x, 0., 0.);
+
+	// Advertise topic to communicate with VelocityControl plugin
+	std::string model_name = _model.Name(ecm);
+	std::string cmd_vel_topic = "/model/" + model_name + "/cmd_vel";
+	_platform_twist_pub = _node.Advertise<gz::msgs::Twist>(cmd_vel_topic);
+}
+
+void MovingPlatformController::PreUpdate(const gz::sim::UpdateInfo &_info, gz::sim::EntityComponentManager &ecm)
+{
+	updatePose(ecm);
+
+	// Initially, ramp up the velocity with constant acceleration. Otherwise
+	// we can't really simulate fast velocities as the drone just slips off
+	// the platform.
+
+	double max_accel_mag = 2.; // m/s^2
+	double dt = std::chrono::duration<double>(_info.dt).count(); // s
+
+	const gz::math::Vector3d vel_sp_err = _target_vel_sp - _current_vel_sp;
+
+	if (vel_sp_err.Length() < max_accel_mag * dt) {
+		// we are already almost there. set directly to avoid division by 0.
+		_current_vel_sp = _target_vel_sp;
+	} else {
+		// acclerate towards target velocity.
+		const gz::math::Vector3d accel = vel_sp_err.Normalized() * max_accel_mag;
+		_current_vel_sp = _current_vel_sp + accel * dt;
+	}
+
+	updateVelocityCommands(_current_vel_sp);
+
+	sendVelocityCommands();
+}
+
+void MovingPlatformController::updateVelocityCommands(const gz::math::Vector3d &velocity_setpoint)
+{
+	// 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 + velocity_setpoint;
+	_platform_w = ampl_w * _noise_w_lowpass;
+
+	// feedback terms to ensure the random walk (= integral of noise)
+	// stays within a realistic region.
+	{
+		// 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;
path: src/modules/simulation/gz_plugins/moving_platform_controller/MovingPlatformController.cpp
user: login: mbjd
body: @Jaeyoung-Lim I Adressed this in the newest commits, could you re-review? thanks :) 
created_at: 2025-04-02T12:58:02Z
author_association: CONTRIBUTOR
diff_hunk: @@ -0,0 +1,324 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+#include "MovingPlatformController.hpp"
+
+using namespace custom;
+
+// Register the plugin
+GZ_ADD_PLUGIN(
+	MovingPlatformController,
+	gz::sim::System,
+	gz::sim::ISystemPreUpdate,
+	gz::sim::ISystemConfigure
+)
+
+void MovingPlatformController::Configure(const gz::sim::Entity &entity,
+		const std::shared_ptr<const sdf::Element> &sdf,
+		gz::sim::EntityComponentManager &ecm,
+		gz::sim::EventManager &eventMgr)
+{
+	_entity = entity;
+	_model = gz::sim::Model(entity);
+
+	const std::string link_name = sdf->Get<std::string>("link_name");
+	_link_entity = _model.LinkByName(ecm, link_name);
+
+	if (!_link_entity) {
+		throw std::runtime_error("MovingPlatformController::Configure: Link \"" + link_name + "\" was not found. "
+					 "Please ensure that your model contains the corresponding link.");
+	}
+
+	_link = gz::sim::Link(_link_entity);
+
+	// Needed to report linear & angular velocity
+	_link.EnableVelocityChecks(ecm, true);
+
+	_startup_timer = gz::common::Timer();
+	_startup_timer.Start();
+
+	// Get velocity and orientation setpoints from env vars
+	{
+		const double vel_forward = ReadEnvVar("PX4_GZ_PLATFORM_VEL", 1.0);
+		const double heading_deg = ReadEnvVar("PX4_GZ_PLATFORM_HEADING_DEG", 0.0);
+		const double heading = GZ_DTOR(heading_deg);
+
+		_orientation_sp = gz::math::Quaterniond(0., 0., heading);
+
+		// Set initial orientation to match the heading
+		const auto optional_original_pose = _link.WorldPose(ecm);
+
+		if (optional_original_pose.has_value()) {
+			const gz::math::Pose3d original_pose = optional_original_pose.value();
+			const gz::math::Pose3d new_pose(original_pose.Pos(), _orientation_sp);
+			_model.SetWorldPoseCmd(ecm, new_pose);
+
+		} else {
+			gzwarn << "Unable to get initial platform pose. Heading will not be set" << std::endl;
+		}
+
+		// Velocity setpoint in world frame.
+		const gz::math::Vector3d _body_velocity_sp(vel_forward, 0., 0.);
+		_velocity_sp = _orientation_sp * _body_velocity_sp;
+	}
+
+	// Get gravity, model mass, platform height.
+	{
+		const auto world_entity = gz::sim::worldEntity(ecm);
+		const auto world = gz::sim::World(world_entity);
+		const auto gravity = world.Gravity(ecm);
+
+		if (gravity.has_value()) {
+			_gravity = gravity.value().Z();
+
+		} else {
+			gzwarn << "Unable to get gazebo world gravity. Keeping default of " << _gravity << std::endl;
+		}
+
+		auto inertial_component = ecm.Component<gz::sim::components::Inertial>(_link_entity);
+
+		if (inertial_component) {
+			_platform_mass = inertial_component->Data().MassMatrix().Mass();
+			_platform_diag_moments = inertial_component->Data().MassMatrix().DiagonalMoments();
+
+		} else {
+			gzwarn << "Unable to get inertial component for link " << link_name << ". Keeping default mass of " << _platform_mass <<
+			       std::endl;
+		}
+
+		auto pose_component = ecm.Component<gz::sim::components::Pose>(_link_entity);
+
+		if (pose_component) {
+			_platform_height_setpoint = pose_component->Data().Z();
+
+		} else {
+			gzwarn << "Unable to get pose component for link " << link_name << ". Keeping default platform height setpoint of " <<
+			       _platform_height_setpoint << std::endl;
+		}
+	}
+
+}
+
+void MovingPlatformController::PreUpdate(const gz::sim::UpdateInfo &_info, gz::sim::EntityComponentManager &ecm)
+{
+	getPlatformState(ecm);
+
+	const double dt_sec = std::chrono::duration<double>(_info.dt).count();
+	updateNoise(dt_sec);
+
+	// Wait for model to spawn, takes quite some time now...
+	const bool keep_stationary = _startup_timer.ElapsedTime() < 5s;
path: src/modules/simulation/gz_plugins/moving_platform_controller/MovingPlatformController.cpp
user: login: KonradRudin
body: Can you quickly explain why you need to keep it stationary for 5s?
created_at: 2025-04-04T08:16:21Z
author_association: CONTRIBUTOR
diff_hunk: @@ -0,0 +1,324 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+#include "MovingPlatformController.hpp"
+
+using namespace custom;
+
+// Register the plugin
+GZ_ADD_PLUGIN(
+	MovingPlatformController,
+	gz::sim::System,
+	gz::sim::ISystemPreUpdate,
+	gz::sim::ISystemConfigure
+)
+
+void MovingPlatformController::Configure(const gz::sim::Entity &entity,
+		const std::shared_ptr<const sdf::Element> &sdf,
+		gz::sim::EntityComponentManager &ecm,
+		gz::sim::EventManager &eventMgr)
+{
+	_entity = entity;
+	_model = gz::sim::Model(entity);
+
+	const std::string link_name = sdf->Get<std::string>("link_name");
+	_link_entity = _model.LinkByName(ecm, link_name);
+
+	if (!_link_entity) {
+		throw std::runtime_error("MovingPlatformController::Configure: Link \"" + link_name + "\" was not found. "
+					 "Please ensure that your model contains the corresponding link.");
+	}
+
+	_link = gz::sim::Link(_link_entity);
+
+	// Needed to report linear & angular velocity
+	_link.EnableVelocityChecks(ecm, true);
+
+	_startup_timer = gz::common::Timer();
+	_startup_timer.Start();
+
+	// Get velocity and orientation setpoints from env vars
+	{
+		const double vel_forward = ReadEnvVar("PX4_GZ_PLATFORM_VEL", 1.0);
+		const double heading_deg = ReadEnvVar("PX4_GZ_PLATFORM_HEADING_DEG", 0.0);
+		const double heading = GZ_DTOR(heading_deg);
+
+		_orientation_sp = gz::math::Quaterniond(0., 0., heading);
+
+		// Set initial orientation to match the heading
+		const auto optional_original_pose = _link.WorldPose(ecm);
+
+		if (optional_original_pose.has_value()) {
+			const gz::math::Pose3d original_pose = optional_original_pose.value();
+			const gz::math::Pose3d new_pose(original_pose.Pos(), _orientation_sp);
+			_model.SetWorldPoseCmd(ecm, new_pose);
+
+		} else {
+			gzwarn << "Unable to get initial platform pose. Heading will not be set" << std::endl;
+		}
+
+		// Velocity setpoint in world frame.
+		const gz::math::Vector3d _body_velocity_sp(vel_forward, 0., 0.);
+		_velocity_sp = _orientation_sp * _body_velocity_sp;
+	}
+
+	// Get gravity, model mass, platform height.
+	{
+		const auto world_entity = gz::sim::worldEntity(ecm);
+		const auto world = gz::sim::World(world_entity);
+		const auto gravity = world.Gravity(ecm);
+
+		if (gravity.has_value()) {
+			_gravity = gravity.value().Z();
+
+		} else {
+			gzwarn << "Unable to get gazebo world gravity. Keeping default of " << _gravity << std::endl;
+		}
+
+		auto inertial_component = ecm.Component<gz::sim::components::Inertial>(_link_entity);
+
+		if (inertial_component) {
+			_platform_mass = inertial_component->Data().MassMatrix().Mass();
+			_platform_diag_moments = inertial_component->Data().MassMatrix().DiagonalMoments();
+
+		} else {
+			gzwarn << "Unable to get inertial component for link " << link_name << ". Keeping default mass of " << _platform_mass <<
+			       std::endl;
+		}
+
+		auto pose_component = ecm.Component<gz::sim::components::Pose>(_link_entity);
+
+		if (pose_component) {
+			_platform_height_setpoint = pose_component->Data().Z();
+
+		} else {
+			gzwarn << "Unable to get pose component for link " << link_name << ". Keeping default platform height setpoint of " <<
+			       _platform_height_setpoint << std::endl;
+		}
+	}
+
+}
+
+void MovingPlatformController::PreUpdate(const gz::sim::UpdateInfo &_info, gz::sim::EntityComponentManager &ecm)
+{
+	getPlatformState(ecm);
+
+	const double dt_sec = std::chrono::duration<double>(_info.dt).count();
+	updateNoise(dt_sec);
+
+	// Wait for model to spawn, takes quite some time now...
+	const bool keep_stationary = _startup_timer.ElapsedTime() < 5s;
+	updateWrenchCommand(_velocity_sp, _orientation_sp, keep_stationary);
+
+	sendWrenchCommand(ecm);
+}
+
+void MovingPlatformController::updateNoise(const double dt)
+{
+	gz::math::Vector3d noise_force = gz::math::Vector3d(
+			gz::math::Rand::DblNormal(),
+			gz::math::Rand::DblNormal(),
+			gz::math::Rand::DblNormal()
+					 );
+
+	gz::math::Vector3d noise_torque = gz::math::Vector3d(
+			gz::math::Rand::DblNormal(),
+			gz::math::Rand::DblNormal(),
+			gz::math::Rand::DblNormal()
+					  );
+
+	// Cutoff frequencies for low-pass filtering the white noise.
+	// larger number = faster movement
+	// Should be between 0 and sample_freq / 2, with sample_freq = 1/dt
+	const double cutoff_freq_force  = 5.;  // 1/s
+	const double cutoff_freq_torque = 5.;  // 1/s
+
+	const double max_cutoff_freq = 0.5 / dt;
+
+	if (cutoff_freq_force < 0 || cutoff_freq_force > max_cutoff_freq ||
+	    cutoff_freq_torque < 0 || cutoff_freq_torque > max_cutoff_freq) {
+		throw std::runtime_error("MovingPlatformController::updateWrenchCommand: invalid cutoff frequency");
+	}
+
+	// Cutoff frequency -> filter coefficient calculation from PX4's AlphaFilter.hpp
+	// from AlphaFilter::setCutoffFreq
+	const double time_constant_force = 1. / (2 * GZ_PI * cutoff_freq_force);
+	const double time_constant_torque = 1. / (2 * GZ_PI * cutoff_freq_torque);
+
+	// from AlphaFilter::setParameters
+	const double filter_coef_force = dt / (dt + time_constant_force);
+	const double filter_coef_torque = dt / (dt + time_constant_torque);
+
+	_noise_lowpass_force = (1. - filter_coef_force) * _noise_lowpass_force + filter_coef_force * noise_force;
path: src/modules/simulation/gz_plugins/moving_platform_controller/MovingPlatformController.cpp
user: login: KonradRudin
body: Nitpicking. This is more an aceleration and not a force, right?
created_at: 2025-04-04T08:16:55Z
author_association: CONTRIBUTOR
diff_hunk: @@ -0,0 +1,324 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+#include "MovingPlatformController.hpp"
+
+using namespace custom;
+
+// Register the plugin
+GZ_ADD_PLUGIN(
+	MovingPlatformController,
+	gz::sim::System,
+	gz::sim::ISystemPreUpdate,
+	gz::sim::ISystemConfigure
+)
+
+void MovingPlatformController::Configure(const gz::sim::Entity &entity,
+		const std::shared_ptr<const sdf::Element> &sdf,
+		gz::sim::EntityComponentManager &ecm,
+		gz::sim::EventManager &eventMgr)
+{
+	_entity = entity;
+	_model = gz::sim::Model(entity);
+
+	const std::string link_name = sdf->Get<std::string>("link_name");
+	_link_entity = _model.LinkByName(ecm, link_name);
+
+	if (!_link_entity) {
+		throw std::runtime_error("MovingPlatformController::Configure: Link \"" + link_name + "\" was not found. "
+					 "Please ensure that your model contains the corresponding link.");
+	}
+
+	_link = gz::sim::Link(_link_entity);
+
+	// Needed to report linear & angular velocity
+	_link.EnableVelocityChecks(ecm, true);
+
+	_startup_timer = gz::common::Timer();
+	_startup_timer.Start();
+
+	// Get velocity and orientation setpoints from env vars
+	{
+		const double vel_forward = ReadEnvVar("PX4_GZ_PLATFORM_VEL", 1.0);
+		const double heading_deg = ReadEnvVar("PX4_GZ_PLATFORM_HEADING_DEG", 0.0);
+		const double heading = GZ_DTOR(heading_deg);
+
+		_orientation_sp = gz::math::Quaterniond(0., 0., heading);
+
+		// Set initial orientation to match the heading
+		const auto optional_original_pose = _link.WorldPose(ecm);
+
+		if (optional_original_pose.has_value()) {
+			const gz::math::Pose3d original_pose = optional_original_pose.value();
+			const gz::math::Pose3d new_pose(original_pose.Pos(), _orientation_sp);
+			_model.SetWorldPoseCmd(ecm, new_pose);
+
+		} else {
+			gzwarn << "Unable to get initial platform pose. Heading will not be set" << std::endl;
+		}
+
+		// Velocity setpoint in world frame.
+		const gz::math::Vector3d _body_velocity_sp(vel_forward, 0., 0.);
+		_velocity_sp = _orientation_sp * _body_velocity_sp;
+	}
+
+	// Get gravity, model mass, platform height.
+	{
+		const auto world_entity = gz::sim::worldEntity(ecm);
+		const auto world = gz::sim::World(world_entity);
+		const auto gravity = world.Gravity(ecm);
+
+		if (gravity.has_value()) {
+			_gravity = gravity.value().Z();
+
+		} else {
+			gzwarn << "Unable to get gazebo world gravity. Keeping default of " << _gravity << std::endl;
+		}
+
+		auto inertial_component = ecm.Component<gz::sim::components::Inertial>(_link_entity);
+
+		if (inertial_component) {
+			_platform_mass = inertial_component->Data().MassMatrix().Mass();
+			_platform_diag_moments = inertial_component->Data().MassMatrix().DiagonalMoments();
+
+		} else {
+			gzwarn << "Unable to get inertial component for link " << link_name << ". Keeping default mass of " << _platform_mass <<
+			       std::endl;
+		}
+
+		auto pose_component = ecm.Component<gz::sim::components::Pose>(_link_entity);
+
+		if (pose_component) {
+			_platform_height_setpoint = pose_component->Data().Z();
+
+		} else {
+			gzwarn << "Unable to get pose component for link " << link_name << ". Keeping default platform height setpoint of " <<
+			       _platform_height_setpoint << std::endl;
+		}
+	}
+
+}
+
+void MovingPlatformController::PreUpdate(const gz::sim::UpdateInfo &_info, gz::sim::EntityComponentManager &ecm)
+{
+	getPlatformState(ecm);
+
+	const double dt_sec = std::chrono::duration<double>(_info.dt).count();
+	updateNoise(dt_sec);
+
+	// Wait for model to spawn, takes quite some time now...
+	const bool keep_stationary = _startup_timer.ElapsedTime() < 5s;
path: src/modules/simulation/gz_plugins/moving_platform_controller/MovingPlatformController.cpp
user: login: mbjd
body: because the model takes 5s to spawn. If we don't wait for it, the platform moves away before it spawns. 
created_at: 2025-04-04T08:33:12Z
author_association: CONTRIBUTOR
diff_hunk: @@ -0,0 +1,324 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+#include "MovingPlatformController.hpp"
+
+using namespace custom;
+
+// Register the plugin
+GZ_ADD_PLUGIN(
+	MovingPlatformController,
+	gz::sim::System,
+	gz::sim::ISystemPreUpdate,
+	gz::sim::ISystemConfigure
+)
+
+void MovingPlatformController::Configure(const gz::sim::Entity &entity,
+		const std::shared_ptr<const sdf::Element> &sdf,
+		gz::sim::EntityComponentManager &ecm,
+		gz::sim::EventManager &eventMgr)
+{
+	_entity = entity;
+	_model = gz::sim::Model(entity);
+
+	const std::string link_name = sdf->Get<std::string>("link_name");
+	_link_entity = _model.LinkByName(ecm, link_name);
+
+	if (!_link_entity) {
+		throw std::runtime_error("MovingPlatformController::Configure: Link \"" + link_name + "\" was not found. "
+					 "Please ensure that your model contains the corresponding link.");
+	}
+
+	_link = gz::sim::Link(_link_entity);
+
+	// Needed to report linear & angular velocity
+	_link.EnableVelocityChecks(ecm, true);
+
+	_startup_timer = gz::common::Timer();
+	_startup_timer.Start();
+
+	// Get velocity and orientation setpoints from env vars
+	{
+		const double vel_forward = ReadEnvVar("PX4_GZ_PLATFORM_VEL", 1.0);
+		const double heading_deg = ReadEnvVar("PX4_GZ_PLATFORM_HEADING_DEG", 0.0);
+		const double heading = GZ_DTOR(heading_deg);
+
+		_orientation_sp = gz::math::Quaterniond(0., 0., heading);
+
+		// Set initial orientation to match the heading
+		const auto optional_original_pose = _link.WorldPose(ecm);
+
+		if (optional_original_pose.has_value()) {
+			const gz::math::Pose3d original_pose = optional_original_pose.value();
+			const gz::math::Pose3d new_pose(original_pose.Pos(), _orientation_sp);
+			_model.SetWorldPoseCmd(ecm, new_pose);
+
+		} else {
+			gzwarn << "Unable to get initial platform pose. Heading will not be set" << std::endl;
+		}
+
+		// Velocity setpoint in world frame.
+		const gz::math::Vector3d _body_velocity_sp(vel_forward, 0., 0.);
+		_velocity_sp = _orientation_sp * _body_velocity_sp;
+	}
+
+	// Get gravity, model mass, platform height.
+	{
+		const auto world_entity = gz::sim::worldEntity(ecm);
+		const auto world = gz::sim::World(world_entity);
+		const auto gravity = world.Gravity(ecm);
+
+		if (gravity.has_value()) {
+			_gravity = gravity.value().Z();
+
+		} else {
+			gzwarn << "Unable to get gazebo world gravity. Keeping default of " << _gravity << std::endl;
+		}
+
+		auto inertial_component = ecm.Component<gz::sim::components::Inertial>(_link_entity);
+
+		if (inertial_component) {
+			_platform_mass = inertial_component->Data().MassMatrix().Mass();
+			_platform_diag_moments = inertial_component->Data().MassMatrix().DiagonalMoments();
+
+		} else {
+			gzwarn << "Unable to get inertial component for link " << link_name << ". Keeping default mass of " << _platform_mass <<
+			       std::endl;
+		}
+
+		auto pose_component = ecm.Component<gz::sim::components::Pose>(_link_entity);
+
+		if (pose_component) {
+			_platform_height_setpoint = pose_component->Data().Z();
+
+		} else {
+			gzwarn << "Unable to get pose component for link " << link_name << ". Keeping default platform height setpoint of " <<
+			       _platform_height_setpoint << std::endl;
+		}
+	}
+
+}
+
+void MovingPlatformController::PreUpdate(const gz::sim::UpdateInfo &_info, gz::sim::EntityComponentManager &ecm)
+{
+	getPlatformState(ecm);
+
+	const double dt_sec = std::chrono::duration<double>(_info.dt).count();
+	updateNoise(dt_sec);
+
+	// Wait for model to spawn, takes quite some time now...
+	const bool keep_stationary = _startup_timer.ElapsedTime() < 5s;
path: src/modules/simulation/gz_plugins/moving_platform_controller/MovingPlatformController.cpp
user: login: Jaeyoung-Lim
body: @mbjd Doing this based on time would not be reliable, as this would largely depend on the system. You can check if the entity exists through the gz API.
created_at: 2025-04-04T08:38:15Z
author_association: MEMBER
diff_hunk: @@ -0,0 +1,324 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+#include "MovingPlatformController.hpp"
+
+using namespace custom;
+
+// Register the plugin
+GZ_ADD_PLUGIN(
+	MovingPlatformController,
+	gz::sim::System,
+	gz::sim::ISystemPreUpdate,
+	gz::sim::ISystemConfigure
+)
+
+void MovingPlatformController::Configure(const gz::sim::Entity &entity,
+		const std::shared_ptr<const sdf::Element> &sdf,
+		gz::sim::EntityComponentManager &ecm,
+		gz::sim::EventManager &eventMgr)
+{
+	_entity = entity;
+	_model = gz::sim::Model(entity);
+
+	const std::string link_name = sdf->Get<std::string>("link_name");
+	_link_entity = _model.LinkByName(ecm, link_name);
+
+	if (!_link_entity) {
+		throw std::runtime_error("MovingPlatformController::Configure: Link \"" + link_name + "\" was not found. "
+					 "Please ensure that your model contains the corresponding link.");
+	}
+
+	_link = gz::sim::Link(_link_entity);
+
+	// Needed to report linear & angular velocity
+	_link.EnableVelocityChecks(ecm, true);
+
+	_startup_timer = gz::common::Timer();
+	_startup_timer.Start();
+
+	// Get velocity and orientation setpoints from env vars
+	{
+		const double vel_forward = ReadEnvVar("PX4_GZ_PLATFORM_VEL", 1.0);
+		const double heading_deg = ReadEnvVar("PX4_GZ_PLATFORM_HEADING_DEG", 0.0);
+		const double heading = GZ_DTOR(heading_deg);
+
+		_orientation_sp = gz::math::Quaterniond(0., 0., heading);
+
+		// Set initial orientation to match the heading
+		const auto optional_original_pose = _link.WorldPose(ecm);
+
+		if (optional_original_pose.has_value()) {
+			const gz::math::Pose3d original_pose = optional_original_pose.value();
+			const gz::math::Pose3d new_pose(original_pose.Pos(), _orientation_sp);
+			_model.SetWorldPoseCmd(ecm, new_pose);
+
+		} else {
+			gzwarn << "Unable to get initial platform pose. Heading will not be set" << std::endl;
+		}
+
+		// Velocity setpoint in world frame.
+		const gz::math::Vector3d _body_velocity_sp(vel_forward, 0., 0.);
+		_velocity_sp = _orientation_sp * _body_velocity_sp;
+	}
+
+	// Get gravity, model mass, platform height.
+	{
+		const auto world_entity = gz::sim::worldEntity(ecm);
+		const auto world = gz::sim::World(world_entity);
+		const auto gravity = world.Gravity(ecm);
+
+		if (gravity.has_value()) {
+			_gravity = gravity.value().Z();
+
+		} else {
+			gzwarn << "Unable to get gazebo world gravity. Keeping default of " << _gravity << std::endl;
+		}
+
+		auto inertial_component = ecm.Component<gz::sim::components::Inertial>(_link_entity);
+
+		if (inertial_component) {
+			_platform_mass = inertial_component->Data().MassMatrix().Mass();
+			_platform_diag_moments = inertial_component->Data().MassMatrix().DiagonalMoments();
+
+		} else {
+			gzwarn << "Unable to get inertial component for link " << link_name << ". Keeping default mass of " << _platform_mass <<
+			       std::endl;
+		}
+
+		auto pose_component = ecm.Component<gz::sim::components::Pose>(_link_entity);
+
+		if (pose_component) {
+			_platform_height_setpoint = pose_component->Data().Z();
+
+		} else {
+			gzwarn << "Unable to get pose component for link " << link_name << ". Keeping default platform height setpoint of " <<
+			       _platform_height_setpoint << std::endl;
+		}
+	}
+
+}
+
+void MovingPlatformController::PreUpdate(const gz::sim::UpdateInfo &_info, gz::sim::EntityComponentManager &ecm)
+{
+	getPlatformState(ecm);
+
+	const double dt_sec = std::chrono::duration<double>(_info.dt).count();
+	updateNoise(dt_sec);
+
+	// Wait for model to spawn, takes quite some time now...
+	const bool keep_stationary = _startup_timer.ElapsedTime() < 5s;
+	updateWrenchCommand(_velocity_sp, _orientation_sp, keep_stationary);
+
+	sendWrenchCommand(ecm);
+}
+
+void MovingPlatformController::updateNoise(const double dt)
+{
+	gz::math::Vector3d noise_force = gz::math::Vector3d(
+			gz::math::Rand::DblNormal(),
+			gz::math::Rand::DblNormal(),
+			gz::math::Rand::DblNormal()
+					 );
+
+	gz::math::Vector3d noise_torque = gz::math::Vector3d(
+			gz::math::Rand::DblNormal(),
+			gz::math::Rand::DblNormal(),
+			gz::math::Rand::DblNormal()
+					  );
+
+	// Cutoff frequencies for low-pass filtering the white noise.
+	// larger number = faster movement
+	// Should be between 0 and sample_freq / 2, with sample_freq = 1/dt
+	const double cutoff_freq_force  = 5.;  // 1/s
+	const double cutoff_freq_torque = 5.;  // 1/s
+
+	const double max_cutoff_freq = 0.5 / dt;
+
+	if (cutoff_freq_force < 0 || cutoff_freq_force > max_cutoff_freq ||
+	    cutoff_freq_torque < 0 || cutoff_freq_torque > max_cutoff_freq) {
+		throw std::runtime_error("MovingPlatformController::updateWrenchCommand: invalid cutoff frequency");
+	}
+
+	// Cutoff frequency -> filter coefficient calculation from PX4's AlphaFilter.hpp
+	// from AlphaFilter::setCutoffFreq
+	const double time_constant_force = 1. / (2 * GZ_PI * cutoff_freq_force);
+	const double time_constant_torque = 1. / (2 * GZ_PI * cutoff_freq_torque);
+
+	// from AlphaFilter::setParameters
+	const double filter_coef_force = dt / (dt + time_constant_force);
+	const double filter_coef_torque = dt / (dt + time_constant_torque);
+
+	_noise_lowpass_force = (1. - filter_coef_force) * _noise_lowpass_force + filter_coef_force * noise_force;
path: src/modules/simulation/gz_plugins/moving_platform_controller/MovingPlatformController.cpp
user: login: mbjd
body: can also be thought of that way - I presume you mean because of the scaling by mass and moment of inertia. the way I imagined it: 
 - `_noise_lowpass_force` and `_noise_lowpass_torque` are dimenionless
 - `_force` is an actual force in N, `_torque` is an actual torque in Nm
 - For this to work, `noise_ampl_force` has to be in N and `noise_ampl_torque` in Nm. Compared to them being scaled by mass and moment of inertia respectively, we have to imagine multiplying in the right unit conversion factors. 
 
If you want I can make this explicit and e.g. calculate linear/angular accelerations and only in the end convert to forces.
created_at: 2025-04-04T08:48:17Z
author_association: CONTRIBUTOR
diff_hunk: @@ -0,0 +1,325 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+#include "MovingPlatformController.hpp"
+
+using namespace custom;
+
+// Register the plugin
+GZ_ADD_PLUGIN(
+	MovingPlatformController,
+	gz::sim::System,
+	gz::sim::ISystemPreUpdate,
+	gz::sim::ISystemConfigure
+)
+
+void MovingPlatformController::Configure(const gz::sim::Entity &entity,
+		const std::shared_ptr<const sdf::Element> &sdf,
+		gz::sim::EntityComponentManager &ecm,
+		gz::sim::EventManager &eventMgr)
+{
+	_entity = entity;
+	_model = gz::sim::Model(entity);
+
+	const std::string link_name = sdf->Get<std::string>("link_name");
+	_link_entity = _model.LinkByName(ecm, link_name);
+
+	if (!_link_entity) {
+		throw std::runtime_error("MovingPlatformController::Configure: Link \"" + link_name + "\" was not found. "
+					 "Please ensure that your model contains the corresponding link.");
+	}
+
+	_link = gz::sim::Link(_link_entity);
+
+	// Needed to report linear & angular velocity
+	_link.EnableVelocityChecks(ecm, true);
+
+	_startup_timer = gz::common::Timer();
+	_startup_timer.Start();
+
+	// Get velocity and orientation setpoints from env vars
+	{
+		const double vel_forward = ReadEnvVar("PX4_GZ_PLATFORM_VEL", 1.0);
+		const double heading_deg = ReadEnvVar("PX4_GZ_PLATFORM_HEADING_DEG", 0.0);
+		const double heading = GZ_DTOR(heading_deg);
+
+		_orientation_sp = gz::math::Quaterniond(0., 0., heading);
+
+		// Set initial orientation to match the heading
+		const auto optional_original_pose = _link.WorldPose(ecm);
+
+		if (optional_original_pose.has_value()) {
+			const gz::math::Pose3d original_pose = optional_original_pose.value();
+			const gz::math::Pose3d new_pose(original_pose.Pos(), _orientation_sp);
+			_model.SetWorldPoseCmd(ecm, new_pose);
+
+		} else {
+			gzwarn << "Unable to get initial platform pose. Heading will not be set" << std::endl;
+		}
+
+		// Velocity setpoint in world frame.
+		const gz::math::Vector3d _body_velocity_sp(vel_forward, 0., 0.);
+		_velocity_sp = _orientation_sp * _body_velocity_sp;
+	}
+
+	// Get gravity, model mass, platform height.
+	{
+		const auto world_entity = gz::sim::worldEntity(ecm);
+		const auto world = gz::sim::World(world_entity);
+		const auto gravity = world.Gravity(ecm);
+
+		if (gravity.has_value()) {
+			_gravity = gravity.value().Z();
path: src/modules/simulation/gz_plugins/moving_platform_controller/MovingPlatformController.cpp
user: login: Jaeyoung-Lim
body: Nitpick: You can also disable gravity for this link?
created_at: 2025-04-04T09:02:12Z
author_association: MEMBER
diff_hunk: @@ -0,0 +1,324 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+#include "MovingPlatformController.hpp"
+
+using namespace custom;
+
+// Register the plugin
+GZ_ADD_PLUGIN(
+	MovingPlatformController,
+	gz::sim::System,
+	gz::sim::ISystemPreUpdate,
+	gz::sim::ISystemConfigure
+)
+
+void MovingPlatformController::Configure(const gz::sim::Entity &entity,
+		const std::shared_ptr<const sdf::Element> &sdf,
+		gz::sim::EntityComponentManager &ecm,
+		gz::sim::EventManager &eventMgr)
+{
+	_entity = entity;
+	_model = gz::sim::Model(entity);
+
+	const std::string link_name = sdf->Get<std::string>("link_name");
+	_link_entity = _model.LinkByName(ecm, link_name);
+
+	if (!_link_entity) {
+		throw std::runtime_error("MovingPlatformController::Configure: Link \"" + link_name + "\" was not found. "
+					 "Please ensure that your model contains the corresponding link.");
+	}
+
+	_link = gz::sim::Link(_link_entity);
+
+	// Needed to report linear & angular velocity
+	_link.EnableVelocityChecks(ecm, true);
+
+	_startup_timer = gz::common::Timer();
+	_startup_timer.Start();
+
+	// Get velocity and orientation setpoints from env vars
+	{
+		const double vel_forward = ReadEnvVar("PX4_GZ_PLATFORM_VEL", 1.0);
+		const double heading_deg = ReadEnvVar("PX4_GZ_PLATFORM_HEADING_DEG", 0.0);
+		const double heading = GZ_DTOR(heading_deg);
+
+		_orientation_sp = gz::math::Quaterniond(0., 0., heading);
+
+		// Set initial orientation to match the heading
+		const auto optional_original_pose = _link.WorldPose(ecm);
+
+		if (optional_original_pose.has_value()) {
+			const gz::math::Pose3d original_pose = optional_original_pose.value();
+			const gz::math::Pose3d new_pose(original_pose.Pos(), _orientation_sp);
+			_model.SetWorldPoseCmd(ecm, new_pose);
+
+		} else {
+			gzwarn << "Unable to get initial platform pose. Heading will not be set" << std::endl;
+		}
+
+		// Velocity setpoint in world frame.
+		const gz::math::Vector3d _body_velocity_sp(vel_forward, 0., 0.);
+		_velocity_sp = _orientation_sp * _body_velocity_sp;
+	}
+
+	// Get gravity, model mass, platform height.
+	{
+		const auto world_entity = gz::sim::worldEntity(ecm);
+		const auto world = gz::sim::World(world_entity);
+		const auto gravity = world.Gravity(ecm);
+
+		if (gravity.has_value()) {
+			_gravity = gravity.value().Z();
+
+		} else {
+			gzwarn << "Unable to get gazebo world gravity. Keeping default of " << _gravity << std::endl;
+		}
+
+		auto inertial_component = ecm.Component<gz::sim::components::Inertial>(_link_entity);
+
+		if (inertial_component) {
+			_platform_mass = inertial_component->Data().MassMatrix().Mass();
+			_platform_diag_moments = inertial_component->Data().MassMatrix().DiagonalMoments();
+
+		} else {
+			gzwarn << "Unable to get inertial component for link " << link_name << ". Keeping default mass of " << _platform_mass <<
+			       std::endl;
+		}
+
+		auto pose_component = ecm.Component<gz::sim::components::Pose>(_link_entity);
+
+		if (pose_component) {
+			_platform_height_setpoint = pose_component->Data().Z();
+
+		} else {
+			gzwarn << "Unable to get pose component for link " << link_name << ". Keeping default platform height setpoint of " <<
+			       _platform_height_setpoint << std::endl;
+		}
+	}
+
+}
+
+void MovingPlatformController::PreUpdate(const gz::sim::UpdateInfo &_info, gz::sim::EntityComponentManager &ecm)
+{
+	getPlatformState(ecm);
+
+	const double dt_sec = std::chrono::duration<double>(_info.dt).count();
+	updateNoise(dt_sec);
+
+	// Wait for model to spawn, takes quite some time now...
+	const bool keep_stationary = _startup_timer.ElapsedTime() < 5s;
path: src/modules/simulation/gz_plugins/moving_platform_controller/MovingPlatformController.cpp
user: login: mbjd
body: @Jaeyoung-Lim true that's a better solution, done in latest commit. 
created_at: 2025-04-04T12:20:10Z
author_association: CONTRIBUTOR
diff_hunk: @@ -0,0 +1,325 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+#include "MovingPlatformController.hpp"
+
+using namespace custom;
+
+// Register the plugin
+GZ_ADD_PLUGIN(
+	MovingPlatformController,
+	gz::sim::System,
+	gz::sim::ISystemPreUpdate,
+	gz::sim::ISystemConfigure
+)
+
+void MovingPlatformController::Configure(const gz::sim::Entity &entity,
+		const std::shared_ptr<const sdf::Element> &sdf,
+		gz::sim::EntityComponentManager &ecm,
+		gz::sim::EventManager &eventMgr)
+{
+	_entity = entity;
+	_model = gz::sim::Model(entity);
+
+	const std::string link_name = sdf->Get<std::string>("link_name");
+	_link_entity = _model.LinkByName(ecm, link_name);
+
+	if (!_link_entity) {
+		throw std::runtime_error("MovingPlatformController::Configure: Link \"" + link_name + "\" was not found. "
+					 "Please ensure that your model contains the corresponding link.");
+	}
+
+	_link = gz::sim::Link(_link_entity);
+
+	// Needed to report linear & angular velocity
+	_link.EnableVelocityChecks(ecm, true);
+
+	_startup_timer = gz::common::Timer();
+	_startup_timer.Start();
+
+	// Get velocity and orientation setpoints from env vars
+	{
+		const double vel_forward = ReadEnvVar("PX4_GZ_PLATFORM_VEL", 1.0);
+		const double heading_deg = ReadEnvVar("PX4_GZ_PLATFORM_HEADING_DEG", 0.0);
+		const double heading = GZ_DTOR(heading_deg);
+
+		_orientation_sp = gz::math::Quaterniond(0., 0., heading);
+
+		// Set initial orientation to match the heading
+		const auto optional_original_pose = _link.WorldPose(ecm);
+
+		if (optional_original_pose.has_value()) {
+			const gz::math::Pose3d original_pose = optional_original_pose.value();
+			const gz::math::Pose3d new_pose(original_pose.Pos(), _orientation_sp);
+			_model.SetWorldPoseCmd(ecm, new_pose);
+
+		} else {
+			gzwarn << "Unable to get initial platform pose. Heading will not be set" << std::endl;
+		}
+
+		// Velocity setpoint in world frame.
+		const gz::math::Vector3d _body_velocity_sp(vel_forward, 0., 0.);
+		_velocity_sp = _orientation_sp * _body_velocity_sp;
+	}
+
+	// Get gravity, model mass, platform height.
+	{
+		const auto world_entity = gz::sim::worldEntity(ecm);
+		const auto world = gz::sim::World(world_entity);
+		const auto gravity = world.Gravity(ecm);
+
+		if (gravity.has_value()) {
+			_gravity = gravity.value().Z();
path: src/modules/simulation/gz_plugins/moving_platform_controller/MovingPlatformController.cpp
user: login: mbjd
body: Thanks for pointing out, was not aware that this was possible per individual link. 

Do you see a benefit beyond saving maybe 10 lines of code? If yes, I'll happily change it, otherwise I'm leaving it like it is. 
created_at: 2025-04-04T12:44:38Z
author_association: CONTRIBUTOR
diff_hunk: @@ -0,0 +1,325 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+#include "MovingPlatformController.hpp"
+
+using namespace custom;
+
+// Register the plugin
+GZ_ADD_PLUGIN(
+	MovingPlatformController,
+	gz::sim::System,
+	gz::sim::ISystemPreUpdate,
+	gz::sim::ISystemConfigure
+)
+
+void MovingPlatformController::Configure(const gz::sim::Entity &entity,
+		const std::shared_ptr<const sdf::Element> &sdf,
+		gz::sim::EntityComponentManager &ecm,
+		gz::sim::EventManager &eventMgr)
+{
+	_entity = entity;
+	_model = gz::sim::Model(entity);
+
+	const std::string link_name = sdf->Get<std::string>("link_name");
+	_link_entity = _model.LinkByName(ecm, link_name);
+
+	if (!_link_entity) {
+		throw std::runtime_error("MovingPlatformController::Configure: Link \"" + link_name + "\" was not found. "
+					 "Please ensure that your model contains the corresponding link.");
+	}
+
+	_link = gz::sim::Link(_link_entity);
+
+	// Needed to report linear & angular velocity
+	_link.EnableVelocityChecks(ecm, true);
+
+	_startup_timer = gz::common::Timer();
+	_startup_timer.Start();
+
+	// Get velocity and orientation setpoints from env vars
+	{
+		const double vel_forward = ReadEnvVar("PX4_GZ_PLATFORM_VEL", 1.0);
+		const double heading_deg = ReadEnvVar("PX4_GZ_PLATFORM_HEADING_DEG", 0.0);
+		const double heading = GZ_DTOR(heading_deg);
+
+		_orientation_sp = gz::math::Quaterniond(0., 0., heading);
+
+		// Set initial orientation to match the heading
+		const auto optional_original_pose = _link.WorldPose(ecm);
+
+		if (optional_original_pose.has_value()) {
+			const gz::math::Pose3d original_pose = optional_original_pose.value();
+			const gz::math::Pose3d new_pose(original_pose.Pos(), _orientation_sp);
+			_model.SetWorldPoseCmd(ecm, new_pose);
+
+		} else {
+			gzwarn << "Unable to get initial platform pose. Heading will not be set" << std::endl;
+		}
+
+		// Velocity setpoint in world frame.
+		const gz::math::Vector3d _body_velocity_sp(vel_forward, 0., 0.);
+		_velocity_sp = _orientation_sp * _body_velocity_sp;
+	}
+
+	// Get gravity, model mass, platform height.
+	{
+		const auto world_entity = gz::sim::worldEntity(ecm);
+		const auto world = gz::sim::World(world_entity);
+		const auto gravity = world.Gravity(ecm);
+
+		if (gravity.has_value()) {
+			_gravity = gravity.value().Z();
path: src/modules/simulation/gz_plugins/moving_platform_controller/MovingPlatformController.cpp
user: login: Jaeyoung-Lim
body: I think it is good enough to go in at this point, I just think there are ways to improve it
created_at: 2025-04-06T18:14:01Z
author_association: MEMBER

PR comments:
user: login: dakejahl
created_at: 2025-03-08T02:15:58Z
body: I think what you want is a system plugin and a model plugin. 
https://gazebosim.org/api/sim/8/createsystemplugins.html

The system plugin is added to the server.config
https://github.com/PX4/PX4-Autopilot/blob/main/src/modules/simulation/gz_bridge/server.config#L16

The model plugin is added to the model (in the link below it's a sensor plugin, but same idea)
https://github.com/PX4/PX4-gazebo-models/blob/main/models/optical_flow/model.sdf#L72-L76

The system plugin is responsible for finding and registering the other plugin
https://github.com/PX4/PX4-Autopilot/blob/main/src/modules/simulation/gz_plugins/optical_flow/OpticalFlowSystem.cpp#L57-L92

You should probably then publish a TargetPose message in gz::transport. You can either create a new message like I did for optical flow (see gz_msgs/) or there might already exist in gz a suitable message. In GZBridge you would setup a callback for that topic (let's say LandingTargetPose) and then publish onto the uORB LandingTargetPose (add body velocities), or create a new uORB topic. This way the data interface is generic with respect to a "moving target". This way there could be multiple moving targets. All of the names should come directly from the sdf. If you have to hard code a name somewhere it's an indication that something isn't structured right.
author_association: CONTRIBUTOR
user: login: dakejahl
created_at: 2025-03-08T02:17:26Z
body: Also I would recommend downloading the gazebo repositories and looking at the examples.
https://github.com/orgs/gazebosim/repositories?type=all
author_association: CONTRIBUTOR
user: login: mbjd
created_at: 2025-03-10T09:37:49Z
body: @dakejahl thanks for the suggestions!

 - I removed the hardcoded model name in the plugin in the latest commit.
 - I removed the callback in GZBridge entirely to decrease scope of this PR. The reason being that in our use case not PX4, but applications on a companion computer need to be informed about the state of the landing platform. Therefore we consider it preferable to handle this communication with an external tool rather than going through PX4 and "polluting" upstream. 
 - System/model plugin separation: Forgive the bluntness, but why? It seems to me that adding a system plugin (via server.config) in addition to the model plugin (via sdf) strictly adds complexity. If there is a concrete benefit that I'm currently missing, of course I'm happy to adapt. 

author_association: CONTRIBUTOR
user: login: dakejahl
created_at: 2025-03-11T03:40:31Z
body: > System/model plugin separation: Forgive the bluntness, but why? It seems to me that adding a system plugin (via server.config) in addition to the model plugin (via sdf) strictly adds complexity. If there is a concrete benefit that I'm currently missing, of course I'm happy to adapt.

Ah okay then I must have misunderstood how the system plugins work. I thought they only worked when added as world plugins. I  know the docs say otherwise but I couldn't figure out how to make my optical flow plugin work when added to the model directly rather than to the world. Great! This is actually better.

> Therefore we consider it preferable to handle this communication with an external tool rather than going through PX4 and "polluting" upstream.

I think a moving target is a great addition to upstream, but it takes slightly more effort to make it generic. Ultimately it's up to you. I would create a custom proto (let's say `LandingTarget`) which contains the same GPS lat/lon/vel information as NavSat, but the message also includes an ID. This way the GZBridge can regex subscribe to all publishers of this new message. This way you avoid needing to know the model name to subscribe to the NavSat msg, and you can support multiple mvoing platforms. The lat/lon/vel you publish could come directly from the model pose, or you could add the NavSat sensor to your model sdf subscribe to the NavSat callback within your plugin (I think you can derive the topic path from the model sdf).
author_association: CONTRIBUTOR
user: login: mbjd
created_at: 2025-03-11T08:42:29Z
body: Great to hear about the plugin architecture :) 

As for the rest, I think we'll proceed for now without making a new proto message and interfacing to PX4, and rely just on the navsat and IMU plugin to get info out from the platform. In the future we can make this generic and extensible by publishing the info on a topic independent of the specific SDF structure and reading it in GZBridge as you describe, but I'd like to keep it separate from this PR. 
author_association: CONTRIBUTOR
user: login: mbjd
created_at: 2025-03-12T09:20:25Z
body: Reabsed & marked ready for review as now this does not depend on the specific implementation in the gz submodule anymore :) 
author_association: CONTRIBUTOR
user: login: mbjd
created_at: 2025-03-13T08:44:25Z
body: Added a README, removed template comments, removed erroneous gazebo classic changes (thanks for pointing out!) 

Also, small mod so it doesn't start at full velocity but ramps up at constant acceleration. This makes it possible to choose relatively fast velocities without the aircraft sliding off the platform immediately. 
author_association: CONTRIBUTOR
user: login: mbjd
created_at: 2025-03-13T09:28:46Z
body: @Jaeyoung-Lim about commit history, yes, definitely agree. feel free to squash when merging, or I can squash manually if you prefer. 
author_association: CONTRIBUTOR
user: login: Jaeyoung-Lim
created_at: 2025-03-13T09:35:35Z
body: @mbjd Please write proper commit messages such that the changes are referred properly: [git best practices](https://community.atlassian.com/forums/Bitbucket-articles/Git-Best-Practices/ba-p/1628803#:~:text=Write%20Good%20Commit%20Messages,by%20including%20a%20blank%20line)
author_association: MEMBER
user: login: dakejahl
created_at: 2025-03-13T21:42:40Z
body: Github let's you squash and merge and add a new commit message at the time of merging. I would actually prefer that since it preserves history during development. When someone rebases you can't tell what's changed and you have to review the entire file again to be 100% sure things didn't regress. My 2 cents
author_association: CONTRIBUTOR
user: login: Jaeyoung-Lim
created_at: 2025-03-13T22:57:42Z
body: > Github let's you squash and merge and add a new commit message at the time of merging. I would actually prefer that since it preserves history during development. When someone rebases you can't tell what's changed and you have to review the entire file again to be 100% sure things didn't regress. My 2 cents

@dakejahl I agree with your comments, but on the basis that the commit history is already inline with the best practices. This is just my opinion.
- If the commit histories are not organized properly (e.g. including messages like "Its working!", as was in this PR), the history during development is not worth preserving IMHO. I would even go on and say that if the commit messages are meaningful, it is better to do a rebase and merge than a squash. 
- If one of the maintainers are to merge the PR, I don't think it is so nice to rewrite the intent of the commit. I think this should come from the author, which should be written on the basis of the review comment the person received during the PR process.
author_association: MEMBER
user: login: dakejahl
created_at: 2025-03-13T23:07:40Z
body: > I don't think it is so nice to rewrite the intent of the commit. I think this should come from the author

I agree with this for sure.

The reason I don't like rebasing is entirely because of the loss of history. When someone rebases it's not possible to know if they slipped in some malicious code unless you re-review the entire PR. For large PRs this is unrealistic. I'm thinking about this from a zero trust perspective. Whereas a single commit I can see exactly what the author changed inbetween reviews.

> I would even go on and say that if the commit messages are meaningful, it is better to do a rebase and merge than a squash.

Yeah... maybe. IMO every commit in main should build. Otherwise bisecting is impossible. During development it's a lot of extra work to have good multi-commits. At least for me, when I'm working I commit frequently and the commits are often not worthy of a good description and may not build. The commit is more about saving your work.

Anyway. This PR looks good to me! Merge when ready.
author_association: CONTRIBUTOR
user: login: mbjd
created_at: 2025-04-01T14:03:31Z
body: Update: changed to wrench implementation as recommended [here](https://github.com/PX4/PX4-Autopilot/pull/24471#discussion_r1998634343). Now it works purely using forces and torques rather than setting velocities directly. 

Other updates:
 - Also make heading configurable by env var in addition to velocity
 - Cleaner error handling
 - Read parameters (gravity, platform mass & height) from model rather than hardcoding
 - Update README with new env vars, usage in sdf, etc.



@Jaeyoung-Lim @dakejahl please feel free to review in as much detail as you deem necessary after you've already reviewed the previous version. Thanks again for all the feedback! 
author_association: CONTRIBUTOR
user: login: hamishwillee
created_at: 2025-04-01T23:47:06Z
body: @mbjd When this is done, would be great to have docs (hence the tag addition). This would be an addition to the world of course, but I suspect more would be needed. I'm not sure what that would be since I am not a domain expert, but enough so that someone can take all this cool work you have done and use it in a turnkey way. Perhaps a dedicated doc or section on simulation of landing on moving targets.

author_association: CONTRIBUTOR
user: login: mbjd
created_at: 2025-04-02T07:21:44Z
body: Rebased on main again & removed commits switching gz submodule branches to avoid merge conflicts. 

Build is failing at the moment because something in the CMakeLists changed upstream. Working on a fix. (edit: fixed, updated it according to https://github.com/PX4/PX4-Autopilot/pull/24518)

@hamishwillee yes, definitely. the current [readme](https://github.com/PX4/PX4-Autopilot/blob/7f3cfeca4f5eb9c02b03dde33376d7f62e930974/src/modules/simulation/gz_plugins/moving_platform_controller/README.md) provides a basic overview but is hardly discoverable to users. Would this ["argument" list](https://docs.px4.io/main/en/sim_gazebo_gz/#syntax) be a suitable place to add it in your opinion, or would you prefer adding an extra section? Because it is more of a special case of the existing PX4_GZ_WORLD rather than just another environment variable...
author_association: CONTRIBUTOR