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/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/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/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/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 c6d4a8c..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_effector = "{{end_effector}}";
+ static constexpr std::array end_effectors ={"{{join(end_effectors, "\", \"")}}"};
using Configuration = FloatVector;
using ConfigurationArray = std::array;
@@ -181,8 +181,9 @@ struct {{name}}
{{ccfkee_code}}
{% include "ccfk" %}
- // attaching at {{ end_effector }}
- set_attachment_pose(environment, to_isometry(&y[{{ccfkee_code_output - 12}}]));
+ // attaching at {{ end_effectors }}
+ // 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/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..6afbd2b 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,27 @@ struct RobotInfo
extract_spheres();
- if (not end_effector)
+ for (const auto &end_effector : end_effectors)
{
- end_effector_name = model.frames[model.nframes - 1].name;
- fmt::print("No EE provided, using distal link `{}`.\n", end_effector_name);
+ if (not model.existFrame(end_effector))
+ {
+ throw std::runtime_error(fmt::format("Invalid EE name {}", end_effector));
+ }
+ else
+ {
+ end_effector_names.emplace_back(end_effector);
+ }
}
- else if (not model.existFrame(*end_effector))
+ if (end_effector_names.empty())
{
- throw std::runtime_error(fmt::format("Invalid EE name {}", *end_effector));
+ end_effector_names.emplace_back(model.frames[model.nframes - 1].name);
+ fmt::print("No EE provided, using distal link `{}`.\n", end_effector_names[0]);
}
- else
+
+ for (const auto &end_effector_name : end_effector_names)
{
- end_effector_name = *end_effector;
+ end_effector_indexes.emplace_back(model.getFrameId(end_effector_name));
}
-
- end_effector_index = model.getFrameId(end_effector_name);
}
auto json() -> nlohmann::json
@@ -136,8 +142,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;
+ 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();
@@ -183,31 +190,35 @@ 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)
+ for (const auto end_effector_index : end_effector_indexes)
{
- if (model.frames[i].parentJoint == end_effector_joint)
+ 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 +410,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 +493,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 +522,14 @@ 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 +610,16 @@ 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());