Allow to fix base link from command-link args

src/main.cpp

@@ -46,6 +46,7 @@ int main(int argc, char * argv[])
       ("without-mc-rtc-gui", po::bool_switch(), "Disable mc_rtc GUI")
       ("with-collisions", po::bool_switch(), "Visualize collisions model")
       ("without-visuals", po::bool_switch(), "Disable visuals display")
+      ("fix-base-link", po::bool_switch(), "Freeze the root joint of all mc_rtc robots.")
       ("sync", po::bool_switch(&config.sync_real_time), "Synchronize mc_mujoco simulation time with real time");
     // clang-format on
     po::variables_map vm;
@@ -67,6 +68,7 @@ int main(int argc, char * argv[])
     {
       config.visualize_collisions = vm["with-collisions"].as<bool>();
     }
+    config.fix_base_link = vm["fix-base-link"].as<bool>();
   }
   mc_mujoco::MjSim mj_sim(config);
 

src/mj_configuration.h

@@ -27,6 +27,8 @@ struct MjConfiguration
   std::string mc_config = "";
   /** Use torque-control rather than position control */
   bool torque_control = false;
+  /** Freeze root joints of all mc_rtc robots by increasing damping and removing ground */
+  bool fix_base_link = false;
 };
 
 } // namespace mc_mujoco

src/mj_sim.cpp

@@ -251,7 +251,7 @@ void MjObject::initialize(mjModel * model)
   }
 }
 
-void MjRobot::initialize(mjModel * model, const mc_rbdyn::Robot & robot)
+void MjRobot::initialize(mjModel * model, const mc_rbdyn::Robot & robot, bool fix_base = false)
 {
   mj_jnt_ids.resize(0);
   for(const auto & j : mj_jnt_names)
@@ -286,6 +286,23 @@ void MjRobot::initialize(mjModel * model, const mc_rbdyn::Robot & robot)
     root_qpos_idx = model->jnt_qposadr[root_joint_id];
     root_qvel_idx = model->jnt_dofadr[root_joint_id];
   }
+
+  // fix base if needed
+  if(fix_base)
+  {
+    if(!root_joint.empty())
+    {
+      auto root_joint_id = mj_name2id(model, mjOBJ_JOINT, root_joint.c_str());
+      for(unsigned int j = 0; j < model->nv; j++)
+      {
+        if(model->dof_jntid[j] == root_joint_id)
+        {
+          model->dof_damping[j] = mjMAXVAL;
+        }
+      }
+    }
+  }
+
   auto init_sensor_id = [&](const char * mj_name, const char * mc_name, const std::string & sensor_name,
                             const char * suffix, mjtSensor type, std::unordered_map<std::string, int> & mapping)
   {
@@ -464,7 +481,7 @@ void MjSimImpl::setSimulationInitialState()
     for(auto & r : robots)
     {
       const auto & robot = controller->robots().robot(r.name);
-      r.initialize(model, robot);
+      r.initialize(model, robot, config.fix_base_link);
       setPosW(r, robot.posW());
       for(size_t i = 0; i < r.mj_jnt_ids.size(); ++i)
       {
@@ -617,7 +634,7 @@ void MjSimImpl::startSimulation()
 
   for(auto & r : robots)
   {
-    r.initialize(model, controller->robot(r.name));
+    r.initialize(model, controller->robot(r.name), config.fix_base_link);
     controller->setEncoderValues(r.name, r.encoders);
   }
   for(const auto & r : robots)

src/mj_sim_impl.h

@@ -151,7 +151,7 @@ struct MjRobot
   std::vector<double> mj_next_ctrl_jointTorque;
 
   /** Initialize some data after the simulation has started */
-  void initialize(mjModel * model, const mc_rbdyn::Robot & robot);
+  void initialize(mjModel * model, const mc_rbdyn::Robot & robot, bool fix_base);
 
   /** Reset the state based on the mc_rtc robot state */
   void reset(const mc_rbdyn::Robot & robot);

src/mj_utils_merge_mujoco_models.cpp

@@ -347,17 +347,17 @@ static void merge_mujoco_model(const std::string & robot, const std::string & xm
       bfs::path include_file_path(inc.attribute("file").value());
       if(!include_file_path.is_absolute())
       {
-	bfs::path xmlPath = bfs::path(xmlFile).parent_path();
-	include_file_path = xmlPath / include_file_path;
+        bfs::path xmlPath = bfs::path(xmlFile).parent_path();
+        include_file_path = xmlPath / include_file_path;
       }
       pugi::xml_document includeDoc;
-      if (!includeDoc.load_file(include_file_path.c_str()))
+      if(!includeDoc.load_file(include_file_path.c_str()))
       {
-	mc_rtc::log::error_and_throw<std::runtime_error>("Failed to load {}", include_file_path.c_str());
+        mc_rtc::log::error_and_throw<std::runtime_error>("Failed to load {}", include_file_path.c_str());
       }
       for(pugi::xml_node include_node : includeDoc.child("mujoco").children())
       {
-	out.append_copy(include_node);
+        out.append_copy(include_node);
       }
     }
   };