RoboidControl/RoboidControl-cpp
RoboidControl/RoboidControl-cpp
1
0
Fork 0
Code Issues 1 Pull Requests Actions Packages Releases 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);
}
void DifferentialDrive::SetTwistSpeed(Vector3 linear,
float yaw,
float pitch,
void DifferentialDrive::SetTwistSpeed(Vector3 linear, float yaw, float pitch,
float roll) {
SetTwistSpeed(linear.Forward(), yaw);
}

38
NetworkSync.cpp
View File

@ -5,7 +5,7 @@
#ifdef RC_DEBUG
#include <Arduino.h>
#if ESP32
#define SERIALPORT Serial0
#define SERIALPORT Serial
#else
#define SERIALPORT Serial
#endif
@ -189,7 +189,7 @@ void NetworkSync::SendDestroyThing(InterestingThing* thing) {
}
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;
if (thing->GetLinearVelocity().distance > 0 ||
@ -204,7 +204,8 @@ void NetworkSync::SendPose(Thing* thing, bool recurse) {
#if RC_DEBUG
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
}
@ -292,6 +293,7 @@ void NetworkSync::PublishTrackedObject(Roboid* roboid,
}
void NetworkSync::SendPoseMsg(Buffer sendBuffer, Roboid *roboid) {
// SERIALPORT.printf("old send pose?\n");
Polar velocity = roboid->propulsion->GetVelocity();
Vector2 worldVelocity2 =
Vector2::Rotate(Vector2::forward * velocity.distance, velocity.angle);
@ -389,8 +391,7 @@ void NetworkSync::SendInt(const int x) {
// Low-level functions
void NetworkSync::SendVector3(unsigned char* data,
unsigned char* startIndex,
void NetworkSync::SendVector3(unsigned char *data, unsigned char *startIndex,
const Vector3 v) {
SendSingle100(data, *startIndex, v.Right());
(*startIndex) += 4;
@ -400,8 +401,7 @@ void NetworkSync::SendVector3(unsigned char* data,
(*startIndex) += 4;
}
void NetworkSync::SendQuaternion(unsigned char* data,
const int startIndex,
void NetworkSync::SendQuaternion(unsigned char *data, const int startIndex,
const Quaternion q) {
Vector3 angles = Quaternion::ToAngles(q);
int ix = startIndex;
@ -410,30 +410,26 @@ void NetworkSync::SendQuaternion(unsigned char* data,
SendAngle8(data, ix++, angles.Forward());
}
void NetworkSync::SendPolar(unsigned char* data,
unsigned char* startIndex,
void NetworkSync::SendPolar(unsigned char *data, unsigned char *startIndex,
Polar p) {
SendAngle8(data, *startIndex, (const float)p.angle.InDegrees());
SendSingle100(data, (*startIndex) + 1, p.distance);
}
void NetworkSync::SendSpherical16(unsigned char *data,
unsigned char* startIndex,
Spherical16 s) {
unsigned char *startIndex, Spherical16 s) {
SendAngle8(data, (*startIndex)++, s.direction.horizontal.InDegrees());
SendAngle8(data, (*startIndex)++, s.direction.vertical.InDegrees());
SendFloat16(data, startIndex, s.distance);
}
void NetworkSync::SendSwingTwist(unsigned char* data,
unsigned char* ix,
void NetworkSync::SendSwingTwist(unsigned char *data, unsigned char *ix,
const SwingTwist16 r) {
Quaternion q = r.ToQuaternion();
SendQuat32(buffer, ix, q);
}
void NetworkSync::SendQuat32(unsigned char* data,
unsigned char* startIndex,
void NetworkSync::SendQuat32(unsigned char *data, unsigned char *startIndex,
const Quaternion q) {
unsigned char qx = (char)(q.x * 127 + 128);
unsigned char qy = (char)(q.y * 127 + 128);
@ -452,8 +448,7 @@ void NetworkSync::SendQuat32(unsigned char* data,
data[(*startIndex)++] = qw;
}
void NetworkSync::SendAngle8(unsigned char* data,
unsigned int startIndex,
void NetworkSync::SendAngle8(unsigned char *data, unsigned int startIndex,
const float angle) {
Angle8 packedAngle2 = Angle8::Degrees(angle);
data[startIndex] = packedAngle2.GetBinary();
@ -480,16 +475,14 @@ void NetworkSync::SendAngle8(unsigned char* data,
// // data[startIndex + 3]);
// }
void NetworkSync::SendSingle100(unsigned char* data,
unsigned int startIndex,
void NetworkSync::SendSingle100(unsigned char *data, unsigned int startIndex,
float value) {
// Sends a float with truncated 2 decimal precision
Int32 intValue = value * 100;
SendInt32(data, startIndex, intValue);
}
void NetworkSync::SendFloat16(unsigned char* data,
unsigned char* startIndex,
void NetworkSync::SendFloat16(unsigned char *data, unsigned char *startIndex,
float value) {
float16 value16 = float16(value);
short binary = value16.getBinary();
@ -498,8 +491,7 @@ void NetworkSync::SendFloat16(unsigned char* data,
data[(*startIndex)++] = binary & 0xFF;
}
void NetworkSync::SendInt32(unsigned char* data,
unsigned int startIndex,
void NetworkSync::SendInt32(unsigned char *data, unsigned int startIndex,
Int32 value) {
for (unsigned char ix = 0; ix < 4; 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->propulsion = nullptr;
this->networkSync = nullptr;
// this->actuation = nullptr;
this->worldPosition = Spherical16::zero;
this->worldOrientation = SwingTwist16::identity;
// this->worldOrientation = Quaternion::identity;
// this->worldAngleAxis = AngleAxisOf<float>();
}
Roboid::Roboid(Propulsion *propulsion) : Roboid() {
@ -33,10 +31,9 @@ Roboid::Roboid(Propulsion* propulsion) : Roboid() {
propulsion->roboid = this;
}
void Passer::RoboidControl::Roboid::SetName(char* name) {
this->name = name;
}
void Passer::RoboidControl::Roboid::SetName(char *name) { this->name = name; }
#include <Arduino.h>
void Roboid::Update(unsigned long currentTimeMs) {
if (perception != nullptr)
perception->Update(currentTimeMs);
@ -46,13 +43,28 @@ void Roboid::Update(unsigned long currentTimeMs) {
float deltaTime = (float)(currentTimeMs - lastUpdateTimeMs) / 1000;
SetPosition(this->worldPosition +
this->worldOrientation * Spherical16::forward *
this->propulsion->GetVelocity().distance * deltaTime);
SetOrientation(this->worldOrientation *
SwingTwist16::AngleAxis(
this->propulsion->GetAngularVelocity() * deltaTime,
Direction16::up));
// Conversion from old units
Polar polarVelocity = this->propulsion->GetVelocity();
this->linearVelocity = Spherical16(
polarVelocity.distance,
Angle16::Degrees(polarVelocity.angle.InDegrees()), Angle16());
float oldAngular = this->propulsion->GetAngularVelocity();
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) {
@ -66,32 +78,9 @@ void Roboid::Update(unsigned long currentTimeMs) {
lastUpdateTimeMs = currentTimeMs;
}
Spherical16 Roboid::GetPosition() {
return this->worldPosition;
}
// Vector2 Roboid::GetPosition2D() {
// return Vector2(this->worldPosition.Right(), this->worldPosition.Forward());
// }
Spherical16 Roboid::GetPosition() { return this->worldPosition; }
SwingTwist16 Roboid::GetOrientation() {
// 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);
// }
SwingTwist16 Roboid::GetOrientation() { return this->worldOrientation; }
void Roboid::SetPosition(Spherical16 newWorldPosition) {
SwingTwist16 roboidOrientation = this->GetOrientation();
@ -103,35 +92,24 @@ void Roboid::SetPosition(Spherical16 newWorldPosition) {
distance, angle); // Polar(angle.InDegrees(), Angle::Degrees(distance));
if (perception != nullptr)
perception->UpdatePose(polarTranslation);
this->worldPosition = newWorldPosition;
if (networkSync != nullptr)
// networkSync->SendPosition(this->worldPosition);
networkSync->SendPose(this->worldPosition, roboidOrientation);
this->position = newWorldPosition; // roboid is the root?
// World position should be set in the update recursion
// this->worldPosition = newWorldPosition;
// if (networkSync != nullptr)
// // networkSync->SendPosition(this->worldPosition);
// networkSync->SendPose(this->worldPosition, roboidOrientation);
}
#include <math.h>
void Roboid::SetOrientation(SwingTwist16 worldOrientation) {
// float angle;
// Vector3 axis;
// worldOrientation.ToAngleAxis(&angle, &axis);
void Roboid::SetOrientation(SwingTwist16 newOrientation) {
SwingTwist16 delta =
SwingTwist16::Inverse(GetOrientation()) * worldOrientation;
SwingTwist16::Inverse(this->orientation) * newOrientation;
if (perception != nullptr)
perception->UpdatePose(delta);
// AngleAxisOf<float> angleAxis =
// 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();
this->orientation = newOrientation;
}
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
/// received though network synchronization
virtual SwingTwist16 GetOrientation();
// float GetOrientation2D();
/// @brief Update the current position of the roboid
/// @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),
/// as these are local to the roboid' orientation.
virtual void SetOrientation(SwingTwist16 worldOrientation);
// void SetOrientation2D(float angle);
virtual Vector3 GetVelocity();
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
/// positions from an external tracking system. These values should not be set
/// directly, but SetPosition should be used instead.
Spherical16 worldPosition = Spherical16::zero;
// Spherical16 worldPosition = Spherical16::zero;
/// @brief The orientation of the roboid in world space
/// @details The position may be set when NetworkSync is used to receive
/// 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
/// parent's position and orientation
Spherical16 position;
Spherical16 worldPosition;
/// @brief The orientation of this Thing
/// @remark When this Thing has a parent, the orientation is relative to the
/// parent's orientation