From 6c5fbd050227e0f08bd5d9485553984d3c35cd05 Mon Sep 17 00:00:00 2001
From: Andreas Bihlmaier <andreas.bihlmaier@gmx.de>
Date: Mon, 28 Oct 2024 02:00:03 -0700
Subject: [PATCH] use gz-physics#283 to implement joint_states/effort feedback
 (#186)

(cherry picked from commit cc66e7342c6954bd5c3950e12a45567e2ca1652c)
---
 gz_ros2_control/src/gz_system.cpp | 46 +++++++++++++++++++++++--------
 1 file changed, 35 insertions(+), 11 deletions(-)

diff --git a/gz_ros2_control/src/gz_system.cpp b/gz_ros2_control/src/gz_system.cpp
index d331cd27..5c9952b9 100644
--- a/gz_ros2_control/src/gz_system.cpp
+++ b/gz_ros2_control/src/gz_system.cpp
@@ -23,14 +23,17 @@
 #include <utility>
 #include <vector>
 
+#include <gz/physics/Geometry.hh>
 #include <gz/sim/components/AngularVelocity.hh>
 #include <gz/sim/components/Imu.hh>
-#include <gz/sim/components/JointForce.hh>
+#include <gz/sim/components/JointAxis.hh>
 #include <gz/sim/components/JointForceCmd.hh>
 #include <gz/sim/components/JointPosition.hh>
 #include <gz/sim/components/JointPositionReset.hh>
-#include <gz/sim/components/JointVelocity.hh>
+#include <gz/sim/components/JointTransmittedWrench.hh>
+#include <gz/sim/components/JointType.hh>
 #include <gz/sim/components/JointVelocityCmd.hh>
+#include <gz/sim/components/JointVelocity.hh>
 #include <gz/sim/components/JointVelocityReset.hh>
 #include <gz/sim/components/LinearAcceleration.hh>
 #include <gz/sim/components/Name.hh>
@@ -50,6 +53,12 @@ struct jointData
   /// \brief Joint's names.
   std::string name;
 
+  /// \brief Joint's type.
+  sdf::JointType joint_type;
+
+  /// \brief Joint's axis.
+  sdf::JointAxis joint_axis;
+
   /// \brief Current joint position
   double joint_position;
 
@@ -250,6 +259,10 @@ bool GazeboSimSystem::initSim(
 
     sim::Entity simjoint = enableJoints[joint_name];
     this->dataPtr->joints_[j].sim_joint = simjoint;
+    this->dataPtr->joints_[j].joint_type = _ecm.Component<sim::components::JointType>(
+      simjoint)->Data();
+    this->dataPtr->joints_[j].joint_axis = _ecm.Component<sim::components::JointAxis>(
+      simjoint)->Data();
 
     // Create joint position component if one doesn't exist
     if (!_ecm.EntityHasComponentType(
@@ -267,12 +280,12 @@ bool GazeboSimSystem::initSim(
       _ecm.CreateComponent(simjoint, sim::components::JointVelocity());
     }
 
-    // Create joint force component if one doesn't exist
+    // Create joint transmitted wrench component if one doesn't exist
     if (!_ecm.EntityHasComponentType(
         simjoint,
-        sim::components::JointForce().TypeId()))
+        sim::components::JointTransmittedWrench().TypeId()))
     {
-      _ecm.CreateComponent(simjoint, sim::components::JointForce());
+      _ecm.CreateComponent(simjoint, sim::components::JointTransmittedWrench());
     }
 
     // Accept this joint and continue configuration
@@ -533,11 +546,10 @@ hardware_interface::return_type GazeboSimSystem::read(
       this->dataPtr->ecm->Component<sim::components::JointVelocity>(
       this->dataPtr->joints_[i].sim_joint);
 
-    // TODO(ahcorde): Revisit this part gazebosim/ign-physics#124
-    // Get the joint force
-    // const auto * jointForce =
-    //   _ecm.Component<sim::components::JointForce>(
-    //   this->dataPtr->sim_joints_[j]);
+    // Get the joint force via joint transmitted wrench
+    const auto * jointWrench =
+      this->dataPtr->ecm->Component<sim::components::JointTransmittedWrench>(
+      this->dataPtr->joints_[i].sim_joint);
 
     // Get the joint position
     const auto * jointPositions =
@@ -546,7 +558,19 @@ hardware_interface::return_type GazeboSimSystem::read(
 
     this->dataPtr->joints_[i].joint_position = jointPositions->Data()[0];
     this->dataPtr->joints_[i].joint_velocity = jointVelocity->Data()[0];
-    // this->dataPtr->joint_effort_[j] = jointForce->Data()[0];
+    gz::physics::Vector3d force_or_torque;
+    if (this->dataPtr->joints_[i].joint_type == sdf::JointType::PRISMATIC) {
+      force_or_torque = {jointWrench->Data().force().x(),
+        jointWrench->Data().force().y(), jointWrench->Data().force().z()};
+    } else {  // REVOLUTE and CONTINUOUS
+      force_or_torque = {jointWrench->Data().torque().x(),
+        jointWrench->Data().torque().y(), jointWrench->Data().torque().z()};
+    }
+    // Calculate the scalar effort along the joint axis
+    this->dataPtr->joints_[i].joint_effort = force_or_torque.dot(
+      gz::physics::Vector3d{this->dataPtr->joints_[i].joint_axis.Xyz()[0],
+        this->dataPtr->joints_[i].joint_axis.Xyz()[1],
+        this->dataPtr->joints_[i].joint_axis.Xyz()[2]});
   }
 
   for (unsigned int i = 0; i < this->dataPtr->imus_.size(); ++i) {