add 0 eef

Author: ShrutheeshIR

README.md

@@ -66,15 +66,15 @@ 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"}],
     "output": "panda_fk.hh"
 }
 ```
 
-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.

resources/templates/fk_template.hh

@@ -18,7 +18,7 @@ struct {{name}}
     static constexpr std::size_t resolution = {{resolution}};
 
     static constexpr std::array<std::string_view, dimension> joint_names = {"{{join(joint_names, "\", \"")}}"};
-    static constexpr char* end_effectors[] ={ {% for eef in end_effectors %}'{{eef}}'{%endfor %} };
+    static constexpr std::array<std::string_view, {{num_end_effectors}}> end_effectors ={"{{join(end_effectors, "\", \"")}}"};
 
     using Configuration = FloatVector<dimension>;
     using ConfigurationArray = std::array<FloatT, dimension>;
@@ -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<float, {{n_q}}> &x) noexcept -> Eigen::Isometry3f
+    static inline auto eefk(const std::array<float, {{n_q}}> &x) noexcept -> std::array<Eigen::Isometry3f, {{num_end_effectors}}>
     {
         std::array<float, {{eefk_code_vars}}> v;
         std::array<float, {{eefk_code_output}}> y;
 
         {{eefk_code}}
 
-        return to_isometry(y.data());
+        return to_isometries<{{num_end_effectors}}>(y.data());
     }
 };
 }

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<float>(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();