Improve use of position/orientation

DifferentialDrive.cpp

@@ -6,9 +6,9 @@
 
 DifferentialDrive::DifferentialDrive() {};
 
-DifferentialDrive::DifferentialDrive(Motor* leftMotor, Motor* rightMotor) {
+DifferentialDrive::DifferentialDrive(Motor *leftMotor, Motor *rightMotor) {
   this->motorCount = 2;
-  this->motors = new Motor*[2];
+  this->motors = new Motor *[2];
   this->motors[0] = leftMotor;
   this->motors[1] = rightMotor;
 
@@ -35,7 +35,7 @@ void DifferentialDrive::SetDimensions(float wheelDiameter,
 void DifferentialDrive::SetMotorTargetSpeeds(float leftSpeed,
                                              float rightSpeed) {
   for (unsigned int motorIx = 0; motorIx < this->motorCount; motorIx++) {
-    Motor* motor = motors[motorIx];
+    Motor *motor = motors[motorIx];
     if (motor == nullptr)
       continue;
 
@@ -64,9 +64,7 @@ void DifferentialDrive::SetTwistSpeed(Vector2 linear, float 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) {
   SetTwistSpeed(linear.Forward(), yaw);
 }
@@ -76,8 +74,8 @@ void DifferentialDrive::SetVelocity(Polar velocity) {
 }
 
 Polar DifferentialDrive::GetVelocity() {
-  Motor* leftMotor = motors[0];
+  Motor *leftMotor = motors[0];
-  Motor* rightMotor = motors[1];
+  Motor *rightMotor = motors[1];
   float leftSpeed = leftMotor->GetActualSpeed();   // in revolutions per second
   float rightSpeed = rightMotor->GetActualSpeed(); // in revolutions per second
 
@@ -93,8 +91,8 @@ Polar DifferentialDrive::GetVelocity() {
 }
 
 float DifferentialDrive::GetAngularVelocity() {
-  Motor* leftMotor = motors[0];
+  Motor *leftMotor = motors[0];
-  Motor* rightMotor = motors[1];
+  Motor *rightMotor = motors[1];
   float leftSpeed = leftMotor->GetActualSpeed();   // in revolutions per second
   float rightSpeed = rightMotor->GetActualSpeed(); // in revolutions per second
 

NetworkSync.cpp

@@ -5,7 +5,7 @@
 #ifdef RC_DEBUG
 #include <Arduino.h>
 #if ESP32
-#define SERIALPORT Serial0
+#define SERIALPORT Serial
 #else
 #define SERIALPORT Serial
 #endif
@@ -18,12 +18,12 @@
 
 #include <string.h>
 
-NetworkSync::NetworkSync(Roboid* roboid) {
+NetworkSync::NetworkSync(Roboid *roboid) {
   this->roboid = roboid;
   this->networkId = 0;
 }
 
-void NetworkSync::ReceiveMessage(Roboid* roboid, unsigned char bytecount) {
+void NetworkSync::ReceiveMessage(Roboid *roboid, unsigned char bytecount) {
   networkPerception->ProcessPacket(roboid, buffer, bytecount);
 
   switch (buffer[0]) {
@@ -42,7 +42,7 @@ void NetworkSync::ReceiveNetworkId() {
   SendName(roboid);
   SendModel(roboid);
   for (unsigned char childIx = 0; childIx < roboid->childCount; childIx++) {
-    Thing* child = roboid->GetChild(childIx);
+    Thing *child = roboid->GetChild(childIx);
     if (child != nullptr)
       PublishRelativeThing(child, true);
   }
@@ -59,12 +59,12 @@ void NetworkSync::PublishDevice() {
 #endif
 }
 
-void NetworkSync::PublishState(Roboid* roboid) {
+void NetworkSync::PublishState(Roboid *roboid) {
   SendPose(roboid);
   PublishPerception(roboid);
 }
 
-void NetworkSync::NewObject(InterestingThing* thing) {
+void NetworkSync::NewObject(InterestingThing *thing) {
   if (thing == nullptr || thing->networkId != 0x00)
     return;
 
@@ -82,8 +82,8 @@ void NetworkSync::NewObject(InterestingThing* thing) {
   PublishTrackedObject(roboid, thing);
 }
 
-void NetworkSync::PublishRelativeThing(Thing* thing, bool recurse) {
+void NetworkSync::PublishRelativeThing(Thing *thing, bool recurse) {
-  Thing* parentThing = thing->GetParent();
+  Thing *parentThing = thing->GetParent();
 
   unsigned char ix = 0;
   buffer[ix++] = RelativePoseMsg;
@@ -99,14 +99,14 @@ void NetworkSync::PublishRelativeThing(Thing* thing, bool recurse) {
 
   if (recurse) {
     for (unsigned char childIx = 0; childIx < thing->childCount; childIx++) {
-      Thing* child = thing->GetChild(childIx);
+      Thing *child = thing->GetChild(childIx);
       if (child != nullptr)
         PublishRelativeThing(child, true);
     }
   }
 }
 
-void NetworkSync::SendName(Roboid* roboid) {
+void NetworkSync::SendName(Roboid *roboid) {
   if (roboid->name == nullptr)
     return;
 
@@ -130,7 +130,7 @@ void NetworkSync::SendName(Roboid* roboid) {
 #endif
 }
 
-void NetworkSync::SendModel(Roboid* roboid) {
+void NetworkSync::SendModel(Roboid *roboid) {
   if (roboid->modelUrl == nullptr)
     return;
 
@@ -152,7 +152,7 @@ void NetworkSync::SendModel(Roboid* roboid) {
   SendBuffer(ix);
 }
 
-void NetworkSync::SendModel(Thing* thing) {
+void NetworkSync::SendModel(Thing *thing) {
   if (thing->modelUrl == nullptr)
     return;
 
@@ -174,7 +174,7 @@ void NetworkSync::SendModel(Thing* thing) {
   SendBuffer(ix);
 }
 
-void NetworkSync::SendDestroyThing(InterestingThing* thing) {
+void NetworkSync::SendDestroyThing(InterestingThing *thing) {
   if (networkId == 0) // We're not connected to a site yet
     return;
 
@@ -188,8 +188,8 @@ void NetworkSync::SendDestroyThing(InterestingThing* thing) {
 #endif
 }
 
-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;
 
   if (thing->GetLinearVelocity().distance > 0 ||
@@ -204,21 +204,22 @@ 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
   }
 
   if (recurse) {
     for (unsigned char childIx = 0; childIx < thing->childCount; childIx++) {
-      Thing* child = thing->GetChild(childIx);
+      Thing *child = thing->GetChild(childIx);
       if (child != nullptr)
         SendPose(thing->GetChild(childIx), true);
     }
   }
 }
 
-void NetworkSync::PublishState(Sensor* sensor) {
+void NetworkSync::PublishState(Sensor *sensor) {
-  float* value = (float*)sensor->GetValue();
+  float *value = (float *)sensor->GetValue();
   if (value == nullptr)
     return;
 
@@ -229,14 +230,14 @@ void NetworkSync::PublishState(Sensor* sensor) {
   PublishBuffer(ix);
 }
 
-void NetworkSync::PublishPerception(Roboid* roboid) {
+void NetworkSync::PublishPerception(Roboid *roboid) {
-  Perception* perception = roboid->perception;
+  Perception *perception = roboid->perception;
   if (perception == nullptr)
     return;
 
   for (unsigned int sensorIx = 0; sensorIx < perception->sensorCount;
        sensorIx++) {
-    Sensor* sensor = perception->sensors[sensorIx];
+    Sensor *sensor = perception->sensors[sensorIx];
     if (sensor == nullptr)
       continue;
     PublishState(sensor);
@@ -244,14 +245,14 @@ void NetworkSync::PublishPerception(Roboid* roboid) {
   PublishTrackedObjects(roboid, roboid->perception->GetTrackedObjects());
 }
 
-void NetworkSync::PublishTrackedObjects(Roboid* roboid,
+void NetworkSync::PublishTrackedObjects(Roboid *roboid,
-                                        InterestingThing** objects) {
+                                        InterestingThing **objects) {
   if (networkId == 0) // We're not connected to a site yet
     return;
 
   int n = 0;
   for (unsigned char objIx = 0; objIx < Perception::maxObjectCount; objIx++) {
-    InterestingThing* obj = objects[objIx];
+    InterestingThing *obj = objects[objIx];
     if (obj == nullptr)
       continue;
 
@@ -262,8 +263,8 @@ void NetworkSync::PublishTrackedObjects(Roboid* roboid,
   }
 }
 
-void NetworkSync::PublishTrackedObject(Roboid* roboid,
+void NetworkSync::PublishTrackedObject(Roboid *roboid,
-                                       InterestingThing* object) {
+                                       InterestingThing *object) {
   if (object == nullptr || object->updated == false ||
       object->networkId != 0x00) {
     return;
@@ -291,7 +292,8 @@ void NetworkSync::PublishTrackedObject(Roboid* roboid,
   object->updated = false;
 }
 
-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();
   Vector2 worldVelocity2 =
       Vector2::Rotate(Vector2::forward * velocity.distance, velocity.angle);
@@ -339,7 +341,7 @@ void NetworkSync::SendPoseMsg(Buffer sendBuffer, Roboid* roboid) {
 // #endif
 // }
 
-void NetworkSync::SendInvestigate(InterestingThing* thing) {
+void NetworkSync::SendInvestigate(InterestingThing *thing) {
   unsigned char ix = 0;
   buffer[ix++] = InvestigateMsg;
   buffer[ix++] = thing->networkId;
@@ -350,7 +352,7 @@ void NetworkSync::SendInvestigate(InterestingThing* thing) {
 #endif
 }
 
-void NetworkSync::SendText(const char* s) {
+void NetworkSync::SendText(const char *s) {
   unsigned char length;
   for (length = 0; length < 253; length++) {
     if (s[length] == '\0')
@@ -389,8 +391,7 @@ void NetworkSync::SendInt(const int x) {
 
 // Low-level functions
 
-void NetworkSync::SendVector3(unsigned char* data,
+void NetworkSync::SendVector3(unsigned char *data, unsigned char *startIndex,
-                              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,
+void NetworkSync::SendQuaternion(unsigned char *data, const int startIndex,
-                                 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,
+void NetworkSync::SendPolar(unsigned char *data, unsigned char *startIndex,
-                            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,
+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.vertical.InDegrees());
   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) {
   Quaternion q = r.ToQuaternion();
   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) {
   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,
+void NetworkSync::SendAngle8(unsigned char *data, unsigned int startIndex,
-                             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,
+void NetworkSync::SendSingle100(unsigned char *data, unsigned int startIndex,
-                                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,
+void NetworkSync::SendFloat16(unsigned char *data, unsigned char *startIndex,
-                              unsigned char* startIndex,
                               float value) {
   float16 value16 = float16(value);
   short binary = value16.getBinary();
@@ -498,11 +491,10 @@ void NetworkSync::SendFloat16(unsigned char* data,
   data[(*startIndex)++] = binary & 0xFF;
 }
 
-void NetworkSync::SendInt32(unsigned char* data,
+void NetworkSync::SendInt32(unsigned char *data, unsigned int startIndex,
-                            unsigned int startIndex,
                             Int32 value) {
   for (unsigned char ix = 0; ix < 4; ix++) {
-    data[startIndex++] = ((unsigned char*)&value)[ix];
+    data[startIndex++] = ((unsigned char *)&value)[ix];
   }
 }
 

Roboid.cpp

@@ -20,23 +20,20 @@ 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() {
+Roboid::Roboid(Propulsion *propulsion) : Roboid() {
   this->propulsion = propulsion;
   if (propulsion != nullptr)
     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) {
   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 +
+    // 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) {
@@ -66,32 +78,9 @@ void Roboid::Update(unsigned long 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) {
   SwingTwist16 roboidOrientation = this->GetOrientation();
@@ -103,41 +92,30 @@ 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)
+  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>
-void Roboid::SetOrientation(SwingTwist16 worldOrientation) {
+void Roboid::SetOrientation(SwingTwist16 newOrientation) {
-  // float angle;
-  // Vector3 axis;
-  // worldOrientation.ToAngleAxis(&angle, &axis);
-
   SwingTwist16 delta =
-      SwingTwist16::Inverse(GetOrientation()) * worldOrientation;
+      SwingTwist16::Inverse(this->orientation) * newOrientation;
   if (perception != nullptr)
     perception->UpdatePose(delta);
 
-  // AngleAxisOf<float> angleAxis =
+  this->orientation = newOrientation;
-  //     AngleAxisOf<float>(angle, DirectionOf<float>(axis));
-  // this->worldAngleAxis = angleAxis;
 }
 
-// void Roboid::SetOrientation2D(float angle) {
+void Roboid::AddChild(Thing *child) {
-//   this->worldAngleAxis = AngleAxisOf<float>(angle, DirectionOf<float>::up);
-// }
-
-Vector3 Passer::RoboidControl::Roboid::GetVelocity() {
-  return Vector3();
-}
-
-void Roboid::AddChild(Thing* child) {
   Thing::AddChild(child);
   if (child->IsSensor()) {
-    Sensor* childSensor = (Sensor*)child;
+    Sensor *childSensor = (Sensor *)child;
     this->perception->AddSensor(childSensor);
   }
 }

Roboid.h

@@ -12,16 +12,16 @@ class NetworkSync;
 
 /// @brief A Roboid is used to control autonomous robots
 class Roboid : public Thing {
- public:
+public:
   /// @brief Default constructor for a Roboid
   Roboid();
   /// @brief Creates a Roboid with Perception and Propulsion abilities
   /// @param perception The Perception implementation to use for this Roboid
   /// @param propulsion The Propulsion implementation to use for this Roboid
-  Roboid(Propulsion* propulsion);
+  Roboid(Propulsion *propulsion);
 
-  char* name = nullptr;
+  char *name = nullptr;
-  void SetName(char* name);
+  void SetName(char *name);
 
   /// @brief Update the state of the Roboid
   /// @param currentTimeMs The time in milliseconds when calling this
@@ -29,13 +29,13 @@ class Roboid : public Thing {
   void Update(unsigned long currentTimeMs);
 
   /// @brief The Perception module of this Roboid
-  Perception* perception = nullptr;
+  Perception *perception = nullptr;
   /// @brief The Propulsion module of this Roboid
-  Propulsion* propulsion = nullptr;
+  Propulsion *propulsion = nullptr;
   // ServoMotor *actuation = nullptr;
 
   /// @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
   /// @return The position in carthesian coordinates in world space
@@ -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,18 +65,15 @@ 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;
 
-  virtual void AddChild(Thing* child) override;
+private:
-
- private:
   /// @brief The position of the roboid in carthesian coordinates in world space
   /// @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

Thing.h

@@ -10,7 +10,7 @@ namespace RoboidControl {
 
 /// @brief A thing is a functional component on a robot
 class Thing {
- public:
+public:
   /// @brief Default constructor for a Thing
   Thing(unsigned char id);
 
@@ -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
@@ -59,27 +60,27 @@ class Thing {
   /// @brief Sets the parent Thing
   /// @param parent The Thing which should become the parnet
   /// @remark This is equivalent to calling parent->AddChild(this);
-  virtual void SetParent(Thing* parent);
+  virtual void SetParent(Thing *parent);
   /// @brief Gets the parent Thing
   /// @return The parent Thing
-  Thing* GetParent();
+  Thing *GetParent();
 
   /// @brief Add a child Thing to this Thing
   /// @param child The Thing which should become a child
   /// @remark When the Thing is already a child, it will not be added again
-  virtual void AddChild(Thing* child);
+  virtual void AddChild(Thing *child);
   /// @brief Get the child at the given index
   /// @param childIx The index of the child
   /// @return The child at the given index or nullptr when the index is invalid
   /// or the child could not be found
-  Thing* GetChild(unsigned char childIx);
+  Thing *GetChild(unsigned char childIx);
 
   /// @brief Sets the location from where the 3D model of this Thing can be
   /// loaded from
   /// @param url The url of the model
   /// @remark Although the roboid implementation is not dependent on the model,
   /// the only official supported model format is .obj
-  void SetModel(const char* url);
+  void SetModel(const char *url);
 
   /// @brief Updates the state of the thing
   /// @param currentTimeMs The current clock time in milliseconds
@@ -87,9 +88,9 @@ class Thing {
 
   unsigned char childCount = 0;
 
-  const char* modelUrl = nullptr;
+  const char *modelUrl = nullptr;
 
- protected:
+protected:
   /// @brief Bitmask for Motor type
   static const unsigned int MotorType = 0x8000;
   /// @brief Bitmap for Sensor type
@@ -112,10 +113,10 @@ class Thing {
     ExternalSensor,
   };
 
-  Thing* parent = nullptr;
+  Thing *parent = nullptr;
-  Thing** children = nullptr;
+  Thing **children = nullptr;
 
- public:
+public:
   Spherical16 angularVelocity = Spherical16::zero;
   Spherical16 linearVelocity = Spherical16::zero;
 };