CMakeLists.txt

+
 cmake_minimum_required(VERSION 3.22.1 FATAL_ERROR)
 
+set(CMAKE_CXX_STANDARD 17)
+
 project(gz-gep-tools)
 
+
 set(CMAKE_VERBOSE_MAKEFILE TRUE)
 
 # Find the Gazebo Transport library
 find_package(gz-transport13 QUIET REQUIRED OPTIONAL_COMPONENTS log)
 set(GZ_TRANSPORT_VER ${gz-transport13_VERSION_MAJOR})
 
-add_library(gz_transport_hw_interface
+add_library(gz_transport_hw_interface SHARED
   src/joint_state_interface.cc
+  src/control_over_gz.cc
 )
-target_compile_features(gz_transport_hw_interface PUBLIC cxx_std_17)
 target_include_directories(gz_transport_hw_interface PUBLIC
   $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>    
   $<INSTALL_INTERFACE:include>)
 target_link_libraries(gz_transport_hw_interface gz-transport${GZ_TRANSPORT_VER}::core)
 
 
-add_executable(publisher publisher.cc)
-target_include_directories(publisher PUBLIC
+add_executable(control_loop ./tools/control_loop.cc)
+target_include_directories(control_loop PUBLIC
   $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>    
   $<INSTALL_INTERFACE:include>)
-target_compile_features(publisher PUBLIC cxx_std_17)
-target_link_libraries(publisher gz_transport_hw_interface)
+target_link_libraries(control_loop gz_transport_hw_interface)
 

include/gz_gep_tools/control_over_gz.hh

+/// Standard includes
+#include <vector>
+#include <string>
+
+/// GZ includes
+#include <gz/msgs.hh>
+#include <gz/transport.hh>
+
+namespace gz_transport_hw_tools {
+
+class ControlOverGz {
+ public:
+
+  /// Provides the gazebo name.
+  ControlOverGz(std::string &world_prefix);
+
+  /// Pause Gazebosim
+  bool Pause();
+
+  /// Start Gazebo
+  bool Start();
+  
+  /// Reset Gazebosim
+  bool Reset();
+  
+ protected:
+
+  /// Send a Pause request
+  bool SendPauseRequest(bool abool);
+  
+  /// World prefix 
+  std::string world_prefix_;
+
+  /// list of services
+  std::string control_gzsim_;
+
+  /// GZ node
+  gz::transport::Node node_;
+
+  
+};
+
+}
+

include/gz_gep_tools/gz_gep_tools.hh

+#include <gz_gep_tools/joint_state_interface.hh>
+#include <gz_gep_tools/control_over_gz.hh>
+

include/gz_gep_tools/joint_state_interface.hh

@@ -40,6 +40,9 @@ class JointStateInterface {
   
   /// 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_;
   
   /// Vector of string describing the topics to command forces on actuators.
   std::vector<std::string> cmd_force_topics_;

src/control_over_gz.cc

+#include <gz_gep_tools/control_over_gz.hh>
+#include <gz/msgs.hh>
+#include <gz/transport.hh>
+
+namespace gz_transport_hw_tools {
+
+ControlOverGz::ControlOverGz(std::string &world_prefix)
+    : world_prefix_(world_prefix) {
+  control_gzsim_ = world_prefix+std::string("/control");  
+}
+
+bool ControlOverGz::Reset()
+{
+  gz::msgs::WorldControl req_world_ctrl;
+  gz::msgs::WorldReset req_world_reset;
+  gz::msgs::Boolean rep_bool;
+  unsigned int timeout = 5000;
+  
+  /// Set reset for everything
+  req_world_reset.set_all(true);
+  /// Set the world reset inside the world control reset.
+  req_world_ctrl.set_allocated_reset(&req_world_reset);
+  /// Do request.
+  bool result;
+  node_.Request( control_gzsim_, req_world_ctrl, timeout, rep_bool, result);
+
+  return result;
+}
+
+bool ControlOverGz::SendPauseRequest(bool abool)
+{
+  gz::msgs::WorldControl req_world_ctrl;
+  gz::msgs::Boolean rep_bool;
+  unsigned int timeout = 5000;
+  
+  /// Set the world reset inside the world control reset.
+  req_world_ctrl.set_pause(abool);
+  
+  /// Do request.
+  bool result;
+  node_.Request( control_gzsim_, req_world_ctrl, timeout, rep_bool, result);
+  return result;
+}
+
+bool ControlOverGz::Pause() { return SendPauseRequest(true); }
+
+bool ControlOverGz::Start() { return SendPauseRequest(false); }
+
+
+
+}

src/joint_state_interface.cc

@@ -22,19 +22,26 @@ void JointStateInterface::SetListOfJoints(
 {
   list_of_joints_.clear();
   list_of_joints_ = a_list_of_joints;
-  
+
   cmd_force_topics_.clear();
   gz_pub_cmd_forces_.clear();
 
+  unsigned int idx=0;
   for (auto jointItr = a_list_of_joints.begin();
        jointItr != a_list_of_joints.end();
        jointItr++) {
+    /// Build a new string
     std::string a_new_cmd_force = prefix_model_root_ + std::string("joint/") +
         *jointItr + std::string("/cmd_force");
     cmd_force_topics_.push_back(a_new_cmd_force);
+    /// Build advertise
     gz::transport::Node::Publisher a_gz_pub_cmd_force =
         node_.Advertise<gz::msgs::Double>(a_new_cmd_force);
     gz_pub_cmd_forces_.push_back(a_gz_pub_cmd_force);
+
+    // Set map relationship between name and idx.
+    map_name_2_indx_[*jointItr] = idx;
+    idx++;
   }
   positions_.clear();
   positions_.resize(a_list_of_joints.size());
@@ -49,29 +56,30 @@ void JointStateInterface::CallbackJointState(
   for (auto jointItr = a_gz_model_msg.joint().begin();
        jointItr != a_gz_model_msg.joint().end();
        jointItr++) {
-    idx_joints++;
 
     const ::gz::msgs::Axis &axis1 = jointItr->axis1();
 
+    long unsigned int local_joint_idx = map_name_2_indx_[jointItr->name()];
 
     if (positions_.size()!=0)
-      positions_[idx_joints] = axis1.position();
+      positions_[local_joint_idx] = axis1.position();
     if (velocities_.size()!=0)
-      velocities_[idx_joints] = axis1.velocity();
+      velocities_[local_joint_idx] = axis1.velocity();
 
+    idx_joints++;
   }
-  
+
 }
 
 bool JointStateInterface::SetCmd( const std::vector<double> &a_cmd_vec_d)
 {
   if (a_cmd_vec_d.size()!=gz_pub_cmd_forces_.size())
     return false;
-  
+
   for (unsigned int i = 0; i < a_cmd_vec_d.size(); i++) {
     gz::msgs::Double msg;
     msg.set_data(a_cmd_vec_d[i]);
-    
+
     if (!gz_pub_cmd_forces_[i].Publish(msg)){
       std::cerr << "Unable to publish on "
                << cmd_force_topics_[i]
@@ -89,7 +97,7 @@ void JointStateInterface::GetPosVel(std::vector<double> &pos_vecd,
       idx++)
   {
     pos_vecd[idx] = positions_[idx];
-    vel_vecd[idx] = velocities_[idx];    
+    vel_vecd[idx] = velocities_[idx];
   }
 }
 };

publisher.cc → tools/control_loop.cc

@@ -25,7 +25,7 @@
 #include <gz/msgs.hh>
 #include <gz/transport.hh>
 
-#include <gz_gep_tools/joint_state_interface.hh>
+#include <gz_gep_tools/gz_gep_tools.hh>
 
 /// \brief Flag used to break the publisher loop and terminate the program.
 static std::atomic<bool> g_terminatePub(false);
@@ -92,6 +92,8 @@ int main(int argc, char **argv)
       a_joint_state_inter(a_prefix_model_root,
                           a_prefix_world);
 
+  gz_transport_hw_tools::ControlOverGz aControlOverGz(a_prefix_world);
+  
   a_joint_state_inter.SetListOfJoints(talos_list_of_joints);
 
   std::vector<double> cmd_vec_d;
@@ -100,11 +102,90 @@ int main(int argc, char **argv)
     cmd_vec_d[i]=100.0;
   
   // Publish messages at 1Hz.
+  std::vector<double> pos_mes_d,vel_mes_d;
+  pos_mes_d.resize(talos_list_of_joints.size());
+  vel_mes_d.resize(talos_list_of_joints.size());
+  
+  // Desired position and velocity
+  std::vector<double> pos_des_d  { 0.25847, 0.173046, -0.0002, -0.525366, 0, 0, 0.1, // arm_left
+    -0.25847, -0.173046, 0.0002, -0.525366, 0, 0, 0.1, // arm_right
+    0, 0, // gripper
+    0, 0, // head
+    0, 0, -0.411354, 0.859395, -0.448041, -0.001708, // leg_left
+    0, 0, -0.411354, 0.859395, -0.448041, -0.001708, // leg_right
+    0, 006761 // torso
+  };
+  std::vector<double> vel_des_d  { 0, 0, 0, 0, 0, 0, 0, // arm_left
+    0, 0, 0, 0, 0, 0, 0, // arm_right
+    0, 0, // gripper
+    0, 0, // head
+    0, 0, 0, 0, 0, 0, 0, // leg_left
+    0, 0, 0, 0, 0, 0, 0, // leg_right
+    0, 0 // torso
+  };
+
+  std::vector<double> Kp { /* 1000.0, 1000.0, 1000.0, 1000.0, 1000.0, 1000.0, 1000.0, // arm_left
+    1000.0, 1000.0, 1000.0, 1000.0, 1000.0, 1000.0, 1000.0, // arm_right
+    100.0, 100.0, // gripper
+    100.0, 100.0, // head*/
+    900.0, 300.0, 900.0, 900.0, 900.0, 900.0, 900.0, // arm_left
+    900.0, 900.0, 900.0, 900.0, 900.0, 900.0, 900.0, // arm_right
+    0.0, 0.0, // gripper
+    0.0, 0.0, // head
+    900.0, 900.0, 900.0, 900.0, 900.0, 900.0, 900.0, // leg_left
+    900.0, 900.0, 900.0, 900.0, 900.0, 900.0, 900.0, // leg_right
+    900.0, 900.0 // torso
+  };
+
+  std::vector<double> Kd { 10.0, 0.0, 10.0, 10.0, 10.0, 10.0, 10.0, // arm_left
+    10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0, // arm_right
+    1.0, 1.0, // gripper
+    1.0, 1.0, // head
+    5.0, 5.0, 5.0, 5.0, 5.0, 5.0, 5.0, // leg_left
+    5.0, 5.0, 5.0, 5.0, 5.0, 5.0, 5.0, // leg_right
+    10.0, 10.0 // torso
+  };
+  unsigned long long int local_time=0;
+
+  aControlOverGz.Reset();
+  aControlOverGz.Start();
   while (!g_terminatePub)
   {
+    /// Sense
+    a_joint_state_inter.GetPosVel(pos_mes_d, vel_mes_d);
+    
+    /// Control
+    for(unsigned int i=0;i<talos_list_of_joints.size();i++)
+    {
+      double pos_err = pos_des_d[i] - pos_mes_d[i];
+      double vel_err = vel_des_d[i] - vel_mes_d[i];
+      std::string nb_joint = std::to_string(i);
+
+      cmd_vec_d[i] = Kp[i] * pos_err + Kd[i]* vel_err ;
+      
+#if 0
+      if (local_time%10==1)      
+        std::cout << "Time : " << local_time
+                  << " pos_err[" + nb_joint +" ] : " << pos_err
+                  << " vel_err[" + nb_joint +" ] :" << vel_err << std::endl;
+#else
+      if (i==1)
+        std::cout   << pos_err << " " 
+                    << pos_des_d[i] << " "
+                    << pos_mes_d[i] << " "
+                    << cmd_vec_d[i] << " "
+                    << Kp[i] << " "
+                    << Kd[i] << std::endl;
+#endif
+    }
     a_joint_state_inter.SetCmd(cmd_vec_d);
-    std::this_thread::sleep_for(std::chrono::milliseconds(1000));
+    
+    std::this_thread::sleep_for(std::chrono::nanoseconds(1000));
+    local_time++;
+    if (local_time>10000)
+      break;
   }
+  aControlOverGz.Pause();
 
   return 0;
 }