RoboidControl/RoboidControl-cpp
RoboidControl/RoboidControl-cpp
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Improve use of position/orientation

Browse Source
This commit is contained in:
Pascal Serrarens 2024-10-21 17:54:27 +02:00
parent 6e996c64eb
commit 0e2f628e3e
5 changed files with 148 additions and 183 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
DifferentialDrive.cpp
View File

@ -64,9 +64,7 @@ void DifferentialDrive::SetTwistSpeed(Vector2 linear, float yaw) {
SetTwistSpeed(linear.y, yaw); SetTwistSpeed(linear.y, yaw);
} }
void DifferentialDrive::SetTwistSpeed(Vector3 linear, void DifferentialDrive::SetTwistSpeed(Vector3 linear, float yaw, float pitch,
float yaw,
float pitch,
float roll) { float roll) {
SetTwistSpeed(linear.Forward(), yaw); SetTwistSpeed(linear.Forward(), yaw);
} }

38
NetworkSync.cpp
View File

@ -5,7 +5,7 @@
#ifdef RC_DEBUG #ifdef RC_DEBUG
#include <Arduino.h> #include <Arduino.h>
#if ESP32 #if ESP32
#define SERIALPORT Serial0 #define SERIALPORT Serial
#else #else
#define SERIALPORT Serial #define SERIALPORT Serial
#endif #endif
@ -189,7 +189,7 @@ void NetworkSync::SendDestroyThing(InterestingThing* thing) {
} }
void NetworkSync::SendPose(Thing *thing, bool recurse) { void NetworkSync::SendPose(Thing *thing, bool recurse) {
if (networkId == 0) // We're not connected to a site yet if (this->networkId == 0) // We're not connected to a site yet
return; return;
if (thing->GetLinearVelocity().distance > 0 || if (thing->GetLinearVelocity().distance > 0 ||
@ -204,7 +204,8 @@ void NetworkSync::SendPose(Thing* thing, bool recurse) {
#if RC_DEBUG #if RC_DEBUG
if (thing->id == 0) if (thing->id == 0)
SERIALPORT.printf("Sent PoseMsg Thing [%d/%d]\n", networkId, buffer[1]); SERIALPORT.printf("Sent PoseMsg Thing [%d/%d]\n", this->networkId,
buffer[1]);
#endif #endif
} }
@ -292,6 +293,7 @@ void NetworkSync::PublishTrackedObject(Roboid* roboid,
} }
void NetworkSync::SendPoseMsg(Buffer sendBuffer, Roboid *roboid) { void NetworkSync::SendPoseMsg(Buffer sendBuffer, Roboid *roboid) {
// SERIALPORT.printf("old send pose?\n");
Polar velocity = roboid->propulsion->GetVelocity(); Polar velocity = roboid->propulsion->GetVelocity();
Vector2 worldVelocity2 = Vector2 worldVelocity2 =
Vector2::Rotate(Vector2::forward * velocity.distance, velocity.angle); Vector2::Rotate(Vector2::forward * velocity.distance, velocity.angle);
@ -389,8 +391,7 @@ void NetworkSync::SendInt(const int x) {
// Low-level functions // Low-level functions
void NetworkSync::SendVector3(unsigned char* data, void NetworkSync::SendVector3(unsigned char *data, unsigned char *startIndex,
unsigned char* startIndex,
const Vector3 v) { const Vector3 v) {
SendSingle100(data, *startIndex, v.Right()); SendSingle100(data, *startIndex, v.Right());
(*startIndex) += 4; (*startIndex) += 4;
@ -400,8 +401,7 @@ void NetworkSync::SendVector3(unsigned char* data,
(*startIndex) += 4; (*startIndex) += 4;
} }
void NetworkSync::SendQuaternion(unsigned char* data, void NetworkSync::SendQuaternion(unsigned char *data, const int startIndex,
const int startIndex,
const Quaternion q) { const Quaternion q) {
Vector3 angles = Quaternion::ToAngles(q); Vector3 angles = Quaternion::ToAngles(q);
int ix = startIndex; int ix = startIndex;
@ -410,30 +410,26 @@ void NetworkSync::SendQuaternion(unsigned char* data,
SendAngle8(data, ix++, angles.Forward()); SendAngle8(data, ix++, angles.Forward());
} }
void NetworkSync::SendPolar(unsigned char* data, void NetworkSync::SendPolar(unsigned char *data, unsigned char *startIndex,
unsigned char* startIndex,
Polar p) { Polar p) {
SendAngle8(data, *startIndex, (const float)p.angle.InDegrees()); SendAngle8(data, *startIndex, (const float)p.angle.InDegrees());
SendSingle100(data, (*startIndex) + 1, p.distance); SendSingle100(data, (*startIndex) + 1, p.distance);
} }
void NetworkSync::SendSpherical16(unsigned char *data, void NetworkSync::SendSpherical16(unsigned char *data,
unsigned char* startIndex, unsigned char *startIndex, Spherical16 s) {
Spherical16 s) {
SendAngle8(data, (*startIndex)++, s.direction.horizontal.InDegrees()); SendAngle8(data, (*startIndex)++, s.direction.horizontal.InDegrees());
SendAngle8(data, (*startIndex)++, s.direction.vertical.InDegrees()); SendAngle8(data, (*startIndex)++, s.direction.vertical.InDegrees());
SendFloat16(data, startIndex, s.distance); SendFloat16(data, startIndex, s.distance);
} }
void NetworkSync::SendSwingTwist(unsigned char* data, void NetworkSync::SendSwingTwist(unsigned char *data, unsigned char *ix,
unsigned char* ix,
const SwingTwist16 r) { const SwingTwist16 r) {
Quaternion q = r.ToQuaternion(); Quaternion q = r.ToQuaternion();
SendQuat32(buffer, ix, q); SendQuat32(buffer, ix, q);
} }
void NetworkSync::SendQuat32(unsigned char* data, void NetworkSync::SendQuat32(unsigned char *data, unsigned char *startIndex,
unsigned char* startIndex,
const Quaternion q) { const Quaternion q) {
unsigned char qx = (char)(q.x * 127 + 128); unsigned char qx = (char)(q.x * 127 + 128);
unsigned char qy = (char)(q.y * 127 + 128); unsigned char qy = (char)(q.y * 127 + 128);
@ -452,8 +448,7 @@ void NetworkSync::SendQuat32(unsigned char* data,
data[(*startIndex)++] = qw; data[(*startIndex)++] = qw;
} }
void NetworkSync::SendAngle8(unsigned char* data, void NetworkSync::SendAngle8(unsigned char *data, unsigned int startIndex,
unsigned int startIndex,
const float angle) { const float angle) {
Angle8 packedAngle2 = Angle8::Degrees(angle); Angle8 packedAngle2 = Angle8::Degrees(angle);
data[startIndex] = packedAngle2.GetBinary(); data[startIndex] = packedAngle2.GetBinary();
@ -480,16 +475,14 @@ void NetworkSync::SendAngle8(unsigned char* data,
// // data[startIndex + 3]); // // data[startIndex + 3]);
// } // }
void NetworkSync::SendSingle100(unsigned char* data, void NetworkSync::SendSingle100(unsigned char *data, unsigned int startIndex,
unsigned int startIndex,
float value) { float value) {
// Sends a float with truncated 2 decimal precision // Sends a float with truncated 2 decimal precision
Int32 intValue = value * 100; Int32 intValue = value * 100;
SendInt32(data, startIndex, intValue); SendInt32(data, startIndex, intValue);
} }
void NetworkSync::SendFloat16(unsigned char* data, void NetworkSync::SendFloat16(unsigned char *data, unsigned char *startIndex,
unsigned char* startIndex,
float value) { float value) {
float16 value16 = float16(value); float16 value16 = float16(value);
short binary = value16.getBinary(); short binary = value16.getBinary();
@ -498,8 +491,7 @@ void NetworkSync::SendFloat16(unsigned char* data,
data[(*startIndex)++] = binary & 0xFF; data[(*startIndex)++] = binary & 0xFF;
} }
void NetworkSync::SendInt32(unsigned char* data, void NetworkSync::SendInt32(unsigned char *data, unsigned int startIndex,
unsigned int startIndex,
Int32 value) { Int32 value) {
for (unsigned char ix = 0; ix < 4; ix++) { for (unsigned char ix = 0; ix < 4; ix++) {
data[startIndex++] = ((unsigned char *)&value)[ix]; data[startIndex++] = ((unsigned char *)&value)[ix];

96
Roboid.cpp
View File

@ -20,11 +20,9 @@ Roboid::Roboid() : Thing(0) {
this->perception->roboid = this; this->perception->roboid = this;
this->propulsion = nullptr; this->propulsion = nullptr;
this->networkSync = nullptr; this->networkSync = nullptr;
// this->actuation = nullptr;
this->worldPosition = Spherical16::zero; this->worldPosition = Spherical16::zero;
this->worldOrientation = SwingTwist16::identity; this->worldOrientation = SwingTwist16::identity;
// this->worldOrientation = Quaternion::identity;
// this->worldAngleAxis = AngleAxisOf<float>();
} }
Roboid::Roboid(Propulsion *propulsion) : Roboid() { Roboid::Roboid(Propulsion *propulsion) : Roboid() {
@ -33,10 +31,9 @@ Roboid::Roboid(Propulsion* propulsion) : Roboid() {
propulsion->roboid = this; propulsion->roboid = this;
} }
void Passer::RoboidControl::Roboid::SetName(char* name) { void Passer::RoboidControl::Roboid::SetName(char *name) { this->name = name; }
this->name = name;
}
#include <Arduino.h>
void Roboid::Update(unsigned long currentTimeMs) { void Roboid::Update(unsigned long currentTimeMs) {
if (perception != nullptr) if (perception != nullptr)
perception->Update(currentTimeMs); perception->Update(currentTimeMs);
@ -46,13 +43,28 @@ void Roboid::Update(unsigned long currentTimeMs) {
float deltaTime = (float)(currentTimeMs - lastUpdateTimeMs) / 1000; float deltaTime = (float)(currentTimeMs - lastUpdateTimeMs) / 1000;
SetPosition(this->worldPosition + // Conversion from old units
this->worldOrientation * Spherical16::forward * Polar polarVelocity = this->propulsion->GetVelocity();
this->propulsion->GetVelocity().distance * deltaTime); this->linearVelocity = Spherical16(
SetOrientation(this->worldOrientation * polarVelocity.distance,
SwingTwist16::AngleAxis( Angle16::Degrees(polarVelocity.angle.InDegrees()), Angle16());
this->propulsion->GetAngularVelocity() * deltaTime, float oldAngular = this->propulsion->GetAngularVelocity();
Direction16::up)); this->angularVelocity =
Spherical16(oldAngular, Angle16(), Angle16::Degrees(90));
SetPosition(this->position + this->orientation * Spherical16::forward *
this->linearVelocity.distance * deltaTime);
this->worldPosition = this->position; // assuming the roboid is the root
SwingTwist16 rotation = SwingTwist16::AngleAxis(
this->angularVelocity.distance * deltaTime, Direction16::up);
if (perception != nullptr)
perception->UpdatePose(rotation);
this->orientation = this->orientation * rotation;
this->worldOrientation =
this->orientation; // assuming the roboid is the root
} }
if (childCount > 0 && children != nullptr) { if (childCount > 0 && children != nullptr) {
@ -66,32 +78,9 @@ void Roboid::Update(unsigned long currentTimeMs) {
lastUpdateTimeMs = currentTimeMs; lastUpdateTimeMs = currentTimeMs;
} }
Spherical16 Roboid::GetPosition() { Spherical16 Roboid::GetPosition() { return this->worldPosition; }
return this->worldPosition;
}
// Vector2 Roboid::GetPosition2D() {
// return Vector2(this->worldPosition.Right(), this->worldPosition.Forward());
// }
SwingTwist16 Roboid::GetOrientation() { SwingTwist16 Roboid::GetOrientation() { return this->worldOrientation; }
// Vector3 axis = this->worldAngleAxis.axis.ToVector3();
// SwingTwist16 q = SwingTwist16::AngleAxis(this->worldAngleAxis.angle, axis);
return this->worldOrientation;
}
// 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.vertical.InDegrees() > maxAngle)
// return this->worldAngleAxis.angle;
// if (this->worldAngleAxis.axis.vertical.InDegrees() < -maxAngle)
// return -this->worldAngleAxis.angle;
// SwingTwist16 q = GetOrientation();
// return Quaternion::GetAngleAround(Vector3::up, q);
// }
void Roboid::SetPosition(Spherical16 newWorldPosition) { void Roboid::SetPosition(Spherical16 newWorldPosition) {
SwingTwist16 roboidOrientation = this->GetOrientation(); SwingTwist16 roboidOrientation = this->GetOrientation();
@ -103,35 +92,24 @@ void Roboid::SetPosition(Spherical16 newWorldPosition) {
distance, angle); // Polar(angle.InDegrees(), Angle::Degrees(distance)); distance, angle); // Polar(angle.InDegrees(), Angle::Degrees(distance));
if (perception != nullptr) if (perception != nullptr)
perception->UpdatePose(polarTranslation); perception->UpdatePose(polarTranslation);
this->worldPosition = newWorldPosition;
if (networkSync != nullptr) this->position = newWorldPosition; // roboid is the root?
// networkSync->SendPosition(this->worldPosition); // World position should be set in the update recursion
networkSync->SendPose(this->worldPosition, roboidOrientation); // this->worldPosition = newWorldPosition;
// if (networkSync != nullptr)
// // networkSync->SendPosition(this->worldPosition);
// networkSync->SendPose(this->worldPosition, roboidOrientation);
} }
#include <math.h> #include <math.h>
void Roboid::SetOrientation(SwingTwist16 worldOrientation) { void Roboid::SetOrientation(SwingTwist16 newOrientation) {
// float angle;
// Vector3 axis;
// worldOrientation.ToAngleAxis(&angle, &axis);
SwingTwist16 delta = SwingTwist16 delta =
SwingTwist16::Inverse(GetOrientation()) * worldOrientation; SwingTwist16::Inverse(this->orientation) * newOrientation;
if (perception != nullptr) if (perception != nullptr)
perception->UpdatePose(delta); perception->UpdatePose(delta);
// AngleAxisOf<float> angleAxis = this->orientation = newOrientation;
// AngleAxisOf<float>(angle, DirectionOf<float>(axis));
// this->worldAngleAxis = angleAxis;
}
// void Roboid::SetOrientation2D(float angle) {
// this->worldAngleAxis = AngleAxisOf<float>(angle, DirectionOf<float>::up);
// }
Vector3 Passer::RoboidControl::Roboid::GetVelocity() {
return Vector3();
} }
void Roboid::AddChild(Thing *child) { void Roboid::AddChild(Thing *child) {

6
Roboid.h
View File

@ -50,7 +50,6 @@ class Roboid : public Thing {
/// used. This value will be Quaternion::identity unless an orientation is /// used. This value will be Quaternion::identity unless an orientation is
/// received though network synchronization /// received though network synchronization
virtual SwingTwist16 GetOrientation(); virtual SwingTwist16 GetOrientation();
// float GetOrientation2D();
/// @brief Update the current position of the roboid /// @brief Update the current position of the roboid
/// @param worldPosition The position of the roboid in carthesian coordinates /// @param worldPosition The position of the roboid in carthesian coordinates
@ -66,9 +65,6 @@ class Roboid : public Thing {
/// perceived objects by the roboid (roboid->perception->perceivedObjets), /// perceived objects by the roboid (roboid->perception->perceivedObjets),
/// as these are local to the roboid' orientation. /// as these are local to the roboid' orientation.
virtual void SetOrientation(SwingTwist16 worldOrientation); virtual void SetOrientation(SwingTwist16 worldOrientation);
// void SetOrientation2D(float angle);
virtual Vector3 GetVelocity();
virtual void AddChild(Thing *child) override; virtual void AddChild(Thing *child) override;
@ -77,7 +73,7 @@ class Roboid : public Thing {
/// @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
/// directly, but SetPosition should be used instead. /// directly, but SetPosition should be used instead.
Spherical16 worldPosition = Spherical16::zero; // Spherical16 worldPosition = Spherical16::zero;
/// @brief The orientation of the roboid in world space /// @brief The orientation of the roboid in world space
/// @details The position may be set when NetworkSync is used to receive /// @details The position may be set when NetworkSync is used to receive
/// orientations from an external tracking system. This value should not be /// orientations from an external tracking system. This value should not be

1
Thing.h
View File

@ -47,6 +47,7 @@ class Thing {
/// @remark When this Thing has a parent, the position is relative to the /// @remark When this Thing has a parent, the position is relative to the
/// parent's position and orientation /// parent's position and orientation
Spherical16 position; Spherical16 position;
Spherical16 worldPosition;
/// @brief The orientation of this Thing /// @brief The orientation of this Thing
/// @remark When this Thing has a parent, the orientation is relative to the /// @remark When this Thing has a parent, the orientation is relative to the
/// parent's orientation /// parent's orientation