Removed Eigen namespace from standard includes header

Author: Jayasinghe, Oshada (Data61, Pullenvale)

include/syropod_highlevel_controller/debug_visualiser.h

@@ -37,7 +37,7 @@ class DebugVisualiser
   /// Publishes visualisation markers which represent the estimated walking plane.
   /// @param[in] walk_plane A Vector representing the walk plane
   /// @param[in] walk_plane_normal A Vector of the normal to the walk plane
-  void generateWalkPlane(const Vector3d& walk_plane, const Vector3d& walk_plane_normal);
+  void generateWalkPlane(const Eigen::Vector3d& walk_plane, const Eigen::Vector3d& walk_plane_normal);
 
   /// Publishes visualisation markers which represent the trajectory of the tip of the input leg.
   /// @param[in] leg A pointer to the leg associated with the tip trajectory that is to be published
@@ -84,7 +84,7 @@ class DebugVisualiser
 
   /// Publishes visualisation markers which represent the estimate of the gravitational acceleration vector.
   /// @param[in] gravity_estimate An estimate of the gravitational acceleration vector
-  void generateGravity(const Vector3d& gravity_estimate);
+  void generateGravity(const Eigen::Vector3d& gravity_estimate);
 
 private:
   ros::Publisher robot_model_publisher_;          ///< Publisher for topic "/shc/visualisation/robot_model"

include/syropod_highlevel_controller/model.h

@@ -23,7 +23,7 @@ class Pose
   /// Pose class constructor.
   /// @param[in] position The vector input to be set as the position vector of the pose
   /// @param[in] rotation The vector input to be set as the rotation vector of the pose
-  inline Pose(const Vector3d& position, const Quaterniond& rotation)
+  inline Pose(const Eigen::Vector3d& position, const Eigen::Quaterniond& rotation)
   {
     position_ = position;
     rotation_ = rotation;
@@ -33,8 +33,8 @@ class Pose
   /// @param[in] pose The geometry_msgs/Pose input to be set as the pose
   inline Pose(const geometry_msgs::Pose& pose)
   {
-    position_ = Vector3d(pose.position.x, pose.position.y, pose.position.z);
-    rotation_ = Quaterniond(pose.orientation.w, pose.orientation.x, pose.orientation.y, pose.orientation.z);
+    position_ = Eigen::Vector3d(pose.position.x, pose.position.y, pose.position.z);
+    rotation_ = Eigen::Quaterniond(pose.orientation.w, pose.orientation.x, pose.orientation.y, pose.orientation.z);
   };
 
   /// Checks for NaN values within pose elements.
@@ -54,8 +54,8 @@ class Pose
   /// @param[in] transform The geometry_msgs/Transform input to be used to construct the pose
   inline Pose(const geometry_msgs::Transform& transform)
   {
-    position_ = Vector3d(transform.translation.x, transform.translation.y, transform.translation.z);
-    rotation_ = Quaterniond(transform.rotation.w, transform.rotation.x, transform.rotation.y, transform.rotation.z);
+    position_ = Eigen::Vector3d(transform.translation.x, transform.translation.y, transform.translation.z);
+    rotation_ = Eigen::Quaterniond(transform.rotation.w, transform.rotation.x, transform.rotation.y, transform.rotation.z);
   };
 
   /// Returns a conversion of this pose object into a geometry_msgs::Pose.
@@ -116,10 +116,10 @@ class Pose
   /// @return The transformed pose
   inline Pose transform(const geometry_msgs::Transform& transform) const
   {
-    Vector3d position(position_ + Vector3d(transform.translation.x,
+    Eigen::Vector3d position(position_ + Eigen::Vector3d(transform.translation.x,
                                            transform.translation.y,
                                            transform.translation.z));
-    Quaterniond rotation(rotation_ * Quaterniond(transform.rotation.w,
+    Eigen::Quaterniond rotation(rotation_ * Eigen::Quaterniond(transform.rotation.w,
                                                  transform.rotation.x,
                                                  transform.rotation.y,
                                                  transform.rotation.z));
@@ -129,31 +129,31 @@ class Pose
   /// Transforms this pose according to an input transformation matrix constructed from DH matrices.
   /// @param[in] transform The input transformation matrix
   /// @return The transformed pose
-  inline Pose transform(const Matrix4d& transform) const
+  inline Pose transform(const Eigen::Matrix4d& transform) const
   {
     Pose return_pose;
     // Position
-    Vector4d result(position_[0], position_[1], position_[2], 1);
+    Eigen::Vector4d result(position_[0], position_[1], position_[2], 1);
     result = transform * result;
-    return_pose.position_ = Vector3d(result[0], result[1], result[2]);
+    return_pose.position_ = Eigen::Vector3d(result[0], result[1], result[2]);
     // Orientation
-    Matrix3d rotation_matrix = transform.block<3, 3>(0, 0);
-    return_pose.rotation_ = (Quaterniond(rotation_matrix) * rotation_).normalized();
+    Eigen::Matrix3d rotation_matrix = transform.block<3, 3>(0, 0);
+    return_pose.rotation_ = (Eigen::Quaterniond(rotation_matrix) * rotation_).normalized();
     return return_pose;
   }
 
   /// Transforms an input vector into the reference frame of this pose.
   /// @param[in] vec The input vector to be transformed into this pose's reference frame
   /// @return The transformed vector
-  inline Vector3d transformVector(const Vector3d& vec) const 
+  inline Eigen::Vector3d transformVector(const Eigen::Vector3d& vec) const 
   { 
     return position_ + rotation_._transformVector(vec);
   };
 
   /// Transforms an input vector from the reference frame of this pose.
   /// @param[in] vec The input vector to be transformed from this pose's reference frame
   /// @return The inversly transformed vector
-  inline Vector3d inverseTransformVector(const Vector3d& vec) const 
+  inline Eigen::Vector3d inverseTransformVector(const Eigen::Vector3d& vec) const 
   { 
     return (~*this).transformVector(vec);
   };
@@ -186,16 +186,16 @@ class Pose
   /// @return The resultant interpolated pose
   inline Pose interpolate (const double& control_input, const Pose& target_pose)
   { 
-    Vector3d position = control_input * target_pose.position_ + (1.0 - control_input) * (*this).position_;
-    Quaterniond rotation = (*this).rotation_.slerp(control_input, target_pose.rotation_);
+    Eigen::Vector3d position = control_input * target_pose.position_ + (1.0 - control_input) * (*this).position_;
+    Eigen::Quaterniond rotation = (*this).rotation_.slerp(control_input, target_pose.rotation_);
     return Pose(position, rotation);
   };
 
   /// Returns pose with position and rotation elements set to identity values.
   /// @return The Identity Pose
   inline static Pose Identity(void) 
   { 
-    return Pose(Vector3d::Zero(), Quaterniond::Identity()); 
+    return Pose(Eigen::Vector3d::Zero(), Eigen::Quaterniond::Identity()); 
   }
 
   /// Returns pose with position and rotation elements all set to undefined values.
@@ -205,8 +205,8 @@ class Pose
     return Pose(UNDEFINED_POSITION, UNDEFINED_ROTATION);
   }
 
-  Vector3d position_;     ///< The position element of the pose class
-  Quaterniond rotation_;  ///< The rotation element of the pose class
+  Eigen::Vector3d position_;     ///< The position element of the pose class
+  Eigen::Quaterniond rotation_;  ///< The rotation element of the pose class
 
 public:
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW

include/syropod_highlevel_controller/pose.h

@@ -37,7 +37,7 @@ class AutoPoser;
 /// requirements and methods. Generation of a user controlled 'manual' body pose and an IMU feedback based automatic
 /// body pose are examples of these sub-poses, which are combined and applied to the robot model body.
 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-typedef vector<shared_ptr<AutoPoser>, aligned_allocator<shared_ptr<AutoPoser>>> AutoPoserContainer;
+typedef vector<shared_ptr<AutoPoser>, Eigen::aligned_allocator<shared_ptr<AutoPoser>>> AutoPoserContainer;
 class PoseController : public enable_shared_from_this<PoseController>
 {
 public:
@@ -76,15 +76,15 @@ class PoseController : public enable_shared_from_this<PoseController>
 
   /// Accessor for error in rotation absement - used in imu posing PID.
   /// @return The error in rotation absement (Difference between current and desired rotation absement for IMU posing PID)
-  inline Vector3d getRotationAbsementError(void) { return rotation_absement_error_; };
+  inline Eigen::Vector3d getRotationAbsementError(void) { return rotation_absement_error_; };
 
   /// Accessor for error in rotation position - used in imu posing PID.
   /// @return The error in rotation position (Difference between current and desired rotation position for IMU posing PID) 
-  inline Vector3d getRotationPositionError(void) { return rotation_position_error_; };
+  inline Eigen::Vector3d getRotationPositionError(void) { return rotation_position_error_; };
 
   /// Accessor for error in rotation velocity - used in imu posing PID.
   /// @return The error in rotation velocity (Difference between current and desired rotation velocity for IMU posing PID)
-  inline Vector3d getRotationVelocityError(void) { return rotation_velocity_error_; };
+  inline Eigen::Vector3d getRotationVelocityError(void) { return rotation_velocity_error_; };
 
   /// Modifier for pose phase length.
   /// @param[in] phase_length The phase length to be set as the phase length of the auto posing cycle
@@ -105,7 +105,7 @@ class PoseController : public enable_shared_from_this<PoseController>
   /// Modifier for manual pose velocity input.
   /// @param[in] translation The value to be set as the velocity input for controlling the translation component of manual pose
   /// @param[in] rotation The value to be set as the velocity input for controlling the rotational component of manual pose
-  inline void setManualPoseInput(const Vector3d& translation, const Vector3d& rotation)
+  inline void setManualPoseInput(const Eigen::Vector3d& translation, const Eigen::Vector3d& rotation)
   {
     translation_velocity_input_ = translation;
     rotation_velocity_input_ = rotation;
@@ -244,7 +244,7 @@ class PoseController : public enable_shared_from_this<PoseController>
 
   /// Estimates the acceleration vector due to gravity.
   /// @return The estimated acceleration vector due to gravity.
-  inline Vector3d estimateGravity(void) { return model_->estimateGravity(); };
+  inline Eigen::Vector3d estimateGravity(void) { return model_->estimateGravity(); };
 
   /// Attempts to generate a pose (x/y linear translation only) to position body such that there is a zero sum of moments
   /// from the force acting on the load bearing feet, allowing the robot to shift its centre of mass away from manually
@@ -258,8 +258,8 @@ class PoseController : public enable_shared_from_this<PoseController>
   shared_ptr<Leg> auto_pose_reference_leg_; ///< Reference leg for auto posing system
 
   PoseResetMode pose_reset_mode_;         ///< Mode for controlling which posing axes to reset to zero
-  Vector3d translation_velocity_input_;   ///< Velocity input for controlling the translation component of manual pose
-  Vector3d rotation_velocity_input_;      ///< Velocity input for controlling the rotational component of manual pose
+  Eigen::Vector3d translation_velocity_input_;   ///< Velocity input for controlling the translation component of manual pose
+  Eigen::Vector3d rotation_velocity_input_;      ///< Velocity input for controlling the rotational component of manual pose
 
   int legs_completed_step_ = 0;           ///< Number of legs having completed the required step in a sequence
   int current_group_ = 0;                 ///< The current leg group executing a stepping maneuver
@@ -303,9 +303,9 @@ class PoseController : public enable_shared_from_this<PoseController>
   int pose_phase_length_ = 0;                       ///< The phase length of the auto posing cycle
   int normaliser_ = 1;                              ///< The value used to scale base posing cycle start/end ratios
 
-  Vector3d rotation_absement_error_;  ///< Difference between current and desired rotation absement for IMU posing PID
-  Vector3d rotation_position_error_;  ///< Difference between current and desired rotation position for IMU posing PID
-  Vector3d rotation_velocity_error_;  ///< Difference between current and desired rotation velocity for IMU posing PID
+  Eigen::Vector3d rotation_absement_error_;  ///< Difference between current and desired rotation absement for IMU posing PID
+  Eigen::Vector3d rotation_position_error_;  ///< Difference between current and desired rotation position for IMU posing PID
+  Eigen::Vector3d rotation_velocity_error_;  ///< Difference between current and desired rotation velocity for IMU posing PID
 
   LegContainer::iterator leg_it_;     ///< Leg iteration member variable used to minimise code
   JointContainer::iterator joint_it_; ///< Joint iteration member variable used to minimise code
@@ -576,7 +576,7 @@ class LegPoser
   Pose target_tip_pose_;           ///< Target tip pose used in bezier curve equations
   ExternalTarget external_target_; ///< Externally set target tip pose object
 
-  vector<Pose, aligned_allocator<Pose>> transition_poses_; ///< Vector of transition target tip poses
+  vector<Pose, Eigen::aligned_allocator<Pose>> transition_poses_; ///< Vector of transition target tip poses
 
   bool leg_completed_step_ = false; ///< Flag denoting if leg has completed its required step in a sequence
 

include/syropod_highlevel_controller/pose_controller.h

@@ -54,15 +54,14 @@
 #define PROGRESS_COMPLETE 100 ///< Value denoting 100% and a completion of progress of various functions
 #define THROTTLE_PERIOD 5  ///< Default throttle period for all throttled rosconsole messages (seconds)
 
-#define UNDEFINED_ROTATION Quaterniond(0,0,0,0)
-#define UNDEFINED_POSITION Vector3d(double(INT_MAX), double(INT_MAX), double(INT_MAX))
+#define UNDEFINED_ROTATION Eigen::Quaterniond(0,0,0,0)
+#define UNDEFINED_POSITION Eigen::Vector3d(double(INT_MAX), double(INT_MAX), double(INT_MAX))
 
 #define GRAVITY_ACCELERATION -9.81 ///< Approximate gravitational acceleration (m/s/s)
 
 using namespace std;
 using namespace ros;
 using namespace tf2_ros;
-using namespace Eigen;
 
 /// Converts Degrees to Radians.
 /// @param[in] degrees Value in degrees to be converted to radians
@@ -132,7 +131,7 @@ inline T clamped(const T& value, const double& magnitude)
 template <class T>
 inline T clamped(const T& value, const T& limit)
 {
-  VectorXd result(value.size());
+  Eigen::VectorXd result(value.size());
   for (int i = 0; i < value.size(); ++i)
   {
     result[i] = clamped(value[i], -limit[i], limit[1]);
@@ -153,9 +152,9 @@ inline double setPrecision(const double& value, const int& precision)
 /// @param[in] vector The input vector
 /// @param[in] precision The required precision
 /// @return The input vector with a precision defined by the precision input
-inline Vector3d setPrecision(const Vector3d& vector, const int& precision)
+inline Eigen::Vector3d setPrecision(const Eigen::Vector3d& vector, const int& precision)
 {
-  return Vector3d(roundToInt(vector[0] * pow(10, precision)) / pow(10, precision),
+  return Eigen::Vector3d(roundToInt(vector[0] * pow(10, precision)) / pow(10, precision),
                   roundToInt(vector[1] * pow(10, precision)) / pow(10, precision),
                   roundToInt(vector[2] * pow(10, precision)) / pow(10, precision));
 }
@@ -174,11 +173,11 @@ inline double smoothStep(const double& control_input)
 /// @param[in] a The input vector
 /// @param[in] b The reference vector
 /// @return The resultant projection vector
-inline Vector3d getProjection(const Vector3d& a, const Vector3d& b)
+inline Eigen::Vector3d getProjection(const Eigen::Vector3d& a, const Eigen::Vector3d& b)
 {
   if (a.norm() == 0.0 || b.norm() == 0.0)
   {
-    return Vector3d::Zero();
+    return Eigen::Vector3d::Zero();
   }
   else
   {
@@ -191,7 +190,7 @@ inline Vector3d getProjection(const Vector3d& a, const Vector3d& b)
 /// @param[in] a The input vector
 /// @param[in] b The reference vector
 /// @return The resultant rejection vector
-inline Vector3d getRejection(const Vector3d& a, const Vector3d& b)
+inline Eigen::Vector3d getRejection(const Eigen::Vector3d& a, const Eigen::Vector3d& b)
 {
   return a - getProjection(a, b);
 }
@@ -212,11 +211,11 @@ inline T interpolate(const T& origin, const T& target, const double& control_inp
 /// @param[in] test The test rotation to check for shorter rotation path
 /// @param[in] reference The reference rotation for the target rotation
 /// @return Rotation with shorter path between identical rotations on quaternion
-inline Quaterniond correctRotation(const Quaterniond& test, const Quaterniond& reference)
+inline Eigen::Quaterniond correctRotation(const Eigen::Quaterniond& test, const Eigen::Quaterniond& reference)
 {
   if (test.dot(reference) < 0.0)
   {
-    return Quaterniond(-test.w(), -test.x(), -test.y(), -test.z());
+    return Eigen::Quaterniond(-test.w(), -test.x(), -test.y(), -test.z());
   }
   else
   {
@@ -228,19 +227,19 @@ inline Quaterniond correctRotation(const Quaterniond& test, const Quaterniond& r
 /// @param[in] euler Eigen Vector3d of Euler angles (roll, pitch, yaw) to be converted
 /// @param[in] intrinsic Defines whether conversion occurs intrinsically or extrinsically
 /// @return Eigen Quaternion generated using given Euler Angles
-inline Quaterniond eulerAnglesToQuaternion(const Vector3d& euler, const bool& intrinsic = false)
+inline Eigen::Quaterniond eulerAnglesToQuaternion(const Eigen::Vector3d& euler, const bool& intrinsic = false)
 {
   if (intrinsic)
   {
-    return Quaterniond(AngleAxisd(euler[0], Vector3d::UnitX()) *
-                       AngleAxisd(euler[1], Vector3d::UnitY()) *
-                       AngleAxisd(euler[2], Vector3d::UnitZ()));
+    return Eigen::Quaterniond(Eigen::AngleAxisd(euler[0], Eigen::Vector3d::UnitX()) *
+                       Eigen::AngleAxisd(euler[1], Eigen::Vector3d::UnitY()) *
+                       Eigen::AngleAxisd(euler[2], Eigen::Vector3d::UnitZ()));
   }
   else
   {
-    return Quaterniond(AngleAxisd(euler[2], Vector3d::UnitZ()) *
-                       AngleAxisd(euler[1], Vector3d::UnitY()) *
-                       AngleAxisd(euler[0], Vector3d::UnitX()));
+    return Eigen::Quaterniond(Eigen::AngleAxisd(euler[2], Eigen::Vector3d::UnitZ()) *
+                       Eigen::AngleAxisd(euler[1], Eigen::Vector3d::UnitY()) *
+                       Eigen::AngleAxisd(euler[0], Eigen::Vector3d::UnitX()));
   }
 }
 
@@ -249,9 +248,9 @@ inline Quaterniond eulerAnglesToQuaternion(const Vector3d& euler, const bool& in
 /// @param[in] rotation Eigen Quaterniond of rotation to be converted
 /// @param[in] intrinsic Defines whether conversion occurs intrinsically or extrinsically
 /// @return Euler Angles generated using given Eigen Quaternoid
-inline Vector3d quaternionToEulerAngles(const Quaterniond& rotation, const bool& intrinsic = false)
+inline Eigen::Vector3d quaternionToEulerAngles(const Eigen::Quaterniond& rotation, const bool& intrinsic = false)
 {
-  Vector3d result(0,0,0);
+  Eigen::Vector3d result(0,0,0);
 
   // Intrinsic rotation (roll, pitch, yaw order)
   if (intrinsic)
@@ -292,7 +291,7 @@ inline Vector3d quaternionToEulerAngles(const Quaterniond& rotation, const bool&
     }
   }
 
-  return intrinsic ? result : Vector3d(result[2], result[1], result[0]);
+  return intrinsic ? result : Eigen::Vector3d(result[2], result[1], result[0]);
 }
 
 /// Returns a string representation of the input value.
@@ -377,9 +376,9 @@ inline T quarticBezierDot(const T* points, const double& t)
 /// @param[in] r The DH parameter representing length of the common normal
 /// @param[in] alpha The DH parameter representing angle about common normal, form old z-axis to new z-axis
 /// @return Classical Denavit-Hartenberg (DH) matrix generated using the given DH parameters as input
-inline Matrix4d createDHMatrix(const double& d, const double& theta, const double& r, const double& alpha)
+inline Eigen::Matrix4d createDHMatrix(const double& d, const double& theta, const double& r, const double& alpha)
 {
-  Matrix4d m;
+  Eigen::Matrix4d m;
   m << cos(theta), -sin(theta)*cos(alpha),  sin(theta)*sin(alpha), r* cos(theta),
   sin(theta),  cos(theta)*cos(alpha), -cos(theta)*sin(alpha), r* sin(theta),
   0,             sin(alpha),             cos(alpha),            d,