.clang-format

+Language: Cpp
+BasedOnStyle: Microsoft
+
+AlwaysBreakTemplateDeclarations: Yes
+BinPackParameters: false
+
+BreakBeforeBinaryOperators: NonAssignment
+BreakConstructorInitializers: BeforeComma
+BreakInheritanceList: BeforeComma
+
+ConstructorInitializerIndentWidth: 0
+ContinuationIndentWidth: 2
+
+IndentWidth: 2
+
+NamespaceIndentation: All
+
+PackConstructorInitializers: Never
+
+PenaltyReturnTypeOnItsOwnLine: 10
+PointerAlignment: Middle
+
+SpaceAfterTemplateKeyword: false
+
+ColumnLimit: 100
+
+SortIncludes: false
+
+IndentPPDirectives: BeforeHash
+
+AlignAfterOpenBracket: AlwaysBreak

.pre-commit-config.yaml

+ci:
+  autoupdate_branch: main
+  autofix_prs: false
+  autoupdate_schedule: quarterly
+repos:
+  - repo: https://github.com/pre-commit/mirrors-clang-format
+    rev: v19.1.6
+    hooks:
+      - id: clang-format
+        types_or: []
+        types: [text]
+        files: \.(cpp|cxx|c|h|hpp|hxx|txx)$
+  - repo: https://github.com/pre-commit/pre-commit-hooks
+    rev: v5.0.0
+    hooks:
+      - id: check-added-large-files
+      - id: check-case-conflict
+      - id: check-yaml
+        exclude: ^packaging/conda/
+      - id: detect-private-key
+      - id: end-of-file-fixer
+      - id: mixed-line-ending
+      - id: check-merge-conflict
+      - id: trailing-whitespace
+        exclude: |
+          (?x)^(
+              doc/doxygen-awesome.*
+          )$
+  - repo: https://github.com/astral-sh/ruff-pre-commit
+    rev: v0.8.6
+    hooks:
+      - id: ruff
+      - id: ruff-format
+  - repo: https://github.com/cheshirekow/cmake-format-precommit
+    rev: v0.6.13
+    hooks:
+      - id: cmake-format
+        additional_dependencies: [pyyaml>=5.1]
+        exclude: pinocchiopy.pc.cmake
+  - repo: https://github.com/Lucas-C/pre-commit-hooks
+    rev: v1.5.5
+    hooks:
+      - id: forbid-tabs
+  - repo: https://github.com/jumanjihouse/pre-commit-hook-yamlfmt
+    rev: 0.2.3
+    hooks:
+      - id: yamlfmt
+        args: [--mapping=2, --offset=2, --sequence=4, --implicit_start, --preserve-quotes]
\ No newline at end of file

include/gz_gep_tools/joint_state_interface.hh

@@ -9,35 +9,79 @@
 #include <gz/transport.hh>
 
 namespace gz_transport_hw_tools {
-
+/// This class is to be used by the user to provide:
+/// @param Kp: Gain to reach the desired position pos_des
+/// @param Kd: Gain to reach the desired velocity vel_des
+/// @param Ki; Gain for the integral error
+/// @param i_clamp: Clamping the integral action
+/// @param pos_des: Desired position (used for the initailization phase, i.e. at
+/// start up)
+/// @param vel_des: Velocity position (used for the initailization phase, i.e.
+/// at start up)
+/// Each joint has such a structure which can initialized in a container here a map.
 class ControlJointValue {
  public:
+
+  ControlJointValue( double Kp,
+                     double Kd,
+                     double Ki,
+                     double i_clamp,
+                     double pos_des,
+                     double vel_des);
+
+  ControlJointValue(const ControlJointValue &other);
+
+  ControlJointValue(const ControlJointValue &&other) noexcept;
+
+  double Cmd() const { return cmd_;}
+  double PosMes() const { return pos_mes_;}
+  double VelMes() const { return vel_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 ComputeCmd();
+
+ private:
+
   /// Desired quantities
-  double pos_des, vel_des;
+  double pos_des_, vel_des_;
 
   // Control gains
-  double Kp, Kd;
+  double Kp_, Kd_, Ki_;
 
+  // Clamping i value to i_clamp
+  double i_clamp_;
+  
   // Measured quantities
-  double pos_mes, vel_mes;
+  double pos_mes_, vel_mes_;
+
+  /// Command
+  double cmd_;
+
+  /// Cumulative error
+  double cumul_err_;
 
-  double cmd;
 
-  void compute_cmd()
-  {
-    cmd = Kp*(pos_des - pos_mes) + Kd*(vel_des -vel_mes);
-  }
 };
 
+/// Definition of the type handling a map of controlled joint
 typedef std::map<std::string, ControlJointValue> RobotCtrlJointInfos;
 
 bool SaveCmd(const RobotCtrlJointInfos & a_rbt_ctrl_joint_infos,
-                   const std::string &afilename);
+             const std::string &afilename);
 bool SavePos(const RobotCtrlJointInfos & a_rbt_ctrl_joint_infos,
-                   const std::string &afilename);                     
-
-
-class JointValues {
+             const std::string &afilename);
+bool SavePosDes(const RobotCtrlJointInfos & a_rbt_ctrl_joint_infos,
+                const std::string &afilename);
+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;
@@ -50,18 +94,18 @@ class JointValues {
 };
 
 
-class RobotJoints {
+class GZRobotJoints {
  public:
   /// Time
   int64_t time_sec_;
   int64_t time_nsec_;
 
   /// Map of joints values
-  std::map<std::string, JointValues> dict_joint_values;
-  
+  std::map<std::string, GZJointValues> dict_joint_values;
+
   std::mutex lock_state_access_;
-  
-  RobotJoints();
+
+  GZRobotJoints();
 };
 
 
@@ -96,20 +140,14 @@ class JointStateInterface {
   /// Prefix world
   std::string prefix_world_;
 
-  /// List of joints
-  std::vector<std::string> list_of_joints_;
-
-  /// Map joints to index in list
-  std::map<std::string, std::size_t> map_name_2_indx_;
-
   /// Topic name joint state
   std::string joint_state_topic_;
 
   /// GZ node
   gz::transport::Node node_;
 
-  /// Last state of the robot
-  RobotJoints robot_joints_;
+  /// Last state of the robot from Gazebo
+  GZRobotJoints gz_robot_joints_;
 
   bool debug_level_;
 };

include/gz_gep_tools/perception_action_loop.hh

@@ -34,7 +34,9 @@ class PerceptionActionLoop {
   double state_gz_time_;
   double pre_state_gz_time_;
   unsigned long long int local_time_;
-  
+
+  /// Debug level
+  bool debug_level_;
 };
 
 }

src/control_over_gz.cc

@@ -214,7 +214,7 @@ bool ControlOverGz::SendWorldControlStateToInitECM(const RobotCtrlJointInfos &rb
     auto a_ctrl_joint_info = rbt_ctrl_joint_infos.find(jointName);
     if (a_ctrl_joint_info != rbt_ctrl_joint_infos.end())
     {
-      one_vecd.push_back(a_ctrl_joint_info->second.pos_des);
+      one_vecd.push_back(a_ctrl_joint_info->second.PosDes());
       //    std::cout << " " << one_vecd[0] << std::endl;
       aJoint.ResetPosition(Robot_ECM_,one_vecd);
     }

src/joint_state_interface.cc

@@ -7,33 +7,104 @@
 
 namespace gz_transport_hw_tools {
 
+ControlJointValue::ControlJointValue(double Kp,
+                                     double Kd,
+                                     double Ki,
+                                     double i_clamp,
+                                     double pos_des,
+                                     double vel_des):
+    pos_des_(pos_des), vel_des_(vel_des), Kp_(Kp),
+    Kd_(Kd), Ki_(Ki), i_clamp_(i_clamp), pos_mes_(0.0),
+    vel_mes_(0.0), cmd_(0.0), cumul_err_(0.0) {
+}
+ControlJointValue::ControlJointValue(const ControlJointValue &other)
+    : ControlJointValue(other.Kp_, other.Kd_, other.Ki_,
+                        other.i_clamp_,
+                        other.pos_des_, other.vel_des_)
+{
+  pos_mes_ = other.pos_mes_;
+  vel_mes_ = other.vel_mes_;
+  cmd_ = other.cmd_;
+  cumul_err_ = other.cumul_err_;
+}
+
+ControlJointValue::ControlJointValue(const ControlJointValue &&other) noexcept
+    :  ControlJointValue(other.Kp_, other.Kd_, other.Ki_,
+                         other.i_clamp_,
+                         other.pos_des_, other.vel_des_ )
+{
+  pos_mes_ = other.pos_mes_;
+  vel_mes_ = other.vel_mes_;
+  cmd_ = other.cmd_;
+  cumul_err_ = other.cumul_err_;
+}
+
+void ControlJointValue::ComputeCmd()
+{
+  double err_pos = pos_des_ - pos_mes_;
+  double i_term = Ki_*cumul_err_;
+  if (i_term > i_clamp_)
+    i_term = i_clamp_;
+  else if (i_term < -i_clamp_)
+    i_term = - i_clamp_;
+
+  cmd_ = Kp_* err_pos + Kd_*(vel_des_ -vel_mes_) + i_term;
+  cumul_err_ += err_pos;
+}
+
 bool SavePos(const RobotCtrlJointInfos &a_rbt_ctrl_joint_infos,
-             const std::string &afilename) 
+             const std::string &afilename)
+{
+  std::ofstream ofs_position_robot(afilename.c_str(), std::ios::app);
+  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.PosMes() << " ";
+  ofs_position_robot << std::endl;
+  ofs_position_robot.close();
+  return true;
+}
+
+bool SavePosDes(const RobotCtrlJointInfos &a_rbt_ctrl_joint_infos,
+             const std::string &afilename)
 {
   std::ofstream ofs_position_robot(afilename.c_str(), std::ios::app);
   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.pos_mes << " ";
+    ofs_position_robot << it_joint_info->second.PosDes() << " ";
   ofs_position_robot << std::endl;
   ofs_position_robot.close();
   return true;
 }
 
 bool SaveCmd(const RobotCtrlJointInfos &a_rbt_ctrl_joint_infos,
-             const std::string &afilename) 
+             const std::string &afilename)
 {
   std::ofstream ofs_position_robot(afilename.c_str(), std::ios::app);
   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() << " ";
   ofs_position_robot << std::endl;
   ofs_position_robot.close();
   return true;
 }
 
-RobotJoints::RobotJoints() :
+bool SaveListOfNamedJoints(const RobotCtrlJointInfos &a_rbt_ctrl_joint_infos,
+                           const std::string &afilename)
+{
+  std::ofstream ofs_position_robot(afilename.c_str(), std::ios::app);
+  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.close();
+  return true;
+}
+
+GZRobotJoints::GZRobotJoints() :
     time_sec_(0),
     time_nsec_(0)
 {}
@@ -57,7 +128,7 @@ JointStateInterface::~JointStateInterface() {}
 void JointStateInterface::SetListOfJoints(
     const RobotCtrlJointInfos &rbt_ctrl_joint_infos)
 {
-  robot_joints_.dict_joint_values.clear();
+  gz_robot_joints_.dict_joint_values.clear();
 
   for (auto jointItr = rbt_ctrl_joint_infos.begin();
        jointItr != rbt_ctrl_joint_infos.end();
@@ -65,13 +136,13 @@ void JointStateInterface::SetListOfJoints(
     /// Build a new string
     std::string a_new_cmd_force = prefix_model_root_ + std::string("joint/") +
         jointItr->first + std::string("/cmd_force");
-    
+
     /// Build advertise
-    robot_joints_.dict_joint_values[jointItr->first].gz_pub_cmd_force = 
+    gz_robot_joints_.dict_joint_values[jointItr->first].gz_pub_cmd_force =
         node_.Advertise<gz::msgs::Double>(a_new_cmd_force);
-    
-    robot_joints_.dict_joint_values[jointItr->first].cmd_force_topic = a_new_cmd_force;    
-    
+
+    gz_robot_joints_.dict_joint_values[jointItr->first].cmd_force_topic = a_new_cmd_force;
+
   }
 
 }
@@ -80,7 +151,7 @@ void JointStateInterface::CallbackJointState(
     const gz::msgs::Model&a_gz_model_msg
     ) {
 
-  std::lock_guard(robot_joints_.lock_state_access_);
+  std::lock_guard(gz_robot_joints_.lock_state_access_);
 
   for (auto jointItr = a_gz_model_msg.joint().begin();
        jointItr != a_gz_model_msg.joint().end();
@@ -88,61 +159,61 @@ void JointStateInterface::CallbackJointState(
 
     const ::gz::msgs::Axis &axis1 = jointItr->axis1();
 
-    robot_joints_.dict_joint_values[jointItr->name()].pos_mes = axis1.position();
-    robot_joints_.dict_joint_values[jointItr->name()].vel_mes = axis1.velocity();
+    gz_robot_joints_.dict_joint_values[jointItr->name()].pos_mes = axis1.position();
+    gz_robot_joints_.dict_joint_values[jointItr->name()].vel_mes = axis1.velocity();
 
   }
 
   if (debug_level_)
   {
     std::cerr << "JointStateInterface::CallbackJointState: time: "
-              << " " << robot_joints_.time_sec_
-              << " " << robot_joints_.time_nsec_;
-    if (robot_joints_.dict_joint_values["arm_left_1_joint"].pos_mes)
-      std::cerr << " " << robot_joints_.dict_joint_values["arm_left_1_joint"].pos_mes;
+              << " " << gz_robot_joints_.time_sec_
+              << " " << gz_robot_joints_.time_nsec_;
+    if (gz_robot_joints_.dict_joint_values["arm_left_1_joint"].pos_mes)
+      std::cerr << " " << gz_robot_joints_.dict_joint_values["arm_left_1_joint"].pos_mes;
 
     std::cerr << std::endl;
   }
-  robot_joints_.time_sec_ = a_gz_model_msg.header().stamp().sec();
-  robot_joints_.time_nsec_ = a_gz_model_msg.header().stamp().nsec();
+  gz_robot_joints_.time_sec_ = a_gz_model_msg.header().stamp().sec();
+  gz_robot_joints_.time_nsec_ = a_gz_model_msg.header().stamp().nsec();
 
 
 }
 
 bool JointStateInterface::SetCmd( const RobotCtrlJointInfos &rbt_ctrl_joint_infos)
 {
-  if (rbt_ctrl_joint_infos.size()>robot_joints_.dict_joint_values.size())
+  if (rbt_ctrl_joint_infos.size()>gz_robot_joints_.dict_joint_values.size())
   {
     std::cerr << "rbt_ctrl_joint_infos.size(): "
               << rbt_ctrl_joint_infos.size()
-              << " robot_joints_.dict_joint_values.size()"
-              << robot_joints_.dict_joint_values.size()
+              << " gz_robot_joints_.dict_joint_values.size()"
+              << gz_robot_joints_.dict_joint_values.size()
               << std::endl;
     return false;
   }
   // Iterate over the cmd map.
   for (auto cmd_it = rbt_ctrl_joint_infos.begin();
        cmd_it != rbt_ctrl_joint_infos.end();
-       cmd_it++)    
+       cmd_it++)
   {
     gz::msgs::Double msg;
-    msg.set_data(cmd_it->second.cmd);
+    msg.set_data(cmd_it->second.Cmd());
 
-    auto it_robot_joint_value = robot_joints_.dict_joint_values.find(cmd_it->first);
+    auto it_robot_joint_value = gz_robot_joints_.dict_joint_values.find(cmd_it->first);
     // If the joint is not found go to the next iteration.
-    if (it_robot_joint_value == robot_joints_.dict_joint_values.end())
+    if (it_robot_joint_value == gz_robot_joints_.dict_joint_values.end())
       continue;
-    
+
     if (!it_robot_joint_value->second.gz_pub_cmd_force.Publish(msg)){
       std::cerr << "Unable to publish on "
                 << it_robot_joint_value->second.cmd_force_topic
                 << std::endl;
       continue;
-    } 
+    }
     if (debug_level_)
     {
-      std::cout << " Publish " << cmd_it->second.cmd << " on "
-                << robot_joints_.dict_joint_values[cmd_it->first].cmd_force_topic
+      std::cout << " Publish " << cmd_it->second.Cmd() << " on "
+                << gz_robot_joints_.dict_joint_values[cmd_it->first].cmd_force_topic
                 << std::endl;
     }
   }
@@ -152,28 +223,37 @@ bool JointStateInterface::SetCmd( const RobotCtrlJointInfos &rbt_ctrl_joint_info
 bool JointStateInterface::GetPosVel(RobotCtrlJointInfos &rbt_ctrl_joint_infos,
                                     double &time)
 {
-  if (rbt_ctrl_joint_infos.size()> robot_joints_.dict_joint_values.size())
+  if (rbt_ctrl_joint_infos.size()> gz_robot_joints_.dict_joint_values.size())
   {
     std::cerr << "rbt_ctrl_joint_infos.size(): " << rbt_ctrl_joint_infos.size()
-              << " robot_joints_.dict_joint_values.size():"  << robot_joints_.dict_joint_values.size()
+              << " gz_robot_joints_.dict_joint_values.size():"  << gz_robot_joints_.dict_joint_values.size()
               << std::endl;
       return false;
   }
-  std::lock_guard(robot_joints_.lock_state_access_);
-  time=(double)robot_joints_.time_sec_ + 1e-9*(double)robot_joints_.time_nsec_;
-  for (auto joint_it = rbt_ctrl_joint_infos.begin();
-       joint_it != rbt_ctrl_joint_infos.end();
-       joint_it++)
+  std::lock_guard(gz_robot_joints_.lock_state_access_);
+  time=(double)gz_robot_joints_.time_sec_ + 1e-9*(double)gz_robot_joints_.time_nsec_;
+  for (auto ctrl_joint_it = rbt_ctrl_joint_infos.begin();
+       ctrl_joint_it != rbt_ctrl_joint_infos.end();
+       ctrl_joint_it++)
   {
-    joint_it->second.pos_mes = robot_joints_.dict_joint_values[joint_it->first].pos_mes;
-    joint_it->second.vel_des = robot_joints_.dict_joint_values[joint_it->first].vel_mes;
+    ctrl_joint_it->second.SetPosMes(gz_robot_joints_.dict_joint_values[ctrl_joint_it->first].pos_mes);
+    ctrl_joint_it->second.SetVelMes(gz_robot_joints_.dict_joint_values[ctrl_joint_it->first].vel_mes);
   }
   if (debug_level_)
+  {
     std::cerr << "JointStateInterface::GetPosVel: time: "
               << time << " "
-              << rbt_ctrl_joint_infos["arm_left_1_joint"].pos_mes << " "
               << std::endl;
+    for (auto ctrl_joint_it = rbt_ctrl_joint_infos.begin();
+         ctrl_joint_it != rbt_ctrl_joint_infos.end();
+         ctrl_joint_it++)
+    {
+
+      std::cerr << ctrl_joint_it->second.PosMes() << " ";
 
+    }
+    std::cerr << std::endl;
+  }
   return true;
 }
 

src/perception_action_loop.cc

@@ -28,13 +28,14 @@ void signal_handler(int _signal)
 
 namespace gz_transport_hw_tools {
 
-PerceptionActionLoop::PerceptionActionLoop(gz_transport_hw_tools::RobotCtrlJointInfos & a_robot_ctrl_joint_infos,
-                                           std::string &a_prefix_model_root,
-                                           std::string &a_prefix_world,
-                                           bool debug_level):
-    robot_ctrl_joint_infos_(a_robot_ctrl_joint_infos),
-    joint_state_interface_(a_prefix_model_root,a_prefix_world,debug_level),
-    control_over_gz_(a_prefix_world,debug_level) {
+PerceptionActionLoop::PerceptionActionLoop(
+    gz_transport_hw_tools::RobotCtrlJointInfos &a_robot_ctrl_joint_infos,
+    std::string &a_prefix_model_root, std::string &a_prefix_world,
+    bool debug_level)
+    : robot_ctrl_joint_infos_(a_robot_ctrl_joint_infos),
+      joint_state_interface_(a_prefix_model_root, a_prefix_world, debug_level),
+      control_over_gz_(a_prefix_world, debug_level), debug_level_(debug_level)
+      {
 
   joint_state_interface_.SetListOfJoints(a_robot_ctrl_joint_infos);
 
@@ -42,6 +43,9 @@ PerceptionActionLoop::PerceptionActionLoop(gz_transport_hw_tools::RobotCtrlJoint
   std::signal(SIGINT,  signal_handler);
   std::signal(SIGTERM, signal_handler);
 
+  // Save list of joint named
+  std::string filename_list_named_joints("/tmp/list_of_joints.dat");
+  SaveListOfNamedJoints(robot_ctrl_joint_infos_,filename_list_named_joints);
 }
 
 int PerceptionActionLoop::InitGz()
@@ -58,7 +62,7 @@ int PerceptionActionLoop::InitGz()
 
   using namespace std::chrono_literals;
   // Wait 1 ms to perform reset.
-  std::this_thread::sleep_for(std::chrono::nanoseconds(1ms));
+  //std::this_thread::sleep_for(std::chrono::nanoseconds(1ms));
   // Start simulation.
   control_over_gz_.Step();
   control_over_gz_.ReadWorldStateToInitECM();
@@ -66,11 +70,12 @@ int PerceptionActionLoop::InitGz()
   // Second reset to set the robot state to a specific position.
   if (!control_over_gz_.Reset())  {
     std::cerr << "Reset failed" << std::endl;
-  }  
+  }
   control_over_gz_.SendWorldControlStateToInitECM(robot_ctrl_joint_infos_);
-  control_over_gz_.DisplayLinkValues();
+  if (debug_level_)
+    control_over_gz_.DisplayLinkValues();
   control_over_gz_.Step();
-  
+
   /// Synchronize simulation wait for starting.
   unsigned long int i=0;
   while(std::isnan(pre_state_gz_time_) ||
@@ -80,9 +85,9 @@ int PerceptionActionLoop::InitGz()
     pre_state_gz_time_=control_over_gz_.GetSimTime();
     if ((i%10==0) && (i>1)){
       control_over_gz_.Reset();
-      std::this_thread::sleep_for(std::chrono::nanoseconds(2ms));
+      //  std::this_thread::sleep_for(std::chrono::nanoseconds(2ms));
       control_over_gz_.Step();
-      std::this_thread::sleep_for(std::chrono::nanoseconds(2ms));      
+      //std::this_thread::sleep_for(std::chrono::nanoseconds(2ms));
       if (i>100) {
         std::cerr << "control_loop stuck in the waiting loop for starting."
                   << std::endl
@@ -98,13 +103,18 @@ int PerceptionActionLoop::InitGz()
 }
 
 int PerceptionActionLoop::MainLoop()
-{    
+{
   unsigned long int internal_timer = 0;
   std::string filename_pos("/tmp/position.dat");
+  std::string filename_pos_des("/tmp/position_des.dat");
   std::string filename_cmd("/tmp/cmd.dat");
   std::ofstream ofs_position_robot(filename_pos),
-      ofs_cmd_robot(filename_cmd);  
-  
+      ofs_cmd_robot(filename_cmd),
+      ofs_position_des_robot(filename_pos_des);
+  ofs_position_robot.close();
+  ofs_cmd_robot.close();
+  ofs_position_des_robot.close();
+
   /// CTRL-C loop
   while (!g_terminatePub)
   {
@@ -131,7 +141,7 @@ int PerceptionActionLoop::MainLoop()
             it_ctrl_joint_i!=robot_ctrl_joint_infos_.end();
             it_ctrl_joint_i++)
         {
-          it_ctrl_joint_i->second.compute_cmd();
+          it_ctrl_joint_i->second.ComputeCmd();
         }
       }
 
@@ -142,9 +152,10 @@ int PerceptionActionLoop::MainLoop()
       if (joint_state_interface_.SetCmd(robot_ctrl_joint_infos_))
       {
         SavePos(robot_ctrl_joint_infos_,filename_pos);
-        SaveCmd(robot_ctrl_joint_infos_,filename_cmd);            
+        SavePosDes(robot_ctrl_joint_infos_,filename_pos_des);
+        SaveCmd(robot_ctrl_joint_infos_,filename_cmd);
       }
-      
+
       // Start simulation.
       // TODO : Verify that step has converged before returning.
       control_over_gz_.Step();

tools/control_loop.cc

@@ -33,6 +33,7 @@
 #include <gz_gep_tools/perception_action_loop.hh>
 
 
+using namespace gz_transport_hw_tools;
 //////////////////////////////////////////////////
 int main(int argc, char **argv)
 {
@@ -50,72 +51,72 @@ int main(int argc, char **argv)
 
   // Create a transport node and advertise a topic.
   gz::transport::Node node;
-  gz_transport_hw_tools::RobotCtrlJointInfos talos_ctrl_joint_infos {
-    { "arm_left_1_joint", { 0.25847 , 0.0, 5000.0, 1.0, 0.0, 0.0, 0.0}, },    
-    { "arm_left_2_joint", { 0.173046, 0.0, 5000.0, 1.0, 0.0, 0.0, 0.0} },
-    { "arm_left_3_joint", {-0.0002  , 0.0, 2000.0, 10.0, 0.0, 0.0, 0.0} },
-    { "arm_left_4_joint", {-0.525366, 0.0, 2000.0, 10.0, 0.0, 0.0, 0.0} }, 
-    { "arm_left_5_joint", { 0.0     , 0.0, 250.0, 1.0, 0.0, 0.0, 0.0} },    
-    { "arm_left_6_joint", { 0.0     , 0.0, 250.0, 1.0, 0.0, 0.0, 0.0} },
-    { "arm_left_7_joint", { 0.1     , 0.0, 250.0, 1.0, 0.0, 0.0, 0.0} },
-    { "arm_right_1_joint", {-0.25847 , 0.0, 5000.0, 1.0, 0.0, 0.0, 0.0} },
-    { "arm_right_2_joint", {-0.173046, 0.0, 5000.0, 1.0, 0.0, 0.0, 0.0}},
-    { "arm_right_3_joint", { 0.0002  , 0.0, 2000.0, 10.0, 0.0, 0.0, 0.0} },
-    { "arm_right_4_joint", {-0.525366, 0.0, 2000.0, 10.0, 0.0, 0.0, 0.0} },
-    { "arm_right_5_joint", { 0.0     , 0.0, 250.0, 1.0, 0.0, 0.0, 0.0}},
-    { "arm_right_6_joint", { 0.0     , 0.0, 250.0, 1.0, 0.0, 0.0, 0.0}},
-    { "arm_right_7_joint", { 0.1     , 0.0, 250.0, 1.0, 0.0, 0.0, 0.0}},
-    { "gripper_left_joint", { 0.0     , 0.0, 1000.0, 10.0, 0.0, 0.0, 0.0}},
-    { "gripper_right_joint", { 0.0     , 0.0, 1000.0, 10.0, 0.0, 0.0, 0.0}},
-    { "head_1_joint", { 0.     , 0.0, 300.0, 0.1, 0.0, 0.0, 0.0}},
-    { "head_2_joint", { 0.     , 0.0, 300.0, 0.1, 0.0, 0.0, 0.0} },
-    { "leg_left_1_joint",{ 0.0     , 0.0, 5000.0, 5.0, 0.0, 0.0, 0.0}},
-    { "leg_left_2_joint",{ 0.0     , 0.0, 5000.0, 5.0, 0.0, 0.0, 0.0}},
-    { "leg_left_3_joint",{-0.411354, 0.0, 5000.0, 5.0, 0.0, 0.0, 0.0}},
-    { "leg_left_4_joint",{ 0.859395, 0.0, 5000.0, 5.0, 0.0, 0.0, 0.0}},
-    { "leg_left_5_joint",{-0.448041, 0.0, 5000.0, 5.0, 0.0, 0.0, 0.0}},
-    { "leg_left_6_joint",{-0.001708, 0.0, 5000.0, 5.0, 0.0, 0.0, 0.0}},
-    { "leg_right_1_joint",{ 0.0     , 0.0, 5000.0, 5.0, 0.0, 0.0, 0.0}},
-    { "leg_right_2_joint",{ 0.0     , 0.0, 5000.0, 5.0, 0.0, 0.0, 0.0}},
-    { "leg_right_3_joint",{-0.411354, 0.0, 5000.0, 5.0, 0.0, 0.0, 0.0}},
-    { "leg_right_4_joint",{0.859395 , 0.0, 5000.0, 5.0, 0.0, 0.0, 0.0}},
-    { "leg_right_5_joint",{-0.448041, 0.0, 5000.0, 5.0, 0.0, 0.0, 0.0}},
-    { "leg_right_6_joint",{-0.001708, 0.0, 5000.0, 5.0, 0.0, 0.0, 0.0}},
-    { "torso_1_joint",{ 0.0     , 0.0, 10000.0, 10.0, 0.0, 0.0, 0.0}},
-    { "torso_2_joint",{ 0.006761, 0.0, 10000.0, 10.0, 0.0, 0.0, 0.0}}
+  RobotCtrlJointInfos talos_ctrl_joint_infos {
+    { "arm_left_1_joint", ControlJointValue(  10000.0, 0.01, 1.0, 14.0, 0.25847 ,  0.0)},
+    { "arm_left_2_joint", ControlJointValue(  10000.0, 0.01, 1.0, 14.0, 0.173046,  0.0)},
+    { "arm_left_3_joint", ControlJointValue(   5000.0, 0.0 , 1.0,  9.0, -0.0002  , 0.0)},
+    { "arm_left_4_joint", ControlJointValue(   5000.0, 0.0 , 1.0,  9.0, -0.525366, 0.0)},
+    { "arm_left_5_joint", ControlJointValue(    500.0, 1.0 , 0.1,  5.0,  0.0,      0.0)},
+    { "arm_left_6_joint", ControlJointValue(    500.0, 1.0 , 0.1,  5.0,  0.0,      0.0)},
+    { "arm_left_7_joint", ControlJointValue(    100.0, 1.0 , 0.0,  3.0,  0.0,      0.0)},
+    { "arm_right_1_joint", ControlJointValue( 10000.0, 0.01, 1.0, 14.0, -0.25847 , 0.0)},
+    { "arm_right_2_joint", ControlJointValue( 10000.0, 0.01, 1.0, 14.0, -0.173046, 0.0)},
+    { "arm_right_3_joint", ControlJointValue(  5000.0, 0.0 , 1.0,  9.0,  0.0002  , 0.0)},
+    { "arm_right_4_joint", ControlJointValue(  5000.0, 0.0 , 1.0,  9.0, -0.525366, 0.0)},
+    { "arm_right_5_joint", ControlJointValue(   500.0, 1.0 , 0.1,  5.0,  0.0,      0.0)},
+    { "arm_right_6_joint", ControlJointValue(   500.0, 1.0 , 0.1,  5.0,  0.0,      0.0)},
+    { "arm_right_7_joint", ControlJointValue(   500.0, 1.0 , 0.1,  5.0,  0.0,      0.0)},
+    { "gripper_left_joint",  ControlJointValue(1000.0,10.0 , 1.0, 10.0,  0.0,      0.0)},
+    { "gripper_right_joint", ControlJointValue(1000.0,10.0 , 1.0, 10.0,  0.0,      0.0)},
+    { "head_1_joint", ControlJointValue(        300.0, 0.1,  1.0,  5.0,  0.0,      0.0)},
+    { "head_2_joint", ControlJointValue(        300.0, 0.1,  1.0,  1.5,  0.0,      0.0)},
+    { "leg_left_1_joint", ControlJointValue(   5000.0,20.0,  5.0,   7.0, 0.0,      0.0)},
+    { "leg_left_2_joint", ControlJointValue(   5000.0,20.0,  5.0,  14.0, 0.0,      0.0)},
+    { "leg_left_3_joint", ControlJointValue(   5000.0,20.0,  5.0,  14.0,-0.448041, 0.0)},
+    { "leg_left_4_joint", ControlJointValue(   5000.0,20.0,  5.0,  25.0, 0.896082, 0.0)},
+    { "leg_left_5_joint", ControlJointValue(   5000.0,20.0,  5.0,  14.0,-0.448041, 0.0)},
+    { "leg_left_6_joint", ControlJointValue(   5000.0,20.0,  5.0,   9.0, 0.0,      0.0)},
+    { "leg_right_1_joint", ControlJointValue(  5000.0,20.0,  5.0,   7.0, 0.0,      0.0)},
+    { "leg_right_2_joint", ControlJointValue(  5000.0,20.0,  5.0,  14.0, 0.0,      0.0)},
+    { "leg_right_3_joint", ControlJointValue(  5000.0,20.0,  5.0,  14.0,-0.448041, 0.0)},
+    { "leg_right_4_joint", ControlJointValue(  5000.0,20.0,  5.0,  25.0, 0.896082, 0.0)},
+    { "leg_right_5_joint", ControlJointValue(  5000.0,20.0,  5.0,   9.0,-0.448041, 0.0)},
+    { "leg_right_6_joint", ControlJointValue(  5000.0,20.0,  5.0,   7.0, 0.0,      0.0)},
+    { "torso_1_joint",     ControlJointValue( 10000.0,10.0,  1.0,  10.0, 0.0,      0.0)},
+    { "torso_2_joint",     ControlJointValue( 10000.0,10.0,  1.0,  10.0, 0.0,      0.0)}
   };
 
   gz_transport_hw_tools::RobotCtrlJointInfos h1_2_ctrl_joint_infos {
-    { "left_shoulder_pitch_joint", { 0.0, 0.0, 10000.0, 1.0, 0.0, 0.0, 0.0}, },    
-    { "left_shoulder_roll_joint",  { 0.0, 0.0, 10000.0, 1.0, 0.0, 0.0, 0.0} },
-    { "left_shoulder_yaw_joint",   { 0.0, 0.0, 5000.0, 10.0, 0.0, 0.0, 0.0} },
-    { "left_elbow_joint",          { 0.0, 0.0, 5000.0, 10.0, 0.0, 0.0, 0.0} }, 
-    { "left_wrist_roll_joint",     { 0.0, 0.0, 500.0, 1.0, 0.0, 0.0, 0.0} },    
-    { "left_wrist_pitch_joint",    { 0.0, 0.0, 500.0, 1.0, 0.0, 0.0, 0.0} },
-    { "left_wrist_yaw_joint",      { 0.0, 0.0, 500.0, 1.0, 0.0, 0.0, 0.0} },
-    { "right_shoulder_pitch_joint",{ 0.0, 0.0, 10000.0, 5.0, 0.0, 0.0, 0.0} },
-    { "right_shoulder_roll_joint", { 0.0, 0.0, 10000.0, 5.0, 0.0, 0.0, 0.0}},
-    { "right_shoulder_yaw_joint",  { 0.0, 0.0, 5000.0, 10.0, 0.0, 0.0, 0.0} },
-    { "right_elbow_joint",         { 0.0, 0.0, 5000.0, 10.0, 0.0, 0.0, 0.0} },
-    { "right_wrist_roll_joint",    { 0.0, 0.0, 500.0, 1.0, 0.0, 0.0, 0.0}},
-    { "right_wrist_pitch_joint",   { 0.0, 0.0, 500.0, 1.0, 0.0, 0.0, 0.0}},
-    { "right_wrist_yaw_joint",     { 0.0, 0.0, 500.0, 1.0, 0.0, 0.0, 0.0}},
-    { "left_hip_yaw_joint",        { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}},
-    { "left_hip_pitch_joint",      { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}},
-    { "left_hip_roll_joint",       { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}},
-    { "left_knee_joint",           { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}},
-    { "left_ankle_pitch_joint",    { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}},
-    { "left_ankle_roll_joint",     { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}},
-    { "right_hip_yaw_joint",       { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}},
-    { "right_hip_pitch_joint",     { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}},
-    { "right_hip_roll_joint",      { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}},
-    { "right_knee_joint",          { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}},
-    { "right_ankle_pitch_joint",   { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}},
-    { "right_ankle_roll_joint",    { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}},
-    { "torso_joint",               { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}},
+    { "left_shoulder_pitch_joint", ControlJointValue( 0.0, 0.0, 0.0,  0.0, 0.0, 0.0) },
+    { "left_shoulder_roll_joint",  ControlJointValue( 0.0, 0.0, 0.0,  0.0, 0.0, 0.0) },
+    { "left_shoulder_yaw_joint",   ControlJointValue( 0.0, 0.0, 0.0,  0.0, 0.0, 0.0) },
+    { "left_elbow_joint",          ControlJointValue( 0.0, 0.0, 0.0,  0.0, 0.0, 0.0) },
+    { "left_wrist_roll_joint",     ControlJointValue( 0.0, 0.0, 0.0,  0.0, 0.0, 0.0) },
+    { "left_wrist_pitch_joint",    ControlJointValue( 0.0, 0.0, 0.0,  0.0, 0.0, 0.0) },
+    { "left_wrist_yaw_joint",      ControlJointValue( 0.0, 0.0, 0.0,  0.0, 0.0, 0.0) },
+    { "right_shoulder_pitch_joint",ControlJointValue( 0.0, 0.0, 0.0,  0.0, 0.0, 0.0) },
+    { "right_shoulder_roll_joint", ControlJointValue( 0.0, 0.0, 0.0,  0.0, 0.0, 0.0) },
+    { "right_shoulder_yaw_joint",  ControlJointValue( 0.0, 0.0, 0.0,  0.0, 0.0, 0.0) },
+    { "right_elbow_joint",         ControlJointValue( 0.0, 0.0, 0.0,  0.0, 0.0, 0.0) },
+    { "right_wrist_roll_joint",    ControlJointValue( 0.0, 0.0, 0.0,  0.0, 0.0, 0.0) },
+    { "right_wrist_pitch_joint",   ControlJointValue( 0.0, 0.0, 0.0,  0.0, 0.0, 0.0) },
+    { "right_wrist_yaw_joint",     ControlJointValue( 0.0, 0.0, 0.0,  0.0, 0.0, 0.0) },
+    { "left_hip_yaw_joint",        ControlJointValue( 0.0, 0.0, 0.0,  0.0, 0.0, 0.0) },
+    { "left_hip_pitch_joint",      ControlJointValue( 0.0, 0.0, 0.0,  0.0, 0.0, 0.0) },
+    { "left_hip_roll_joint",       ControlJointValue( 0.0, 0.0, 0.0,  0.0, 0.0, 0.0) },
+    { "left_knee_joint",           ControlJointValue( 0.0, 0.0, 0.0,  0.0, 0.0, 0.0) },
+    { "left_ankle_pitch_joint",    ControlJointValue( 0.0, 0.0, 0.0,  0.0, 0.0, 0.0) },
+    { "left_ankle_roll_joint",     ControlJointValue( 0.0, 0.0, 0.0,  0.0, 0.0, 0.0) },
+    { "right_hip_yaw_joint",       ControlJointValue( 0.0, 0.0, 0.0,  0.0, 0.0, 0.0) },
+    { "right_hip_pitch_joint",     ControlJointValue( 0.0, 0.0, 0.0,  0.0, 0.0, 0.0) },
+    { "right_hip_roll_joint",      ControlJointValue( 0.0, 0.0, 0.0,  0.0, 0.0, 0.0) },
+    { "right_knee_joint",          ControlJointValue( 0.0, 0.0, 0.0,  0.0, 0.0, 0.0) },
+    { "right_ankle_pitch_joint",   ControlJointValue( 0.0, 0.0, 0.0,  0.0, 0.0, 0.0) },
+    { "right_ankle_roll_joint",    ControlJointValue( 0.0, 0.0, 0.0,  0.0, 0.0, 0.0) },
+    { "torso_joint",               ControlJointValue( 0.0, 0.0, 0.0,  0.0, 0.0, 0.0) },
   };
 
-  
+
   std::string talos_prefix_model_root("/model/Pyrene/");
   std::string talos_prefix_world("/world/empty_talos_gz");
 
@@ -125,14 +126,14 @@ int main(int argc, char **argv)
 
   std::string & a_prefix_model_root = ( choice ==0 ) ?
       h1_v2_prefix_model_root : talos_prefix_model_root;
-      
+
   std::string & a_prefix_world = (choice == 0) ?
       h1_v2_prefix_world: talos_prefix_world;
-     
+
   gz_transport_hw_tools::RobotCtrlJointInfos &
     a_robot_ctrl_joint_infos = (choice==0) ?
       h1_2_ctrl_joint_infos: talos_ctrl_joint_infos ;
-      
+
   bool debug_level = false;