Merge pull request #7 from LemLib/pose-derivatives

✨ Pose Derivatives

include/units/Pose.hpp

@@ -1,5 +1,6 @@
 #pragma once
 
+#include "Angle.hpp"
 #include "units/Vector2D.hpp"
 
 namespace units {
@@ -9,11 +10,9 @@ namespace units {
  * @brief A class that represents a position and orientation in 2D space
  *
  * This class inherits from V2Position, which is identical to Vector2D<Length>. The only difference here
- * is that Pose has an additional member variable, theta, which represents orientation.
+ * is that Pose has an additional member variable, orientation.
  */
 class Pose : public V2Position {
-    protected:
-        Angle theta; /** Orientation */
     public:
         /**
          * @brief Construct a new Pose object
@@ -25,7 +24,7 @@ class Pose : public V2Position {
         /**
          * @brief Construct a new Pose object
          *
-         * This constructor initializes x, y to the given values and orientation to 0
+         * This constructor initializes orientation to 0
          *
          * @param v position
          */
@@ -35,39 +34,182 @@ class Pose : public V2Position {
          * @brief Construct a new Pose object
          *
          * @param v position
-         * @param h orientation
+         * @param orientation orientation
          */
-        Pose(V2Position v, Angle h);
+        Pose(V2Position v, Angle orientation);
 
         /**
          * @brief Construct a new Pose object
          *
-         * @param nx x position
-         * @param ny y position
+         * This constructor initializes orientation to 0
+         *
+         * @param x x position
+         * @param y y position
          */
-        Pose(Length nx, Length ny);
+        Pose(Length x, Length y);
 
         /**
          * @brief Construct a new Pose object
          *
-         * @param nx x position
-         * @param ny y position
-         * @param nh orientation
+         * @param x x position
+         * @param y y position
+         * @param orientation orientation
          */
-        Pose(Length nx, Length ny, Angle nh);
+        Pose(Length x, Length y, Angle orientation);
 
         /**
          * @brief Get the orientation
          *
          * @return Angle orientation
          */
-        Angle getTheta();
+        Angle getOrientation();
 
         /**
          * @brief Set the orientation
          *
-         * @param h orientation
+         * @param orientation orientation
+         */
+        void setOrientation(Angle orientation);
+    protected:
+        Angle orientation; /** Orientation */
+};
+
+/**
+ * @class VelocityPose
+ *
+ * @brief A class that represents a linear velocity and angular velocity in 2D space
+ *
+ * This class inherits from V2Velocity, which is identical to Vector2D<LinearVelocity>. The only difference here
+ * is that VelocityPose has an additional member variable, angularVelocity.
+ */
+class VelocityPose : public V2Velocity {
+        /**
+         * @brief Construct a new VelocityPose object
+         *
+         * This constructor initializes x velocity, y velocity, and angular velocity to 0
+         */
+        VelocityPose();
+
+        /**
+         * @brief Construct a new VelocityPose object
+         *
+         * This constructor initializes angular velocity to 0
+         *
+         * @param v linear velocity vector
+         */
+        VelocityPose(V2Velocity v);
+
+        /**
+         * @brief Construct a new VelocityPose object
+         *
+         * @param v linear velocity vector
+         * @param angularVelocity angular velocity
+         */
+        VelocityPose(V2Velocity v, AngularVelocity angularVelocity);
+
+        /**
+         * @brief Construct a new VelocityPose object
+         *
+         * This constructor initializes angular velocity to 0
+         *
+         * @param x x linear velocity
+         * @param y y linear velocity
+         */
+        VelocityPose(LinearVelocity x, LinearVelocity y);
+
+        /**
+         * @brief Construct a new VelocityPose object
+         *
+         * @param x x linear velocity
+         * @param y y linear velocity
+         * @param angularVelocity  angular velocity
+         */
+        VelocityPose(LinearVelocity x, LinearVelocity y, AngularVelocity angularVelocity);
+
+        /**
+         * @brief Get the angular velocity
+         *
+         * @return AngularVelocity
+         */
+        AngularVelocity getAngularVelocity();
+
+        /**
+         * @brief Set the angular velocity
+         *
+         * @param angularVelocity angular velocity
          */
-        void setTheta(Angle h);
+        void setAngularVelocity(AngularVelocity angularVelocity);
+    protected:
+        AngularVelocity angularVelocity; /** Angular velocity */
+};
+
+/**
+ * @class AccelerationPose
+ *
+ * @brief A class that represents a linear acceleration and angular acceleration in 2D space
+ *
+ * This class inherits from V2Acceleration, which is identical to Vector2D<LinearAcceleration>. The only difference here
+ * is that AccelerationPose has an additional member variable, angularAcceleration.
+ */
+class AccelerationPose : public V2Acceleration {
+    public:
+        /**
+         * @brief Construct a new AccelerationPose object
+         *
+         * This constructor initializes x acceleration, y acceleration, and angular acceleration to 0
+         */
+        AccelerationPose();
+
+        /**
+         * @brief Construct a new AccelerationPose object
+         *
+         * This constructor initializes angular acceleration to 0
+         *
+         * @param v linear acceleration vector
+         */
+        AccelerationPose(V2Acceleration v);
+
+        /**
+         * @brief Construct a new AccelerationPose object
+         *
+         * @param v linear acceleration vector
+         * @param angularAcceleration angular acceleration
+         */
+        AccelerationPose(V2Acceleration v, AngularAcceleration angularAcceleration);
+
+        /**
+         * @brief Construct a new AccelerationPose object
+         *
+         * This constructor initializes angular acceleration to 0
+         *
+         * @param x x linear acceleration
+         * @param y y linear acceleration
+         */
+        AccelerationPose(LinearAcceleration x, LinearAcceleration y);
+
+        /**
+         * @brief Construct a new AccelerationPose object
+         *
+         * @param x x linear acceleration
+         * @param y y linear acceleration
+         * @param angularAcceleration  angular acceleration
+         */
+        AccelerationPose(LinearAcceleration x, LinearAcceleration y, AngularAcceleration angularAcceleration);
+
+        /**
+         * @brief Get the angular acceleration
+         *
+         * @return AngularAcceleration
+         */
+        AngularAcceleration getAngularAcceleration();
+
+        /**
+         * @brief Set the angular acceleration
+         *
+         * @param angularAcceleration angular acceleration
+         */
+        void setAngularAcceleration(AngularAcceleration angularAcceleration);
+    protected:
+        AngularAcceleration angularAcceleration; /** Angular acceleration */
 };
 } // namespace units

src/units/Pose.cpp

@@ -1,15 +1,39 @@
 #include "units/Pose.hpp"
 
 namespace units {
-Pose::Pose() : V2Position(), theta(0.0) {}
+Pose::Pose() : V2Position(), orientation(0.0) {}
 
-Pose::Pose(V2Position v) : V2Position(v), theta(0.0) {}
+Pose::Pose(V2Position v) : V2Position(v), orientation(0.0) {}
 
-Pose::Pose(V2Position v, Angle h) : V2Position(v), theta(0.0) {}
+Pose::Pose(V2Position v, Angle orientation) : V2Position(v), orientation(orientation) {}
 
-Pose::Pose(Length nx, Length ny, Angle nh) : V2Position(nx, ny), theta(nh) {}
+Pose::Pose(Length x, Length y, Angle orientation) : V2Position(x, y), orientation(orientation) {}
 
-Angle Pose::getTheta() { return theta; }
+Angle Pose::getOrientation() { return orientation; }
 
-void Pose::setTheta(Angle h) { theta = h; }
+void Pose::setOrientation(Angle orientation) { this->orientation = orientation; }
+
+VelocityPose::VelocityPose() : V2Velocity(), angularVelocity(0.0) {}
+
+VelocityPose::VelocityPose(V2Velocity v) : V2Velocity(v), angularVelocity(0.0) {}
+
+VelocityPose::VelocityPose(V2Velocity v, AngularVelocity angularVelocity)
+    : V2Velocity(v), angularVelocity(angularVelocity) {}
+
+AngularVelocity VelocityPose::getAngularVelocity() { return angularVelocity; }
+
+void VelocityPose::setAngularVelocity(AngularVelocity angularVelocity) { this->angularVelocity = angularVelocity; }
+
+AccelerationPose::AccelerationPose() : V2Acceleration(), angularAcceleration(0.0) {}
+
+AccelerationPose::AccelerationPose(V2Acceleration v) : V2Acceleration(v), angularAcceleration(0.0) {}
+
+AccelerationPose::AccelerationPose(V2Acceleration v, AngularAcceleration angularAcceleration)
+    : V2Acceleration(v), angularAcceleration(angularAcceleration) {}
+
+AngularAcceleration AccelerationPose::getAngularAcceleration() { return angularAcceleration; }
+
+void AccelerationPose::setAngularAcceleration(AngularAcceleration angularAcceleration) {
+    this->angularAcceleration = angularAcceleration;
+}
 } // namespace units