diff --git a/src/main.cpp b/src/main.cpp index 2d48c0a..89339a4 100644 --- a/src/main.cpp +++ b/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(); } + config.fix_base_link = vm["fix-base-link"].as(); } mc_mujoco::MjSim mj_sim(config); diff --git a/src/mj_configuration.h b/src/mj_configuration.h index e4bf046..513bc4d 100644 --- a/src/mj_configuration.h +++ b/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 diff --git a/src/mj_sim.cpp b/src/mj_sim.cpp index 04c8ea1..0d75583 100644 --- a/src/mj_sim.cpp +++ b/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 & 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) diff --git a/src/mj_sim_impl.h b/src/mj_sim_impl.h index 6e6d2d0..c2e5271 100644 --- a/src/mj_sim_impl.h +++ b/src/mj_sim_impl.h @@ -151,7 +151,7 @@ struct MjRobot std::vector 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); diff --git a/src/mj_utils_merge_mujoco_models.cpp b/src/mj_utils_merge_mujoco_models.cpp index 61f6ebd..a410b41 100644 --- a/src/mj_utils_merge_mujoco_models.cpp +++ b/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("Failed to load {}", include_file_path.c_str()); + mc_rtc::log::error_and_throw("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); } } };