IMU: reads real vehicle linear acceleration

MRPT · Jul 1, 2023 · e1e6689 · e1e6689
1 parent 9db6a02
commit e1e6689
Show file tree

Hide file tree

Showing 3 changed files with 38 additions and 6 deletions.
diff --git a/modules/simulator/include/mvsim/Simulable.h b/modules/simulator/include/mvsim/Simulable.h
@@ -70,6 +70,12 @@ class Simulable
 
 	mrpt::math::TTwist2D getTwist() const;
 
+	/** Last time-step acceleration of the ref. point (global coords).
+	 * Note this is the "coordinate acceleration" vector, not the proper
+	 * acceleration. It is simply estimated as a finite difference of dq_.
+	 */
+	mrpt::math::TVector3D getLinearAcceleration() const;
+
 	/** Manually override vehicle pose (Use with caution!) (purposely set a
 	 * "const")*/
 	void setPose(const mrpt::math::TPose3D& p) const;
@@ -128,7 +134,7 @@ class Simulable
    private:
 	World* simulable_parent_ = nullptr;
 
-	/** protects q_, dq_ */
+	/** protects q_, dq_, ddq_lin_ */
 	mutable std::shared_mutex q_mtx_;
 
 	/** Last time-step pose (of the ref. point, in global coords) */
@@ -137,6 +143,11 @@ class Simulable
 	/** Last time-step velocity (of the ref. point, in global coords) */
 	mrpt::math::TTwist2D dq_{0, 0, 0};
 
+	/// See notes of getLinearAcceleration()
+	mrpt::math::TVector3D ddq_lin_{0, 0, 0};
+
+	mrpt::math::TPose3D former_q_;	//!< Updated in simul_post_timestep()
+
 	// ============ ANIMATION VARIABLES ============
 	/** Initial pose, per configuration XML world file */
 	mrpt::math::TPose3D initial_q_ = mrpt::math::TPose3D::Identity();

diff --git a/modules/simulator/src/Sensors/IMU.cpp b/modules/simulator/src/Sensors/IMU.cpp
@@ -9,6 +9,7 @@
 
 #include <mrpt/core/lock_helper.h>
 #include <mrpt/opengl/stock_objects.h>
+#include <mrpt/version.h>
 #include <mvsim/Sensors/IMU.h>
 #include <mvsim/VehicleBase.h>
 #include <mvsim/World.h>
@@ -113,12 +114,19 @@ void IMU::internal_simulate_imu(const TSimulContext& context)
 	outObs->set(mrpt::obs::IMU_WZ, w.z);
 
 	// linear acceleration:
-	mrpt::math::TVector3D linAcc(0.0, 0.0, 0.0);
-	rng_.drawGaussian1DVector(linAcc, 0.0, linearAccelerationStdNoise_);
+	const auto g = mrpt::math::TVector3D(.0, .0, -world_->get_gravity());
 
-	outObs->set(mrpt::obs::IMU_X_ACC, linAcc.x);
-	outObs->set(mrpt::obs::IMU_Y_ACC, linAcc.y);
-	outObs->set(mrpt::obs::IMU_Z_ACC, linAcc.z);
+	mrpt::math::TVector3D linAccNoise(0, 0, 0);
+	rng_.drawGaussian1DVector(linAccNoise, 0.0, linearAccelerationStdNoise_);
+
+	const mrpt::math::TVector3D linAccLocal =
+		vehicle_.getPose().inverseComposePoint(
+			vehicle_.getLinearAcceleration() + g) +
+		linAccNoise;
+
+	outObs->set(mrpt::obs::IMU_X_ACC, linAccLocal.x);
+	outObs->set(mrpt::obs::IMU_Y_ACC, linAccLocal.y);
+	outObs->set(mrpt::obs::IMU_Z_ACC, linAccLocal.z);
 
 	// Save:
 	{

diff --git a/modules/simulator/src/Simulable.cpp b/modules/simulator/src/Simulable.cpp
@@ -83,6 +83,11 @@ void Simulable::simul_post_timestep(const TSimulContext& context)
 		dq_.vy = vel(1);
 		dq_.omega = w;
 
+		// Estimate acceleration from finite differences:
+		ddq_lin_ =
+			(q_.translation() - former_q_.translation()) * (1.0 / context.dt);
+		former_q_ = q_;
+
 		// Instantaneous collision flag:
 		isInCollision_ = false;
 		if (b2ContactEdge* cl = b2dBody_->GetContactList();
@@ -448,6 +453,14 @@ mrpt::math::TTwist2D Simulable::getTwist() const
 	return ret;
 }
 
+mrpt::math::TVector3D Simulable::getLinearAcceleration() const
+{
+	q_mtx_.lock_shared();
+	auto ret = ddq_lin_;
+	q_mtx_.unlock_shared();
+	return ret;
+}
+
 void Simulable::setTwist(const mrpt::math::TTwist2D& dq) const
 {
 	q_mtx_.lock();