update all velocity and acceleration components of non-link entities

fixes #1866
Signed-off-by: El Jawad Alaa <[email protected]>

src/systems/physics/Physics.cc

@@ -3327,6 +3327,53 @@ void PhysicsPrivate::UpdateSim(EntityComponentManager &_ecm,
 
         return true;
       });
+
+  // body linear velocity
+  _ecm.Each<components::Pose, components::LinearVelocity,
+            components::ParentEntity>(
+    [&](const Entity&, const components::Pose *_pose,
+        components::LinearVelocity *_linearVel,
+        const components::ParentEntity *_parent)->bool
+    {
+      // check if parent entity is a link, e.g. entity is sensor / collision
+      if (auto linkPhys = this->entityLinkMap.Get(_parent->Data()))
+      {
+        const auto entityFrameData =
+            this->LinkFrameDataAtOffset(linkPhys, _pose->Data());
+
+        auto entityWorldPose = math::eigen3::convert(entityFrameData.pose);
+        math::Vector3d entityWorldLinearVel = 
+            math::eigen3::convert(entityFrameData.linearVelocity);
+
+        auto entityBodyLinearVel = 
+            entityWorldPose.Rot().RotateVectorReverse(entityWorldLinearVel);
+        *_linearVel = components::LinearVelocity(entityBodyLinearVel);
+      }
+
+      return true;
+    });
+
+  // world angular velocity
+  _ecm.Each<components::Pose, components::WorldAngularVelocity,
+            components::ParentEntity>(
+      [&](const Entity&,
+          const components::Pose *_pose,
+          components::WorldAngularVelocity *_worldAngularVel,
+          const components::ParentEntity *_parent)->bool
+      {
+        // check if parent entity is a link, e.g. entity is sensor / collision
+        if (auto linkPhys = this->entityLinkMap.Get(_parent->Data()))
+        {
+          const auto entityFrameData = 
+              this->LinkFrameDataAtOffset(linkPhys, _pose->Data());
+
+          // set entity world angular velocity
+          *_worldAngularVel = components::WorldAngularVelocity(
+              math::eigen3::convert(entityFrameData.angularVelocity));
+        }
+
+        return true;
+      });
 
   // body angular velocity
   _ecm.Each<components::Pose, components::AngularVelocity,
@@ -3353,6 +3400,27 @@ void PhysicsPrivate::UpdateSim(EntityComponentManager &_ecm,
 
         return true;
       });
+
+  // world linear acceleration
+  _ecm.Each<components::Pose, components::WorldLinearAcceleration,
+            components::ParentEntity>(
+      [&](const Entity &,
+          const components::Pose *_pose,
+          components::WorldLinearAcceleration *_worldLinearAcc,
+          const components::ParentEntity *_parent)->bool
+      {
+        if (auto linkPhys = this->entityLinkMap.Get(_parent->Data()))
+        {
+          const auto entityFrameData =
+              this->LinkFrameDataAtOffset(linkPhys, _pose->Data());
+
+          // set entity world linear acceleration
+          *_worldLinearAcc = components::WorldLinearAcceleration(
+              math::eigen3::convert(entityFrameData.linearAcceleration));
+        }
+
+        return true;
+      });
 
   // body linear acceleration
   _ecm.Each<components::Pose, components::LinearAcceleration,
@@ -3376,6 +3444,52 @@ void PhysicsPrivate::UpdateSim(EntityComponentManager &_ecm,
           *_linearAcc = components::LinearAcceleration(entityBodyLinearAcc);
         }
 
+        return true;
+      });
+
+  // world angular acceleration
+  _ecm.Each<components::Pose, components::WorldAngularAcceleration,
+            components::ParentEntity>(
+      [&](const Entity &,
+          const components::Pose *_pose,
+          components::WorldAngularAcceleration *_worldAngularAcc,
+          const components::ParentEntity *_parent)->bool
+      {
+        if (auto linkPhys = this->entityLinkMap.Get(_parent->Data()))
+        {
+          const auto entityFrameData =
+              this->LinkFrameDataAtOffset(linkPhys, _pose->Data());
+
+          // set entity world angular acceleration
+          *_worldAngularAcc = components::WorldAngularAcceleration(
+              math::eigen3::convert(entityFrameData.angularAcceleration));
+        }
+
+        return true;
+      });
+
+  // body angular acceleration
+  _ecm.Each<components::Pose, components::AngularAcceleration,
+            components::ParentEntity>(
+      [&](const Entity &,
+          const components::Pose *_pose,
+          components::AngularAcceleration *_angularAcc,
+          const components::ParentEntity *_parent)->bool
+      {
+        if (auto linkPhys = this->entityLinkMap.Get(_parent->Data()))
+        {
+          const auto entityFrameData =
+              this->LinkFrameDataAtOffset(linkPhys, _pose->Data());
+
+          auto entityWorldPose = math::eigen3::convert(entityFrameData.pose);
+          math::Vector3d entityWorldAngularAcc =
+              math::eigen3::convert(entityFrameData.angularAcceleration);
+
+          auto entityBodyAngularAcc =
+              entityWorldPose.Rot().RotateVectorReverse(entityWorldAngularAcc);
+          *_angularAcc = components::AngularAcceleration(entityBodyAngularAcc);
+        }
+
         return true;
       });
   GZ_PROFILE_END();