.gitmodules

+[submodule "cmake"]
+	path = cmake
+	url = git@github.com:jrl-umi3218/jrl-cmakemodules.git

CMakeLists.txt

@@ -11,6 +11,8 @@ 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})
+find_package(gz-sim8 QUIET REQUIRED OPTIONAL_COMPONENTS log)
+set(GZ_SIM_VER ${gz-sim8_VERSION_MAJOR})
 
 add_library(gz_transport_hw_interface SHARED
   src/joint_state_interface.cc
@@ -19,7 +21,9 @@ add_library(gz_transport_hw_interface SHARED
 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)
+target_link_libraries(gz_transport_hw_interface
+  gz-transport${GZ_TRANSPORT_VER}::core
+  gz-sim${GZ_SIM_VER}::core)
 
 
 add_executable(control_loop ./tools/control_loop.cc)

cmake @ 2ede15d1

+Subproject commit 2ede15d1cb9d66401ba96788444ad64c44ffccd8

include/gz_gep_tools/control_over_gz.hh

 /// Standard includes
+#include <gz/sim/EntityComponentManager.hh>
 #include <vector>
 #include <string>
 
@@ -12,33 +13,63 @@ class ControlOverGz {
  public:
 
   /// Provides the gazebo name.
-  ControlOverGz(std::string &world_prefix);
+  ControlOverGz(std::string &world_prefix, bool debug_level=false);
 
   /// Pause Gazebosim
   bool Pause();
 
   /// Start Gazebo
   bool Start();
-  
+
   /// Reset Gazebosim
   bool Reset();
-  
+
+  /// Performing a simulation step
+  bool Step();
+
+  /// Getting the simualted time
+  double GetSimTime();
+
+  /// Send World Control state to Init ECM.
+  bool SendWorldControlStateToInitECM(std::map<std::string, double> &named_pos_des_d);
+
+  /// Initialization of the ECM by reading the world control state
+  bool ReadWorldStateToInitECM();
+
+  /// Display joint values
+  void DisplayJointValues();
+
  protected:
 
+
+  /// Reading GZ sim time
+  void CallbackClock(const gz::msgs::Clock &);
+
   /// Send a Pause request
   bool SendPauseRequest(bool abool);
-  
-  /// World prefix 
+
+  /// World prefix
   std::string world_prefix_;
 
-  /// list of services
-  std::string control_gzsim_;
+  /// Name of the control service
+  std::string control_service_gzsim_;
+
+  /// Name of state service
+  std::string state_service_gzsim_;
+
+  /// Name of wrold control state service.
+  std::string wrld_ctrl_state_srv_gzsim_;
 
   /// GZ node
   gz::transport::Node node_;
 
-  
+  /// Time
+  double gz_sim_time_;
+
+  bool debug_level_;
+
+  /// Robot Entity Component Manager
+  gz::sim::EntityComponentManager Robot_ECM_;
 };
 
 }
-

include/gz_gep_tools/joint_state_interface.hh

+#pragma once
 /// Standard includes
 #include <vector>
 #include <string>
@@ -8,13 +9,40 @@
 
 namespace gz_transport_hw_tools {
 
+class LastState {
+ public:
+  /// Time
+  int64_t time_sec_;
+  int64_t time_nsec_;
+
+  /// Position
+  std::vector<double> positions_;
+
+  /// Velocity
+  std::vector<double> velocities_;
+
+  void resize(std::size_t asize);
+
+  std::mutex lock_state_access_;
+  LastState();
+};
+
+class JointValues {
+  double pos_mes;
+  double vel_mes;
+  double force_ctrl;
+  std::string cmd_force_topic;
+  gz::transport::Node::Publisher;
+};
+
 /// This class handles the interface to the joints and the sensors
 /// of a robot simulated by Gazebo (Harmonic)
 class JointStateInterface {
  public:
   /// Constructor
   JointStateInterface(std::string &a_prefix_model_root,
-                      std::string &a_prefix_world);
+                      std::string &a_prefix_world,
+                      bool debug_level=false);
   /// Destructor
   ~JointStateInterface();
 
@@ -24,26 +52,27 @@ class JointStateInterface {
 
   bool SetCmd(const std::vector<double> &cmd_vec);
 
-  void GetPosVel(std::vector<double> &pos_vecd,
-                 std::vector<double> &vel_vecd);
-  
+  bool GetPosVel(std::vector<double> &pos_vecd,
+                 std::vector<double> &vel_vecd,
+                 double &time);
+
   private:
 
   /// Callback function for state model update.
   void CallbackJointState(const gz::msgs::Model &a_gz_model_msg);
-  
+
   /// Root of the prefix model
   std::string prefix_model_root_;
 
   /// 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_;
-  
+
   /// Vector of string describing the topics to command forces on actuators.
   std::vector<std::string> cmd_force_topics_;
 
@@ -56,11 +85,10 @@ class JointStateInterface {
   /// GZ node
   gz::transport::Node node_;
 
-  /// Position
-  std::vector<double> positions_;
+  /// Last state of the robot
+  LastState last_state_;
 
-  /// Velocity
-  std::vector<double> velocities_;
+  bool debug_level_;
 };
 
 }

src/control_over_gz.cc

+#include <cmath>
+#include <cstdint>
+#include <cstring>
+
+#include <gz/msgs/empty.pb.h>
 #include <gz_gep_tools/control_over_gz.hh>
 #include <gz/msgs.hh>
 #include <gz/transport.hh>
 
+#include <gz/sim/EntityComponentManager.hh>
+#include <gz/sim/components/Joint.hh>
+#include <gz/sim/Joint.hh>
+
 namespace gz_transport_hw_tools {
 
-ControlOverGz::ControlOverGz(std::string &world_prefix)
-    : world_prefix_(world_prefix) {
-  control_gzsim_ = world_prefix+std::string("/control");  
+ControlOverGz::ControlOverGz(std::string &world_prefix, bool debug_level)
+    : world_prefix_(world_prefix), debug_level_(debug_level)
+{
+  /// Create control_gzsim topic name
+  control_service_gzsim_ = world_prefix+std::string("/control");
+
+  /// State gzsim service
+  state_service_gzsim_ = world_prefix+std::string("/state");
+
+  std::string gazebo_clock_topic_name;
+  gazebo_clock_topic_name = world_prefix + std::string("/clock");
+  node_.Subscribe<ControlOverGz,gz::msgs::Clock>(gazebo_clock_topic_name,
+                                                 &ControlOverGz::CallbackClock,
+                                                 this);
+  gz_sim_time_ = std::nan("1");
+
+  wrld_ctrl_state_srv_gzsim_ = control_service_gzsim_ + std::string("/state");
+}
+
+void ControlOverGz::CallbackClock(const gz::msgs::Clock &a_gz_time_msg) {
+  if (a_gz_time_msg.has_sim())
+  {
+    gz_sim_time_ = a_gz_time_msg.sim().sec() +
+        1e-9*a_gz_time_msg.sim().nsec();
+    if (debug_level_)
+      std::cerr << "ControlOverGz::CallbackClock: Time: "  << a_gz_time_msg.sim().sec()
+                << " "  <<  a_gz_time_msg.sim().nsec()
+                << std::endl;
+  }
+}
+
+double ControlOverGz::GetSimTime() {
+  return gz_sim_time_;
+}
+
+bool ControlOverGz::Step()
+{
+  gz::msgs::WorldControl req_world_ctrl;
+  gz::msgs::Boolean rep_bool;
+  unsigned int timeout = 3000;
+
+  /// Set reset for everything
+  req_world_ctrl.set_multi_step(1);
+  req_world_ctrl.set_pause(true);
+
+  /// Do request.
+  bool result=true;
+  if (!node_.Request( control_service_gzsim_, req_world_ctrl, timeout, rep_bool, result)) {
+
+    std::cerr << "Unable to send reset request !"
+              << std::endl;
+    result =false;
+  }
+      ///set_allocated_reset(&req_world_reset);
+  return result;
 }
 
 bool ControlOverGz::Reset()
 {
   gz::msgs::WorldControl req_world_ctrl;
-  gz::msgs::WorldReset req_world_reset;
+  gz::msgs::WorldReset * req_world_reset;
   gz::msgs::Boolean rep_bool;
-  unsigned int timeout = 5000;
-  
+  unsigned int timeout = 3000;
+
   /// Set reset for everything
-  req_world_reset.set_all(true);
+  req_world_reset = new gz::msgs::WorldReset();
+  req_world_reset->set_all(true);
+  req_world_ctrl.set_pause(true);
   /// Set the world reset inside the world control reset.
-  req_world_ctrl.set_allocated_reset(&req_world_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);
+  if (!node_.Request( control_service_gzsim_, req_world_ctrl, timeout, rep_bool, result)) {
 
+    std::cerr << "Unable to send reset request !"
+              << std::endl;
+    result =false;
+  }
+  /// World Reset will be released because out of scope.
   return result;
 }
 
@@ -32,13 +100,13 @@ 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);
+  node_.Request( control_service_gzsim_, req_world_ctrl, timeout, rep_bool, result);
   return result;
 }
 
@@ -46,6 +114,111 @@ bool ControlOverGz::Pause() { return SendPauseRequest(true); }
 
 bool ControlOverGz::Start() { return SendPauseRequest(false); }
 
+bool ControlOverGz::ReadWorldStateToInitECM()
+{
+  /// Gazebo messages
+  /// Response as Serialized map of the entities
+  gz::msgs::SerializedStepMap rep_serialized_step_map;
+  /// Request does not have request argument.
+  gz::msgs::Empty req_empty_msg;
+  unsigned int timeout = 3000;
+  gz::msgs::Boolean rep_bool;
+  bool result=true;
+
+  /// Get World Control State.
+  /// by calling the /prefix_world/state Gazebo service.
+  if (!node_.Request(state_service_gzsim_,
+                     req_empty_msg,
+                     timeout,
+                     rep_serialized_step_map,
+                     result)) {
+
+    std::cerr << "Unable to send reset request ! timeout "
+              << state_service_gzsim_ << " "
+              << result
+              << std::endl;
+    result =false;
+  }
+
+  /// Initialization of the robot's Entity Component Manager.
+  Robot_ECM_.SetState(*rep_serialized_step_map.mutable_state());
 
+  return result;
+}
+
+void ControlOverGz::DisplayJointValues()
+{
+  auto entities = Robot_ECM_.EntitiesByComponents( gz::sim::components::Joint());
+
+  for (auto it_Entity = entities.begin();
+       it_Entity != entities.end();
+       it_Entity++)
+  {
+    gz::sim::Joint aJoint(*it_Entity);
+    //    std::cout << "Joint name:" << aJoint.Name(Robot_ECM_).value_or("No joint name");
+    std::vector<double> empty_vecd;
+    std::vector<double> pos_vecd = aJoint.Position(Robot_ECM_).value_or(empty_vecd);
+    if (pos_vecd.size()!=0)
+      std::cout << " " << "Position: " << pos_vecd[0];
+    else
+      std::cout << " no pos ";
+    std::vector<double> vel_vecd = aJoint.Velocity(Robot_ECM_).value_or(empty_vecd);
+    if (vel_vecd.size()!=0)
+      std::cout << " " << "Velocity: " << vel_vecd[1];
+    else
+      std::cout << " no vel ";
+    std::cout << std::endl;
+  }
+}
 
+bool ControlOverGz::SendWorldControlStateToInitECM(std::map<std::string, double> &named_pos_d)
+{
+  gz::msgs::WorldControlState req_world_ctrl_state;
+  gz::msgs::SerializedState * req_serialized_state;
+  unsigned int timeout = 3000;
+  gz::msgs::Boolean rep_bool;
+  bool result=true;
+
+
+  // Modify joint entities in Robot_ECM.
+  auto entities = Robot_ECM_.EntitiesByComponents( gz::sim::components::Joint());
+
+  for (auto it_Entity = entities.begin();
+       it_Entity != entities.end();
+       it_Entity++)
+  {
+    gz::sim::Joint aJoint(*it_Entity);
+    std::string jointName =  aJoint.Name(Robot_ECM_).value_or("No joint name");
+    //    std::cout << "Joint name:" << jointName;
+
+    std::vector<double> one_vecd;
+    one_vecd.push_back(named_pos_d[jointName]);
+    //    std::cout << " " << one_vecd[0] << std::endl;
+    aJoint.ResetPosition(Robot_ECM_,one_vecd);
+  }
+
+  /// Create the serialized state.
+  req_serialized_state = new gz::msgs::SerializedState();
+  ///  Generate the serialized state msg from the ECM.
+  *req_serialized_state = Robot_ECM_.State();
+  /// Link it in the WorldControlState msg
+  req_world_ctrl_state.set_allocated_state(req_serialized_state);
+
+  /// Set World Control State.
+  /// by calling the /prefix_world/state/control Gazebo service.
+  if (!node_.Request(wrld_ctrl_state_srv_gzsim_,
+                     req_world_ctrl_state,
+                     timeout,
+                     rep_bool,
+                     result)) {
+
+    std::cerr << "Unable to send reset request ! timeout "
+              << wrld_ctrl_state_srv_gzsim_ << " "
+              << result
+              << std::endl;
+    result =false;
+  }
+
+  return true;
+}
 }

src/joint_state_interface.cc

@@ -4,8 +4,27 @@
 
 namespace gz_transport_hw_tools {
 
+LastState::LastState() :
+    time_sec_(0),
+    time_nsec_(0)
+{}
+
+void LastState::resize(size_t asize) {
+  positions_.clear();
+  positions_.resize(asize);
+  velocities_.clear();
+  velocities_.resize(asize);
+  for(unsigned i=0;i<positions_.size();i++)
+  {
+    positions_[i]  = 0.0;
+    velocities_[i] = 0.0;
+  }
+}
+
 JointStateInterface::JointStateInterface(std::string &a_prefix_model_root,
-                                         std::string &a_prefix_world) {
+                                         std::string &a_prefix_world,
+                                         bool debug_level):
+    debug_level_(debug_level) {
   prefix_model_root_ = a_prefix_model_root;
   prefix_world_ = a_prefix_world ;
   joint_state_topic_ =a_prefix_world +  a_prefix_model_root + "joint_state";
@@ -43,32 +62,48 @@ void JointStateInterface::SetListOfJoints(
     map_name_2_indx_[*jointItr] = idx;
     idx++;
   }
-  positions_.clear();
-  positions_.resize(a_list_of_joints.size());
-  velocities_.clear();
-  velocities_.resize(a_list_of_joints.size());
+
+  last_state_.resize(a_list_of_joints.size());
+
 }
 
 void JointStateInterface::CallbackJointState(
     const gz::msgs::Model&a_gz_model_msg
     ) {
+
+  std::lock_guard(last_state_.lock_state_access_);
   unsigned int idx_joints=0;
   for (auto jointItr = a_gz_model_msg.joint().begin();
        jointItr != a_gz_model_msg.joint().end();
        jointItr++) {
 
+
     const ::gz::msgs::Axis &axis1 = jointItr->axis1();
 
     long unsigned int local_joint_idx = map_name_2_indx_[jointItr->name()];
 
-    if (positions_.size()!=0)
-      positions_[local_joint_idx] = axis1.position();
-    if (velocities_.size()!=0)
-      velocities_[local_joint_idx] = axis1.velocity();
+    if (last_state_.positions_.size()!=0)
+      last_state_.positions_[local_joint_idx] = axis1.position();
+    if (last_state_.velocities_.size()!=0)
+      last_state_.velocities_[local_joint_idx] = axis1.velocity();
 
     idx_joints++;
   }
 
+  if (debug_level_)
+  {
+    std::cerr << "JointStateInterface::CallbackJointState: time: "
+              << " " << last_state_.time_sec_
+              << " " << last_state_.time_nsec_;
+    if (last_state_.positions_.size()>1)
+      std::cerr << " " << last_state_.positions_[1];
+
+    std::cerr << std::endl;
+  }
+  last_state_.time_sec_ = a_gz_model_msg.header().stamp().sec();
+  last_state_.time_nsec_ = a_gz_model_msg.header().stamp().nsec();
+
+
 }
 
 bool JointStateInterface::SetCmd( const std::vector<double> &a_cmd_vec_d)
@@ -76,7 +111,8 @@ 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++) {
+  for (unsigned int i = 0; i < a_cmd_vec_d.size(); i++)
+  {
     gz::msgs::Double msg;
     msg.set_data(a_cmd_vec_d[i]);
 
@@ -84,20 +120,37 @@ bool JointStateInterface::SetCmd( const std::vector<double> &a_cmd_vec_d)
       std::cerr << "Unable to publish on "
                << cmd_force_topics_[i]
                << std::endl;
+      return false;
     }
   }
   return true;
 }
 
-void JointStateInterface::GetPosVel(std::vector<double> &pos_vecd,
-                                    std::vector<double> &vel_vecd)
+bool JointStateInterface::GetPosVel(std::vector<double> &pos_vecd,
+                                    std::vector<double> &vel_vecd,
+                                    double &time)
 {
+  if ((pos_vecd.size()!= last_state_.positions_.size()) ||
+      (vel_vecd.size()!= last_state_.velocities_.size()))
+      return false;
+
+  std::lock_guard(last_state_.lock_state_access_);
+  time=last_state_.time_sec_ + 1e-9*last_state_.time_nsec_;
   for(std::vector<double>::size_type idx=0;
       idx < pos_vecd.size();
       idx++)
   {
-    pos_vecd[idx] = positions_[idx];
-    vel_vecd[idx] = velocities_[idx];
+    pos_vecd[idx] = last_state_.positions_[idx];
+    vel_vecd[idx] = last_state_.velocities_[idx];
   }
+  if (debug_level_)
+    std::cerr << "JointStateInterface::GetPosVel: time: "
+              << time << " "
+              << pos_vecd[1] << " "
+              << std::endl;
+
+  return true;
 }
+
+
 };

tools/control_loop.cc

@@ -44,6 +44,8 @@ void signal_handler(int _signal)
 //////////////////////////////////////////////////
 int main(int argc, char **argv)
 {
+  using namespace std::chrono_literals;
+
   // Install a signal handler for SIGINT and SIGTERM.
   std::signal(SIGINT,  signal_handler);
   std::signal(SIGTERM, signal_handler);
@@ -82,30 +84,34 @@ int main(int argc, char **argv)
     "leg_right_5_joint",
     "leg_right_6_joint",
     "torso_1_joint",
-    "torso_2_joint",    
+    "torso_2_joint",
   };
 
   std::string a_prefix_model_root("/model/Pyrene/");
   std::string a_prefix_world("/world/empty_talos_gz");
-  
+
+  bool debug_level = false;
   gz_transport_hw_tools::JointStateInterface
       a_joint_state_inter(a_prefix_model_root,
-                          a_prefix_world);
+                          a_prefix_world,
+                          debug_level);
+
+  gz_transport_hw_tools::ControlOverGz aControlOverGz(a_prefix_world,debug_level);
 
-  gz_transport_hw_tools::ControlOverGz aControlOverGz(a_prefix_world);
-  
   a_joint_state_inter.SetListOfJoints(talos_list_of_joints);
 
   std::vector<double> cmd_vec_d;
   cmd_vec_d.resize(talos_list_of_joints.size());
   for(unsigned int i=0;i<talos_list_of_joints.size();i++)
     cmd_vec_d[i]=100.0;
-  
+
   // Publish messages at 1Hz.
-  std::vector<double> pos_mes_d,vel_mes_d;
+  std::vector<double> pos_mes_d,vel_mes_d, pos_err_d,vel_err_d;
   pos_mes_d.resize(talos_list_of_joints.size());
   vel_mes_d.resize(talos_list_of_joints.size());
-  
+  pos_err_d.resize(talos_list_of_joints.size());
+  vel_err_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
@@ -113,8 +119,15 @@ int main(int argc, char **argv)
     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
+    0, 0.006761 // torso
   };
+  std::map<std::string, double> named_pos_des_d;
+  if (talos_list_of_joints.size()!=pos_des_d.size())
+    std::cerr << "Size between talos_list_of_joints and pos_des_d do not match." << std::endl;
+
+  for(unsigned int i=0;i<talos_list_of_joints.size();i++)
+    named_pos_des_d[talos_list_of_joints[i]]= pos_des_d[i];
+
   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
@@ -128,63 +141,142 @@ int main(int argc, char **argv)
     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
+    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
     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
+    0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, // leg_left
+    0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, // leg_right
+    1000.0, 1000.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
+  std::vector<double> Kd { 10.0, 10.0, 0.0, 0.0, 0.0, 0.0, 0.0, // arm_left
+    0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, // arm_right
+    0.0, 0.0, // gripper
+    0.0, 0.0, // head
+    0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, // leg_left
+    0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, // leg_right
+    0.0, 0.0 // torso
   };
   unsigned long long int local_time=0;
 
-  aControlOverGz.Reset();
-  aControlOverGz.Start();
+  if (!aControlOverGz.Reset())  {
+    std::cerr << "Reset failed" << std::endl;
+  }
+  double state_gz_time=0.0;
+  double pre_state_gz_time=aControlOverGz.GetSimTime();
+  // Wait 1 ms to perform reset.
+  std::this_thread::sleep_for(std::chrono::nanoseconds(1ms));
+  // Start simulation.
+  aControlOverGz.Step();
+
+  /// Synchronize simulation wait for starting.
+  unsigned long int i=0;
+  while(std::isnan(pre_state_gz_time) ||
+         (pre_state_gz_time>0.001))
+  { ///  Wait (1ms)
+    std::this_thread::sleep_for(std::chrono::nanoseconds(1ms));
+    pre_state_gz_time=aControlOverGz.GetSimTime();
+    if ((i%100==0) && (i>1)){
+      aControlOverGz.Reset();
+      std::this_thread::sleep_for(std::chrono::nanoseconds(1ms));
+      aControlOverGz.Step();
+      /* std::cerr << "control_loop stuck in the waiting loop for starting."
+                << std::endl
+                << pre_state_gz_time << " "
+                << i << " "
+                << std::endl; */
+    }
+    i++;
+  }
+
+  aControlOverGz.ReadWorldStateToInitECM();
+
+  unsigned long int internal_timer = 0;
+  /// CTRL-C loop
   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++)
+    bool is_sim_ready = a_joint_state_inter.GetPosVel(pos_mes_d, vel_mes_d,state_gz_time);
+
+    if (debug_level)
+      std::cerr << "control_loop: "
+                << is_sim_ready << " "
+                << pre_state_gz_time << " "
+                << state_gz_time << " "
+                << std::endl;
+    if ((is_sim_ready) && // If the simulation is ready
+        (pre_state_gz_time<state_gz_time) && // If the pre_state_gz_time is before state_gz_time
+        (fabs(pre_state_gz_time-state_gz_time) < 0.005) // The update of state_gz_time might happen before
+        // reset and then putting state_gz_time to far ahead.
+        ) {
+      /// Control
+      for(unsigned int i=0;i<talos_list_of_joints.size();i++)
+      {
+        pos_err_d[i] = pos_des_d[i] - pos_mes_d[i];
+        vel_err_d[i] = vel_des_d[i] - vel_mes_d[i];
+
+        cmd_vec_d[i] = Kp[i] * pos_err_d[i] + Kd[i]* vel_err_d[i];
+
+      }
+
+      /// Store Gazebo time
+      pre_state_gz_time = state_gz_time;
+
+
+      if (local_time==1) {
+        aControlOverGz.ReadWorldStateToInitECM();
+        aControlOverGz.SendWorldControlStateToInitECM(named_pos_des_d);
+      }
+      else {
+
+        if (local_time>1)
+        {
+          if (a_joint_state_inter.SetCmd(cmd_vec_d))
+          {
+            // std::cout   << pos_err_d[1] << " "
+            //             << vel_err_d[1] << " "
+            //             << pos_des_d[1] << " "
+            //             << pos_mes_d[1] << std::endl;
+            for(unsigned int i=0;i<talos_list_of_joints.size();i++)
+              std::cout << pos_mes_d[i] << " ";
+            // std::cout << Kp[1] << " "
+            //           << Kd[1] << " "
+            //           << state_gz_time << " "
+            //           << pre_state_gz_time << " "
+            std::cout  << std::endl;
+          }
+        }
+
+        // Start simulation.
+        // TODO : Verify that step has converged before returning.
+        aControlOverGz.Step();
+      }
+
+      local_time++;
+
+      internal_timer=0;
+      /// Stop after 0.2 seconds.
+      if (local_time>2000)
+        break;
+    } else
     {
-      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
+
+      // We have missed the Step for one sec.
+      if ((internal_timer>=1000) &&
+          (pre_state_gz_time==state_gz_time))
+      {
+        std::cerr << "internal_timer: " << internal_timer << std::endl;
+        aControlOverGz.Step();
+        internal_timer=0;
+      }
+
     }
-    a_joint_state_inter.SetCmd(cmd_vec_d);
-    
-    std::this_thread::sleep_for(std::chrono::nanoseconds(1000));
-    local_time++;
-    if (local_time>10000)
-      break;
+    std::this_thread::sleep_for(1ms);
+    internal_timer++;
   }
+  //  aControlOverGz.SendWorldControlState();
+
   aControlOverGz.Pause();
 
   return 0;