From 70eb141407633f1a9cbb67ab2a964c7d459cb7cf Mon Sep 17 00:00:00 2001
From: ahb <andreas.bihlmaier@gmx.de>
Date: Sat, 7 Oct 2023 16:58:30 +0200
Subject: [PATCH] use of gz-physics#283 to implement joint_states/effort
 feedback

---
 ign_ros2_control/src/ign_system.cpp | 34 ++++++++++++++++++-----------
 1 file changed, 21 insertions(+), 13 deletions(-)

diff --git a/ign_ros2_control/src/ign_system.cpp b/ign_ros2_control/src/ign_system.cpp
index ac6d4161..55b7ea4c 100644
--- a/ign_ros2_control/src/ign_system.cpp
+++ b/ign_ros2_control/src/ign_system.cpp
@@ -25,11 +25,12 @@
 
 #include <ignition/gazebo/components/AngularVelocity.hh>
 #include <ignition/gazebo/components/Imu.hh>
-#include <ignition/gazebo/components/JointForce.hh>
 #include <ignition/gazebo/components/JointForceCmd.hh>
 #include <ignition/gazebo/components/JointPosition.hh>
-#include <ignition/gazebo/components/JointVelocity.hh>
+#include <ignition/gazebo/components/JointTransmittedWrench.hh>
+#include <ignition/gazebo/components/JointType.hh>
 #include <ignition/gazebo/components/JointVelocityCmd.hh>
+#include <ignition/gazebo/components/JointVelocity.hh>
 #include <ignition/gazebo/components/LinearAcceleration.hh>
 #include <ignition/gazebo/components/Name.hh>
 #include <ignition/gazebo/components/ParentEntity.hh>
@@ -46,6 +47,9 @@ struct jointData
   /// \brief Joint's names.
   std::string name;
 
+  /// \brief Joint's type.
+  sdf::JointType joint_type;
+
   /// \brief Current joint position
   double joint_position;
 
@@ -200,6 +204,7 @@ bool IgnitionSystem::initSim(
 
     ignition::gazebo::Entity simjoint = enableJoints[joint_name];
     this->dataPtr->joints_[j].sim_joint = simjoint;
+    this->dataPtr->joints_[j].joint_type = _ecm.Component<ignition::gazebo::components::JointType>(simjoint)->Data();
 
     // Create joint position component if one doesn't exist
     if (!_ecm.EntityHasComponentType(
@@ -217,12 +222,12 @@ bool IgnitionSystem::initSim(
       _ecm.CreateComponent(simjoint, ignition::gazebo::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,
-        ignition::gazebo::components::JointForce().TypeId()))
+        ignition::gazebo::components::JointTransmittedWrench().TypeId()))
     {
-      _ecm.CreateComponent(simjoint, ignition::gazebo::components::JointForce());
+      _ecm.CreateComponent(simjoint, ignition::gazebo::components::JointTransmittedWrench());
     }
 
     // Accept this joint and continue configuration
@@ -502,11 +507,10 @@ hardware_interface::return_type IgnitionSystem::read(
       this->dataPtr->ecm->Component<ignition::gazebo::components::JointVelocity>(
       this->dataPtr->joints_[i].sim_joint);
 
-    // TODO(ahcorde): Revisit this part ignitionrobotics/ign-physics#124
-    // Get the joint force
-    // const auto * jointForce =
-    //   _ecm.Component<ignition::gazebo::components::JointForce>(
-    //   this->dataPtr->sim_joints_[j]);
+    // Get the joint force via joint transmitted wrench
+    const auto * jointWrench =
+      this->dataPtr->ecm->Component<ignition::gazebo::components::JointTransmittedWrench>(
+      this->dataPtr->joints_[i].sim_joint);
 
     // Get the joint position
     const auto * jointPositions =
@@ -515,7 +519,11 @@ hardware_interface::return_type IgnitionSystem::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];
+    if (this->dataPtr->joints_[i].joint_type == sdf::JointType::PRISMATIC) {
+      this->dataPtr->joints_[i].joint_effort = jointWrench->Data().force().z();
+    } else {  // REVOLUTE and CONTINUOUS
+      this->dataPtr->joints_[i].joint_effort = jointWrench->Data().torque().z();
+    }
   }
 
   for (unsigned int i = 0; i < this->dataPtr->imus_.size(); ++i) {
@@ -705,8 +713,8 @@ hardware_interface::return_type IgnitionSystem::write(
         }
       }
       if (mimic_interface == "effort") {
-        // TODO(ahcorde): Revisit this part ignitionrobotics/ign-physics#124
-        // Get the joint force
+        // TODO(ahb):
+        // Get the joint force via joint transmitted wrench
         // const auto * jointForce =
         //   _ecm.Component<ignition::gazebo::components::JointForce>(
         //   this->dataPtr->sim_joints_[j]);