Add Measured Force.

include/gz_gep_tools/joint_state_interface.hh

@@ -36,11 +36,13 @@ class ControlJointValue {
   double Cmd() const { return cmd_;}
   double PosMes() const { return pos_mes_;}
   double VelMes() const { return vel_mes_;}
+  double ForceMes() const { return force_mes_;}
   double PosDes() const { return pos_des_;}
   double VelDes() const { return vel_des_;}
 
   void SetPosMes(double &pos_mes) { pos_mes_ = pos_mes;}
   void SetVelMes(double &vel_mes) { vel_mes_ = vel_mes;}
+  void SetForceMes(double &force_mes) { force_mes_ = force_mes;}
 
   void ComputeCmd();
 
@@ -54,9 +56,9 @@ class ControlJointValue {
 
   // Clamping i value to i_clamp
   double i_clamp_;
-  
+
   // Measured quantities
-  double pos_mes_, vel_mes_;
+  double pos_mes_, vel_mes_, force_mes_;
 
   /// Command
   double cmd_;
@@ -80,12 +82,13 @@ bool SaveListOfNamedJoints(const RobotCtrlJointInfos &a_rbt_ctrl_joint_infos,
                            const std::string &afilename);
 
 /// Intermediate structure used to read information from Gazebo "joint_name/joint_state" topic.
-/// 
+///
 class GZJointValues {
  public:
   /// Measured quantities
   double pos_mes;
   double vel_mes;
+  double force_mes;
   /// Control
   double force_ctrl;
 };

src/joint_state_interface.cc

@@ -88,7 +88,8 @@ bool SaveCmd(const RobotCtrlJointInfos &a_rbt_ctrl_joint_infos,
   for(auto it_joint_info = a_rbt_ctrl_joint_infos.begin();
       it_joint_info != a_rbt_ctrl_joint_infos.end();
       it_joint_info++)
-    ofs_position_robot << it_joint_info->second.Cmd() << " ";
+    ofs_position_robot << it_joint_info->second.Cmd() << " "
+                       << it_joint_info->second.ForceMes() << " ";
   ofs_position_robot << std::endl;
   ofs_position_robot.close();
   return true;
@@ -97,12 +98,12 @@ bool SaveCmd(const RobotCtrlJointInfos &a_rbt_ctrl_joint_infos,
 bool SaveListOfNamedJoints(const RobotCtrlJointInfos &a_rbt_ctrl_joint_infos,
                            const std::string &afilename)
 {
-  std::ofstream ofs_position_robot(afilename.c_str(), std::ios::app);
+  std::ofstream ofs_position_robot(afilename.c_str());
+  unsigned int idx=1;
   for(auto it_joint_info = a_rbt_ctrl_joint_infos.begin();
       it_joint_info != a_rbt_ctrl_joint_infos.end();
       it_joint_info++)
-    ofs_position_robot << it_joint_info->first << " ";
-  ofs_position_robot << std::endl;
+    ofs_position_robot << idx++ << " - " << it_joint_info->first << " " << std::endl;
   ofs_position_robot.close();
   return true;
 }
@@ -167,6 +168,7 @@ void JointStateInterface::CallbackJointState(
 
     gz_robot_joints_.dict_joint_values[jointItr->name()].pos_mes = axis1.position();
     gz_robot_joints_.dict_joint_values[jointItr->name()].vel_mes = axis1.velocity();
+    gz_robot_joints_.dict_joint_values[jointItr->name()].force_mes = axis1.force();
 
   }