From 74f93852249efeeb7a0e3a7087f675e4863f762f Mon Sep 17 00:00:00 2001 From: ShrutheeshIR Date: Fri, 17 Oct 2025 17:59:51 -0400 Subject: [PATCH 1/5] single arm works --- resources/baxter.json | 2 +- resources/fetch.json | 2 +- resources/fr3.json | 2 +- resources/motoman.json | 2 +- resources/panda.json | 2 +- resources/templates/fk_template.hh | 4 +- resources/ur10e.json | 2 +- resources/ur5.json | 2 +- src/fkcc_gen.cc | 82 +++++++++++++++--------------- 9 files changed, 50 insertions(+), 50 deletions(-) diff --git a/resources/baxter.json b/resources/baxter.json index 6f349ec..c2ec6be 100644 --- a/resources/baxter.json +++ b/resources/baxter.json @@ -2,7 +2,7 @@ "name": "Baxter", "urdf": "baxter/baxter_spherized.urdf", "srdf": "baxter/baxter.srdf", - "end_effector": "right_gripper", + "end_effectors": ["right_gripper"], "resolution": 64, "template": "templates/fk_template.hh", "subtemplates": [{"name": "ccfk", "template": "templates/ccfk_template.hh"}], diff --git a/resources/fetch.json b/resources/fetch.json index 85fe0ca..96f274d 100644 --- a/resources/fetch.json +++ b/resources/fetch.json @@ -2,7 +2,7 @@ "name": "Fetch", "urdf": "fetch/fetch_spherized.urdf", "srdf": "fetch/fetch.srdf", - "end_effector": "gripper_link", + "end_effectors": ["gripper_link"], "resolution": 32, "template": "templates/fk_template.hh", "subtemplates": [{"name": "ccfk", "template": "templates/ccfk_template.hh"}], diff --git a/resources/fr3.json b/resources/fr3.json index 59327b8..e9b87e2 100644 --- a/resources/fr3.json +++ b/resources/fr3.json @@ -2,7 +2,7 @@ "name": "FR3", "urdf": "fr3/fr3_spherized.urdf", "srdf": "fr3/fr3.srdf", - "end_effector": "fr3_hand_tcp", + "end_effectors": ["fr3_hand_tcp"], "resolution": 32, "template": "templates/fk_template.hh", "subtemplates": [{"name": "ccfk", "template": "templates/ccfk_template.hh"}], diff --git a/resources/motoman.json b/resources/motoman.json index 5e94976..dd4148e 100644 --- a/resources/motoman.json +++ b/resources/motoman.json @@ -2,7 +2,7 @@ "name": "Motoman", "urdf": "motoman/motoman_sda10f_spherized.urdf", "srdf": "motoman/motoman_sda10f.srdf", - "end_effector": "arm_right_link_tool0", + "end_effectors": ["arm_right_link_tool0"], "resolution": 64, "template": "templates/fk_template.hh", "subtemplates": [{"name": "ccfk", "template": "templates/ccfk_template.hh"}], diff --git a/resources/panda.json b/resources/panda.json index 1d79874..d46dcd3 100644 --- a/resources/panda.json +++ b/resources/panda.json @@ -2,7 +2,7 @@ "name": "Panda", "urdf": "panda/panda_spherized.urdf", "srdf": "panda/panda.srdf", - "end_effector": "panda_grasptarget", + "end_effectors": ["panda_grasptarget"], "resolution": 32, "template": "templates/fk_template.hh", "subtemplates": [{"name": "ccfk", "template": "templates/ccfk_template.hh"}], diff --git a/resources/templates/fk_template.hh b/resources/templates/fk_template.hh index c6d4a8c..03113a5 100644 --- a/resources/templates/fk_template.hh +++ b/resources/templates/fk_template.hh @@ -18,7 +18,7 @@ struct {{name}} static constexpr std::size_t resolution = {{resolution}}; static constexpr std::array joint_names = {"{{join(joint_names, "\", \"")}}"}; - static constexpr char* end_effector = "{{end_effector}}"; + static constexpr char* end_effectors[] ={ {% for eef in end_effectors %}'{{eef}}'{%endfor %} } using Configuration = FloatVector; using ConfigurationArray = std::array; @@ -181,7 +181,7 @@ struct {{name}} {{ccfkee_code}} {% include "ccfk" %} - // attaching at {{ end_effector }} + // attaching at {{ end_effectors }} set_attachment_pose(environment, to_isometry(&y[{{ccfkee_code_output - 12}}])); // diff --git a/resources/ur10e.json b/resources/ur10e.json index ffc9b69..6d6d94a 100644 --- a/resources/ur10e.json +++ b/resources/ur10e.json @@ -1,7 +1,7 @@ { "name": "UR10e", "urdf": "ur10e/ur_10e_spherized.urdf", - "end_effector": "robotiq_85_base_link", + "end_effectors": ["robotiq_85_base_link"], "resolution": 32, "template": "templates/fk_template.hh", "subtemplates": [{"name": "ccfk", "template": "templates/ccfk_template.hh"}], diff --git a/resources/ur5.json b/resources/ur5.json index f401e06..d366bd7 100644 --- a/resources/ur5.json +++ b/resources/ur5.json @@ -2,7 +2,7 @@ "name": "UR5", "urdf": "ur5/ur5_spherized.urdf", "srdf": "ur5/ur5.srdf", - "end_effector": "robotiq_85_base_link", + "end_effectors": ["robotiq_85_base_link"], "resolution": 32, "template": "templates/fk_template.hh", "subtemplates": [{"name": "ccfk", "template": "templates/ccfk_template.hh"}], diff --git a/src/fkcc_gen.cc b/src/fkcc_gen.cc index a5ab49e..2f3107b 100644 --- a/src/fkcc_gen.cc +++ b/src/fkcc_gen.cc @@ -77,7 +77,7 @@ struct RobotInfo RobotInfo( const std::filesystem::path &urdf_file, const std::optional &srdf_file, - const std::optional &end_effector) + const std::vector &end_effectors) { if (not std::filesystem::exists(urdf_file)) { @@ -105,21 +105,15 @@ struct RobotInfo extract_spheres(); - if (not end_effector) - { - end_effector_name = model.frames[model.nframes - 1].name; - fmt::print("No EE provided, using distal link `{}`.\n", end_effector_name); - } - else if (not model.existFrame(*end_effector)) - { - throw std::runtime_error(fmt::format("Invalid EE name {}", *end_effector)); - } - else - { - end_effector_name = *end_effector; + for(const auto end_effector: end_effectors) { + if (not model.existFrame(end_effector)) + throw std::runtime_error(fmt::format("Invalid EE name {}", end_effector)); + else + end_effector_names.push_back(end_effector); } - end_effector_index = model.getFrameId(end_effector_name); + for(const auto end_effector_name: end_effector_names) + end_effector_indexes.push_back(model.getFrameId(end_effector_name)); } auto json() -> nlohmann::json @@ -136,8 +130,9 @@ struct RobotInfo json["bound_range"] = std::vector(bound_range.data(), bound_range.data() + model.nq); json["bound_descale"] = std::vector(bound_descale.data(), bound_descale.data() + model.nq); json["measure"] = bound_range.prod(); - json["end_effector"] = end_effector_name; - json["end_effector_index"] = end_effector_index; + json["end_effectors"] = end_effector_names; + std::cout << "Num eefs : " << end_effector_names.size() << std::endl; + json["end_effector_indexes"] = end_effector_indexes; json["min_radius"] = min_radius; json["max_radius"] = max_radius; json["joint_names"] = dof_to_joint_names(); @@ -183,31 +178,34 @@ struct RobotInfo auto get_frames_colliding_end_effector() -> std::vector { - std::size_t end_effector_joint = model.frames[end_effector_index].parentJoint; + std::set end_effector_allowed_collisions; - std::vector frames; - for (auto i = 0U; i < model.frames.size(); ++i) - { - if (model.frames[i].parentJoint == end_effector_joint) + for(const auto end_effector_index: end_effector_indexes){ + std::size_t end_effector_joint = model.frames[end_effector_index].parentJoint; + + std::vector frames; + for (auto i = 0U; i < model.frames.size(); ++i) { - if (bounding_spheres.find(i) != bounding_spheres.end()) + if (model.frames[i].parentJoint == end_effector_joint) { - frames.emplace_back(i); + if (bounding_spheres.find(i) != bounding_spheres.end()) + { + frames.emplace_back(i); + } } } - } - std::set end_effector_allowed_collisions; - for (const auto &[first, second] : allowed_link_pairs) - { - if (std::find(frames.begin(), frames.end(), first) != frames.end()) + for (const auto &[first, second] : allowed_link_pairs) { - end_effector_allowed_collisions.emplace(second); - } + if (std::find(frames.begin(), frames.end(), first) != frames.end()) + { + end_effector_allowed_collisions.emplace(second); + } - if (std::find(frames.begin(), frames.end(), second) != frames.end()) - { - end_effector_allowed_collisions.emplace(first); + if (std::find(frames.begin(), frames.end(), second) != frames.end()) + { + end_effector_allowed_collisions.emplace(first); + } } } @@ -399,8 +397,8 @@ struct RobotInfo Model model; GeometryModel collision_model; - std::string end_effector_name; - std::size_t end_effector_index; + std::vector end_effector_names; + std::vector end_effector_indexes; float min_radius{std::numeric_limits::max()}; float max_radius{std::numeric_limits::min()}; @@ -482,7 +480,7 @@ auto trace_sphere_cc_fk( std::size_t n_spheres_data = (spheres) ? info.spheres.size() * 4 : 0; std::size_t n_bounding_spheres_data = (bounding_spheres) ? info.bounding_spheres.size() * 4 : 0; - std::size_t n_fk_data = (fk) ? 12 : 0; + std::size_t n_fk_data = (fk) ? 12 * info.end_effector_indexes.size() : 0; std::size_t n_out = n_spheres_data + n_bounding_spheres_data + n_fk_data; ADVectorXs data(n_out); @@ -511,7 +509,8 @@ auto trace_sphere_cc_fk( if (fk) { - trace_frame(info.end_effector_index, ad_data, data, n_spheres_data + n_bounding_spheres_data); + for(size_t i = 0; i < info.end_effector_indexes.size(); i++) + trace_frame(info.end_effector_indexes[i], ad_data, data, n_spheres_data + n_bounding_spheres_data + 12 *i); } // Create the AD function @@ -592,13 +591,14 @@ int main(int argc, char **argv) srdf_path = parent_path / data["srdf"]; } - std::optional end_effector_name = {}; - if (data.contains("end_effector")) + std::vector end_effector_names; + if (data.contains("end_effectors")) { - end_effector_name = data["end_effector"]; + for (const auto end_effector_name: data["end_effectors"]) + end_effector_names.push_back(end_effector_name); } - RobotInfo robot(parent_path / data["urdf"], srdf_path, end_effector_name); + RobotInfo robot(parent_path / data["urdf"], srdf_path, end_effector_names); data.update(robot.json()); From b229bacddbacbb4851b6c12bd82d0adc5b701398 Mon Sep 17 00:00:00 2001 From: ShrutheeshIR Date: Fri, 17 Oct 2025 18:08:14 -0400 Subject: [PATCH 2/5] added 2nd arm --- resources/bipanda.json | 10 + resources/panda/bipanda.srdf | 227 +++++ resources/panda/bipanda_spherized.urdf | 1248 ++++++++++++++++++++++++ resources/templates/fk_template.hh | 2 +- 4 files changed, 1486 insertions(+), 1 deletion(-) create mode 100644 resources/bipanda.json create mode 100644 resources/panda/bipanda.srdf create mode 100644 resources/panda/bipanda_spherized.urdf diff --git a/resources/bipanda.json b/resources/bipanda.json new file mode 100644 index 0000000..373590a --- /dev/null +++ b/resources/bipanda.json @@ -0,0 +1,10 @@ +{ + "name": "BimanualPanda", + "urdf": "panda/bipanda_spherized.urdf", + "srdf": "panda/bipanda.srdf", + "end_effectors":["left_panda_grasptarget", "right_panda_grasptarget"], + "resolution": 32, + "template": "templates/fk_template.hh", + "subtemplates": [{"name": "ccfk", "template": "templates/ccfk_template.hh"}], + "output": "bimanual_panda_fk.hh" +} diff --git a/resources/panda/bipanda.srdf b/resources/panda/bipanda.srdf new file mode 100644 index 0000000..2a988ff --- /dev/null +++ b/resources/panda/bipanda.srdf @@ -0,0 +1,227 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/resources/panda/bipanda_spherized.urdf b/resources/panda/bipanda_spherized.urdf new file mode 100644 index 0000000..5e12fa1 --- /dev/null +++ b/resources/panda/bipanda_spherized.urdf @@ -0,0 +1,1248 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/resources/templates/fk_template.hh b/resources/templates/fk_template.hh index 03113a5..1579b47 100644 --- a/resources/templates/fk_template.hh +++ b/resources/templates/fk_template.hh @@ -18,7 +18,7 @@ struct {{name}} static constexpr std::size_t resolution = {{resolution}}; static constexpr std::array joint_names = {"{{join(joint_names, "\", \"")}}"}; - static constexpr char* end_effectors[] ={ {% for eef in end_effectors %}'{{eef}}'{%endfor %} } + static constexpr char* end_effectors[] ={ {% for eef in end_effectors %}'{{eef}}'{%endfor %} }; using Configuration = FloatVector; using ConfigurationArray = std::array; From 2b6ed9b1246a081bdcafcda26815801d00d48f1c Mon Sep 17 00:00:00 2001 From: ShrutheeshIR Date: Sat, 18 Oct 2025 04:00:12 -0400 Subject: [PATCH 3/5] add 0 eef --- README.md | 6 +++--- resources/templates/fk_template.hh | 9 +++++---- src/fkcc_gen.cc | 7 ++++++- 3 files changed, 14 insertions(+), 8 deletions(-) diff --git a/README.md b/README.md index 80cbba5..4f288dd 100644 --- a/README.md +++ b/README.md @@ -66,7 +66,7 @@ An example for the Franka Panda is given below: "name": "Panda", "urdf": "panda/panda_spherized.urdf", "srdf": "panda/panda.srdf", - "end_effector": "panda_grasptarget", + "end_effectors": ["panda_grasptarget"], "resolution": 32, "template": "templates/fk_template.hh", "subtemplates": [{"name": "ccfk", "template": "templates/ccfk_template.hh"}], @@ -74,7 +74,7 @@ An example for the Franka Panda is given below: } ``` -For a custom robot, you only need to change the `name`, `urdf`, `srdf`, and `end_effector` fields. +For a custom robot, you only need to change the `name`, `urdf`, `srdf`, and `end_effectors` fields. Some notes: - If your robot does not have an SRDF, then the script will attempt to guess the self-collisions of the robot by randomly sampling one million configurations and seeing what is always in collision and what never collides. However, this is unreliable and probably should just be done by hand. - If you do not provide an end-effector, the last frame in the robot will be used. For serial link manipulators, this is probably the tool frame, but you should set this yourself. @@ -130,7 +130,7 @@ In addition to the specified input fields from the configuration file, the scrip - `bound_lower`: lower bound of joint ranges. - `bound_range`: range between lower and upper bound of joint ranges. - `measure`: total measure of robot joint space. -- `end_effector_index`: frame index of end-effector. +- `end_effector_indexes`: frame index of end-effector. - `min_radius`: minimum sphere radius on robot. - `max_radius`: maximum sphere radius on robot. - `joint_names`: name of joint corresponding to each DoF. diff --git a/resources/templates/fk_template.hh b/resources/templates/fk_template.hh index 1579b47..1137d77 100644 --- a/resources/templates/fk_template.hh +++ b/resources/templates/fk_template.hh @@ -18,7 +18,7 @@ struct {{name}} static constexpr std::size_t resolution = {{resolution}}; static constexpr std::array joint_names = {"{{join(joint_names, "\", \"")}}"}; - static constexpr char* end_effectors[] ={ {% for eef in end_effectors %}'{{eef}}'{%endfor %} }; + static constexpr std::array end_effectors ={"{{join(end_effectors, "\", \"")}}"}; using Configuration = FloatVector; using ConfigurationArray = std::array; @@ -182,7 +182,8 @@ struct {{name}} {% include "ccfk" %} // attaching at {{ end_effectors }} - set_attachment_pose(environment, to_isometry(&y[{{ccfkee_code_output - 12}}])); + // provide the eef pose of all end effectors. + set_attachment_pose(environment, to_isometries<{{num_end_effectors}}>(&y[{{ccfkee_code_output - 12 * num_end_effectors}}])); // // attachment vs. environment collisions @@ -225,14 +226,14 @@ struct {{name}} return true; } - static inline auto eefk(const std::array &x) noexcept -> Eigen::Isometry3f + static inline auto eefk(const std::array &x) noexcept -> std::array { std::array v; std::array y; {{eefk_code}} - return to_isometry(y.data()); + return to_isometries<{{num_end_effectors}}>(y.data()); } }; } diff --git a/src/fkcc_gen.cc b/src/fkcc_gen.cc index 2f3107b..fb52ce9 100644 --- a/src/fkcc_gen.cc +++ b/src/fkcc_gen.cc @@ -111,6 +111,11 @@ struct RobotInfo else end_effector_names.push_back(end_effector); } + if (end_effector_names.size() == 0) + { + end_effector_names.push_back(model.frames[model.nframes - 1].name); + fmt::print("No EE provided, using distal link `{}`.\n", end_effector_names[0]); + } for(const auto end_effector_name: end_effector_names) end_effector_indexes.push_back(model.getFrameId(end_effector_name)); @@ -131,8 +136,8 @@ struct RobotInfo json["bound_descale"] = std::vector(bound_descale.data(), bound_descale.data() + model.nq); json["measure"] = bound_range.prod(); json["end_effectors"] = end_effector_names; - std::cout << "Num eefs : " << end_effector_names.size() << std::endl; json["end_effector_indexes"] = end_effector_indexes; + json["num_end_effectors"] = end_effector_names.size(); json["min_radius"] = min_radius; json["max_radius"] = max_radius; json["joint_names"] = dof_to_joint_names(); From 9829fba36a1e93e3c0e3a72cc40ebf0bdd5e3411 Mon Sep 17 00:00:00 2001 From: ShrutheeshIR Date: Sat, 18 Oct 2025 04:14:44 -0400 Subject: [PATCH 4/5] clangformatted --- src/fkcc_gen.cc | 28 ++++++++++++++++++++++------ 1 file changed, 22 insertions(+), 6 deletions(-) diff --git a/src/fkcc_gen.cc b/src/fkcc_gen.cc index fb52ce9..dc454c6 100644 --- a/src/fkcc_gen.cc +++ b/src/fkcc_gen.cc @@ -105,11 +105,16 @@ struct RobotInfo extract_spheres(); - for(const auto end_effector: end_effectors) { + for (const auto end_effector : end_effectors) + { if (not model.existFrame(end_effector)) + { throw std::runtime_error(fmt::format("Invalid EE name {}", end_effector)); + } else + { end_effector_names.push_back(end_effector); + } } if (end_effector_names.size() == 0) { @@ -117,8 +122,10 @@ struct RobotInfo fmt::print("No EE provided, using distal link `{}`.\n", end_effector_names[0]); } - for(const auto end_effector_name: end_effector_names) + for (const auto end_effector_name : end_effector_names) + { end_effector_indexes.push_back(model.getFrameId(end_effector_name)); + } } auto json() -> nlohmann::json @@ -185,7 +192,8 @@ struct RobotInfo { std::set end_effector_allowed_collisions; - for(const auto end_effector_index: end_effector_indexes){ + for (const auto end_effector_index : end_effector_indexes) + { std::size_t end_effector_joint = model.frames[end_effector_index].parentJoint; std::vector frames; @@ -514,8 +522,14 @@ auto trace_sphere_cc_fk( if (fk) { - for(size_t i = 0; i < info.end_effector_indexes.size(); i++) - trace_frame(info.end_effector_indexes[i], ad_data, data, n_spheres_data + n_bounding_spheres_data + 12 *i); + for (size_t i = 0; i < info.end_effector_indexes.size(); i++) + { + trace_frame( + info.end_effector_indexes[i], + ad_data, + data, + n_spheres_data + n_bounding_spheres_data + 12 * i); + } } // Create the AD function @@ -599,8 +613,10 @@ int main(int argc, char **argv) std::vector end_effector_names; if (data.contains("end_effectors")) { - for (const auto end_effector_name: data["end_effectors"]) + for (const auto end_effector_name : data["end_effectors"]) + { end_effector_names.push_back(end_effector_name); + } } RobotInfo robot(parent_path / data["urdf"], srdf_path, end_effector_names); From e97a810a0b7369e4cda0d434855c85dc29e6b83c Mon Sep 17 00:00:00 2001 From: ShrutheeshIR Date: Tue, 16 Dec 2025 17:11:10 -0500 Subject: [PATCH 5/5] reviewed changes --- src/fkcc_gen.cc | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/fkcc_gen.cc b/src/fkcc_gen.cc index dc454c6..6afbd2b 100644 --- a/src/fkcc_gen.cc +++ b/src/fkcc_gen.cc @@ -105,7 +105,7 @@ struct RobotInfo extract_spheres(); - for (const auto end_effector : end_effectors) + for (const auto &end_effector : end_effectors) { if (not model.existFrame(end_effector)) { @@ -113,18 +113,18 @@ struct RobotInfo } else { - end_effector_names.push_back(end_effector); + end_effector_names.emplace_back(end_effector); } } - if (end_effector_names.size() == 0) + if (end_effector_names.empty()) { - end_effector_names.push_back(model.frames[model.nframes - 1].name); + end_effector_names.emplace_back(model.frames[model.nframes - 1].name); fmt::print("No EE provided, using distal link `{}`.\n", end_effector_names[0]); } - for (const auto end_effector_name : end_effector_names) + for (const auto &end_effector_name : end_effector_names) { - end_effector_indexes.push_back(model.getFrameId(end_effector_name)); + end_effector_indexes.emplace_back(model.getFrameId(end_effector_name)); } }