From a2005fb132c528272768d87633ed1aac55b78258 Mon Sep 17 00:00:00 2001 From: Xinsong Lin Date: Fri, 22 Mar 2024 10:37:23 -0700 Subject: [PATCH 1/6] fixed srdf generation by including all adjacent links --- .../kinematics/pinocchio/pinocchio_model.h | 8 ++++ mplib/planner.py | 48 +++++++++---------- .../pinocchio/pybind_pinocchio_model.cpp | 1 + src/kinematics/pinocchio/pinocchio_model.cpp | 26 ++++++++++ 4 files changed, 57 insertions(+), 26 deletions(-) diff --git a/include/mplib/kinematics/pinocchio/pinocchio_model.h b/include/mplib/kinematics/pinocchio/pinocchio_model.h index 3596185f..6914855b 100644 --- a/include/mplib/kinematics/pinocchio/pinocchio_model.h +++ b/include/mplib/kinematics/pinocchio/pinocchio_model.h @@ -4,6 +4,7 @@ #include #include #include +#include #include #include @@ -239,6 +240,13 @@ class PinocchioModelTpl { */ std::vector getChainJointIndex(const std::string &end_effector) const; + /** + * @brief Get the all adjacent links as a set of pairs. + * + * @return std::set> + */ + std::set> getAdjacentLinks() const; + /** * Get a random configuration. * diff --git a/mplib/planner.py b/mplib/planner.py index 6b92ff88..1dc9d326 100644 --- a/mplib/planner.py +++ b/mplib/planner.py @@ -155,7 +155,7 @@ def replace_package_keyword(self, package_keyword_replacement): out_f.write(content) return rtn_urdf - def generate_collision_pair(self, sample_time=1000000, echo_freq=100000): + def generate_collision_pair(self, sample_time=1000000, echo_freq=10): """ We read the srdf file to get the link pairs that should not collide. If not provided, we need to randomly sample configurations @@ -165,18 +165,6 @@ def generate_collision_pair(self, sample_time=1000000, echo_freq=100000): "Since no SRDF file is provided. We will first detect link pairs that will" " always collide. This may take several minutes." ) - n_link = len(self.user_link_names) - cnt = np.zeros((n_link, n_link), dtype=np.int32) - for i in range(sample_time): - qpos = self.pinocchio_model.get_random_configuration() - self.robot.set_qpos(qpos, True) - collisions = self.planning_world.collide_full() - for collision in collisions: - u = self.link_name_2_idx[collision.link_name1] - v = self.link_name_2_idx[collision.link_name2] - cnt[u][v] += 1 - if i % echo_freq == 0: - print("Finish %.1f%%!" % (i * 100 / sample_time)) import xml.etree.ElementTree as ET from xml.dom import minidom @@ -186,19 +174,27 @@ def generate_collision_pair(self, sample_time=1000000, echo_freq=100000): root.set("name", robot_name) self.srdf = self.urdf.replace(".urdf", ".srdf") - for i in range(n_link): - for j in range(n_link): - if cnt[i][j] == sample_time: - link1 = self.user_link_names[i] - link2 = self.user_link_names[j] - print( - f"Ignore collision pair: ({link1}, {link2}), " - "reason: always collide" - ) - collision = ET.SubElement(root, "disable_collisions") - collision.set("link1", link1) - collision.set("link2", link2) - collision.set("reason", "Default") + for link1, link2 in self.pinocchio_model.get_adjacent_links(): + print(f"Ignore collision pair: ({link1}, {link2}), " "reason: adjacent") + collision = ET.SubElement(root, "disable_collisions") + collision.set("link1", link1) + collision.set("link2", link2) + collision.set("reason", "adjacent") + + leaf_links = self.pinocchio_model.get_leaf_links() + for i in range(len(leaf_links) - 1): + j = i + 1 + link1 = leaf_links[i] + link2 = leaf_links[j] + print( + f"Ignore collision pair: ({link1}, {link2}), " + "reason: leaf links, so highly likely to be gripper parts" + ) + collision = ET.SubElement(root, "disable_collisions") + collision.set("link1", link1) + collision.set("link2", link2) + collision.set("reason", "leaf") + with open(self.srdf, "w") as srdf_file: srdf_file.write( minidom.parseString(ET.tostring(root)).toprettyxml(indent=" ") diff --git a/pybind/kinematics/pinocchio/pybind_pinocchio_model.cpp b/pybind/kinematics/pinocchio/pybind_pinocchio_model.cpp index af110962..3c2bbd4d 100644 --- a/pybind/kinematics/pinocchio/pybind_pinocchio_model.cpp +++ b/pybind/kinematics/pinocchio/pybind_pinocchio_model.cpp @@ -86,6 +86,7 @@ void build_pypinocchio_model(py::module &m) { .def("get_chain_joint_index", &PinocchioModel::getChainJointIndex, py::arg("end_effector"), DOC(mplib, kinematics, pinocchio, PinocchioModelTpl, getChainJointIndex)) + .def("get_adjacent_links", &PinocchioModel::getAdjacentLinks) .def("get_random_configuration", &PinocchioModel::getRandomConfiguration, DOC(mplib, kinematics, pinocchio, PinocchioModelTpl, getRandomConfiguration)) diff --git a/src/kinematics/pinocchio/pinocchio_model.cpp b/src/kinematics/pinocchio/pinocchio_model.cpp index 0446a8fa..2d4826bd 100644 --- a/src/kinematics/pinocchio/pinocchio_model.cpp +++ b/src/kinematics/pinocchio/pinocchio_model.cpp @@ -434,6 +434,32 @@ std::vector PinocchioModelTpl::getChainJointIndex( return ret; } +template +std::set> +PinocchioModelTpl::getAdjacentLinks() const { + auto leaf_links = getLeafLinks(); + std::set> ret; + std::string root_name = urdf_model_->getRoot()->name; + for (auto &leaf_link : leaf_links) { + std::string link_name = leaf_link; + while (link_name != root_name) { + std::string parent_name = urdf_model_->getLink(link_name)->getParent()->name; + ret.insert({parent_name, link_name}); + // if the parent has a fixed link to its parent, also disable collision + if (parent_name != root_name) { + auto parent_joint = urdf_model_->getLink(parent_name)->parent_joint; + if (parent_joint->type == urdf::Joint::FIXED) { + std::string grand_parent_name = + urdf_model_->getLink(parent_name)->getParent()->name; + ret.insert({grand_parent_name, link_name}); + } + } + link_name = parent_name; + } + } + return ret; +} + template VectorX PinocchioModelTpl::qposUser2Pinocchio(const VectorX &q_user) const { VectorX q_pinocchio(model_.nq); From 5b0620925aafca903ac92f1415d758a5160ac652 Mon Sep 17 00:00:00 2001 From: Xinsong Lin Date: Fri, 22 Mar 2024 10:38:58 -0700 Subject: [PATCH 2/6] remove stale arguments --- mplib/planner.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/mplib/planner.py b/mplib/planner.py index 1dc9d326..e0056471 100644 --- a/mplib/planner.py +++ b/mplib/planner.py @@ -155,7 +155,7 @@ def replace_package_keyword(self, package_keyword_replacement): out_f.write(content) return rtn_urdf - def generate_collision_pair(self, sample_time=1000000, echo_freq=10): + def generate_collision_pair(self): """ We read the srdf file to get the link pairs that should not collide. If not provided, we need to randomly sample configurations From f86d6d4c0b9e9d65f9ab36f313e26465ce8cb3ff Mon Sep 17 00:00:00 2001 From: Kolin Guo Date: Thu, 4 Apr 2024 12:04:57 -0700 Subject: [PATCH 3/6] Improve PinocchioModel::getAdjacentLinks() --- .../kinematics/pinocchio/pinocchio_model.h | 17 +++---- .../pinocchio/pybind_pinocchio_model.cpp | 3 +- src/kinematics/pinocchio/pinocchio_model.cpp | 46 ++++++++----------- 3 files changed, 31 insertions(+), 35 deletions(-) diff --git a/include/mplib/kinematics/pinocchio/pinocchio_model.h b/include/mplib/kinematics/pinocchio/pinocchio_model.h index 6914855b..31a67db0 100644 --- a/include/mplib/kinematics/pinocchio/pinocchio_model.h +++ b/include/mplib/kinematics/pinocchio/pinocchio_model.h @@ -1,10 +1,11 @@ #pragma once #include +#include #include #include +#include #include -#include #include #include @@ -74,6 +75,13 @@ class PinocchioModelTpl { */ const std::vector &getLeafLinks() const { return leaf_links_; } + /** + * Get the all adjacent link names. + * + * @return: adjacent link names as a set of pairs of strings + */ + std::set> getAdjacentLinks() const; + /** * Pinocchio might have a different link order or it might add additional links. * @@ -240,13 +248,6 @@ class PinocchioModelTpl { */ std::vector getChainJointIndex(const std::string &end_effector) const; - /** - * @brief Get the all adjacent links as a set of pairs. - * - * @return std::set> - */ - std::set> getAdjacentLinks() const; - /** * Get a random configuration. * diff --git a/pybind/kinematics/pinocchio/pybind_pinocchio_model.cpp b/pybind/kinematics/pinocchio/pybind_pinocchio_model.cpp index 3c2bbd4d..263bb271 100644 --- a/pybind/kinematics/pinocchio/pybind_pinocchio_model.cpp +++ b/pybind/kinematics/pinocchio/pybind_pinocchio_model.cpp @@ -40,6 +40,8 @@ void build_pypinocchio_model(py::module &m) { .def("get_leaf_links", &PinocchioModel::getLeafLinks, DOC(mplib, kinematics, pinocchio, PinocchioModelTpl, getLeafLinks)) + .def("get_adjacent_links", &PinocchioModel::getAdjacentLinks, + DOC(mplib, kinematics, pinocchio, PinocchioModelTpl, getAdjacentLinks)) .def("set_link_order", &PinocchioModel::setLinkOrder, py::arg("names"), DOC(mplib, kinematics, pinocchio, PinocchioModelTpl, setLinkOrder)) @@ -86,7 +88,6 @@ void build_pypinocchio_model(py::module &m) { .def("get_chain_joint_index", &PinocchioModel::getChainJointIndex, py::arg("end_effector"), DOC(mplib, kinematics, pinocchio, PinocchioModelTpl, getChainJointIndex)) - .def("get_adjacent_links", &PinocchioModel::getAdjacentLinks) .def("get_random_configuration", &PinocchioModel::getRandomConfiguration, DOC(mplib, kinematics, pinocchio, PinocchioModelTpl, getRandomConfiguration)) diff --git a/src/kinematics/pinocchio/pinocchio_model.cpp b/src/kinematics/pinocchio/pinocchio_model.cpp index 2d4826bd..0d8cdab0 100644 --- a/src/kinematics/pinocchio/pinocchio_model.cpp +++ b/src/kinematics/pinocchio/pinocchio_model.cpp @@ -236,6 +236,26 @@ void PinocchioModelTpl::dfsParseTree(const urdf::LinkConstSharedPtr &link, if (link->child_links.empty()) leaf_links_.push_back(link->name); } +template +std::set> PinocchioModelTpl::getAdjacentLinks() + const { + std::set> ret; + + const auto root_name = urdf_model_->getRoot()->name; + for (auto link_name : getLeafLinks()) + while (link_name != root_name) { + const auto parent_link = urdf_model_->getLink(link_name)->getParent(); + const auto parent_name = parent_link->name; + ret.insert({parent_name, link_name}); + // if the parent has a fixed link to its parent, also disable collision + if (parent_name != root_name && + parent_link->parent_joint->type == urdf::Joint::FIXED) + ret.insert({parent_link->getParent()->name, link_name}); + link_name = parent_name; + } + return ret; +} + template void PinocchioModelTpl::setLinkOrder(const std::vector &names) { user_link_names_ = names; @@ -434,32 +454,6 @@ std::vector PinocchioModelTpl::getChainJointIndex( return ret; } -template -std::set> -PinocchioModelTpl::getAdjacentLinks() const { - auto leaf_links = getLeafLinks(); - std::set> ret; - std::string root_name = urdf_model_->getRoot()->name; - for (auto &leaf_link : leaf_links) { - std::string link_name = leaf_link; - while (link_name != root_name) { - std::string parent_name = urdf_model_->getLink(link_name)->getParent()->name; - ret.insert({parent_name, link_name}); - // if the parent has a fixed link to its parent, also disable collision - if (parent_name != root_name) { - auto parent_joint = urdf_model_->getLink(parent_name)->parent_joint; - if (parent_joint->type == urdf::Joint::FIXED) { - std::string grand_parent_name = - urdf_model_->getLink(parent_name)->getParent()->name; - ret.insert({grand_parent_name, link_name}); - } - } - link_name = parent_name; - } - } - return ret; -} - template VectorX PinocchioModelTpl::qposUser2Pinocchio(const VectorX &q_user) const { VectorX q_pinocchio(model_.nq); From b0efd4ce5e60cfae03ba6914c877fa3116fc0001 Mon Sep 17 00:00:00 2001 From: Kolin Guo Date: Thu, 4 Apr 2024 14:25:35 -0700 Subject: [PATCH 4/6] Update Planner.generate_collision_pair to match MoveIt2's behavior --- mplib/planner.py | 93 +++++++++++++++++++++++++++++++++++------------- 1 file changed, 69 insertions(+), 24 deletions(-) diff --git a/mplib/planner.py b/mplib/planner.py index e0056471..0bedd081 100644 --- a/mplib/planner.py +++ b/mplib/planner.py @@ -1,8 +1,10 @@ from __future__ import annotations import os +import xml.etree.ElementTree as ET from collections.abc import Callable from typing import Optional, Sequence +from xml.dom import minidom import numpy as np import toppra as ta @@ -11,7 +13,7 @@ from transforms3d.quaternions import mat2quat, quat2mat from mplib.pymp import ArticulatedModel, PlanningWorld -from mplib.pymp.collision_detection import WorldCollisionResult +from mplib.pymp.collision_detection import AllowedCollisionMatrix, WorldCollisionResult from mplib.pymp.planning import ompl @@ -155,52 +157,95 @@ def replace_package_keyword(self, package_keyword_replacement): out_f.write(content) return rtn_urdf - def generate_collision_pair(self): + def generate_collision_pair(self, num_samples=100000): """ We read the srdf file to get the link pairs that should not collide. If not provided, we need to randomly sample configurations to find the link pairs that will always collide. + + :param num_samples: number of samples to find the link that will always collide """ print( "Since no SRDF file is provided. We will first detect link pairs that will" " always collide. This may take several minutes." ) - import xml.etree.ElementTree as ET - from xml.dom import minidom - root = ET.Element("robot") robot_name = self.urdf.split("/")[-1].split(".")[0] root.set("name", robot_name) self.srdf = self.urdf.replace(".urdf", ".srdf") + acm = AllowedCollisionMatrix() + + # 1. disable adjacent link pairs for link1, link2 in self.pinocchio_model.get_adjacent_links(): - print(f"Ignore collision pair: ({link1}, {link2}), " "reason: adjacent") - collision = ET.SubElement(root, "disable_collisions") - collision.set("link1", link1) - collision.set("link2", link2) - collision.set("reason", "adjacent") - - leaf_links = self.pinocchio_model.get_leaf_links() - for i in range(len(leaf_links) - 1): - j = i + 1 - link1 = leaf_links[i] - link2 = leaf_links[j] + print(f"Ignore collision pair: ({link1}, {link2}), reason: Adjacent") + acm.set_entry(link1, link2, True) + _ = ET.SubElement( + root, + "disable_collisions", + attrib={"link1": link1, "link2": link2, "reason": "Adjacent"}, + ) + + # 2. disable all-zeros qpos (default) collision + self.robot.set_qpos(np.zeros(len(self.user_joint_names)), True) + for collision in self.check_for_self_collision(): + link1, link2 = collision.link_name1, collision.link_name2 + if acm.get_entry(link1, link2) is not None: # already ignored + continue print( f"Ignore collision pair: ({link1}, {link2}), " - "reason: leaf links, so highly likely to be gripper parts" + "reason: Default (collides at all-zeros qpos)" + ) + acm.set_entry(link1, link2, True) + _ = ET.SubElement( + root, + "disable_collisions", + attrib={"link1": link1, "link2": link2, "reason": "Default"}, ) - collision = ET.SubElement(root, "disable_collisions") - collision.set("link1", link1) - collision.set("link2", link2) - collision.set("reason", "leaf") + # 3. disable collision pairs that always collide and never collide via sampling + n_links = len(self.user_link_names) + collision_cnt = np.zeros((n_links, n_links), dtype=int) + for _ in range(num_samples): + self.robot.set_qpos(self.pinocchio_model.get_random_configuration(), True) + for collision in self.check_for_self_collision(): + u = self.link_name_2_idx[collision.link_name1] + v = self.link_name_2_idx[collision.link_name2] + collision_cnt[u][v] += 1 + + for i, link1 in enumerate(self.user_link_names): + for j in range(i + 1, n_links): + link2 = self.user_link_names[j] + if acm.get_entry(link1, link2) is not None: # already ignored + continue + if (cnt := (collision_cnt[i][j] + collision_cnt[j][i])) == num_samples: + print( + f"Ignore collision pair: ({link1}, {link2}), " + "reason: Always collide" + ) + _ = ET.SubElement( + root, + "disable_collisions", + attrib={"link1": link1, "link2": link2, "reason": "Always"}, + ) + elif cnt == 0: + print( + f"Ignore collision pair: ({link1}, {link2}), " + "reason: Never collide" + ) + _ = ET.SubElement( + root, + "disable_collisions", + attrib={"link1": link1, "link2": link2, "reason": "Never"}, + ) + + # Save SRDF with open(self.srdf, "w") as srdf_file: srdf_file.write( - minidom.parseString(ET.tostring(root)).toprettyxml(indent=" ") + minidom.parseString(ET.tostring(root)).toprettyxml(indent=" ") ) - srdf_file.close() - print("Saving the SRDF file to %s" % self.srdf) + print(f"Saving the SRDF file to {self.srdf}") def distance_6D(self, p1, q1, p2, q2): """ From 83df166bb9a3d6fe8946b5899136774d1ca48d5a Mon Sep 17 00:00:00 2001 From: Kolin Guo Date: Thu, 4 Apr 2024 14:31:55 -0700 Subject: [PATCH 5/6] Reorder Planner.__init__() methods call --- mplib/planner.py | 24 ++++++++++++------------ 1 file changed, 12 insertions(+), 12 deletions(-) diff --git a/mplib/planner.py b/mplib/planner.py index 0bedd081..4edc8bc6 100644 --- a/mplib/planner.py +++ b/mplib/planner.py @@ -79,14 +79,6 @@ def __init__( self.user_link_names = self.pinocchio_model.get_link_names() self.user_joint_names = self.pinocchio_model.get_joint_names() - self.planning_world = PlanningWorld( - [self.robot], - ["robot"], - kwargs.get("normal_objects", []), - kwargs.get("normal_object_names", []), - ) - self.acm = self.planning_world.get_allowed_collision_matrix() - self.joint_name_2_idx = {} for i, joint in enumerate(self.user_joint_names): self.joint_name_2_idx[joint] = i @@ -94,10 +86,6 @@ def __init__( for i, link in enumerate(self.user_link_names): self.link_name_2_idx[link] = i - if self.srdf == "": - self.generate_collision_pair() - self.robot.update_SRDF(self.srdf) - assert ( move_group in self.user_link_names ), f"end-effector not found as one of the links in {self.user_link_names}" @@ -136,6 +124,18 @@ def __init__( t.startswith("JointModelR") for t in self.joint_types ] & (self.joint_limits[:, 1] - self.joint_limits[:, 0] > 2 * np.pi) + self.planning_world = PlanningWorld( + [self.robot], + ["robot"], + kwargs.get("normal_objects", []), + kwargs.get("normal_object_names", []), + ) + self.acm = self.planning_world.get_allowed_collision_matrix() + + if self.srdf == "": + self.generate_collision_pair() + self.robot.update_SRDF(self.srdf) + self.planner = ompl.OMPLPlanner(world=self.planning_world) def replace_package_keyword(self, package_keyword_replacement): From eee6421705325fd6f90cbe34ce6b691c2d5c96d5 Mon Sep 17 00:00:00 2001 From: Kolin Guo Date: Fri, 22 Mar 2024 17:57:17 -0700 Subject: [PATCH 6/6] Update doc & stubs --- mplib/pymp/kinematics/pinocchio.pyi | 6 ++++++ pybind/docstring/kinematics/pinocchio/pinocchio_model.h | 6 ++++++ 2 files changed, 12 insertions(+) diff --git a/mplib/pymp/kinematics/pinocchio.pyi b/mplib/pymp/kinematics/pinocchio.pyi index 2d8463ee..0181ecde 100644 --- a/mplib/pymp/kinematics/pinocchio.pyi +++ b/mplib/pymp/kinematics/pinocchio.pyi @@ -161,6 +161,12 @@ class PinocchioModel: frame of the link :return: 6 x n jacobian of the link """ + def get_adjacent_links(self) -> set[tuple[str, str]]: + """ + Get the all adjacent link names. + + :return: adjacent link names as a set of pairs of strings + """ def get_chain_joint_index(self, end_effector: str) -> list[int]: """ Get the joint indices of the joints in the chain from the root to the given diff --git a/pybind/docstring/kinematics/pinocchio/pinocchio_model.h b/pybind/docstring/kinematics/pinocchio/pinocchio_model.h index 6c1e6730..fcf0b1ed 100644 --- a/pybind/docstring/kinematics/pinocchio/pinocchio_model.h +++ b/pybind/docstring/kinematics/pinocchio/pinocchio_model.h @@ -118,6 +118,12 @@ Constructs a PinocchioModel from URDF string :param verbose: print debug information. Default: ``False``. :return: a unique_ptr to PinocchioModel)doc"; +static const char *__doc_mplib_kinematics_pinocchio_PinocchioModelTpl_getAdjacentLinks = +R"doc( +Get the all adjacent link names. + +:return: adjacent link names as a set of pairs of strings)doc"; + static const char *__doc_mplib_kinematics_pinocchio_PinocchioModelTpl_getChainJointIndex = R"doc( Get the joint indices of the joints in the chain from the root to the given