use gz-physics#283 to implement joint_states/effort feedback (backport #186) (#607)

Co-authored-by: Andreas Bihlmaier <andreas.bihlmaier@gmx.de>

Author: christophfroehlich

gz_ros2_control/src/gz_system.cpp

@@ -24,14 +24,17 @@
 #ifdef GZ_HEADERS
 #include <gz/msgs/imu.pb.h>
 
+#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>
@@ -41,17 +44,22 @@
 #include <gz/transport/Node.hh>
 #define GZ_TRANSPORT_NAMESPACE gz::transport::
 #define GZ_MSGS_NAMESPACE gz::msgs::
+#define GZ_PHYSICS_NAMESPACE gz::physics::
+#define GZ_VECTOR_DOT dot
 #else
 #include <ignition/msgs/imu.pb.h>
 
+#include <ignition/math/Vector3.hh>
 #include <ignition/gazebo/components/AngularVelocity.hh>
 #include <ignition/gazebo/components/Imu.hh>
-#include <ignition/gazebo/components/JointForce.hh>
+#include <ignition/gazebo/components/JointAxis.hh>
 #include <ignition/gazebo/components/JointForceCmd.hh>
 #include <ignition/gazebo/components/JointPosition.hh>
 #include <ignition/gazebo/components/JointPositionReset.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/JointVelocityReset.hh>
 #include <ignition/gazebo/components/LinearAcceleration.hh>
 #include <ignition/gazebo/components/Name.hh>
@@ -61,6 +69,8 @@
 #include <ignition/transport/Node.hh>
 #define GZ_TRANSPORT_NAMESPACE ignition::transport::
 #define GZ_MSGS_NAMESPACE ignition::msgs::
+#define GZ_PHYSICS_NAMESPACE ignition::math::
+#define GZ_VECTOR_DOT Dot
 #endif
 
 #include <hardware_interface/hardware_info.hpp>
@@ -70,6 +80,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;
 
@@ -236,6 +252,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(
@@ -253,12 +273,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
@@ -576,11 +596,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 ignitionrobotics/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 =
@@ -589,7 +608,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_NAMESPACE 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.GZ_VECTOR_DOT(
+      GZ_PHYSICS_NAMESPACE 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) {