From f9853589005e537ea9ee4404c868e39fbb5dcc34 Mon Sep 17 00:00:00 2001 From: Sylvain Joyeux Date: Thu, 13 Dec 2018 17:21:03 -0200 Subject: [PATCH] clarify RigidBodyAcceleration's usage --- src/samples/RigidBodyAcceleration.cpp | 7 +++++- src/samples/RigidBodyAcceleration.hpp | 35 ++++++++++++++++++++------- 2 files changed, 32 insertions(+), 10 deletions(-) diff --git a/src/samples/RigidBodyAcceleration.cpp b/src/samples/RigidBodyAcceleration.cpp index 0db5f9a8..b41f6f86 100644 --- a/src/samples/RigidBodyAcceleration.cpp +++ b/src/samples/RigidBodyAcceleration.cpp @@ -2,7 +2,12 @@ namespace base { namespace samples { -void RigidBodyAcceleration::invalidateOrientation() +RigidBodyAcceleration::RigidBodyAcceleration() +{ + invalidate(); +} + +void RigidBodyAcceleration::invalidate() { cov_acceleration = Eigen::Matrix3d::Identity(); cov_acceleration *= INFINITY; diff --git a/src/samples/RigidBodyAcceleration.hpp b/src/samples/RigidBodyAcceleration.hpp index 5f1f76b7..8e02f397 100644 --- a/src/samples/RigidBodyAcceleration.hpp +++ b/src/samples/RigidBodyAcceleration.hpp @@ -5,24 +5,41 @@ #include namespace base { namespace samples { - struct RigidBodyAcceleration + /** + * Representation of accelerations of a body in a given (unspecified) fixed + * frame of reference + * + * While the frame of reference is unspecified, it will usually be the + * inertial frame. We encourage Rock code to use the body-fixed frame + * as the frame of expression. + * + * Indeed, Sensors (e.g. gyros, accelerometers) and models (e.g. + * hydrodynamic models) give access to velocities and accelerations w.r.t. + * the inertial frame, but expressed in the body frame. However, expressing + * the velocities/accelerations in the world frame from these + * sensing/estimation methods would require to have an estimate + * of the system's pose in the world, which is a harder thing to get. + */ + struct RigidBodyAcceleration { + RigidBodyAcceleration(); + base::Time time; - /** Linear acceleration in m/s2, world fixed frame of reference (East-North-Up) */ + /** Linear acceleration in m/s2 */ base::Vector3d acceleration; - /** Covariance matrix of the linear acceleration - */ + /** Covariance matrix of the linear acceleration + */ base::Matrix3d cov_acceleration; - /** Angular acceleration in rad/s2, world fixed frame of reference (East-North-Up) */ + /** Angular acceleration in rad/s2 */ base::Vector3d angular_acceleration; - /** Covariance matrix of the angular acceleration - */ + /** Covariance matrix of the angular acceleration + */ base::Matrix3d cov_angular_acceleration; - void invalidateOrientation(); - + void invalidate(); + }; }}