From c3eed38ccddd5d73f905abf75d01ae05221a3410 Mon Sep 17 00:00:00 2001 From: "Rohan P. Singh" Date: Thu, 7 Dec 2023 16:28:10 +0900 Subject: [PATCH 1/4] Allow to fix base link from command-link args --- src/main.cpp | 2 ++ src/mj_configuration.h | 2 ++ src/mj_sim.cpp | 32 +++++++++++++++++++++++++++++--- src/mj_sim_impl.h | 2 +- 4 files changed, 34 insertions(+), 4 deletions(-) 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 0992df8..76ccd7d 100644 --- a/src/mj_sim.cpp +++ b/src/mj_sim.cpp @@ -173,6 +173,15 @@ MjSimImpl::MjSimImpl(const MjConfiguration & config) // load all robots named in mc-rtc config for(const auto & r : controller->robots()) { + // fix base if needed + if(config.fix_base_link) + { + if (r.name()=="ground") + { + continue; + } + } + const auto & robot_cfg_path = get_robot_cfg_path(r.module().name); if(!robot_cfg_path.empty()) { @@ -251,7 +260,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 +295,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 +490,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 +643,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); From 22464127776f6c893279bd86f7d37b2274c0d575 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Thu, 7 Dec 2023 11:34:51 +0000 Subject: [PATCH 2/4] [pre-commit.ci] auto fixes from pre-commit.com hooks for more information, see https://pre-commit.ci --- src/mj_sim.cpp | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/src/mj_sim.cpp b/src/mj_sim.cpp index 76ccd7d..0692323 100644 --- a/src/mj_sim.cpp +++ b/src/mj_sim.cpp @@ -176,9 +176,9 @@ MjSimImpl::MjSimImpl(const MjConfiguration & config) // fix base if needed if(config.fix_base_link) { - if (r.name()=="ground") + if(r.name() == "ground") { - continue; + continue; } } @@ -260,7 +260,7 @@ void MjObject::initialize(mjModel * model) } } -void MjRobot::initialize(mjModel * model, const mc_rbdyn::Robot & robot, bool fix_base=false) +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) @@ -299,15 +299,15 @@ void MjRobot::initialize(mjModel * model, const mc_rbdyn::Robot & robot, bool fi // fix base if needed if(fix_base) { - if (!root_joint.empty()) + 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++) + for(unsigned int j = 0; j < model->nv; j++) { - if (model->dof_jntid[j] == root_joint_id) - { - model->dof_damping[j] = mjMAXVAL; - } + if(model->dof_jntid[j] == root_joint_id) + { + model->dof_damping[j] = mjMAXVAL; + } } } } From bf39a6c30ea42853817fdfebb526d8a6a3b8c40c Mon Sep 17 00:00:00 2001 From: "Rohan P. Singh" Date: Fri, 8 Dec 2023 21:08:53 +0900 Subject: [PATCH 3/4] do not filter ground explicitly for fixed-base --- src/mj_sim.cpp | 9 --------- 1 file changed, 9 deletions(-) diff --git a/src/mj_sim.cpp b/src/mj_sim.cpp index 0692323..8800cdf 100644 --- a/src/mj_sim.cpp +++ b/src/mj_sim.cpp @@ -173,15 +173,6 @@ MjSimImpl::MjSimImpl(const MjConfiguration & config) // load all robots named in mc-rtc config for(const auto & r : controller->robots()) { - // fix base if needed - if(config.fix_base_link) - { - if(r.name() == "ground") - { - continue; - } - } - const auto & robot_cfg_path = get_robot_cfg_path(r.module().name); if(!robot_cfg_path.empty()) { From cf54e8e7fa57b5267dfe94545037ce967f66daee Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Thu, 4 Apr 2024 10:59:55 +0000 Subject: [PATCH 4/4] [pre-commit.ci] auto fixes from pre-commit.com hooks for more information, see https://pre-commit.ci --- src/mj_utils_merge_mujoco_models.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) 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); } } };