Publish Joint forces

src/systems/joint_state_publisher/JointStatePublisher.cc

@@ -129,7 +129,8 @@ void JointStatePublisher::CreateComponents(EntityComponentManager &_ecm,
   }
 
   // Create joint force component if one doesn't exist
-  if (!_ecm.EntityHasComponentType(_joint, components::JointForce().TypeId()))
+  if (!_ecm.EntityHasComponentType(_joint,
+        components::JointForce().TypeId()))
   {
     _ecm.CreateComponent(_joint, components::JointForce());
   }

src/systems/physics/Physics.cc

@@ -116,10 +116,13 @@
 #include "gz/sim/components/JointPositionLimitsCmd.hh"
 #include "gz/sim/components/JointPositionReset.hh"
 #include "gz/sim/components/JointType.hh"
+#include "gz/sim/components/JointForce.hh"
 #include "gz/sim/components/JointVelocity.hh"
 #include "gz/sim/components/JointVelocityCmd.hh"
 #include "gz/sim/components/JointVelocityLimitsCmd.hh"
 #include "gz/sim/components/JointVelocityReset.hh"
+#include "gz/sim/components/JointForce.hh"
+#include "gz/sim/components/JointForceCmd.hh"
 #include "gz/sim/components/LinearAcceleration.hh"
 #include "gz/sim/components/LinearVelocity.hh"
 #include "gz/sim/components/LinearVelocityCmd.hh"
@@ -130,7 +133,6 @@
 #include "gz/sim/components/ParentLinkName.hh"
 #include "gz/sim/components/ExternalWorldWrenchCmd.hh"
 #include "gz/sim/components/JointTransmittedWrench.hh"
-#include "gz/sim/components/JointForceCmd.hh"
 #include "gz/sim/components/Physics.hh"
 #include "gz/sim/components/PhysicsEnginePlugin.hh"
 #include "gz/sim/components/Pose.hh"
@@ -2791,6 +2793,7 @@ void PhysicsPrivate::ResetPhysics(EntityComponentManager &_ecm)
         {
           jointPhys->SetVelocity(i, 0.0);
           jointPhys->SetPosition(i, 0.0);
+          jointPhys->SetForce(i, 0.0);
         }
 
         return true;
@@ -3692,6 +3695,22 @@ void PhysicsPrivate::UpdateSim(EntityComponentManager &_ecm,
         }
         return true;
       });
+  // Update joint Forces
+  _ecm.Each<components::Joint, components::JointForce>(
+      [&](const Entity &_entity, components::Joint *,
+          components::JointForce *_jointForce) -> bool
+      {
+        if (auto jointPhys = this->entityJointMap.Get(_entity))
+        {
+          _jointForce->Data().resize(jointPhys->GetDegreesOfFreedom());
+          for (std::size_t i = 0; i < jointPhys->GetDegreesOfFreedom();
+               ++i)
+          {
+            _jointForce->Data()[i] = jointPhys->GetForce(i);
+          }
+        }
+        return true;
+      });
   GZ_PROFILE_END();
 
   // Update joint transmitteds