From 5b4dfbb0ae05e394200990506373b9587cbbc154 Mon Sep 17 00:00:00 2001 From: Pascal Serrarens Date: Mon, 16 Sep 2024 15:21:47 +0200 Subject: [PATCH] Improved angular support --- LinearAlgebra | 2 +- NetworkSync.cpp | 70 ++++++++++++++++++++++++++--------------------- NetworkSync.h | 8 +++--- Roboid.cpp | 18 ++++++------ Roboid.h | 2 +- ServoMotor.cpp | 11 ++++---- ServoMotor.h | 2 +- Thing.cpp | 8 ++++++ Thing.h | 11 ++++++-- TrackedObject.cpp | 4 +-- TrackedObject.h | 2 +- 11 files changed, 80 insertions(+), 58 deletions(-) diff --git a/LinearAlgebra b/LinearAlgebra index 09e25a8..3363388 160000 --- a/LinearAlgebra +++ b/LinearAlgebra @@ -1 +1 @@ -Subproject commit 09e25a8a218690157ba46ca5f7ccd5f33eab1c07 +Subproject commit 3363388a95d8fe9708b615fc2ecdc9474f4930a1 diff --git a/NetworkSync.cpp b/NetworkSync.cpp index 2ef3298..dec63ae 100644 --- a/NetworkSync.cpp +++ b/NetworkSync.cpp @@ -49,8 +49,7 @@ void NetworkSync::PublishState(Roboid* roboid) { // if (roboid->updated == false) // return; - if (roboid->GetVelocity().magnitude() > 0) - SendPose(roboid); + SendPose(roboid); PublishPerception(roboid); } @@ -177,39 +176,48 @@ void NetworkSync::SendDestroyThing(InterestingThing* thing) { #endif } -void NetworkSync::SendPose(Roboid* roboid, bool recurse) { +// void NetworkSync::SendPose(Roboid* roboid, bool recurse) { +// if (networkId == 0) // We're not connected to a site yet +// return; + +// if (roboid->GetVelocity().magnitude() > 0) { +// unsigned char ix = 0; +// buffer[ix++] = PoseMsg; +// buffer[ix++] = 0x00; +// buffer[ix++] = Pose_Position | Pose_Orientation; +// SendSpherical(buffer, &ix, +// Spherical::FromVector3(roboid->GetPosition())); SendQuat32(buffer, &ix, +// roboid->GetOrientation()); SendBuffer(ix); +// } + +// #if RC_DEBUG +// printf("Sent PoseMsg [%d/%d]\n", networkId, buffer[1]); +// #endif + +// if (recurse) { +// for (unsigned char childIx = 0; childIx < roboid->childCount; childIx++) +// { +// Thing* child = roboid->GetChild(childIx); +// if (child != nullptr) +// SendPose(child, true); +// } +// } +// } + +void NetworkSync::SendPose(Thing* thing, bool recurse) { if (networkId == 0) // We're not connected to a site yet return; - unsigned char ix = 0; - buffer[ix++] = PoseMsg; - buffer[ix++] = 0x00; - buffer[ix++] = Pose_Position | Pose_Orientation; - SendSpherical(buffer, &ix, Spherical::FromVector3(roboid->GetPosition())); - SendQuat32(buffer, &ix, roboid->GetOrientation()); - SendBuffer(ix); - -#if RC_DEBUG - printf("Sent PoseMsg [%d/%d]\n", networkId, buffer[1]); -#endif - - if (recurse) { - for (unsigned char childIx = 0; childIx < roboid->childCount; childIx++) { - Thing* child = roboid->GetChild(childIx); - if (child != nullptr) - SendPose(child, true); - } + if (thing->GetLinearVelocity().distance > 0 || + thing->GetAngularVelocity().angle > 0) { + unsigned char ix = 0; + buffer[ix++] = PoseMsg; + buffer[ix++] = thing->id; + buffer[ix++] = Pose_Position | Pose_Orientation; + SendSpherical16(buffer, &ix, thing->position); + SendQuat32(buffer, &ix, thing->orientation); + SendBuffer(ix); } -} - -void NetworkSync::SendPose(Thing* thing, bool recurse) { - unsigned char ix = 0; - buffer[ix++] = PoseMsg; - buffer[ix++] = thing->id; - buffer[ix++] = Pose_Position | Pose_Orientation; - SendSpherical16(buffer, &ix, thing->position); - SendQuat32(buffer, &ix, thing->orientation); - SendBuffer(ix); #if RC_DEBUG printf("Sent PoseMsg Thing [%d/%d]\n", networkId, buffer[1]); diff --git a/NetworkSync.h b/NetworkSync.h index 0d59d32..7614a77 100644 --- a/NetworkSync.h +++ b/NetworkSync.h @@ -77,7 +77,7 @@ class NetworkSync { virtual void SendPosition(Vector3 worldPosition) {}; virtual void SendPose(Vector3 worldPosition, Quaternion worldOrientation) {}; - void SendPose(Roboid* roboid, bool recurse = true); + // void SendPose(Roboid* roboid, bool recurse = true); void SendPose(Thing* thing, bool recurse = true); virtual void SendText(const char* s); @@ -90,9 +90,9 @@ class NetworkSync { void PublishState(Sensor* sensor); void PublishTrackedObject(Roboid* roboid, InterestingThing* object); - void PublishRelativeObject(Buffer sendBuffer, - UInt8 parentId, - InterestingThing* object); + // void PublishRelativeObject(Buffer sendBuffer, + // UInt8 parentId, + // InterestingThing* object); void SendSingle100(unsigned char* data, unsigned int startIndex, float value); void SendFloat16(unsigned char* data, unsigned char* startIndex, float value); diff --git a/Roboid.cpp b/Roboid.cpp index 8ecfb5d..cab60a0 100644 --- a/Roboid.cpp +++ b/Roboid.cpp @@ -23,7 +23,7 @@ Roboid::Roboid() : Thing(0) { // this->actuation = nullptr; this->worldPosition = Vector3::zero; // this->worldOrientation = Quaternion::identity; - this->worldAngleAxis = AngleAxis(); + this->worldAngleAxis = AngleAxisOf(); } Roboid::Roboid(Propulsion* propulsion) : Roboid() { @@ -74,8 +74,7 @@ Vector2 Roboid::GetPosition2D() { Quaternion Roboid::GetOrientation() { Vector3 axis = this->worldAngleAxis.axis.ToVector3(); - Quaternion q = - Quaternion::AngleAxis(this->worldAngleAxis.angle.ToFloat(), axis); + Quaternion q = Quaternion::AngleAxis(this->worldAngleAxis.angle, axis); return q; } @@ -83,10 +82,10 @@ float Roboid::GetOrientation2D() { float maxAngle = 90 - Float::epsilon; // note: range vertical angle = -90..90 // rotation axis is vertical, so we have a simple 2D orientation - if (this->worldAngleAxis.axis.verticalAngle.ToFloat() > maxAngle) - return this->worldAngleAxis.angle.ToFloat(); - if (this->worldAngleAxis.axis.verticalAngle.ToFloat() < -maxAngle) - return -this->worldAngleAxis.angle.ToFloat(); + if (this->worldAngleAxis.axis.vertical.InDegrees() > maxAngle) + return this->worldAngleAxis.angle; + if (this->worldAngleAxis.axis.vertical.InDegrees() < -maxAngle) + return -this->worldAngleAxis.angle; Quaternion q = GetOrientation(); return Quaternion::GetAngleAround(Vector3::up, q); @@ -118,12 +117,13 @@ void Roboid::SetOrientation(Quaternion worldOrientation) { if (perception != nullptr) perception->UpdatePose(delta); - AngleAxis angleAxis = AngleAxis(angle, Direction(axis)); + AngleAxisOf angleAxis = + AngleAxisOf(angle, DirectionOf(axis)); this->worldAngleAxis = angleAxis; } void Roboid::SetOrientation2D(float angle) { - this->worldAngleAxis = AngleAxis(angle, Direction::up); + this->worldAngleAxis = AngleAxisOf(angle, DirectionOf::up); } Vector3 Passer::RoboidControl::Roboid::GetVelocity() { diff --git a/Roboid.h b/Roboid.h index 13ba6c0..a308101 100644 --- a/Roboid.h +++ b/Roboid.h @@ -84,7 +84,7 @@ class Roboid : public Thing { /// set directly, but SetOrientation should be used instead. // Quaternion worldOrientation = Quaternion::identity; - AngleAxis worldAngleAxis = AngleAxis(); + AngleAxis worldAngleAxis = AngleAxis(); unsigned long lastUpdateTimeMs = 0; }; diff --git a/ServoMotor.cpp b/ServoMotor.cpp index b7b3608..b9f0495 100644 --- a/ServoMotor.cpp +++ b/ServoMotor.cpp @@ -13,7 +13,7 @@ ServoMotor::ServoMotor() void ServoMotor::SetTargetAngle(Angle16 angle) { angle = Float::Clamp(angle.ToFloat(), minAngle.ToFloat(), maxAngle.ToFloat()); - if (maxVelocity == 0.0F) { + if (maxSpeed == 0.0F) { SetAngle(angle); this->limitedTargetAngle = angle; } @@ -28,12 +28,11 @@ Angle16 ServoMotor::GetTargetAngle() { } void ServoMotor::SetMaximumVelocity(float maxVelocity) { - this->maxVelocity = maxVelocity; + this->maxSpeed = maxVelocity; } void ServoMotor::SetTargetVelocity(float targetVelocity) { - targetVelocity = - Float::Clamp(targetVelocity, -this->maxVelocity, maxVelocity); + targetVelocity = Float::Clamp(targetVelocity, -this->maxSpeed, maxSpeed); this->controlMode = ControlMode::Velocity; this->targetVelocity = targetVelocity; @@ -62,12 +61,12 @@ void ServoMotor::Update(unsigned long currentTimeMs) { // Position control if (controlMode == ControlMode::Position) { - if (maxVelocity == 0.0F || hasTargetAngle == false) { + if (maxSpeed == 0.0F || hasTargetAngle == false) { this->lastUpdateTimeMs = currentTimeMs; return; } else { - float angleStep = maxVelocity * deltaTime; + float angleStep = maxSpeed * deltaTime; float deltaAngle = this->targetAngle.ToFloat() - this->limitedTargetAngle.ToFloat(); float absDeltaAngle = (deltaAngle < 0) ? -deltaAngle : deltaAngle; diff --git a/ServoMotor.h b/ServoMotor.h index 5c7c8c9..1045c16 100644 --- a/ServoMotor.h +++ b/ServoMotor.h @@ -36,7 +36,7 @@ class ServoMotor : public Thing { Angle16 targetAngle = 0.0F; Angle16 actualAngle = 0.0F; - float maxVelocity = 0.0F; + float maxSpeed = 0.0F; float targetVelocity = 0.0F; diff --git a/Thing.cpp b/Thing.cpp index 1f346a0..c1e5d32 100644 --- a/Thing.cpp +++ b/Thing.cpp @@ -75,4 +75,12 @@ Thing* Thing::GetChild(unsigned char childIx) { return this->children[childIx]; } else return nullptr; +} + +Spherical16 Thing::GetLinearVelocity() { + return this->linearVelocity; +} + +AngleAxis16 Thing::GetAngularVelocity() { + return this->angularVelocity; } \ No newline at end of file diff --git a/Thing.h b/Thing.h index 68e72ac..6257d55 100644 --- a/Thing.h +++ b/Thing.h @@ -1,5 +1,6 @@ #pragma once +#include "LinearAlgebra/AngleAxis.h" #include "LinearAlgebra/Quaternion.h" #include "LinearAlgebra/Spherical.h" @@ -17,8 +18,8 @@ class Thing { /// @brief The type of Thing unsigned int type; - // I hate this, better is to have an additional field stating the thing classificaton - // Motor, Sensor etc. + // I hate this, better is to have an additional field stating the thing + // classificaton Motor, Sensor etc. /// @brief The type of a switch sensor static const unsigned int SwitchType; /// @brief The type of a distance sensor @@ -51,6 +52,9 @@ class Thing { Quaternion orientation; Quaternion worldOrientation; + virtual Spherical16 GetLinearVelocity(); + virtual AngleAxis16 GetAngularVelocity(); + /// @brief Sets the parent Thing /// @param parent The Thing which should become the parnet /// @remark This is equivalent to calling parent->AddChild(this); @@ -109,6 +113,9 @@ class Thing { Thing* parent = nullptr; Thing** children = nullptr; + + AngleAxis16 angularVelocity = AngleAxis16::zero; + Spherical16 linearVelocity = Spherical16::zero; }; } // namespace RoboidControl diff --git a/TrackedObject.cpp b/TrackedObject.cpp index a2a8aa3..5e883eb 100644 --- a/TrackedObject.cpp +++ b/TrackedObject.cpp @@ -21,7 +21,7 @@ InterestingThing::InterestingThing(Sensor* sensor, float angle; Vector3 axis; orientation.ToAngleAxis(&angle, &axis); - this->orientation = AngleAxis(angle, axis); + this->orientation = AngleAxisOf(angle, axis); this->updated = true; } @@ -85,7 +85,7 @@ void InterestingThing::Refresh(Spherical16 position, Quaternion orientation) { float angle; Vector3 axis; orientation.ToAngleAxis(&angle, &axis); - this->orientation = AngleAxis(angle, axis); + this->orientation = AngleAxisOf(angle, axis); this->confidence = maxConfidence; this->updated = true; } diff --git a/TrackedObject.h b/TrackedObject.h index 57ad432..54c743e 100644 --- a/TrackedObject.h +++ b/TrackedObject.h @@ -69,7 +69,7 @@ class InterestingThing { /// @brief The current position of the object Spherical16 position = Spherical16::zero; /// @brief The current orientation of the object - AngleAxis orientation = AngleAxis(); + AngleAxisOf orientation = AngleAxisOf(); // Quaternion orientation = Quaternion::identity; /// @brief The sensor which provided that lastet pose this object Sensor* sensor = nullptr;