Add pose3D

include/gz_gep_tools/control_over_gz.hh

@@ -37,7 +37,8 @@ class ControlOverGz {
   double GetSimTime();
 
   /// Send World Control state to Init ECM.
-  bool SendWorldControlStateToInitECM(const RobotCtrlJointInfos &rbt_ctrl_joint_infos);
+  bool SendWorldControlStateToInitECM(const RobotCtrlJointInfos &rbt_ctrl_joint_infos,
+                                      std::vector<double> &aPose3d);
 
   /// Set Pose
   bool SetPose(double x, double y, double z,

include/gz_gep_tools/perception_action_loop.hh

@@ -10,6 +10,7 @@ class PerceptionActionLoop {
  public:
   // Default constructore
   PerceptionActionLoop(gz_transport_hw_tools::RobotCtrlJointInfos & a_robot_ctrl_joint_infos,
+                       std::vector<double> & a_pose3d,
                        std::string & a_prefix_model_root,
                        std::string & a_prefix_world,
                        bool debug_level);
@@ -19,11 +20,11 @@ class PerceptionActionLoop {
 
   /// Main loop
   int MainLoop();
-  
+
  protected:
   /// Information for control
   gz_transport_hw_tools::RobotCtrlJointInfos & robot_ctrl_joint_infos_;
-  
+
   /// Informmation from perception and the simulator
   gz_transport_hw_tools::JointStateInterface joint_state_interface_;
 
@@ -35,6 +36,9 @@ class PerceptionActionLoop {
   double pre_state_gz_time_;
   unsigned long long int local_time_;
 
+  /// Pose3d when starting
+  std::vector<double> pose3d_;
+
   /// Debug level
   bool debug_level_;
 };

src/control_over_gz.cc

@@ -3,6 +3,7 @@
 #include <cstdint>
 #include <cstring>
 
+#include <gz/math/Pose3.hh>
 #include <gz/msgs/empty.pb.h>
 #include <gz_gep_tools/control_over_gz.hh>
 #include <gz/msgs.hh>
@@ -11,8 +12,10 @@
 #include <gz/sim/EntityComponentManager.hh>
 #include <gz/sim/components/Joint.hh>
 #include <gz/sim/components/Link.hh>
+#include <gz/sim/components/Model.hh>
 #include <gz/sim/Joint.hh>
 #include <gz/sim/Link.hh>
+#include <gz/sim/Model.hh>
 
 namespace gz_transport_hw_tools {
 
@@ -89,6 +92,11 @@ bool ControlOverGz::Reset()
   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_multi_step(0);
+  req_world_ctrl.set_step (false);
+  req_world_ctrl.set_pause(true);
+  req_world_ctrl.set_seed(0);
+
   /// Do request.
   bool result;
   if (!node_.Request( control_service_gzsim_, req_world_ctrl, timeout, rep_bool, result)) {
@@ -193,7 +201,8 @@ void ControlOverGz::DisplayJointValues()
   }
 }
 
-bool ControlOverGz::SendWorldControlStateToInitECM(const RobotCtrlJointInfos &rbt_ctrl_joint_infos)
+bool ControlOverGz::SendWorldControlStateToInitECM(const RobotCtrlJointInfos &rbt_ctrl_joint_infos,
+                                                   std::vector<double> &aPose3d)
 {
   gz::msgs::WorldControlState req_world_ctrl_state;
   gz::msgs::SerializedState * req_serialized_state;
@@ -223,6 +232,24 @@ bool ControlOverGz::SendWorldControlStateToInitECM(const RobotCtrlJointInfos &rb
     }
   }
 
+  auto mentities = Robot_ECM_.EntitiesByComponents( gz::sim::components::Model());
+  for (auto it_Entity = mentities.begin();
+       it_Entity != mentities.end();
+       it_Entity++)
+  {
+    gz::sim::Model aModel(*it_Entity);
+    std::string modelName =  aModel.Name(Robot_ECM_);
+    std::cout << "Model name:" << modelName << std::endl;
+
+    if (modelName=="Pyrene")
+    {
+      gz::math::Pose3d aPose;
+      aPose.Set( aPose3d[0], aPose3d[1], aPose3d[2],
+                 aPose3d[3], aPose3d[4], aPose3d[5]);
+      aModel.SetWorldPoseCmd(Robot_ECM_,aPose);
+    }
+  }
+
 
   /// Create the serialized state.
   req_serialized_state = new gz::msgs::SerializedState();

src/perception_action_loop.cc

@@ -30,9 +30,11 @@ namespace gz_transport_hw_tools {
 
 PerceptionActionLoop::PerceptionActionLoop(
     gz_transport_hw_tools::RobotCtrlJointInfos &a_robot_ctrl_joint_infos,
+    std::vector<double> &a_pose3d,
     std::string &a_prefix_model_root, std::string &a_prefix_world,
     bool debug_level)
     : robot_ctrl_joint_infos_(a_robot_ctrl_joint_infos),
+      pose3d_(a_pose3d),
       joint_state_interface_(a_prefix_model_root, a_prefix_world, debug_level),
       control_over_gz_(a_prefix_world, debug_level), debug_level_(debug_level)
       {
@@ -67,11 +69,24 @@ int PerceptionActionLoop::InitGz()
   control_over_gz_.Step();
   control_over_gz_.ReadWorldStateToInitECM();
 
+  while (pre_state_gz_time_==state_gz_time_) {
+    std::this_thread::sleep_for(std::chrono::nanoseconds(2ms));
+    state_gz_time_=control_over_gz_.GetSimTime();
+  }
   // Second reset to set the robot state to a specific position.
+  state_gz_time_=0.0;
+  pre_state_gz_time_=control_over_gz_.GetSimTime();
   if (!control_over_gz_.Reset())  {
     std::cerr << "Reset failed" << std::endl;
   }
-  control_over_gz_.SendWorldControlStateToInitECM(robot_ctrl_joint_infos_);
+
+  while (pre_state_gz_time_==state_gz_time_) {
+    std::this_thread::sleep_for(std::chrono::nanoseconds(2ms));
+    state_gz_time_=control_over_gz_.GetSimTime();
+  }
+
+  control_over_gz_.SendWorldControlStateToInitECM(robot_ctrl_joint_infos_,
+                                                  pose3d_);
   if (debug_level_)
     control_over_gz_.DisplayLinkValues();
   control_over_gz_.Step();

tools/control_loop.cc

@@ -85,6 +85,7 @@ int main(int argc, char **argv)
     { "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)}
   };
+  std::vector<double> talos_pose3d {0.0 , 0.0, 1.02, 0.0, 0.0, 0.0};
 
   gz_transport_hw_tools::RobotCtrlJointInfos h1_2_ctrl_joint_infos {
     { "left_shoulder_pitch_joint", ControlJointValue( 10000.0, 0.01, 1.0, 14.0,  0.0, 0.0) },
@@ -115,6 +116,7 @@ int main(int argc, char **argv)
     { "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) },
   };
+    std::vector<double> h1_v2_pose3d {0.0 , 0.0, 0.0, 0.0, 0.0, 0.0};
 
 
   std::string talos_prefix_model_root("/model/Pyrene/");
@@ -130,17 +132,21 @@ int main(int argc, char **argv)
   std::string & a_prefix_world = (choice == 0) ?
       h1_v2_prefix_world: talos_prefix_world;
 
+  std::vector<double> & a_pose3d = (choice ==0) ?
+      h1_v2_pose3d : talos_pose3d;
+
   gz_transport_hw_tools::RobotCtrlJointInfos &
     a_robot_ctrl_joint_infos = (choice==0) ?
       h1_2_ctrl_joint_infos: talos_ctrl_joint_infos ;
 
-  bool debug_level = true;
+  bool debug_level = false;
 
 
   gz_transport_hw_tools::PerceptionActionLoop a_pal(a_robot_ctrl_joint_infos,
-                             a_prefix_model_root,
-                             a_prefix_world,
-                             debug_level);
+                                                    a_pose3d,
+                                                    a_prefix_model_root,
+                                                    a_prefix_world,
+                                                    debug_level);
 
 
   a_pal.InitGz();