Use AngleAxis for tracked thing orientation (but this will change again in the future)

This commit is contained in:
Pascal Serrarens 2024-08-06 12:10:35 +02:00
parent 10700da996
commit c421fd4db7
7 changed files with 43 additions and 32 deletions

@ -1 +1 @@
Subproject commit 51772a18433c702244af814e0e7c24b28e904ada Subproject commit 353cb1bc7f3c12f703c0f963d89d08d567fc446a

View File

@ -211,7 +211,8 @@ void NetworkSync::PublishTrackedObject(Roboid* roboid,
// Vector3 localPosition = object->position.ToVector3(); // Vector3 localPosition = object->position.ToVector3();
Vector3 localPosition = Vector3(object->position); Vector3 localPosition = Vector3(object->position);
Vector3 worldPosition = roboidPosition + roboidOrientation * localPosition; Vector3 worldPosition = roboidPosition + roboidOrientation * localPosition;
Quaternion worldOrientation = roboidOrientation * object->orientation; Quaternion worldOrientation =
roboidOrientation * object->orientation.ToQuaternion();
unsigned char ix = 0; unsigned char ix = 0;
buffer[ix++] = PoseMsg; // Position2DMsg; buffer[ix++] = PoseMsg; // Position2DMsg;

View File

@ -306,8 +306,8 @@ InterestingThing* Perception::AddTrackedObject(Sensor* sensor,
// this->roboid->networkSync->SendText("Update\0"); // this->roboid->networkSync->SendText("Update\0");
// else // else
// this->roboid->networkSync->SendText("Refresh\0"); // this->roboid->networkSync->SendText("Refresh\0");
this->trackedObjects[thingIx]->Refresh(thing->position, this->trackedObjects[thingIx]->Refresh(
thing->orientation); thing->position, thing->orientation.ToQuaternion());
delete thing; delete thing;
return this->trackedObjects[thingIx]; return this->trackedObjects[thingIx];
} }

View File

@ -23,7 +23,7 @@ Roboid::Roboid() : Thing(0) {
// this->actuation = nullptr; // this->actuation = nullptr;
this->worldPosition = Vector3::zero; this->worldPosition = Vector3::zero;
// this->worldOrientation = Quaternion::identity; // this->worldOrientation = Quaternion::identity;
this->worldAngleAxis = AngleAxis(); this->worldAngleAxis = AngleAxis<float>();
} }
Roboid::Roboid(Propulsion* propulsion) : Roboid() { Roboid::Roboid(Propulsion* propulsion) : Roboid() {
@ -92,9 +92,9 @@ void Roboid::SetPosition(Vector3 newWorldPosition) {
Quaternion roboidOrientation = this->GetOrientation(); Quaternion roboidOrientation = this->GetOrientation();
Vector3 translation = newWorldPosition - this->worldPosition; Vector3 translation = newWorldPosition - this->worldPosition;
float distance = translation.magnitude(); float distance = translation.magnitude();
float angle = Vector3::SignedAngle(roboidOrientation * Vector3::forward, AngleOf<float> angle = Vector3::SignedAngle(
translation, Vector3::up); roboidOrientation * Vector3::forward, translation, Vector3::up);
Polar polarTranslation = Polar(angle, distance); Polar polarTranslation = Polar(angle.ToFloat(), distance);
if (perception != nullptr) if (perception != nullptr)
perception->UpdatePose(polarTranslation); perception->UpdatePose(polarTranslation);
this->worldPosition = newWorldPosition; this->worldPosition = newWorldPosition;
@ -114,12 +114,12 @@ void Roboid::SetOrientation(Quaternion worldOrientation) {
if (perception != nullptr) if (perception != nullptr)
perception->UpdatePose(delta); perception->UpdatePose(delta);
AngleAxis angleAxis = AngleAxis(angle, Axis(axis)); AngleAxis<float> angleAxis = AngleAxis<float>(angle, Direction<float>(axis));
this->worldAngleAxis = angleAxis; this->worldAngleAxis = angleAxis;
} }
void Roboid::SetOrientation2D(float angle) { void Roboid::SetOrientation2D(float angle) {
this->worldAngleAxis = AngleAxis(angle, Axis::up); this->worldAngleAxis = AngleAxis<float>(angle, Direction<float>::up);
} }
void Roboid::AddChild(Thing* child) { void Roboid::AddChild(Thing* child) {

View File

@ -12,13 +12,13 @@ class NetworkSync;
/// @brief A Roboid is used to control autonomous robots /// @brief A Roboid is used to control autonomous robots
class Roboid : public Thing { class Roboid : public Thing {
public: public:
/// @brief Default constructor for a Roboid /// @brief Default constructor for a Roboid
Roboid(); Roboid();
/// @brief Creates a Roboid with Perception and Propulsion abilities /// @brief Creates a Roboid with Perception and Propulsion abilities
/// @param perception The Perception implementation to use for this Roboid /// @param perception The Perception implementation to use for this Roboid
/// @param propulsion The Propulsion implementation to use for this Roboid /// @param propulsion The Propulsion implementation to use for this Roboid
Roboid(Propulsion *propulsion); Roboid(Propulsion* propulsion);
/// @brief Update the state of the Roboid /// @brief Update the state of the Roboid
/// @param currentTimeMs The time in milliseconds when calling this /// @param currentTimeMs The time in milliseconds when calling this
@ -26,13 +26,13 @@ public:
void Update(unsigned long currentTimeMs); void Update(unsigned long currentTimeMs);
/// @brief The Perception module of this Roboid /// @brief The Perception module of this Roboid
Perception *perception = nullptr; Perception* perception = nullptr;
/// @brief The Propulsion module of this Roboid /// @brief The Propulsion module of this Roboid
Propulsion *propulsion = nullptr; Propulsion* propulsion = nullptr;
// ServoMotor *actuation = nullptr; // ServoMotor *actuation = nullptr;
/// @brief The reference to the module to synchronize states across a network /// @brief The reference to the module to synchronize states across a network
NetworkSync *networkSync = nullptr; NetworkSync* networkSync = nullptr;
/// @brief Retrieve the current position of the roboid /// @brief Retrieve the current position of the roboid
/// @return The position in carthesian coordinates in world space /// @return The position in carthesian coordinates in world space
@ -65,9 +65,9 @@ public:
virtual void SetOrientation(Quaternion worldOrientation); virtual void SetOrientation(Quaternion worldOrientation);
void SetOrientation2D(float angle); void SetOrientation2D(float angle);
virtual void AddChild(Thing *child) override; virtual void AddChild(Thing* child) override;
private: private:
/// @brief The position of the roboid in carthesian coordinates in world space /// @brief The position of the roboid in carthesian coordinates in world space
/// @details This position may be set when NetworkSync is used to receive /// @details This position may be set when NetworkSync is used to receive
/// positions from an external tracking system. These values should not be set /// positions from an external tracking system. These values should not be set
@ -79,7 +79,7 @@ private:
/// set directly, but SetOrientation should be used instead. /// set directly, but SetOrientation should be used instead.
// Quaternion worldOrientation = Quaternion::identity; // Quaternion worldOrientation = Quaternion::identity;
AngleAxis worldAngleAxis = AngleAxis(); AngleAxis<float> worldAngleAxis = AngleAxis<float>();
unsigned long lastUpdateTimeMs = 0; unsigned long lastUpdateTimeMs = 0;
}; };

View File

@ -17,7 +17,10 @@ InterestingThing::InterestingThing(Sensor* sensor,
this->confidence = maxConfidence; this->confidence = maxConfidence;
this->sensor = sensor; this->sensor = sensor;
this->position = position; this->position = position;
this->orientation = orientation; float angle;
Vector3 axis;
orientation.ToAngleAxis(&angle, &axis);
this->orientation = AngleAxis<float>(angle, axis);
this->updated = true; this->updated = true;
} }
@ -76,7 +79,11 @@ void InterestingThing::Refresh(Polar position) {
void InterestingThing::Refresh(Spherical position, Quaternion orientation) { void InterestingThing::Refresh(Spherical position, Quaternion orientation) {
this->position = position; this->position = position;
this->orientation = orientation;
float angle;
Vector3 axis;
orientation.ToAngleAxis(&angle, &axis);
this->orientation = AngleAxis<float>(angle, axis);
this->confidence = maxConfidence; this->confidence = maxConfidence;
this->updated = true; this->updated = true;
} }

View File

@ -1,5 +1,6 @@
#pragma once #pragma once
#include "LinearAlgebra/AngleAxis.h"
#include "LinearAlgebra/Polar.h" #include "LinearAlgebra/Polar.h"
#include "LinearAlgebra/Quaternion.h" #include "LinearAlgebra/Quaternion.h"
#include "LinearAlgebra/Spherical.h" #include "LinearAlgebra/Spherical.h"
@ -10,12 +11,13 @@ namespace RoboidControl {
/// @brief An object tracked by the roboid /// @brief An object tracked by the roboid
class InterestingThing { class InterestingThing {
public: public:
/// @brief An object tracked by the roboid /// @brief An object tracked by the roboid
/// @param sensor The Sensor which detected this object /// @param sensor The Sensor which detected this object
/// @param position The position in polar coordinates local to the roboid /// @param position The position in polar coordinates local to the roboid
InterestingThing(Sensor *sensor, Polar position); InterestingThing(Sensor* sensor, Polar position);
InterestingThing(Sensor *sensor, Spherical position, InterestingThing(Sensor* sensor,
Spherical position,
Quaternion orientation = Quaternion::identity); Quaternion orientation = Quaternion::identity);
/// @brief Update the position of the object /// @brief Update the position of the object
@ -39,7 +41,7 @@ public:
/// @return Returns true when both objects are considered the same /// @return Returns true when both objects are considered the same
/// The result of this check depends on the equalityDistance and equalityAngle /// The result of this check depends on the equalityDistance and equalityAngle
/// value. /// value.
bool IsTheSameAs(InterestingThing *otherObj); bool IsTheSameAs(InterestingThing* otherObj);
/// @brief The maximum difference in distance from the roboid in which two /// @brief The maximum difference in distance from the roboid in which two
/// objects may be considered the same /// objects may be considered the same
/// @details When the difference in distance is exactly this /// @details When the difference in distance is exactly this
@ -66,15 +68,16 @@ public:
/// @brief The current position of the object /// @brief The current position of the object
Spherical position = Spherical::zero; Spherical position = Spherical::zero;
/// @brief The current orientation of the object /// @brief The current orientation of the object
Quaternion orientation = Quaternion::identity; AngleAxis<float> orientation = AngleAxis<float>();
// Quaternion orientation = Quaternion::identity;
/// @brief The sensor which provided that lastet pose this object /// @brief The sensor which provided that lastet pose this object
Sensor *sensor = nullptr; Sensor* sensor = nullptr;
unsigned char type = 0x00; unsigned char type = 0x00;
unsigned char confidence; unsigned char confidence;
bool updated = false; bool updated = false;
protected: protected:
static constexpr unsigned char maxConfidence = 255; static constexpr unsigned char maxConfidence = 255;
static constexpr unsigned char confidenceDropSpeed = 10; // 150; // 2; static constexpr unsigned char confidenceDropSpeed = 10; // 150; // 2;
}; };