CMakeLists.txt

@@ -29,7 +29,9 @@ target_include_directories(gz_transport_hw_interface PUBLIC
   $<INSTALL_INTERFACE:include>)
 target_link_libraries(gz_transport_hw_interface
   gz-transport${GZ_TRANSPORT_VER}::core
-  gz-sim${GZ_SIM_VER}::core)
+  gz-sim${GZ_SIM_VER}::core
+  ${PROJECT_NAME}-msgs
+)
 
 
 add_executable(control_loop ./tools/control_loop.cc)

include/gz_gep_tools/control_over_gz.hh

@@ -6,6 +6,9 @@
 
 /// GZ includes
 #include <gz/msgs.hh>
+#include <gz/msgs/clock.pb.h>
+#include <gz/msgs/world_control.pb.h>
+#include <gz/msgs/world_control_state.pb.h>
 #include <gz/transport.hh>
 
 #include "joint_state_interface.hh"
@@ -36,6 +39,10 @@ class ControlOverGz {
   /// Send World Control state to Init ECM.
   bool SendWorldControlStateToInitECM(const RobotCtrlJointInfos &rbt_ctrl_joint_infos);
 
+  /// Set Pose
+  bool SetPose(double x, double y, double z,
+               double qx, double qy, double qz, double qw);
+
   /// Initialization of the ECM by reading the world control state
   bool ReadWorldStateToInitECM();
 
@@ -44,7 +51,7 @@ class ControlOverGz {
 
   /// Display link values
   void DisplayLinkValues();
-  
+
  protected:
 
 
@@ -63,9 +70,12 @@ class ControlOverGz {
   /// Name of state service
   std::string state_service_gzsim_;
 
-  /// Name of wrold control state service.
+  /// Name of world control state service.
   std::string wrld_ctrl_state_srv_gzsim_;
 
+  /// Name of set pose service.
+  std::string set_pose_service_gzsim_;
+
   /// GZ node
   gz::transport::Node node_;
 

include/gz_gep_tools/joint_state_interface.hh

@@ -5,7 +5,7 @@
 #include <memory>
 
 /// GZ includes
-#include <gz/msgs.hh>
+#include <gz/msgs/model.pb.h>
 #include <gz/transport.hh>
 
 namespace gz_transport_hw_tools {
@@ -88,9 +88,6 @@ class GZJointValues {
   double vel_mes;
   /// Control
   double force_ctrl;
-
-  std::string cmd_force_topic;
-  gz::transport::Node::Publisher gz_pub_cmd_force;
 };
 
 
@@ -150,6 +147,9 @@ class JointStateInterface {
   GZRobotJoints gz_robot_joints_;
 
   bool debug_level_;
+
+  std::string topic_name_gz_pub_named_joints_forces_;
+  gz::transport::Node::Publisher gz_pub_named_joints_forces_;
 };
 
 }

plugin/ApplyJointsForces.cc

@@ -32,7 +32,7 @@
 #include "gz/sim/Model.hh"
 #include "gz/sim/Util.hh"
 
-#include <gz/msgs/map_named_joint_forces.pb.h>
+#include <gz/msgs/map_named_joints_forces.pb.h>
 
 using namespace gz;
 using namespace sim;
@@ -48,26 +48,9 @@ class ActuatedJoint
 
   /// \brief Constructor that properly configures the actuated joint
   /// \param[in] _entity Entity of the joint
-  /// \param[in] _params All parameters of the joint required for its
   /// configuration
-  public: ActuatedJoint(const Entity &_entity);
+ public: ActuatedJoint(const Entity &an_entity): entity(an_entity) {};
 
-  /// \brief Setup components required for control of this joint
-  /// \param[in,out] _ecm Gazebo Entity Component Manager
-  public: void SetupComponents(
-              gz::sim::EntityComponentManager &_ecm) const;
-
-  /// \brief Set Torque of the joint that the controller will attempt to reach
-  /// \param[in] _jointIndex Index of the joint, used to determine what index
-  /// of `_targetPoint` to use
-  public: void SetJointForce(
-              const gz::msgs::Double &JointTorqueForce);
-
-  /// \brief Update command force that is applied on the joint
-  /// \param[in,out] _ecm Gazebo Entity Component Manager
-  /// \param[in] _dt Time difference to update for
-  public: void Update(gz::sim::EntityComponentManager &_ecm,
-                      const std::chrono::steady_clock::duration &_dt);
 
   /// \brief Entity of the joint
   public: Entity entity;
@@ -103,7 +86,7 @@ class gz::sim::systems::ApplyJointsForcesPrivate
 
   /// \brief Callback for joint force subscription
   /// \param[in] _msg Joint force message
- public: void CallbackCmdForce(const gz::msgs::MapNamedJointForces &_msg);
+ public: void CallbackCmdForce(const gz::msgs::MapNamedJointsForces &_msg);
 
   /// \brief Gazebo communication node.
   public: transport::Node node;
@@ -123,7 +106,7 @@ class gz::sim::systems::ApplyJointsForcesPrivate
   public: Model model{kNullEntity};
   //! [modelDeclaration]<q
 
-  public: 
+  public:
   template <typename T>
     std::vector<T> Parse(
     const std::shared_ptr<const sdf::Element> &_sdf,
@@ -209,11 +192,9 @@ void ApplyJointsForces::Configure(const Entity &_entity,
   //! [cmdTopic]
   auto topic = _sdf->Get<std::string>("topic");
   //! [cmdTopic]
-  if (topic.empty())
-  {
-    transport::TopicUtils::AsValidTopic("/model/" +
-      this->dataPtr->model.Name(_ecm) + "/joints/cmd_forces");
-  }
+  // TODO: Use the model namespace.
+  topic = "/model/" + this->dataPtr->model.Name(_ecm) + "/joints/" + topic;
+
 
   // Make sure the topic is valid
   const auto validApplyJointsForcesTopic = transport::TopicUtils::AsValidTopic(
@@ -222,12 +203,14 @@ void ApplyJointsForces::Configure(const Entity &_entity,
   {
     gzerr << "[ApplyJointsForces] Cannot subscribe to invalid topic ["
            << topic << "].\n";
+    gzerr << "/model/" +
+        this->dataPtr->model.Name(_ecm) + "/joints/cmd_forces\n";
     return;
   }
  // Subscribe
   gzmsg << "[ApplyJointsForces] Subscribing to ApplyJointsForces"
             " commands on topic [" << validApplyJointsForcesTopic << "].\n";
- 
+
   //! [cmdSub]
   this->dataPtr->node.Subscribe(validApplyJointsForcesTopic,
                                 &ApplyJointsForcesPrivate::CallbackCmdForce,
@@ -267,19 +250,19 @@ void ApplyJointsForces::PreUpdate(const UpdateInfo &_info,
           this->dataPtr->model.JointByName(_ecm, an_actuated_joint->first);
     }
     //! [findJoint]
-    
+
     if (an_actuated_joint->second.entity == kNullEntity)
       continue;
-    
+
 
     // Update joint force
     //! [jointForceComponent]
     auto force = _ecm.Component<components::JointForceCmd>(
         an_actuated_joint->second.entity);
     //! [jointForceComponent]
-    
+
     std::lock_guard<std::mutex> lock(this->dataPtr->jointForceCmdMutex);
-    
+
     //! [modifyComponent]
     if (force == nullptr)
     {
@@ -289,7 +272,7 @@ void ApplyJointsForces::PreUpdate(const UpdateInfo &_info,
     }
     else
     {
-      force->Data()[0] += an_actuated_joint->second.jointForceCmd;
+      force->Data()[0] = an_actuated_joint->second.jointForceCmd;
     }
   }
     //! [modifyComponent]
@@ -298,11 +281,11 @@ void ApplyJointsForces::PreUpdate(const UpdateInfo &_info,
 //////////////////////////////////////////////////
 //! [setForce]
 void ApplyJointsForcesPrivate::CallbackCmdForce(
-    const gz::msgs::MapNamedJointForces &mnjf_msg)
+    const gz::msgs::MapNamedJointsForces &mnjf_msg)
 {
   std::lock_guard<std::mutex> lock(this->jointForceCmdMutex);
-  for ( auto mnjf_it =  mnjf_msg.jointforces().begin();
-        mnjf_it !=  mnjf_msg.jointforces().end();
+  for ( auto mnjf_it =  mnjf_msg.jointsforces().begin();
+        mnjf_it !=  mnjf_msg.jointsforces().end();
         mnjf_it++)
   {
     actuatedJoints[mnjf_it->first].jointForceCmd = mnjf_it->second;
@@ -413,7 +396,7 @@ std::vector<Entity> ApplyJointsForcesPrivate::GetEnabledJoints(
   return output;
 }
 
-    
+
 
 GZ_ADD_PLUGIN(ApplyJointsForces,
                     System,

plugin/CMakeLists.txt

-add_subdirectory(msgs)
-
 find_package(gz_sim_vendor REQUIRED)
 find_package(gz-sim REQUIRED)
 
@@ -14,10 +12,12 @@ add_library(${gz_gep_tools_plugin}-system SHARED
 target_link_libraries(${gz_gep_tools_plugin}-system PRIVATE
   gz-sim::gz-sim
   gz-plugin::register
-  {$PROJECT_NAME}-msgs
+  ${PROJECT_NAME}-msgs
 )
 
 # Install directories
 install(TARGETS ${gz_gep_tools_plugin}-system
   DESTINATION lib
 )
+
+add_subdirectory(msgs)

plugin/msgs/CMakeLists.txt

@@ -9,7 +9,7 @@ set(GZ_MSGS_VER ${gz-msgs10_VERSION_MAJOR})
 
 # Define a variable 'MSGS_PROTOS' listing the .proto files
 set(MSGS_PROTOS
-   ${CMAKE_CURRENT_SOURCE_DIR}/gz/msgs/map_named_joint_forces.proto
+   ${CMAKE_CURRENT_SOURCE_DIR}/gz/msgs/map_named_joints_forces.proto
 )
 
 # Call 'gz_msgs_generate_messages()' to process the .proto files

plugin/msgs/gz/msgs/map_named_joint_forces.proto → plugin/msgs/gz/msgs/map_named_joints_forces.proto

@@ -2,6 +2,6 @@ syntax = "proto3";
 
 package gz.msgs;
 
-message MapNamedJointForces {
-  map<string, double> JointForces= 1;
+message MapNamedJointsForces {
+  map<string, double> JointsForces= 1;
 }
\ No newline at end of file

src/control_over_gz.cc

@@ -25,6 +25,9 @@ ControlOverGz::ControlOverGz(std::string &world_prefix, bool debug_level)
   /// State gzsim service
   state_service_gzsim_ = world_prefix+std::string("/state");
 
+  /// Set pose service
+  set_pose_service_gzsim_ = world_prefix + std::string("/set_pose");
+
   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,
@@ -152,7 +155,7 @@ bool ControlOverGz::ReadWorldStateToInitECM()
 void ControlOverGz::DisplayLinkValues()
 {
   auto entities = Robot_ECM_.EntitiesByComponents( gz::sim::components::Link());
-  
+
   for (auto it_entity = entities.begin();
        it_entity != entities.end();
        it_entity++)
@@ -220,6 +223,7 @@ bool ControlOverGz::SendWorldControlStateToInitECM(const RobotCtrlJointInfos &rb
     }
   }
 
+
   /// Create the serialized state.
   req_serialized_state = new gz::msgs::SerializedState();
   ///  Generate the serialized state msg from the ECM.
@@ -244,4 +248,36 @@ bool ControlOverGz::SendWorldControlStateToInitECM(const RobotCtrlJointInfos &rb
 
   return true;
 }
+
+bool ControlOverGz::SetPose(double x, double y, double z,
+                            double qx, double qy, double qz, double qw)
+{
+  gz::msgs::Pose pose_msg;
+  gz::msgs::Boolean rep_bool;
+  unsigned int timeout = 3000;
+  bool result;
+
+  gz::msgs::Vector3d * aPosition = pose_msg.mutable_position();
+  aPosition->set_x(x); aPosition->set_y(y); aPosition->set_z(z);
+
+  gz::msgs::Quaternion * aQuaternion  = pose_msg.mutable_orientation();
+  aQuaternion->set_x(qx); aQuaternion->set_y(qy); aQuaternion->set_z(qz); aQuaternion->set_w(qw);
+
+  /// Set Robot Pose
+  /// by calling the /prefix_world/set_pose Gazebo service.
+  if (!node_.Request(set_pose_service_gzsim_,
+                     pose_msg,
+                     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

@@ -3,8 +3,11 @@
 
 #include <gz_gep_tools/joint_state_interface.hh>
 #include <gz/msgs.hh>
+#include <gz/msgs/model.pb.h>
 #include <gz/transport.hh>
 
+#include <gz/msgs/map_named_joints_forces.pb.h>
+
 namespace gz_transport_hw_tools {
 
 ControlJointValue::ControlJointValue(double Kp,
@@ -118,9 +121,16 @@ JointStateInterface::JointStateInterface(std::string &a_prefix_model_root,
   prefix_world_ = a_prefix_world ;
   joint_state_topic_ =a_prefix_world +  a_prefix_model_root + "joint_state";
 
+  if (debug_level)
+    std::cout << "Subscribe to " << joint_state_topic_ << std::endl;
+
   node_.Subscribe<JointStateInterface,gz::msgs::Model>(joint_state_topic_,
                                                        &JointStateInterface::CallbackJointState,
                                                        this);
+
+  topic_name_gz_pub_named_joints_forces_ = a_prefix_model_root + "joints/cmd_forces";
+  gz_pub_named_joints_forces_ =
+      node_.Advertise<gz::msgs::MapNamedJointsForces>(topic_name_gz_pub_named_joints_forces_);
 }
 
 JointStateInterface::~JointStateInterface() {}
@@ -133,15 +143,11 @@ void JointStateInterface::SetListOfJoints(
   for (auto jointItr = rbt_ctrl_joint_infos.begin();
        jointItr != rbt_ctrl_joint_infos.end();
        jointItr++) {
-    /// Build a new string
-    std::string a_new_cmd_force = prefix_model_root_ + std::string("joint/") +
-        jointItr->first + std::string("/cmd_force");
-
     /// Build advertise
-    gz_robot_joints_.dict_joint_values[jointItr->first].gz_pub_cmd_force =
-        node_.Advertise<gz::msgs::Double>(a_new_cmd_force);
-
-    gz_robot_joints_.dict_joint_values[jointItr->first].cmd_force_topic = a_new_cmd_force;
+    GZJointValues & agzjointvalues = gz_robot_joints_.dict_joint_values[jointItr->first];
+    agzjointvalues.pos_mes = 0.0;
+    agzjointvalues.vel_mes = 0.0;
+    agzjointvalues.force_ctrl = 0.0;
 
   }
 
@@ -164,7 +170,7 @@ void JointStateInterface::CallbackJointState(
 
   }
 
-  if (debug_level_)
+  //  if (debug_level_)
   {
     std::cerr << "JointStateInterface::CallbackJointState: time: "
               << " " << gz_robot_joints_.time_sec_
@@ -191,31 +197,24 @@ bool JointStateInterface::SetCmd( const RobotCtrlJointInfos &rbt_ctrl_joint_info
               << std::endl;
     return false;
   }
+
+  gz::msgs::MapNamedJointsForces mnjf_msg;
+  auto ljointsforces = mnjf_msg.mutable_jointsforces();
+
   // Iterate over the cmd map.
   for (auto cmd_it = rbt_ctrl_joint_infos.begin();
        cmd_it != rbt_ctrl_joint_infos.end();
        cmd_it++)
   {
-    gz::msgs::Double msg;
-    msg.set_data(cmd_it->second.Cmd());
-
-    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 == 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 "
-                << gz_robot_joints_.dict_joint_values[cmd_it->first].cmd_force_topic
-                << std::endl;
-    }
+
+    (*ljointsforces)[cmd_it->first]= cmd_it->second.Cmd();
+  }
+
+  if (!gz_pub_named_joints_forces_.Publish(mnjf_msg)) {
+       std::cerr << "Unable to publish on "
+                 << topic_name_gz_pub_named_joints_forces_
+                 << std::endl;
+       return false;
   }
   return true;
 }

src/perception_action_loop.cc

@@ -122,12 +122,13 @@ int PerceptionActionLoop::MainLoop()
     bool is_sim_ready = joint_state_interface_.GetPosVel(robot_ctrl_joint_infos_,
                                                          state_gz_time_);
 
-    std::cout << "state_gz_time:" << state_gz_time_ << std::endl;
-    //    if (debug_level)
+
+    if (debug_level_)
       std::cerr << "control_loop: "
                 << is_sim_ready << " "
                 << pre_state_gz_time_<< " "
                 << state_gz_time_ << " "
+                << internal_timer
                 << 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_
@@ -171,10 +172,11 @@ int PerceptionActionLoop::MainLoop()
 
       // We have missed the Step for one sec.
       if ((internal_timer>=1000) &&
-          (pre_state_gz_time_==state_gz_time_))
+          (pre_state_gz_time_>=state_gz_time_))
       {
         std::cerr << "internal_timer: " << internal_timer << std::endl;
         control_over_gz_.Step();
+        std::cout << "Step in internal timer" << std::endl;
         internal_timer=0;
       }
 

tools/control_loop.cc

@@ -87,33 +87,33 @@ int main(int argc, char **argv)
   };
 
   gz_transport_hw_tools::RobotCtrlJointInfos h1_2_ctrl_joint_infos {
-    { "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) },
+    { "left_shoulder_pitch_joint", ControlJointValue( 10000.0, 0.01, 1.0, 14.0,  0.0, 0.0) },
+    { "left_shoulder_roll_joint",  ControlJointValue( 10000.0, 0.01, 1.0, 14.0,  0.0, 0.0) },
+    { "left_shoulder_yaw_joint",   ControlJointValue(  5000.0, 0.0,  1.0,  9.0,  0.0, 0.0) },
+    { "left_elbow_joint",          ControlJointValue(  5000.0, 0.0,  1.0,  9.0,  0.0, 0.0) },
+    { "left_wrist_roll_joint",     ControlJointValue(  500.0,  1.0,  0.1,  5.0,  0.0, 0.0) },
+    { "left_wrist_pitch_joint",    ControlJointValue(  500.0,  1.0,  0.1,  5.0,  0.0, 0.0) },
+    { "left_wrist_yaw_joint",      ControlJointValue(  500.0,  1.0,  0.1,  3.0,  0.0, 0.0) },
+    { "right_shoulder_pitch_joint",ControlJointValue(10000.0, 0.01,  1.0,  0.0,  0.0, 0.0) },
+    { "right_shoulder_roll_joint", ControlJointValue(10000.0, 0.01,  1.0,  0.0,  0.0, 0.0) },
+    { "right_shoulder_yaw_joint",  ControlJointValue( 5000.0,  0.0,  1.0,  0.0,  0.0, 0.0) },
+    { "right_elbow_joint",         ControlJointValue( 5000.0,  0.0,  1.0,  0.0,  0.0, 0.0) },
+    { "right_wrist_roll_joint",    ControlJointValue(  500.0,  1.0,  0.1,  0.0,  0.0, 0.0) },
+    { "right_wrist_pitch_joint",   ControlJointValue(  500.0,  1.0,  0.1,  0.0,  0.0, 0.0) },
+    { "right_wrist_yaw_joint",     ControlJointValue(  500.0,  1.0,  0.1,  0.0,  0.0, 0.0) },
+    { "left_hip_yaw_joint",        ControlJointValue( 5000.0, 20.0,  5.0,  7.0,  0.0, 0.0) },
+    { "left_hip_pitch_joint",      ControlJointValue( 5000.0, 20.0,  5.0, 14.0,  0.0, 0.0) },
+    { "left_hip_roll_joint",       ControlJointValue( 5000.0, 20.0,  5.0, 14.0,  0.0, 0.0) },
+    { "left_knee_joint",           ControlJointValue( 5000.0, 20.0,  5.0, 25.0,  0.0, 0.0) },
+    { "left_ankle_pitch_joint",    ControlJointValue( 5000.0, 20.0,  5.0, 14.0,  0.0, 0.0) },
+    { "left_ankle_roll_joint",     ControlJointValue( 5000.0, 20.0,  5.0,  9.0,  0.0, 0.0) },
+    { "right_hip_yaw_joint",       ControlJointValue( 5000.0, 20.0,  5.0,  7.0,  0.0, 0.0) },
+    { "right_hip_pitch_joint",     ControlJointValue( 5000.0, 20.0,  5.0, 14.0,  0.0, 0.0) },
+    { "right_hip_roll_joint",      ControlJointValue( 5000.0, 20.0,  5.0, 14.0,  0.0, 0.0) },
+    { "right_knee_joint",          ControlJointValue( 5000.0, 20.0,  5.0, 25.0,  0.0, 0.0) },
+    { "right_ankle_pitch_joint",   ControlJointValue( 5000.0, 20.0,  5.0, 14.0,  0.0, 0.0) },
+    { "right_ankle_roll_joint",    ControlJointValue( 5000.0, 20.0,  5.0,  9.0,  0.0, 0.0) },
+    { "torso_joint",               ControlJointValue(10000.0, 20.0,  1.0, 10.0,  0.0, 0.0) },
   };
 
 
@@ -134,7 +134,7 @@ int main(int argc, char **argv)
     a_robot_ctrl_joint_infos = (choice==0) ?
       h1_2_ctrl_joint_infos: talos_ctrl_joint_infos ;
 
-  bool debug_level = false;
+  bool debug_level = true;
 
 
   gz_transport_hw_tools::PerceptionActionLoop a_pal(a_robot_ctrl_joint_infos,