From 6cce388a7d05981d7778e09136d0fec8da73f4fe Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Tue, 2 Jan 2024 11:02:00 +0100 Subject: [PATCH] Moved position/orientation to roboid --- Propulsion.cpp | 23 +---------------------- Propulsion.h | 15 ++++++--------- Roboid.cpp | 21 +++++++++++++++++++++ Roboid.h | 44 ++++++++++++++++++++++++++++++++++++++++++-- 4 files changed, 70 insertions(+), 33 deletions(-) diff --git a/Propulsion.cpp b/Propulsion.cpp index d23510d..9b40f18 100644 --- a/Propulsion.cpp +++ b/Propulsion.cpp @@ -43,25 +43,4 @@ void Propulsion::SetTwistSpeed(Vector3 linear, float yaw, float pitch, Polar Propulsion::GetVelocity() { return Polar(0, 0); } -float Propulsion::GetAngularVelocity() { return 0; } - -Vector3 Propulsion::GetPosition() { return this->worldPosition; } - -Quaternion Propulsion::GetOrientation() { return this->worldOrientation; } - -void Propulsion::SetPosition(Vector3 newWorldPosition) { - Vector3 translation = newWorldPosition - this->worldPosition; - float distance = translation.magnitude(); - float angle = Vector3::SignedAngle(this->worldOrientation * Vector3::forward, - translation, Vector3::up); - Polar polarTranslation = Polar(angle, distance); - roboid->perception->UpdatePose(polarTranslation); - this->worldPosition = newWorldPosition; -} - -void Propulsion::SetOrientation(Quaternion worldOrientation) { - Quaternion delta = - Quaternion::Inverse(this->worldOrientation) * worldOrientation; - roboid->perception->UpdatePose(delta); - this->worldOrientation = worldOrientation; -} \ No newline at end of file +float Propulsion::GetAngularVelocity() { return 0; } \ No newline at end of file diff --git a/Propulsion.h b/Propulsion.h index 8cad843..47d4b91 100644 --- a/Propulsion.h +++ b/Propulsion.h @@ -63,15 +63,15 @@ public: virtual void SetTwistSpeed(Vector3 linear, float yaw = 0.0F, float pitch = 0.0F, float roll = 0.0F); + /// @brief Retrieve the current velocity of the roboid + /// @return The velocity in polar coordinates + /// The actual units of the velocity depend on the implementation virtual Polar GetVelocity(); + /// @brief Retrieve the current angular velocity of the roboid + /// @return The angular velocity + /// The actual unit of the angular velocity depend on the implementation virtual float GetAngularVelocity(); - virtual Vector3 GetPosition(); - virtual Quaternion GetOrientation(); - - virtual void SetPosition(Vector3 worldPosition); - virtual void SetOrientation(Quaternion worldOrientation); - Roboid *roboid = nullptr; protected: @@ -79,9 +79,6 @@ protected: unsigned int motorCount = 0; /// @brief The Placement of the motors used for Propulsion Placement *placement = nullptr; - - Vector3 worldPosition = Vector3::zero; - Quaternion worldOrientation = Quaternion::identity; }; } // namespace RoboidControl diff --git a/Roboid.cpp b/Roboid.cpp index 6da0535..b1e3fd0 100644 --- a/Roboid.cpp +++ b/Roboid.cpp @@ -16,4 +16,25 @@ void Roboid::Update(float currentTimeMs) { perception->Update(currentTimeMs); if (networkSync != nullptr) networkSync->NetworkUpdate(this); +} + +Vector3 Roboid::GetPosition() { return this->worldPosition; } + +Quaternion Roboid::GetOrientation() { return this->worldOrientation; } + +void Roboid::SetPosition(Vector3 newWorldPosition) { + Vector3 translation = newWorldPosition - this->worldPosition; + float distance = translation.magnitude(); + float angle = Vector3::SignedAngle(this->worldOrientation * Vector3::forward, + translation, Vector3::up); + Polar polarTranslation = Polar(angle, distance); + perception->UpdatePose(polarTranslation); + this->worldPosition = newWorldPosition; +} + +void Roboid::SetOrientation(Quaternion worldOrientation) { + Quaternion delta = + Quaternion::Inverse(this->worldOrientation) * worldOrientation; + perception->UpdatePose(delta); + this->worldOrientation = worldOrientation; } \ No newline at end of file diff --git a/Roboid.h b/Roboid.h index 063c3ad..babc90e 100644 --- a/Roboid.h +++ b/Roboid.h @@ -24,11 +24,51 @@ public: void Update(float currentTimeMs); /// @brief The Perception module of this Roboid - Perception *perception; + Perception *perception = nullptr; /// @brief The Propulsion module of this Roboid - Propulsion *propulsion; + Propulsion *propulsion = nullptr; + /// @brief The reference to the module to synchronize states across a network NetworkSync *networkSync = nullptr; + + /// @brief Retrieve the current position of the roboid + /// @return The position in carthesian coordinates in world space + /// The origin and units of the position depends on the position tracking + /// system used. This value will be Vector3::zero unless a position is + /// received through network synchronisation + virtual Vector3 GetPosition(); + /// @brief Retrieve the current orientation of the roboid + /// @return The orientation quaternion in world space + /// The origin orientation depends on the position tracking system used. This + /// value will be Quaternion::identity unless an orientation is received + /// though network synchronization + virtual Quaternion GetOrientation(); + + /// @brief Update the current position of the roboid + /// @param worldPosition The position of the roboid in carthesian coordinates + /// in world space The use of this function will also update the positions and + /// orientations of the perceived objects by the roboid + /// (roboid->perception->perceivedObjects), as these are local to the + /// roboid's position. + virtual void SetPosition(Vector3 worldPosition); + /// @brief Update the current orientation of the roboid + /// @param worldOrientation The orientation of the roboid in world space + /// The use of this function will also update the orientations of the + /// perceived objects by the roboid (roboid->perception->perceivedObjets), + /// as these are local to the roboid' orientation. + virtual void SetOrientation(Quaternion worldOrientation); + +private: + /// @brief The position of the roboid in carthesian coordinates in world space + /// This position may be set when NetworkSync is used to receive + /// positions from an external tracking system. These values should not be set + /// directly, but SetPosition should be used instead. + Vector3 worldPosition = Vector3::zero; + /// @brief The orientation of the roboid in world space + /// The position may be set when NetworkSync is used to receive orientations + /// from an external tracking system. This value should not be set directly, + /// but SetOrientation should be used instead. + Quaternion worldOrientation = Quaternion::identity; }; } // namespace RoboidControl