diff --git a/.github/dependabot.yml b/.github/dependabot.yml index 203f3c88..4ad7bd50 100644 --- a/.github/dependabot.yml +++ b/.github/dependabot.yml @@ -3,4 +3,4 @@ updates: - package-ecosystem: "github-actions" directory: "/" schedule: - interval: "weekly" + interval: "monthly" diff --git a/.github/workflows/nix.yml b/.github/workflows/nix.yml index 8f469ead..e0a43639 100644 --- a/.github/workflows/nix.yml +++ b/.github/workflows/nix.yml @@ -17,13 +17,14 @@ jobs: matrix: os: [ubuntu] # TODO steps: - - uses: actions/checkout@v5 + - uses: actions/checkout@v6 - uses: cachix/install-nix-action@v31 - uses: cachix/cachix-action@v16 with: name: gepetto authToken: '${{ secrets.CACHIX_AUTH_TOKEN }}' - run: nix flake check -L + - run: nix build -L check: if: always() name: check-macos-linux-nix diff --git a/.github/workflows/update-flake-lock.yml b/.github/workflows/update-flake-lock.yml index d32267f0..bca13295 100644 --- a/.github/workflows/update-flake-lock.yml +++ b/.github/workflows/update-flake-lock.yml @@ -6,14 +6,28 @@ on: - cron: '0 16 13 * *' jobs: - lockfile: - runs-on: ubuntu-latest + update-flake-inputs: + runs-on: ubuntu-slim + permissions: + contents: write + pull-requests: write steps: + - name: Generate GitHub App Token + id: app-token + uses: actions/create-github-app-token@v2 + with: + app-id: ${{ secrets.GEPETTO_NIX_APP_ID }} + private-key: ${{ secrets.GEPETTO_NIX_APP_PRIVATE_KEY }} - name: Checkout repository - uses: actions/checkout@v5 - - name: Install Nix - uses: DeterminateSystems/nix-installer-action@main - - name: Update flake.lock - uses: DeterminateSystems/update-flake-lock@main + uses: actions/checkout@v6 + with: + token: ${{ steps.app-token.outputs.token }} + - name: Setup Nix + uses: cachix/install-nix-action@v31 + - name: Update flake inputs + uses: mic92/update-flake-inputs@v1 with: - token: ${{ secrets.GH_TOKEN_FOR_UPDATES }} + github-token: ${{ steps.app-token.outputs.token }} + pr-labels: 'no-changelog' + git-author-name: 'hrp2-14' + git-author-email: '40568249+hrp2-14@users.noreply.github.com' diff --git a/.gitmodules b/.gitmodules deleted file mode 100644 index 2b7a4fe1..00000000 --- a/.gitmodules +++ /dev/null @@ -1,3 +0,0 @@ -[submodule "cmake"] - path = cmake - url = https://github.com/jrl-umi3218/jrl-cmakemodules.git diff --git a/.mergify.yml b/.mergify.yml index 1e2dd5a7..8ece7ea1 100644 --- a/.mergify.yml +++ b/.mergify.yml @@ -5,6 +5,7 @@ pull_request_rules: - check-success = "pre-commit.ci - pr" - or: - author = dependabot[bot] + - author = gepetto-flake-updater[bot] - author = github-actions[bot] - author = hrp2-14 - author = pre-commit-ci[bot] diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 35972c41..7c9180b7 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -1,8 +1,8 @@ ci: - autoupdate_branch: devel + autoupdate_schedule: quarterly repos: - repo: https://github.com/astral-sh/ruff-pre-commit - rev: v0.12.12 + rev: v0.15.5 hooks: - id: ruff args: @@ -14,11 +14,11 @@ repos: hooks: - id: cmake-format - repo: https://github.com/pappasam/toml-sort - rev: v0.24.2 + rev: v0.24.3 hooks: - id: toml-sort-fix - repo: https://github.com/pre-commit/mirrors-clang-format - rev: v21.1.0 + rev: v22.1.0 hooks: - id: clang-format args: diff --git a/CMakeLists.txt b/CMakeLists.txt index 37b2abe1..aeabfdbe 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -100,7 +100,6 @@ add_project_dependency(hpp-util REQUIRED) add_project_dependency(hpp-pinocchio REQUIRED) add_project_dependency(hpp-constraints REQUIRED) add_project_dependency(hpp-core REQUIRED) -add_project_dependency(hpp-corbaserver REQUIRED) add_project_dependency(hpp-manipulation REQUIRED) add_project_dependency(hpp-manipulation-urdf REQUIRED) diff --git a/cmake b/cmake deleted file mode 160000 index 8456ad46..00000000 --- a/cmake +++ /dev/null @@ -1 +0,0 @@ -Subproject commit 8456ad466d18b415b3c6ddd4c21f712c83160315 diff --git a/doc/configure.py b/doc/configure.py index 068d108d..b9e95cd9 100755 --- a/doc/configure.py +++ b/doc/configure.py @@ -116,7 +116,13 @@ def _xmlDirFromPkgConfig(pkg): nsToPackage = { "hpp::core": _xmlDirFromPkgConfig("hpp-core"), + "hpp::core::path": _xmlDirFromPkgConfig("hpp-core"), "hpp::constraints": _xmlDirFromPkgConfig("hpp-constraints"), + "hpp::constraints::solver": _xmlDirFromPkgConfig("hpp-constraints"), + "hpp::pinocchio": _xmlDirFromPkgConfig("hpp-pinocchio"), + "hpp::manipulation": _xmlDirFromPkgConfig("hpp-manipulation"), + "hpp::manipulation::graph": _xmlDirFromPkgConfig("hpp-manipulation"), + "hpp::manipulation::pathPlanner": _xmlDirFromPkgConfig("hpp-manipulation"), } @@ -142,7 +148,7 @@ def make_doc_string(brief, detailled): def make_args_string(args): - return "(" + ",".join(['arg("' + a + '")' for a in args]) + ")" + return "(" + ",".join(['boost::python::arg("' + a + '")' for a in args]) + ")" def substitute(istr, ostr): diff --git a/doc/doxygen_xml_parser.py b/doc/doxygen_xml_parser.py index 7079c793..c4aa0400 100644 --- a/doc/doxygen_xml_parser.py +++ b/doc/doxygen_xml_parser.py @@ -16,45 +16,112 @@ def _getCompound(self, kind, name): def classDoc(self, name): el = self._getCompound("class", name) - return ClassDoc(path.join(self.directory, el.get("refid") + ".xml")) + return ClassDoc( + path.join(self.directory, el.get("refid") + ".xml"), self.directory + ) class ClassDoc: - def __init__(self, filename): + def __init__(self, filename, directory=None): self.tree = etree.parse(filename) - self.compound = self.tree.find("/compounddef") + self.compound = self.tree.find("./compounddef") self.classname = self.compound.find("compoundname").text.strip() + self.directory = directory if directory else path.dirname(filename) + self._group_cache = {} @staticmethod def _getDoc(el): b = el.find("briefdescription") d = el.find("detaileddescription") - return etree.tostring(b, method="text").strip(), d.text.strip() + brief = etree.tostring(b, method="text", encoding="unicode").strip() + detailed = d.text.strip() if d.text else "" + return brief, detailed + + def _getMemberFromGroup(self, memberDefKind, name): + """Look for memberdef in a group file via member refid.""" + members = self.compound.xpath( + "sectiondef/member[@kind='" + memberDefKind + "' and name='" + name + "']" + ) + if not members: + return None + member = members[0] + refid = member.get("refid") + if not refid: + return None + group_id = refid.rsplit("_1", 1)[0] + if group_id not in self._group_cache: + group_file = path.join(self.directory, group_id + ".xml") + if not path.isfile(group_file): + return None + self._group_cache[group_id] = etree.parse(group_file) + group_tree = self._group_cache[group_id] + memberdefs = group_tree.xpath( + "//memberdef[@id='" + refid + "' and @kind='" + memberDefKind + "']" + ) + if memberdefs: + return memberdefs[0] + return None + + def _getMemberFromInherited(self, memberDefKind, name): + """Look for memberdef in base class XML via listofallmembers member refid.""" + members = self.compound.xpath("listofallmembers/member[name='" + name + "']") + if not members: + return None + member = members[0] + refid = member.get("refid") + if not refid: + return None + # Extract base class file from refid + # e.g., "classhpp_1_1pinocchio_1_1AbstractDevice_1a312..." -> "classhpp_1_1pinocchio_1_1AbstractDevice" + parts = refid.rsplit("_1", 1) + if len(parts) < 2: + return None + base_class_id = parts[0] + base_class_file = path.join(self.directory, base_class_id + ".xml") + if not path.isfile(base_class_file): + return None + if base_class_id not in self._group_cache: + self._group_cache[base_class_id] = etree.parse(base_class_file) + base_tree = self._group_cache[base_class_id] + memberdefs = base_tree.xpath( + "//memberdef[@id='" + refid + "' and @kind='" + memberDefKind + "']" + ) + if memberdefs: + return memberdefs[0] + return None def _getMember(self, sectionKind, memberDefKind, name): - # return self.compound.xpath ("sectiondef[@kind='" + sectionKind - try: - return self.compound.xpath( - "sectiondef[re:test(@kind,'" - + sectionKind - + "')]/memberdef[@kind='" - + memberDefKind - + "' and name='" - + name - + "']", - namespaces={"re": "http://exslt.org/regular-expressions"}, - )[0] - except IndexError as e: - msg = ( - "Error: Could not find member (" - + sectionKind - + ") " - + name - + " of class " - + self.classname - ) - print(msg + "\n" + str(e), file=sys.stderr) - raise IndexError(msg) + # First try to find memberdef directly in class XML + results = self.compound.xpath( + "sectiondef[re:test(@kind,'" + + sectionKind + + "')]/memberdef[@kind='" + + memberDefKind + + "' and name='" + + name + + "']", + namespaces={"re": "http://exslt.org/regular-expressions"}, + ) + if results: + return results[0] + # Fall back to looking in group files (for @ingroup documented classes) + memberdef = self._getMemberFromGroup(memberDefKind, name) + if memberdef is not None: + return memberdef + # Fall back to looking in base class files (for inherited methods) + memberdef = self._getMemberFromInherited(memberDefKind, name) + if memberdef is not None: + return memberdef + msg = ( + "Error: Could not find member (" + + sectionKind + + ") " + + name + + " of class " + + self.classname + ) + print(msg, file=sys.stderr) + raise IndexError(msg) def getClassDoc(self): return self._getDoc(self.compound) @@ -76,14 +143,16 @@ def getClassMethodDoc(self, methodname): ] else: args = [] - args += [el.text.strip() for el in member.xpath("param/declname")] + args += [el.text.strip() for el in member.xpath("param/declname") if el.text] for parameters in dd.xpath("para/parameterlist/parameteritem"): pargs = [ el.text.strip() for el in parameters.find("parameternamelist").findall("parametername") + if el.text ] pns = " ".join(pargs) - pd = parameters.find("parameterdescription").find("para").text.strip() + para_el = parameters.find("parameterdescription").find("para") + pd = para_el.text.strip() if para_el is not None and para_el.text else "" d += "\n:param " + pns + ":" + pd return b, d, args diff --git a/flake.lock b/flake.lock index 3f2ef499..a1c93df9 100644 --- a/flake.lock +++ b/flake.lock @@ -5,11 +5,11 @@ "nixpkgs-lib": "nixpkgs-lib" }, "locked": { - "lastModified": 1760948891, - "narHash": "sha256-TmWcdiUUaWk8J4lpjzu4gCGxWY6/Ok7mOK4fIFfBuU4=", + "lastModified": 1769996383, + "narHash": "sha256-AnYjnFWgS49RlqX7LrC4uA+sCCDBj0Ry/WOJ5XWAsa0=", "owner": "hercules-ci", "repo": "flake-parts", - "rev": "864599284fc7c0ba6357ed89ed5e2cd5040f0c04", + "rev": "57928607ea566b5db3ad13af0e57e921e6b12381", "type": "github" }, "original": { @@ -36,36 +36,82 @@ "type": "github" } }, - "gepetto": { + "gazebros2nix": { "inputs": { "flake-parts": "flake-parts", + "gepetto-lib": "gepetto-lib", "nix-ros-overlay": "nix-ros-overlay", - "nix-system-graphics": "nix-system-graphics", "nixpkgs": [ "gepetto", + "gazebros2nix", "nix-ros-overlay", "nixpkgs" ], - "src-agimus-controller": "src-agimus-controller", - "src-agimus-msgs": "src-agimus-msgs", - "src-franka-description": "src-franka-description", + "pyproject-build-systems": "pyproject-build-systems", + "pyproject-nix": "pyproject-nix", + "systems": [ + "gepetto", + "gazebros2nix", + "nix-ros-overlay", + "flake-utils", + "systems" + ], + "treefmt-nix": "treefmt-nix", + "uv2nix": "uv2nix" + }, + "locked": { + "lastModified": 1772816781, + "narHash": "sha256-Ac0KEl+8ygy+BnDgczNHgTumw8HpCasp/zJU5Yx3kQs=", + "owner": "gepetto", + "repo": "gazebros2nix", + "rev": "ea8aff2fca6d45fa85fe5e90ef3c71fe0fcc0d12", + "type": "github" + }, + "original": { + "owner": "gepetto", + "repo": "gazebros2nix", + "type": "github" + } + }, + "gepetto": { + "inputs": { + "flake-parts": [ + "gepetto", + "gazebros2nix", + "flake-parts" + ], + "gazebros2nix": "gazebros2nix", + "nix-ros-overlay": [ + "gepetto", + "gazebros2nix", + "nix-ros-overlay" + ], + "nix-system-graphics": "nix-system-graphics", + "nixpkgs": [ + "gepetto", + "gazebros2nix", + "nixpkgs" + ], "src-odri-control-interface": "src-odri-control-interface", "src-odri-masterboard-sdk": "src-odri-masterboard-sdk", "system-manager": "system-manager", "systems": [ "gepetto", - "nix-ros-overlay", - "flake-utils", + "gazebros2nix", "systems" ], - "treefmt-nix": "treefmt-nix" + "treefmt-nix": [ + "gepetto", + "gazebros2nix", + "treefmt-nix" + ] }, "locked": { - "lastModified": 1761305265, - "narHash": "sha256-eEuunrRx0tfi0oq05CuQsV0/U/q6yQYCKUEUbjT0Am8=", + "lastModified": 1772825496, + "narHash": "sha256-ZCgGWufV1suEVlft03k9TGOD190kGRCA3rrO8qsjeQ0=", "owner": "gepetto", "repo": "nix", - "rev": "e82291718d5bc01d145781811db48b4f22eb6ce1", + "rev": "5c1a5edffd02c51e267c42f8dfd36a13c7817950", "type": "github" }, "original": { @@ -74,17 +120,32 @@ "type": "github" } }, + "gepetto-lib": { + "locked": { + "lastModified": 1770945346, + "narHash": "sha256-L88f+oJbpIkMm9Ln1GP9SFyGztMvnOowbdshQHBeGGs=", + "owner": "Gepetto", + "repo": "nix-lib", + "rev": "82ef58cdf50514f6b1fde96b9d5b38fd8d3e83f5", + "type": "github" + }, + "original": { + "owner": "Gepetto", + "repo": "nix-lib", + "type": "github" + } + }, "nix-ros-overlay": { "inputs": { "flake-utils": "flake-utils", "nixpkgs": "nixpkgs" }, "locked": { - "lastModified": 1760993928, - "narHash": "sha256-eojj3Mggmd8fAHlEsl+c9pbPWwsmQqmn8u/FoYX/az4=", + "lastModified": 1771885942, + "narHash": "sha256-TlBFvE4YHNlbhKVkayP/FWBNAAv+yG9APA8vMR+5NBw=", "owner": "lopsided98", "repo": "nix-ros-overlay", - "rev": "8a927d104d10114d64528d3731eb7663bea5d843", + "rev": "f891b118c8f4ddb2b6f38d6ce1bfe2f8079552ba", "type": "github" }, "original": { @@ -102,11 +163,11 @@ ] }, "locked": { - "lastModified": 1737457219, - "narHash": "sha256-nX9dxoATDCSQgWw/iv6BngXDJEyHVYYEvHEVQ7Ig3fI=", + "lastModified": 1763296639, + "narHash": "sha256-K9JBscC7ApwCnl0wR0sVkxrKFsoDYVqXN5fOujvyBWA=", "owner": "soupglasses", "repo": "nix-system-graphics", - "rev": "9c875e0c56cf2eb272b9102a4f3e24e4e31629fd", + "rev": "ac37f0f3ec0cb15d63a520918433c794d01d9dac", "type": "github" }, "original": { @@ -133,11 +194,11 @@ }, "nixpkgs-lib": { "locked": { - "lastModified": 1754788789, - "narHash": "sha256-x2rJ+Ovzq0sCMpgfgGaaqgBSwY+LST+WbZ6TytnT9Rk=", + "lastModified": 1769909678, + "narHash": "sha256-cBEymOf4/o3FD5AZnzC3J9hLbiZ+QDT/KDuyHXVJOpM=", "owner": "nix-community", "repo": "nixpkgs.lib", - "rev": "a73b9c743612e4244d865a2fdee11865283c04e6", + "rev": "72716169fe93074c333e8d0173151350670b824c", "type": "github" }, "original": { @@ -146,77 +207,87 @@ "type": "github" } }, - "root": { + "pyproject-build-systems": { "inputs": { - "flake-parts": [ - "gepetto", - "flake-parts" - ], - "gepetto": "gepetto", - "nix-ros-overlay": [ - "gepetto", - "nix-ros-overlay" - ], "nixpkgs": [ "gepetto", + "gazebros2nix", "nixpkgs" ], - "systems": [ + "pyproject-nix": [ "gepetto", - "systems" + "gazebros2nix", + "pyproject-nix" ], - "treefmt-nix": [ + "uv2nix": [ "gepetto", - "treefmt-nix" + "gazebros2nix", + "uv2nix" ] - } - }, - "src-agimus-controller": { - "flake": false, + }, "locked": { - "lastModified": 1761049897, - "narHash": "sha256-tJieqjLTKS0QUHmUew8qgjCYTejhMfYMFrndFUV6rJ4=", - "owner": "agimus-project", - "repo": "agimus_controller", - "rev": "1a4eff6ded0e08b06bfaddb8087ad01a74356c73", + "lastModified": 1771423342, + "narHash": "sha256-7uXPiWB0YQ4HNaAqRvVndYL34FEp1ZTwVQHgZmyMtC8=", + "owner": "pyproject-nix", + "repo": "build-system-pkgs", + "rev": "04e9c186e01f0830dad3739088070e4c551191a4", "type": "github" }, "original": { - "owner": "agimus-project", - "repo": "agimus_controller", + "owner": "pyproject-nix", + "repo": "build-system-pkgs", "type": "github" } }, - "src-agimus-msgs": { - "flake": false, + "pyproject-nix": { + "inputs": { + "nixpkgs": [ + "gepetto", + "gazebros2nix", + "nixpkgs" + ] + }, "locked": { - "lastModified": 1759994370, - "narHash": "sha256-QuYtUR7VTOOtRBrYCxDjSLUP77wh0NlHbMtxZ1nSJFM=", - "owner": "agimus-project", - "repo": "agimus_msgs", - "rev": "e8e48c8c7b942cc2d2feba01f5e2d319c7915816", + "lastModified": 1771518446, + "narHash": "sha256-nFJSfD89vWTu92KyuJWDoTQJuoDuddkJV3TlOl1cOic=", + "owner": "pyproject-nix", + "repo": "pyproject.nix", + "rev": "eb204c6b3335698dec6c7fc1da0ebc3c6df05937", "type": "github" }, "original": { - "owner": "agimus-project", - "repo": "agimus_msgs", + "owner": "pyproject-nix", + "repo": "pyproject.nix", "type": "github" } }, - "src-franka-description": { - "flake": false, - "locked": { - "lastModified": 1759137774, - "narHash": "sha256-D7vkjZ1B9qKecqUCmnpwHcxzZpakvoqp0qAhv4jGwRI=", - "owner": "agimus-project", - "repo": "franka_description", - "rev": "3cad53b368ea19f10ce1248f4fd781abfae1bdc6", - "type": "github" - }, - "original": { - "owner": "agimus-project", - "repo": "franka_description", - "type": "github" + "root": { + "inputs": { + "flake-parts": [ + "gepetto", + "flake-parts" + ], + "gazebros2nix": [ + "gepetto", + "gazebros2nix" + ], + "gepetto": "gepetto", + "nix-ros-overlay": [ + "gepetto", + "nix-ros-overlay" + ], + "nixpkgs": [ + "gepetto", + "nixpkgs" + ], + "systems": [ + "gepetto", + "systems" + ], + "treefmt-nix": [ + "gepetto", + "treefmt-nix" + ] } }, "src-odri-control-interface": { @@ -261,11 +332,11 @@ ] }, "locked": { - "lastModified": 1761207312, - "narHash": "sha256-x/E0qSvHP5YHBaf+Ee2eFr6Ph14upcSEQ1WC0uLrcmQ=", + "lastModified": 1770135975, + "narHash": "sha256-J3qmZ4rTfmgyjrsQRrQWT7ZIYVtYqtLomMNDUibuw2k=", "owner": "numtide", "repo": "system-manager", - "rev": "f6597102b6e053ee6f8d797e1e8bf8e2a3bdd7d7", + "rev": "413f296fb1fd210c44e38744e270b3afc4c733d7", "type": "github" }, "original": { @@ -293,15 +364,16 @@ "inputs": { "nixpkgs": [ "gepetto", + "gazebros2nix", "nixpkgs" ] }, "locked": { - "lastModified": 1760945191, - "narHash": "sha256-ZRVs8UqikBa4Ki3X4KCnMBtBW0ux1DaT35tgsnB1jM4=", + "lastModified": 1770228511, + "narHash": "sha256-wQ6NJSuFqAEmIg2VMnLdCnUc0b7vslUohqqGGD+Fyxk=", "owner": "numtide", "repo": "treefmt-nix", - "rev": "f56b1934f5f8fcab8deb5d38d42fd692632b47c2", + "rev": "337a4fe074be1042a35086f15481d763b8ddc0e7", "type": "github" }, "original": { @@ -309,6 +381,33 @@ "repo": "treefmt-nix", "type": "github" } + }, + "uv2nix": { + "inputs": { + "nixpkgs": [ + "gepetto", + "gazebros2nix", + "nixpkgs" + ], + "pyproject-nix": [ + "gepetto", + "gazebros2nix", + "pyproject-nix" + ] + }, + "locked": { + "lastModified": 1772187362, + "narHash": "sha256-gCojeIlQ/rfWMe3adif3akyHsT95wiMkLURpxTeqmPc=", + "owner": "pyproject-nix", + "repo": "uv2nix", + "rev": "abe65de114300de41614002fe9dce2152ac2ac23", + "type": "github" + }, + "original": { + "owner": "pyproject-nix", + "repo": "uv2nix", + "type": "github" + } } }, "root": "root", diff --git a/flake.nix b/flake.nix index 68f3cac6..d9edb38b 100644 --- a/flake.nix +++ b/flake.nix @@ -3,6 +3,7 @@ inputs = { gepetto.url = "github:gepetto/nix"; + gazebros2nix.follows = "gepetto/gazebros2nix"; flake-parts.follows = "gepetto/flake-parts"; nixpkgs.follows = "gepetto/nixpkgs"; nix-ros-overlay.follows = "gepetto/nix-ros-overlay"; @@ -12,26 +13,14 @@ outputs = inputs: - inputs.flake-parts.lib.mkFlake { inherit inputs; } { - systems = import inputs.systems; - imports = [ inputs.gepetto.flakeModule ]; - perSystem = - { - lib, - pkgs, - self', - ... - }: - { - apps = lib.filterAttrs (_n: v: v.program.meta.available && !v.program.meta.broken) { - default = { - type = "app"; - program = pkgs.python3.withPackages (_: [ self'.packages.default ]); - }; - }; - packages = { - default = self'.packages.hpp-python; - hpp-python = pkgs.python3Packages.hpp-python.overrideAttrs (super: { + inputs.flake-parts.lib.mkFlake { inherit inputs; } ( + { lib, ... }: + { + systems = import inputs.systems; + imports = [ + inputs.gepetto.flakeModule + { + gazebros2nix.pyOverrides.hpp-python = _final: _python-final: { src = lib.fileset.toSource { root = ./.; fileset = lib.fileset.unions [ @@ -43,12 +32,9 @@ ./tests ]; }; - patches = [ ]; - propagatedBuildInputs = (super.propagatedBuildInputs or [ ]) ++ [ - pkgs.python3Packages.hpp-tutorial - ]; - }); - }; - }; - }; + }; + } + ]; + } + ); } diff --git a/include/pyhpp/constraints/fwd.hh b/include/pyhpp/constraints/fwd.hh index 0cf1c510..b2162c40 100644 --- a/include/pyhpp/constraints/fwd.hh +++ b/include/pyhpp/constraints/fwd.hh @@ -42,6 +42,8 @@ void exposeExplicit(); void exposeLockedJoint(); void exposeHierarchicalIterativeSolver(); void exposeBySubstitution(); +void exposeRelativeCom(); + } // namespace constraints } // namespace pyhpp diff --git a/include/pyhpp/corbaserver/fwd.hh b/include/pyhpp/corbaserver/fwd.hh deleted file mode 100644 index 28357e67..00000000 --- a/include/pyhpp/corbaserver/fwd.hh +++ /dev/null @@ -1,45 +0,0 @@ -// -// Copyright (c) 2018 CNRS -// Authors: Joseph Mirabel -// -// Redistribution and use in source and binary forms, with or without -// modification, are permitted provided that the following conditions -// are met: - -// 1. Redistributions of source code must retain the above copyright -// notice, this list of conditions and the following disclaimer. - -// 2. Redistributions in binary form must reproduce the above -// copyright notice, this list of conditions and the following -// disclaimer in the documentation and/or other materials provided -// with the distribution. - -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -// COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, -// INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES -// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR -// SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) -// HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, -// STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED -// OF THE POSSIBILITY OF SUCH DAMAGE. - -#ifndef PYHPP_CORBASERVER_FWD_HH -#define PYHPP_CORBASERVER_FWD_HH - -#include - -namespace pyhpp { -namespace corbaserver { -void exposeServer(); -// void exposeProblemSolverMap(); -namespace wholebodyStep { -void exposeServer(); -} -} // namespace corbaserver -} // namespace pyhpp - -#endif // PYHPP_CORBASERVER_FWD_HH diff --git a/include/pyhpp/core/fwd.hh b/include/pyhpp/core/fwd.hh index 4204a2d2..64cc175c 100644 --- a/include/pyhpp/core/fwd.hh +++ b/include/pyhpp/core/fwd.hh @@ -58,7 +58,6 @@ void exposePathPlanner(); void exposeParameter(); void exposeProblem(); -void exposeProblemSolver(); // forward declaration of some classes class PathPlanner; diff --git a/include/pyhpp/core/problem.hh b/include/pyhpp/core/problem.hh index 5e2f1423..595aa686 100644 --- a/include/pyhpp/core/problem.hh +++ b/include/pyhpp/core/problem.hh @@ -56,11 +56,11 @@ typedef hpp::core::ProblemTargetPtr_t ProblemTargetPtr_t; typedef hpp::core::DistancePtr_t DistancePtr_t; typedef hpp::core::value_type value_type; typedef hpp::core::size_type size_type; +typedef hpp::core::Configuration_t Configuration_t; // Wrapper class for hpp::core::Problem struct Problem { hpp::core::ProblemPtr_t obj; - Problem(const DevicePtr_t& robot); Problem(hpp::core::ProblemPtr_t problemPtr) : obj(problemPtr) {} @@ -102,6 +102,51 @@ struct Problem { bool isManipulationProblem() const { return bool(HPP_DYNAMIC_PTR_CAST(hpp::manipulation::Problem, obj)); } + + // Constraints utility functions + + void addPartialCom(const std::string& name, boost::python::list pyjointNames); + hpp::pinocchio::vector3_t getPartialCom(const std::string& name); + std::map + centerOfMassComputations; + hpp::constraints::ImplicitPtr_t createRelativeComConstraint( + const char* constraintName, const char* comName, const char* jointName, + hpp::pinocchio::vector3_t point, boost::python::list mask); + + hpp::constraints::ImplicitPtr_t createTransformationConstraint( + const char* constraintName, const char* joint1Name, + const char* joint2Name, const hpp::pinocchio::Transform3s& M, + boost::python::list mask); + + hpp::constraints::ImplicitPtr_t createTransformationConstraint2( + const char* constraintName, const char* joint1Name, + const char* joint2Name, const hpp::pinocchio::Transform3s& M1, + const hpp::pinocchio::Transform3s& M2, const boost::python::list mask); + + void setConstantRightHandSide(hpp::constraints::ImplicitPtr_t constraint, + bool constant); + + boost::python::tuple applyConstraints(ConfigurationIn_t config); + boost::python::tuple isConfigValid(ConfigurationIn_t dofArray); + void setConstraints(hpp::core::ConstraintSetPtr_t constraints); + void setRightHandSideFromConfig(ConfigurationIn_t configIn); + + void addNumericalConstraintsToConfigProjector1( + const char* configProjName, boost::python::list constraints, + boost::python::list priorities); + void addNumericalConstraintsToConfigProjector2( + const char* configProjName, boost::python::list constraints); + hpp::constraints::ImplicitPtr_t createComBetweenFeet( + const char* constraintName, const char* comName, const char* jointLName, + const char* jointRName, const hpp::pinocchio::vector3_t& pointL, + const hpp::pinocchio::vector3_t& pointR, const char* jointRefName, + const hpp::pinocchio::vector3_t& pointRef, boost::python::list mask); + + boost::python::tuple directPath(ConfigurationIn_t start, + ConfigurationIn_t end, bool validate); + hpp::core::ConstraintSetPtr_t constraints_; + value_type errorThreshold_; + size_type maxIterProjection_; }; } // namespace core diff --git a/include/pyhpp/manipulation/fwd.hh b/include/pyhpp/manipulation/fwd.hh index bfdf2406..3030cbdb 100644 --- a/include/pyhpp/manipulation/fwd.hh +++ b/include/pyhpp/manipulation/fwd.hh @@ -53,7 +53,8 @@ void exposeGraph(); void exposePathPlanners(); void exposeProblem(); void exposePathProjector(); -void exposeGraphSteeringMethod(); +void exposeManipSteeringMethod(); +void exposePathOptimizers(); } // namespace manipulation } // namespace pyhpp diff --git a/include/pyhpp/util.hh b/include/pyhpp/util.hh index 5756a0a2..ce461d4c 100644 --- a/include/pyhpp/util.hh +++ b/include/pyhpp/util.hh @@ -86,11 +86,20 @@ struct VectorOfPtr { return *v[i]; } }; - template std::vector extract_vector(boost::python::list py_list) { - return std::vector(boost::python::stl_input_iterator(py_list), - boost::python::stl_input_iterator()); + std::vector result; + result.reserve(boost::python::len(py_list)); + + for (int i = 0; i < boost::python::len(py_list); ++i) { + boost::python::extract extractor(py_list[i]); + if (extractor.check()) { + result.push_back(extractor()); + } else { + throw std::runtime_error("Failed to extract element"); + } + } + return result; } template boost::python::list to_python_list(const std::vector& vec) { diff --git a/package.xml b/package.xml index bea53e45..cb904b19 100644 --- a/package.xml +++ b/package.xml @@ -28,7 +28,4 @@ hpp-core hpp-core hpp-core - hpp-corbaserver - hpp-corbaserver - hpp-corbaserver diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 31c06b4b..b45a2fb2 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -69,6 +69,8 @@ endmacro() python_install_on_site(pyhpp __init__.py) python_install_on_site(pyhpp/manipulation constraint_graph_factory.py) +python_install_on_site(pyhpp/core static_stability_constraint_factory.py) +python_install_on_site(pyhpp/manipulation security_margins.py) add_python_library( pyhpp/pinocchio @@ -94,6 +96,7 @@ add_python_library( pyhpp/constraints/iterative-solver.cc pyhpp/constraints/by-substitution.cc pyhpp/constraints/locked-joint.cc + pyhpp/constraints/relative-com.cc pyhpp/constraints/bindings.cc LINK_LIBRARIES hpp-constraints::hpp-constraints) @@ -118,7 +121,6 @@ add_python_library( pyhpp/core/problem-target.cc pyhpp/core/parameter.cc pyhpp/core/problem.cc - pyhpp/core/problem-solver.cc pyhpp/core/bindings.cc LINK_LIBRARIES hpp-core::hpp-core) @@ -142,10 +144,6 @@ add_python_library( pyhpp/core/path_optimization/spline-gradient-based-abstract.cc pyhpp/core/path_optimization/bindings.cc LINK_LIBRARIES hpp-core::hpp-core) -add_python_library( - pyhpp/corbaserver FILES pyhpp/corbaserver/bindings.cc - pyhpp/corbaserver/server.cc LINK_LIBRARIES hpp-corbaserver::hpp-corbaserver) - add_python_library( pyhpp/manipulation FILES @@ -157,6 +155,7 @@ add_python_library( pyhpp/manipulation/path-planner.hh pyhpp/manipulation/path-projector.cc pyhpp/manipulation/steering-method.cc + pyhpp/manipulation/path-optimizer.cc LINK_LIBRARIES hpp-manipulation::hpp-manipulation) diff --git a/src/pyhpp/__init__.py b/src/pyhpp/__init__.py index a8506fb6..a7ffa60d 100644 --- a/src/pyhpp/__init__.py +++ b/src/pyhpp/__init__.py @@ -1 +1,9 @@ -import eigenpy # noqa: F401 +import warnings + +warnings.filterwarnings( + "ignore", + message="to-Python converter .* already registered", + category=RuntimeWarning, +) + +import eigenpy # noqa: E402, F401 diff --git a/src/pyhpp/constraints/bindings.cc b/src/pyhpp/constraints/bindings.cc index cbe1c5fb..dcc26dba 100644 --- a/src/pyhpp/constraints/bindings.cc +++ b/src/pyhpp/constraints/bindings.cc @@ -49,4 +49,5 @@ BOOST_PYTHON_MODULE(bindings) { pyhpp::constraints::exposeExplicitConstraintSet(); pyhpp::constraints::exposeHierarchicalIterativeSolver(); pyhpp::constraints::exposeBySubstitution(); + pyhpp::constraints::exposeRelativeCom(); } diff --git a/src/pyhpp/constraints/by-substitution.cc b/src/pyhpp/constraints/by-substitution.cc index c2b38627..6b2e3c92 100644 --- a/src/pyhpp/constraints/by-substitution.cc +++ b/src/pyhpp/constraints/by-substitution.cc @@ -31,6 +31,8 @@ #include #include +// DocNamespace(hpp::constraints::solver) + using namespace boost::python; namespace pyhpp { @@ -45,15 +47,24 @@ tuple BySubstitution_solve(const BySubstitution& hs, const vector_t& q) { } void exposeBySubstitution() { + enum_("SolverStatus") + .value("ERROR_INCREASED", HierarchicalIterative::ERROR_INCREASED) + .value("MAX_ITERATION_REACHED", + HierarchicalIterative::MAX_ITERATION_REACHED) + .value("INFEASIBLE", HierarchicalIterative::INFEASIBLE) + .value("SUCCESS", HierarchicalIterative::SUCCESS); + + // DocClass(BySubstitution) class_ >( "BySubstitution", init()) .def("explicitConstraintSetHasChanged", - &BySubstitution::explicitConstraintSetHasChanged) + &BySubstitution::explicitConstraintSetHasChanged, + DocClassMethod(explicitConstraintSetHasChanged)) .def("solve", &BySubstitution_solve) .def("explicitConstraintSet", static_cast( &BySubstitution::explicitConstraintSet), - return_internal_reference<>()) + return_internal_reference<>(), DocClassMethod(explicitConstraintSet)) .add_property("errorThreshold", static_cast( &BySubstitution::errorThreshold), diff --git a/src/pyhpp/constraints/explicit-constraint-set.cc b/src/pyhpp/constraints/explicit-constraint-set.cc index 26578a83..032577ea 100644 --- a/src/pyhpp/constraints/explicit-constraint-set.cc +++ b/src/pyhpp/constraints/explicit-constraint-set.cc @@ -32,6 +32,8 @@ #include #include +// DocNamespace(hpp::constraints) + using namespace boost::python; namespace pyhpp { @@ -39,10 +41,11 @@ namespace constraints { using namespace hpp::constraints; void exposeExplicitConstraintSet() { + // DocClass(ExplicitConstraintSet) class_("ExplicitConstraintSet", init()) .def("__str__", &to_str) - .def("add", &ExplicitConstraintSet::add); + .def("add", &ExplicitConstraintSet::add, DocClassMethod(add)); } } // namespace constraints } // namespace pyhpp diff --git a/src/pyhpp/constraints/explicit.cc b/src/pyhpp/constraints/explicit.cc index 4b076f8d..fe6c9cfb 100644 --- a/src/pyhpp/constraints/explicit.cc +++ b/src/pyhpp/constraints/explicit.cc @@ -31,6 +31,8 @@ #include #include +// DocNamespace(hpp::constraints) + using namespace boost::python; namespace pyhpp { @@ -61,6 +63,7 @@ ExplicitPtr_t createExplicit(const LiegroupSpacePtr_t& configSpace, } void exposeExplicit() { + // DocClass(Explicit) class_("Explicit", no_init) .def("create", &createExplicit) .staticmethod("create"); diff --git a/src/pyhpp/constraints/generic-transformation.cc b/src/pyhpp/constraints/generic-transformation.cc index df647b3c..425c0dfd 100644 --- a/src/pyhpp/constraints/generic-transformation.cc +++ b/src/pyhpp/constraints/generic-transformation.cc @@ -31,6 +31,8 @@ #include #include +// DocNamespace(hpp::constraints) + using namespace boost::python; namespace pyhpp { @@ -54,9 +56,8 @@ void exposeAbsoluteGenericTransformation(const char* name) { class_, typename GT_t::Ptr_t, boost::noncopyable>(name, no_init) - .def("create", &AbsoluteGenericTransformation_create, - args("name", "device", "joint2", "frame2", "frame1", "mask")) - .staticmethod("create"); + .def("__init__", + make_constructor(&AbsoluteGenericTransformation_create)); } template @@ -75,10 +76,8 @@ template void exposeRelativeGenericTransformation(const char* name) { class_, typename GT_t::Ptr_t, boost::noncopyable>(name, no_init) - .def("create", &RelativeGenericTransformation_create, - args("name", "device", "joint1", "joint2", "frame1", "frame2", - "mask")) - .staticmethod("create"); + .def("__init__", + make_constructor(&RelativeGenericTransformation_create)); } void exposeGenericTransformations() { diff --git a/src/pyhpp/constraints/implicit.cc b/src/pyhpp/constraints/implicit.cc index b83e4f83..a5cd89c0 100644 --- a/src/pyhpp/constraints/implicit.cc +++ b/src/pyhpp/constraints/implicit.cc @@ -37,6 +37,8 @@ #include #include +// DocNamespace(hpp::constraints) + using namespace boost::python; namespace pyhpp { @@ -53,14 +55,22 @@ void exposeImplicit() { .value("EqualToZero", EqualToZero) .value("Superior", Superior) .value("Inferior", Inferior); + // DocClass(Implicit) class_("Implicit", no_init) - .def("create", &Implicit::create) - .staticmethod("create") - .PYHPP_DEFINE_GETTER_SETTER_INTERNAL_REF(Implicit, comparisonType, - const ComparisonTypes_t&) - .PYHPP_DEFINE_METHOD_INTERNAL_REF(Implicit, function) - .def("parameterSize", &Implicit::parameterSize) - .def("rightHandSideSize", &Implicit::rightHandSideSize) + .def("__init__", make_constructor(&Implicit::create)) + .def("comparisonType", + static_cast( + &Implicit::comparisonType), + return_internal_reference<>(), DocClassMethod(comparisonType)) + .def("comparisonType", + static_cast( + &Implicit::comparisonType)) + .def("function", &Implicit::function, return_internal_reference<>(), + DocClassMethod(function)) + .def("parameterSize", &Implicit::parameterSize, + DocClassMethod(parameterSize)) + .def("rightHandSideSize", &Implicit::rightHandSideSize, + DocClassMethod(rightHandSideSize)) .def("getFunctionOutputSize", &getFunctionOutputSize) .staticmethod("getFunctionOutputSize"); } diff --git a/src/pyhpp/constraints/iterative-solver.cc b/src/pyhpp/constraints/iterative-solver.cc index 6325ea48..f9e01d4c 100644 --- a/src/pyhpp/constraints/iterative-solver.cc +++ b/src/pyhpp/constraints/iterative-solver.cc @@ -37,6 +37,8 @@ #include #include +// DocNamespace(hpp::constraints) + using namespace boost::python; namespace pyhpp { @@ -48,10 +50,11 @@ void exposeHierarchicalIterativeSolver() { class_("ComparisonTypes") .def(vector_indexing_suite()); + // DocClass(solver::HierarchicalIterative) class_("HierarchicalIterative", init()) .def("__str__", &to_str) - .def("add", &HierarchicalIterative::add) + .def("add", &HierarchicalIterative::add, DocClassMethod(add)) .add_property( "errorThreshold", diff --git a/src/pyhpp/constraints/locked-joint.cc b/src/pyhpp/constraints/locked-joint.cc index 796dd6bb..7cd14c13 100644 --- a/src/pyhpp/constraints/locked-joint.cc +++ b/src/pyhpp/constraints/locked-joint.cc @@ -25,6 +25,8 @@ #include #include +// DocNamespace(hpp::constraints) + using namespace boost::python; namespace pyhpp { @@ -64,10 +66,8 @@ LockedJointPtr_t createLockedJointWithComp(const DevicePtr_t& robot, void exposeLockedJoint() { class_, LockedJointPtr_t, boost::noncopyable>( "LockedJoint", no_init) - .def("create", &createLockedJoint) - .staticmethod("create") - .def("createWithComp", &createLockedJointWithComp) - .staticmethod("create"); + .def("__init__", make_constructor(&createLockedJoint)) + .def("__init__", make_constructor(&createLockedJointWithComp)); } } // namespace constraints } // namespace pyhpp diff --git a/src/pyhpp/constraints/relative-com.cc b/src/pyhpp/constraints/relative-com.cc new file mode 100644 index 00000000..5672a0c8 --- /dev/null +++ b/src/pyhpp/constraints/relative-com.cc @@ -0,0 +1,79 @@ +// +// Copyright (c) 2025 CNRS +// Authors: Paul Sardin +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions +// are met: + +// 1. Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. + +// 2. Redistributions in binary form must reproduce the above +// copyright notice, this list of conditions and the following +// disclaimer in the documentation and/or other materials provided +// with the distribution. + +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +// COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, +// INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +// SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) +// HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, +// STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED +// OF THE POSSIBILITY OF SUCH DAMAGE. + +#include +#include +#include +#include +#include + +// DocNamespace(hpp::constraints) + +using namespace boost::python; + +namespace pyhpp { +namespace constraints { +using namespace hpp::constraints; + +static RelativeComPtr_t create1(std::string& name, const DevicePtr_t& robot, + const JointPtr_t& joint, + const vector3_t reference, + std::vector mask) { + CenterOfMassComputationPtr_t comc = CenterOfMassComputation::create(robot); + comc->add(robot->rootJoint()); + return RelativeCom::create(name, robot, comc, joint, reference, mask); +} +static RelativeComPtr_t create2(const DevicePtr_t& robot, + const CenterOfMassComputationPtr_t& comc, + const JointPtr_t& joint, + const vector3_t reference, + std::vector mask) { + return RelativeCom::create("RelativeCom", robot, comc, joint, reference, + mask); +} +static RelativeComPtr_t create3(const std::string& name, + const DevicePtr_t& robot, + const CenterOfMassComputationPtr_t& comc, + const JointPtr_t& joint, + const vector3_t reference, + std::vector mask) { + return RelativeCom::create(name, robot, comc, joint, reference, mask); +} + +void exposeRelativeCom() { + // DocClass(RelativeCom) + class_, + boost::noncopyable>("RelativeCom", no_init) + .def("create", &create1, DocClassMethod(create)) + .def("create", &create2) + .def("create", &create3) + .staticmethod("create"); +} +} // namespace constraints +} // namespace pyhpp diff --git a/src/pyhpp/corbaserver/bindings.cc b/src/pyhpp/corbaserver/bindings.cc deleted file mode 100644 index a6cff2e7..00000000 --- a/src/pyhpp/corbaserver/bindings.cc +++ /dev/null @@ -1,40 +0,0 @@ -// -// Copyright (c) 2018 CNRS -// Authors: Joseph Mirabel -// -// Redistribution and use in source and binary forms, with or without -// modification, are permitted provided that the following conditions -// are met: - -// 1. Redistributions of source code must retain the above copyright -// notice, this list of conditions and the following disclaimer. - -// 2. Redistributions in binary form must reproduce the above -// copyright notice, this list of conditions and the following -// disclaimer in the documentation and/or other materials provided -// with the distribution. - -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -// COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, -// INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES -// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR -// SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) -// HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, -// STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED -// OF THE POSSIBILITY OF SUCH DAMAGE. - -#include -#include -#include - -BOOST_PYTHON_MODULE(bindings) { - INIT_PYHPP_MODULE; - - boost::python::import("pyhpp.core"); - - pyhpp::corbaserver::exposeServer(); -} diff --git a/src/pyhpp/corbaserver/server.cc b/src/pyhpp/corbaserver/server.cc deleted file mode 100644 index c732982a..00000000 --- a/src/pyhpp/corbaserver/server.cc +++ /dev/null @@ -1,68 +0,0 @@ -// Copyright (c) 2018, Joseph Mirabel -// Authors: Joseph Mirabel (joseph.mirabel@laas.fr) -// -// Redistribution and use in source and binary forms, with or without -// modification, are permitted provided that the following conditions -// are met: - -// 1. Redistributions of source code must retain the above copyright -// notice, this list of conditions and the following disclaimer. - -// 2. Redistributions in binary form must reproduce the above -// copyright notice, this list of conditions and the following -// disclaimer in the documentation and/or other materials provided -// with the distribution. - -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -// COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, -// INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES -// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR -// SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) -// HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, -// STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED -// OF THE POSSIBILITY OF SUCH DAMAGE. - -#include -#include -#include -#include -#include - -using namespace boost::python; - -namespace pyhpp { -namespace corbaserver { -using hpp::core::ProblemSolverPtr_t; -using namespace hpp::corbaServer; - -struct SWrapper { - static Server* init1(ProblemSolverPtr_t ps, bool multithread = false) { - return new Server(ps, 0, NULL, multithread); - } - static Server* init2(ProblemSolverPtr_t ps, const char* name, - bool multithread = false) { - const char** argv = new const char*[2]; - const char* arg0 = "--name"; - argv[0] = arg0; - argv[1] = name; - Server* server = new Server(ps, 2, argv, multithread); - delete[] argv; - return server; - } -}; - -void exposeServer() { - class_("Server", init()) - .def("__init__", make_constructor(&SWrapper::init1)) - .def("__init__", make_constructor(&SWrapper::init2)) - .def("initialize", &Server::initialize) - .PYHPP_DEFINE_METHOD(Server, startCorbaServer) - .PYHPP_DEFINE_METHOD(Server, processRequest) - .PYHPP_DEFINE_METHOD_INTERNAL_REF(Server, mainContextId); -} -} // namespace corbaserver -} // namespace pyhpp diff --git a/src/pyhpp/corbaserver/wholebodyStep/bindings.cc b/src/pyhpp/corbaserver/wholebodyStep/bindings.cc deleted file mode 100644 index daa8d9e1..00000000 --- a/src/pyhpp/corbaserver/wholebodyStep/bindings.cc +++ /dev/null @@ -1,41 +0,0 @@ -// -// Copyright (c) 2018 CNRS -// Authors: Joseph Mirabel -// -// -// Redistribution and use in source and binary forms, with or without -// modification, are permitted provided that the following conditions -// are met: - -// 1. Redistributions of source code must retain the above copyright -// notice, this list of conditions and the following disclaimer. - -// 2. Redistributions in binary form must reproduce the above -// copyright notice, this list of conditions and the following -// disclaimer in the documentation and/or other materials provided -// with the distribution. - -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -// COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, -// INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES -// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR -// SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) -// HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, -// STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED -// OF THE POSSIBILITY OF SUCH DAMAGE. - -#include -#include -#include - -BOOST_PYTHON_MODULE(bindings) { - INIT_PYHPP_MODULE; - - boost::python::import("pyhpp.corbaserver"); - - pyhpp::corbaserver::wholebodyStep::exposeServer(); -} diff --git a/src/pyhpp/corbaserver/wholebodyStep/server.cc b/src/pyhpp/corbaserver/wholebodyStep/server.cc deleted file mode 100644 index 46c6b159..00000000 --- a/src/pyhpp/corbaserver/wholebodyStep/server.cc +++ /dev/null @@ -1,64 +0,0 @@ -// Copyright (c) 2018, Joseph Mirabel -// Authors: Joseph Mirabel (joseph.mirabel@laas.fr) -// -// Redistribution and use in source and binary forms, with or without -// modification, are permitted provided that the following conditions -// are met: - -// 1. Redistributions of source code must retain the above copyright -// notice, this list of conditions and the following disclaimer. - -// 2. Redistributions in binary form must reproduce the above -// copyright notice, this list of conditions and the following -// disclaimer in the documentation and/or other materials provided -// with the distribution. - -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -// COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, -// INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES -// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR -// SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) -// HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, -// STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED -// OF THE POSSIBILITY OF SUCH DAMAGE. - -#include -#include -#include -#include -#include -#include - -using namespace boost::python; - -namespace pyhpp { -namespace corbaserver { -namespace wholebodyStep { -typedef hpp::corbaServer::Server CorbaServer; -typedef hpp::wholebodyStep::Server WholebodyServer; - -struct SWrapper { - static WholebodyServer* init(CorbaServer* server, bool multithread = false) { - WholebodyServer* s = new WholebodyServer(0, NULL, multithread); - s->setProblemSolverMap(server->problemSolverMap()); - return s; - } - static void startCorbaServer(WholebodyServer* wbs, - const CorbaServer* server) { - wbs->startCorbaServer(server->mainContextId(), "corbaserver", - "wholebodyStep", "problem"); - } -}; - -void exposeServer() { - class_("Server", no_init) - .def("__init__", make_constructor(&SWrapper::init)) - .PYHPP_DEFINE_METHOD(SWrapper, startCorbaServer); -} -} // namespace wholebodyStep -} // namespace corbaserver -} // namespace pyhpp diff --git a/src/pyhpp/core/bindings.cc b/src/pyhpp/core/bindings.cc index 8079a45b..3646a57f 100644 --- a/src/pyhpp/core/bindings.cc +++ b/src/pyhpp/core/bindings.cc @@ -44,7 +44,6 @@ BOOST_PYTHON_MODULE(bindings) { pyhpp::core::exposeParameter(); pyhpp::core::exposeProblem(); - pyhpp::core::exposeProblemSolver(); pyhpp::core::exposeConfigValidation(); pyhpp::core::exposeConfigurationShooter(); diff --git a/src/pyhpp/core/config-validation.cc b/src/pyhpp/core/config-validation.cc index 9c0bf84f..6f2c1a8c 100644 --- a/src/pyhpp/core/config-validation.cc +++ b/src/pyhpp/core/config-validation.cc @@ -69,7 +69,7 @@ void exposeConfigValidation() { .PYHPP_DEFINE_METHOD2(ConfigValidations, add, DocClassMethod(add)) .PYHPP_DEFINE_METHOD2(ConfigValidations, numberConfigValidations, DocClassMethod(numberConfigValidations)) - .PYHPP_DEFINE_METHOD2(ConfigValidations, clear, DocClassMethod(clear)); + .def("clear", &ConfigValidations::clear); } } // namespace core } // namespace pyhpp diff --git a/src/pyhpp/core/configuration-shooter.cc b/src/pyhpp/core/configuration-shooter.cc index 36d0c39d..a6807200 100644 --- a/src/pyhpp/core/configuration-shooter.cc +++ b/src/pyhpp/core/configuration-shooter.cc @@ -32,6 +32,8 @@ #include #include +// DocNamespace(hpp::core) + using namespace boost::python; namespace pyhpp { @@ -43,9 +45,10 @@ struct CSWrapper { }; void exposeConfigurationShooter() { + // DocClass(ConfigurationShooter) class_( "ConfigurationShooter", no_init) - .def("shoot", &CSWrapper::shoot); + .def("shoot", &CSWrapper::shoot, DocClassMethod(shoot)); } } // namespace core } // namespace pyhpp diff --git a/src/pyhpp/core/connected-component.cc b/src/pyhpp/core/connected-component.cc index fdc31841..399b5e13 100644 --- a/src/pyhpp/core/connected-component.cc +++ b/src/pyhpp/core/connected-component.cc @@ -35,6 +35,8 @@ #include #include +// DocNamespace(hpp::core) + using namespace boost::python; namespace pyhpp { @@ -49,11 +51,38 @@ struct CCWrapper { } return to_python_list(res); } + static boost::python::list reachableFrom(ConnectedComponent& cc) { + std::vector res; + for (ConnectedComponent::RawPtrs_t::const_iterator itcc = + cc.reachableFrom().begin(); + itcc != cc.reachableFrom().end(); ++itcc) { + res.push_back((*itcc)->self()); + } + return to_python_list(res); + } + static boost::python::list reachableTo(ConnectedComponent& cc) { + std::vector res; + for (ConnectedComponent::RawPtrs_t::const_iterator itcc = + cc.reachableTo().begin(); + itcc != cc.reachableTo().end(); ++itcc) { + res.push_back((*itcc)->self()); + } + return to_python_list(res); + } + // Test that raw pointers are the same. + static bool equality(ConnectedComponent& cc1, ConnectedComponent& cc2) { + return &cc1 == &cc2; + } }; void exposeConnectedComponent() { + // DocClass(ConnectedComponent) class_( "ConnectedComponent", no_init) - .def("nodes", &CCWrapper::nodes); + .def("nodes", &CCWrapper::nodes, DocClassMethod(nodes)) + .def("reachableFrom", &CCWrapper::reachableFrom, + DocClassMethod(reachableFrom)) + .def("reachableTo", &CCWrapper::reachableTo, DocClassMethod(reachableTo)) + .def("__eq__", &CCWrapper::equality); } } // namespace core } // namespace pyhpp diff --git a/src/pyhpp/core/constraint.cc b/src/pyhpp/core/constraint.cc index 74f21215..4e76df84 100644 --- a/src/pyhpp/core/constraint.cc +++ b/src/pyhpp/core/constraint.cc @@ -36,6 +36,8 @@ #include #include +// DocNamespace(hpp::core) + using namespace boost::python; namespace pyhpp { @@ -62,38 +64,77 @@ struct CWrapper { static ConstraintPtr_t copy(const Constraint* cs) { return cs->copy(); } }; +static ConstraintSetPtr_t createConstraintSet( + const hpp::pinocchio::DevicePtr_t& device, const std::string& name) { + return core::ConstraintSet::create(device, name); +} + +static ConfigProjectorPtr_t createConfigProjector( + const hpp::pinocchio::DevicePtr_t& device, const std::string& name, + value_type threshold, size_type iterations) { + return core::ConfigProjector::create(device, name, threshold, iterations); +} + +static void rightHandSideFromConfig(ConfigProjectorPtr_t& configProj, + ConfigurationIn_t config) { + configProj->rightHandSideFromConfig(config); +} + +static void setRightHandSideOfConstraint( + ConfigProjectorPtr_t& configProj, + hpp::constraints::ImplicitPtr_t constraint, ConfigurationIn_t config) { + configProj->rightHandSide(constraint, config); +} + void exposeConstraint() { + // DocClass(Constraint) class_("Constraint", no_init) .def("__str__", &to_str_from_operator) - .PYHPP_DEFINE_METHOD_INTERNAL_REF(Constraint, name) - .PYHPP_DEFINE_METHOD(CWrapper, apply) + .def("name", &Constraint::name, return_internal_reference<>(), + DocClassMethod(name)) + .def("apply", &CWrapper::apply) .def("isSatisfied", &CWrapper::isSatisfied1) .def("isSatisfied", &CWrapper::isSatisfied2) - .PYHPP_DEFINE_METHOD(CWrapper, copy); + .def("copy", &CWrapper::copy); + // DocClass(ConstraintSet) class_ >("ConstraintSet", no_init) - .PYHPP_DEFINE_METHOD(ConstraintSet, addConstraint) - .PYHPP_DEFINE_METHOD(ConstraintSet, configProjector); + .def("__init__", make_constructor(&createConstraintSet)) + .def("addConstraint", &ConstraintSet::addConstraint, + DocClassMethod(addConstraint)) + .def("configProjector", &ConstraintSet::configProjector, + DocClassMethod(configProjector)); + // DocClass(ConfigProjector) class_ >("ConfigProjector", no_init) + .def("__init__", make_constructor(&createConfigProjector)) .def("solver", static_cast( &ConfigProjector::solver), - return_internal_reference<>()) - .def("add", &ConfigProjector::add) - .PYHPP_DEFINE_GETTER_SETTER(ConfigProjector, lastIsOptional, bool) - .PYHPP_DEFINE_GETTER_SETTER(ConfigProjector, maxIterations, size_type) + return_internal_reference<>(), DocClassMethod(solver)) + .def("add", &ConfigProjector::add, DocClassMethod(add)) + .def("lastIsOptional", static_cast( + &ConfigProjector::lastIsOptional)) + .def("lastIsOptional", static_cast( + &ConfigProjector::lastIsOptional)) + .def("maxIterations", static_cast( + &ConfigProjector::maxIterations)) + .def("maxIterations", static_cast( + &ConfigProjector::maxIterations)) .def("errorThreshold", static_cast( &ConfigProjector::errorThreshold)) .def("errorThreshold", static_cast( &ConfigProjector::errorThreshold)) - .PYHPP_DEFINE_METHOD(ConfigProjector, residualError) + .def("residualError", &ConfigProjector::residualError, + DocClassMethod(residualError)) + .def("setRightHandSideFromConfig", &rightHandSideFromConfig) + .def("setRightHandSideOfConstraint", &setRightHandSideOfConstraint) .def("sigma", &ConfigProjector::sigma, - return_value_policy()); + return_value_policy(), DocClassMethod(sigma)); } } // namespace core } // namespace pyhpp diff --git a/src/pyhpp/core/distance.cc b/src/pyhpp/core/distance.cc index 4bd9621a..a96f3f40 100644 --- a/src/pyhpp/core/distance.cc +++ b/src/pyhpp/core/distance.cc @@ -33,6 +33,8 @@ #include #include +// DocNamespace(hpp::core) + using namespace boost::python; namespace pyhpp { @@ -55,12 +57,14 @@ struct DistanceWrapper { }; void exposeDistance() { + // DocClass(Distance) class_("Distance", no_init) - .def("compute", &DistanceWrapper::compute); + .def("compute", &DistanceWrapper::compute, DocClassMethod(compute)); + + // DocClass(WeighedDistance) class_, WeighedDistancePtr_t, boost::noncopyable>("WeighedDistance", no_init) - .def("create", &WeighedDistance::create) - .staticmethod("create") + .def("__init__", make_constructor(&WeighedDistance::create)) .def("asDistancePtr_t", &DistanceWrapper::AsDistancePtr_t) .def("getWeights", &DistanceWrapper::getWeights) .def("setWeights", &DistanceWrapper::setWeights); diff --git a/src/pyhpp/core/node.cc b/src/pyhpp/core/node.cc index d702fe20..1ffc5c56 100644 --- a/src/pyhpp/core/node.cc +++ b/src/pyhpp/core/node.cc @@ -34,6 +34,8 @@ #include #include +// DocNamespace(hpp::core) + using namespace boost::python; namespace pyhpp { @@ -42,23 +44,26 @@ namespace core { using namespace hpp::core; void exposeNode() { + // DocClass(Node) class_, boost::noncopyable>("Node", no_init) .def(init()) .def(init()) - .PYHPP_DEFINE_METHOD(Node, addOutEdge) - .PYHPP_DEFINE_METHOD(Node, addInEdge) + .def("addOutEdge", &Node::addOutEdge, DocClassMethod(addOutEdge)) + .def("addInEdge", &Node::addInEdge, DocClassMethod(addInEdge)) .def("connectedComponent", static_cast( &Node::connectedComponent)) .def("connectedComponent", static_cast( &Node::connectedComponent)) - .PYHPP_DEFINE_METHOD_INTERNAL_REF(Node, outEdges) - .PYHPP_DEFINE_METHOD_INTERNAL_REF(Node, inEdges) - .PYHPP_DEFINE_METHOD(Node, isOutNeighbor) - .PYHPP_DEFINE_METHOD(Node, isInNeighbor) - .PYHPP_DEFINE_METHOD_INTERNAL_REF(Node, configuration) - .PYHPP_DEFINE_METHOD_INTERNAL_REF(Node, print); + .def("outEdges", &Node::outEdges, return_internal_reference<>(), + DocClassMethod(outEdges)) + .def("inEdges", &Node::inEdges, return_internal_reference<>(), + DocClassMethod(inEdges)) + .def("isOutNeighbor", &Node::isOutNeighbor, DocClassMethod(isOutNeighbor)) + .def("isInNeighbor", &Node::isInNeighbor, DocClassMethod(isInNeighbor)) + .def("configuration", &Node::configuration, return_internal_reference<>(), + DocClassMethod(configuration)); } } // namespace core } // namespace pyhpp diff --git a/src/pyhpp/core/parameter.cc b/src/pyhpp/core/parameter.cc index 530c524a..36dc3a46 100644 --- a/src/pyhpp/core/parameter.cc +++ b/src/pyhpp/core/parameter.cc @@ -31,6 +31,8 @@ #include #include +// DocNamespace(hpp::core) + using namespace boost::python; namespace pyhpp { @@ -80,6 +82,7 @@ Parameter create(object param) { Parameter createBool(bool param) { return Parameter(param); } void exposeParameter() { + // DocClass(Parameter) class_("Parameter", no_init) .def("create", &create) .staticmethod("create") diff --git a/src/pyhpp/core/path-optimizer.cc b/src/pyhpp/core/path-optimizer.cc index 4a3b3b12..1eac091a 100644 --- a/src/pyhpp/core/path-optimizer.cc +++ b/src/pyhpp/core/path-optimizer.cc @@ -29,10 +29,19 @@ // OF THE POSSIBILITY OF SUCH DAMAGE. #include +#include +#include +#include +#include +#include +#include #include +#include #include #include +// DocNamespace(hpp::core) + using namespace boost::python; namespace pyhpp { @@ -40,8 +49,72 @@ namespace core { using namespace hpp::core; void exposePathOptimizer() { + // DocClass(PathOptimizer) class_("PathOptimizer", - no_init); + no_init) + .def("problem", &PathOptimizer::problem, DocClassMethod(problem)) + .def("optimize", &PathOptimizer::optimize, DocClassMethod(optimize)) + .def("interrupt", &PathOptimizer::interrupt, DocClassMethod(interrupt)) + .def("maxIterations", &PathOptimizer::maxIterations, + DocClassMethod(maxIterations)) + .def("timeOut", &PathOptimizer::timeOut, DocClassMethod(timeOut)); + + class_, + bases, boost::noncopyable>("RandomShortcut", no_init) + .def("__init__", + make_constructor(&pathOptimization::RandomShortcut::create)); + + class_, + bases, boost::noncopyable>("SimpleShortcut", no_init) + .def("__init__", + make_constructor(&pathOptimization::SimpleShortcut::create)); + + class_, + bases, boost::noncopyable>("PartialShortcut", no_init) + .def("__init__", + make_constructor(&pathOptimization::PartialShortcut::create)); + + class_, + bases, boost::noncopyable>("SimpleTimeParameterization", + no_init) + .def("__init__", + make_constructor( + &pathOptimization::SimpleTimeParameterization::create)); + + class_, + bases, boost::noncopyable>("RSTimeParameterization", + no_init) + .def("__init__", + make_constructor(&pathOptimization::RSTimeParameterization::create)); + + class_, + std::shared_ptr< + pathOptimization::SplineGradientBased >, + bases, boost::noncopyable>( + "SplineGradientBased_bezier1", no_init) + .def("__init__", make_constructor(&pathOptimization::SplineGradientBased< + path::BernsteinBasis, 1>::create)); + + class_, + std::shared_ptr< + pathOptimization::SplineGradientBased >, + bases, boost::noncopyable>( + "SplineGradientBased_bezier3", no_init) + .def("__init__", make_constructor(&pathOptimization::SplineGradientBased< + path::BernsteinBasis, 3>::create)); + + class_, + std::shared_ptr< + pathOptimization::SplineGradientBased >, + bases, boost::noncopyable>( + "SplineGradientBased_bezier5", no_init) + .def("__init__", make_constructor(&pathOptimization::SplineGradientBased< + path::BernsteinBasis, 5>::create)); } } // namespace core } // namespace pyhpp diff --git a/src/pyhpp/core/path-planner.cc b/src/pyhpp/core/path-planner.cc index c09e37bb..bf1e33c2 100644 --- a/src/pyhpp/core/path-planner.cc +++ b/src/pyhpp/core/path-planner.cc @@ -39,6 +39,8 @@ #include #include +// DocNamespace(hpp::core) + namespace pyhpp { namespace core { @@ -142,20 +144,33 @@ void PathPlanner::stopWhenProblemIsSolved(bool enable) { PathVectorPtr_t PathPlanner::computePath() const { return obj->computePath(); } void exposePathPlanner() { + // DocClass(PathPlanner) class_("PathPlanner", no_init) - .PYHPP_DEFINE_METHOD_CONST_REF(PathPlanner, roadmap) - .PYHPP_DEFINE_METHOD(PathPlanner, problem) - .PYHPP_DEFINE_METHOD(PathPlanner, startSolve) - .PYHPP_DEFINE_METHOD(PathPlanner, solve) - .PYHPP_DEFINE_METHOD(PathPlanner, tryConnectInitAndGoals) - .PYHPP_DEFINE_METHOD(PathPlanner, oneStep) - .PYHPP_DEFINE_METHOD(PathPlanner, finishSolve) - .PYHPP_DEFINE_METHOD(PathPlanner, interrupt) - .PYHPP_DEFINE_GETTER_SETTER_CONST_REF(PathPlanner, maxIterations, - unsigned long int) - .PYHPP_DEFINE_GETTER_SETTER_CONST_REF(PathPlanner, timeOut, double) - .PYHPP_DEFINE_METHOD(PathPlanner, stopWhenProblemIsSolved) - .PYHPP_DEFINE_METHOD(PathPlanner, computePath); + .def("roadmap", &PathPlanner::roadmap, + return_value_policy(), DocClassMethod(roadmap)) + .def("problem", &PathPlanner::problem, DocClassMethod(problem)) + .def("startSolve", &PathPlanner::startSolve, DocClassMethod(startSolve)) + .def("solve", &PathPlanner::solve, DocClassMethod(solve)) + .def("tryConnectInitAndGoals", &PathPlanner::tryConnectInitAndGoals, + DocClassMethod(tryConnectInitAndGoals)) + .def("oneStep", &PathPlanner::oneStep, DocClassMethod(oneStep)) + .def("finishSolve", &PathPlanner::finishSolve, + DocClassMethod(finishSolve)) + .def("interrupt", &PathPlanner::interrupt, DocClassMethod(interrupt)) + .def("maxIterations", + static_cast( + &PathPlanner::maxIterations)) + .def("maxIterations", + static_cast( + &PathPlanner::maxIterations)) + .def("timeOut", + static_cast(&PathPlanner::timeOut)) + .def("timeOut", static_cast( + &PathPlanner::timeOut)) + .def("stopWhenProblemIsSolved", &PathPlanner::stopWhenProblemIsSolved, + DocClassMethod(stopWhenProblemIsSolved)) + .def("computePath", &PathPlanner::computePath, + DocClassMethod(computePath)); pyhpp::core::pathPlanner::exposePathPlanners(); } diff --git a/src/pyhpp/core/path-projector.cc b/src/pyhpp/core/path-projector.cc index a001d8cb..c6678db2 100644 --- a/src/pyhpp/core/path-projector.cc +++ b/src/pyhpp/core/path-projector.cc @@ -36,6 +36,8 @@ #include #include +// DocNamespace(hpp::core) + using namespace boost::python; namespace pyhpp { @@ -56,9 +58,10 @@ struct PPWrapper { }; void exposePathProjector() { + // DocClass(PathProjector) class_("PathProjector", no_init) - .def("apply", &PPWrapper::apply) + .def("apply", &PPWrapper::apply, DocClassMethod(apply)) .def("apply", &PPWrapper::py_apply); class_, @@ -78,7 +81,7 @@ void exposePathProjector() { "RecursiveHermiteProjector", no_init); def( - "createNoneProjector", + "NoneProjector", +[](const DistancePtr_t&, const PyWSteeringMethodPtr_t&, const value_type&) -> PathProjectorPtr_t { return PathProjectorPtr_t(); @@ -86,7 +89,7 @@ void exposePathProjector() { (arg("distance"), arg("steeringMethod"), arg("step"))); def( - "createProgressiveProjector", + "ProgressiveProjector", +[](const DistancePtr_t& distance, const PyWSteeringMethodPtr_t& steeringMethodWrapper, const value_type& step) { @@ -96,7 +99,7 @@ void exposePathProjector() { (arg("distance"), arg("steeringMethod"), arg("step"))); def( - "createDichotomyProjector", + "DichotomyProjector", +[](const DistancePtr_t& distance, const PyWSteeringMethodPtr_t& steeringMethodWrapper, const value_type& step) { @@ -106,7 +109,7 @@ void exposePathProjector() { (arg("distance"), arg("steeringMethod"), arg("step"))); def( - "createGlobalProjector", + "GlobalProjector", +[](const DistancePtr_t& distance, const PyWSteeringMethodPtr_t& steeringMethodWrapper, const value_type& step) { @@ -116,7 +119,7 @@ void exposePathProjector() { (arg("distance"), arg("steeringMethod"), arg("step"))); def( - "createRecursiveHermiteProjector", + "RecursiveHermiteProjector", +[](const DistancePtr_t& distance, const PyWSteeringMethodPtr_t& steeringMethodWrapper, const value_type& step) { diff --git a/src/pyhpp/core/path-validation.cc b/src/pyhpp/core/path-validation.cc index 64fc95b0..b46db98e 100644 --- a/src/pyhpp/core/path-validation.cc +++ b/src/pyhpp/core/path-validation.cc @@ -40,6 +40,8 @@ #include #include +// DocNamespace(hpp::core) + using namespace boost::python; namespace pyhpp { @@ -79,9 +81,10 @@ struct PVWrapper { }; void exposePathValidation() { + // DocClass(PathValidation) class_( "PathValidation", no_init) - .def("validate", &PVWrapper::validate) + .def("validate", &PVWrapper::validate, DocClassMethod(validate)) .def("validate", &PVWrapper::py_validate) .def("validateConfiguration", &PVWrapper::validateConfiguration); @@ -97,20 +100,19 @@ void exposePathValidation() { hpp::core::continuousValidation::DichotomyPtr_t, boost::noncopyable>( "Dichotomy", no_init); - def("createDiscretized", &pathValidation::createDiscretizedCollisionChecking, + def("Discretized", &pathValidation::createDiscretizedCollisionChecking, (arg("robot"), arg("stepSize"))); - def("createDiscretizedCollision", + def("DiscretizedCollision", &pathValidation::createDiscretizedCollisionChecking, (arg("robot"), arg("stepSize"))); - def("createDiscretizedJointBound", - &pathValidation::createDiscretizedJointBound, + def("DiscretizedJointBound", &pathValidation::createDiscretizedJointBound, (arg("robot"), arg("stepSize"))); - def("createDiscretizedCollisionAndJointBound", + def("DiscretizedCollisionAndJointBound", &PVWrapper::createDiscretizedJointBoundAndCollisionChecking, (arg("robot"), arg("stepSize"))); - def("createProgressive", &continuousValidation::Progressive::create, + def("Progressive", &continuousValidation::Progressive::create, (arg("robot"), arg("tolerance"))); - def("createDichotomy", &continuousValidation::Dichotomy::create, + def("Dichotomy", &continuousValidation::Dichotomy::create, (arg("robot"), arg("tolerance"))); } } // namespace core diff --git a/src/pyhpp/core/path.cc b/src/pyhpp/core/path.cc index 454739e8..9892e68c 100644 --- a/src/pyhpp/core/path.cc +++ b/src/pyhpp/core/path.cc @@ -36,6 +36,8 @@ #include #include +// DocNamespace(hpp::core) + using namespace boost::python; namespace pyhpp { @@ -141,6 +143,7 @@ struct PathWrap : PathWrapper, wrapper { }; void exposePath() { + // DocClass(Path) class_, boost::noncopyable>("Path", no_init) .def("__str__", &to_str_from_operator) @@ -148,25 +151,29 @@ void exposePath() { .def("__call__", &PathWrap::py_call2) .def("eval", &PathWrap::py_call1) .def("eval", &PathWrap::py_call2) - .PYHPP_DEFINE_METHOD(PathWrap, derivative) + .def("derivative", &PathWrap::derivative) - .def("copy", static_cast(&Path::copy)) + .def("copy", static_cast(&Path::copy), + DocClassMethod(copy)) .def("extract", static_cast(&Path::extract)) + const>(&Path::extract), + DocClassMethod(extract)) .def("extract", static_cast( &Path::extract)) - // .PYHPP_DEFINE_METHOD (Path, timeRange) .def("timeRange", static_cast(&Path::timeRange), - return_internal_reference<>()) - .PYHPP_DEFINE_METHOD_INTERNAL_REF(Path, paramRange) - .PYHPP_DEFINE_METHOD(Path, length) - .PYHPP_DEFINE_METHOD(Path, initial) - .PYHPP_DEFINE_METHOD(Path, end) - .PYHPP_DEFINE_METHOD(PathWrap, constraints) - .PYHPP_DEFINE_METHOD(Path, outputSize) - .PYHPP_DEFINE_METHOD(Path, outputDerivativeSize); + return_internal_reference<>(), DocClassMethod(timeRange)) + .def("reverse", &Path::reverse, DocClassMethod(reverse)) + .def("paramRange", &Path::paramRange, return_internal_reference<>(), + DocClassMethod(paramRange)) + .def("length", &Path::length, DocClassMethod(length)) + .def("initial", &Path::initial, DocClassMethod(initial)) + .def("end", &Path::end, DocClassMethod(end)) + .def("constraints", &PathWrap::constraints) + .def("outputSize", &Path::outputSize, DocClassMethod(outputSize)) + .def("outputDerivativeSize", &Path::outputDerivativeSize, + DocClassMethod(outputDerivativeSize)); class_, hpp::shared_ptr, boost::noncopyable>( "PathWrap", no_init) diff --git a/src/pyhpp/core/path/spline.cc b/src/pyhpp/core/path/spline.cc index 2f6e69b3..5b7b0759 100644 --- a/src/pyhpp/core/path/spline.cc +++ b/src/pyhpp/core/path/spline.cc @@ -34,6 +34,8 @@ #include #include +// DocNamespace(hpp::core::path) + using namespace boost::python; namespace pyhpp { diff --git a/src/pyhpp/core/path/vector.cc b/src/pyhpp/core/path/vector.cc index 6388da39..d08b368e 100644 --- a/src/pyhpp/core/path/vector.cc +++ b/src/pyhpp/core/path/vector.cc @@ -29,10 +29,15 @@ #include #include +#include #include #include +#include #include #include +#include + +// DocNamespace(hpp::core) using namespace boost::python; @@ -41,18 +46,46 @@ namespace core { namespace path { using namespace hpp::core; +void savePathVector(PathVectorPtr_t pathVector, const std::string& filename) { + if (!pathVector) { + throw std::invalid_argument("Cannot save null PathVector"); + } + std::ofstream ofs(filename, std::ios::binary); + if (!ofs.is_open()) { + throw std::runtime_error("Failed to open file for writing: " + filename); + } + hpp::serialization::binary_oarchive ar(ofs); + ar.initialize(); + ar << hpp::serialization::make_nvp("pathVector", pathVector); +} + +PathVectorPtr_t loadPathVector(const std::string& filename) { + std::ifstream ifs(filename, std::ios::binary); + if (!ifs.is_open()) { + throw std::runtime_error("Failed to open file for reading: " + filename); + } + hpp::serialization::binary_iarchive ar(ifs); + ar.initialize(); + PathVectorPtr_t pathVector; + ar >> hpp::serialization::make_nvp("pathVector", pathVector); + return pathVector; +} + void exposeVector() { + // DocClass(PathVector) class_, boost::noncopyable>("Vector", no_init) - .def("create", static_cast( - &PathVector::create)) + .def("create", + static_cast( + &PathVector::create), + DocClassMethod(create)) .staticmethod("create") - .PYHPP_DEFINE_METHOD(PathVector, numberPaths) - .PYHPP_DEFINE_METHOD(PathVector, pathAtRank) - .PYHPP_DEFINE_METHOD(PathVector, rankAtParam) - .PYHPP_DEFINE_METHOD(PathVector, appendPath) - .PYHPP_DEFINE_METHOD(PathVector, concatenate) - .PYHPP_DEFINE_METHOD(PathVector, flatten); + .def("numberPaths", &PathVector::numberPaths, DocClassMethod(numberPaths)) + .def("pathAtRank", &PathVector::pathAtRank, DocClassMethod(pathAtRank)) + .def("rankAtParam", &PathVector::rankAtParam, DocClassMethod(rankAtParam)) + .def("appendPath", &PathVector::appendPath, DocClassMethod(appendPath)) + .def("concatenate", &PathVector::concatenate, DocClassMethod(concatenate)) + .def("flatten", &PathVector::flatten, DocClassMethod(flatten)); class_("Vectors").def( vector_indexing_suite()); diff --git a/src/pyhpp/core/problem-solver.cc b/src/pyhpp/core/problem-solver.cc deleted file mode 100644 index c025c5a4..00000000 --- a/src/pyhpp/core/problem-solver.cc +++ /dev/null @@ -1,230 +0,0 @@ -// -// Copyright (c) 2018, Joseph Mirabel -// Authors: Joseph Mirabel (joseph.mirabel@laas.fr) -// -// -// Redistribution and use in source and binary forms, with or without -// modification, are permitted provided that the following conditions -// are met: - -// 1. Redistributions of source code must retain the above copyright -// notice, this list of conditions and the following disclaimer. - -// 2. Redistributions in binary form must reproduce the above -// copyright notice, this list of conditions and the following -// disclaimer in the documentation and/or other materials provided -// with the distribution. - -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -// COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, -// INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES -// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR -// SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) -// HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, -// STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED -// OF THE POSSIBILITY OF SUCH DAMAGE. - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -// DocNamespace(hpp::core) - -using namespace boost::python; - -#define PYHPP_PROBLEMSOLVER_SELECT_TYPE(type) \ - def(#type "Type", \ - static_cast( \ - &ProblemSolver::type##Type), \ - return_value_policy()) \ - .def(#type "Type", \ - static_cast( \ - &ProblemSolver::type##Type)) - -#define PYHPP_PROBLEMSOLVER_CONTAINER(name) \ - def_readwrite(#name, &ProblemSolver::name) - -namespace pyhpp { -namespace core { -using namespace hpp::core; - -// static list goalConfigs(ProblemSolver& ps) { -// const Configurations_t cfgs = ps.goalConfigs(); -// list ret; -// for (std::size_t i = 0; i < cfgs.size(); ++i) ret.append(*cfgs[i]); -// return ret; -// } -static PathVectorPtr_t path(ProblemSolver& ps, const std::size_t& i) { - if (i > ps.paths().size()) throw std::invalid_argument("Out of range"); - return ps.paths()[i]; -} - -template > -struct Builder { - Builder(PyObject* _callable) : callable(_callable) { incref(callable); } - - ~Builder() { decref(callable); } - - static TypePtr_t call2(PyObject* c, const hpp::core::ProblemConstPtr_t& p) { - object obj = call(c, p); - TypePtr_t ptr = extract(obj); - return ptr; - } - - template - static void add_to_container(hpp::core::Container& c, - const std::string& key, PyObject* callable) { - // TODO check if incref is needed - // incref (callable); - c.add(key, T(boost::bind(&Builder::call2, callable, - boost::placeholders::_1))); - } - - PyObject* callable; -}; - -struct NotABuilder { - template - static void add_to_container(hpp::core::Container& c, - const std::string& key, const T& value) { - c.add(key, value); - } -}; - -/// \tparam Builder_t void means not a Builder. -template -struct exposeContainer { - static void run(const char* name) { - typedef hpp::core::Container C_t; - class_(name, no_init) - .PYHPP_DEFINE_METHOD(C_t, erase) - .PYHPP_DEFINE_METHOD(C_t, clear) - .def("add", &Builder_t::template add_to_container) - .PYHPP_DEFINE_METHOD(C_t, has) - // .PYHPP_DEFINE_METHOD (C_t, get) - .def( - "__getitem__", - static_cast( - &C_t::get), - return_internal_reference<>()) - // .PYHPP_DEFINE_METHOD (C_t, getKeys) - .def("keys", &C_t::template getKeys >); - } -}; - -void exposeProblemSolver() { - // DocClass (ProblemSolver) - using namespace hpp::core; - class_("ProblemSolver", no_init) - .def("create", &ProblemSolver::create, - return_value_policy(), DocClassMethod(create)) - .staticmethod("create") - .def("problem", - static_cast( - &ProblemSolver::problem), - return_value_policy()) - // .def("initConfig", static_cast(&ProblemSolver::initConfig), - // return_internal_reference<>()) - .def("path", path) - .def("initConfig", - static_cast( - &ProblemSolver::initConfig), - return_internal_reference<>()) - .def("initConfig", - static_cast( - &ProblemSolver::initConfig)) - // .PYHPP_DEFINE_METHOD2(ProblemSolver, goalConfigs, - // DocClassMethod(goalConfigs)) - .PYHPP_DEFINE_METHOD2(ProblemSolver, addGoalConfig, - DocClassMethod(addGoalConfig)) - .PYHPP_DEFINE_METHOD2(ProblemSolver, resetGoalConfigs, - DocClassMethod(resetGoalConfigs)) - .PYHPP_DEFINE_METHOD2(ProblemSolver, resetProblem, - DocClassMethod(resetProblem)) - .PYHPP_DEFINE_METHOD2(ProblemSolver, resetRoadmap, - DocClassMethod(resetRoadmap)) - .PYHPP_DEFINE_METHOD2(ProblemSolver, createPathOptimizers, - DocClassMethod(createPathOptimizers)) - .PYHPP_DEFINE_METHOD2(ProblemSolver, prepareSolveStepByStep, - DocClassMethod(prepareSolveStepByStep)) - .PYHPP_DEFINE_METHOD2(ProblemSolver, executeOneStep, - DocClassMethod(executeOneStep)) - .PYHPP_DEFINE_METHOD2(ProblemSolver, finishSolveStepByStep, - DocClassMethod(finishSolveStepByStep)) - .PYHPP_DEFINE_METHOD2(ProblemSolver, solve, DocClassMethod(solve)) - .PYHPP_DEFINE_METHOD2(ProblemSolver, optimizePath, - DocClassMethod(optimizePath)) - - .PYHPP_DEFINE_METHOD2(ProblemSolver, addPath, DocClassMethod(addPath)) - .PYHPP_DEFINE_METHOD2(ProblemSolver, erasePath, DocClassMethod(erasePath)) - .PYHPP_DEFINE_METHOD_INTERNAL_REF(ProblemSolver, paths) - .PYHPP_DEFINE_METHOD2(ProblemSolver, createRobot, - DocClassMethod(createRobot)) - .PYHPP_DEFINE_GETTER_SETTER_INTERNAL_REF(ProblemSolver, robot, - const DevicePtr_t&) - .def("addObstacle", - static_cast( - &ProblemSolver::addObstacle), - DocClassMethod(addObstacle)) - - .PYHPP_PROBLEMSOLVER_SELECT_TYPE(robot) - .PYHPP_PROBLEMSOLVER_SELECT_TYPE(pathPlanner) - .PYHPP_DEFINE_METHOD_INTERNAL_REF(ProblemSolver, pathPlanner) - .PYHPP_PROBLEMSOLVER_SELECT_TYPE(distance) - .PYHPP_PROBLEMSOLVER_SELECT_TYPE(steeringMethod) - .PYHPP_PROBLEMSOLVER_SELECT_TYPE(configurationShooter) - .def("pathValidationType", static_cast( - &ProblemSolver::pathValidationType)) - .def("pathProjectorType", static_cast( - &ProblemSolver::pathProjectorType)) - - .PYHPP_DEFINE_METHOD2(ProblemSolver, addConfigValidation, - DocClassMethod(addConfigValidation)) - .PYHPP_DEFINE_METHOD2(ProblemSolver, configValidationTypes, - DocClassMethod(configValidationTypes)) - .PYHPP_DEFINE_METHOD2(ProblemSolver, clearConfigValidations, - DocClassMethod(clearConfigValidations)) - - .PYHPP_DEFINE_METHOD2(ProblemSolver, addPathOptimizer, - DocClassMethod(addPathOptimizer)) - .PYHPP_DEFINE_METHOD2(ProblemSolver, clearPathOptimizers, - DocClassMethod(clearPathOptimizers)) - //.PYHPP_DEFINE_METHOD_INTERNAL_REF(ProblemSolver, - // pathOptimizer) - - // .PYHPP_PROBLEMSOLVER_CONTAINER(robots) - .PYHPP_PROBLEMSOLVER_CONTAINER(pathPlanners) - .PYHPP_PROBLEMSOLVER_CONTAINER(pathOptimizers) - - .PYHPP_DEFINE_GETTER_SETTER(ProblemSolver, maxIterProjection, size_type) - .PYHPP_DEFINE_GETTER_SETTER(ProblemSolver, maxIterPathPlanning, size_type) - // .PYHPP_DEFINE_GETTER_SETTER (ProblemSolver, errorThreshold , - // value_type) - .def("errorThreshold", static_cast( - &ProblemSolver::errorThreshold)) - .def("errorThreshold", - static_cast( - &ProblemSolver::errorThreshold)); - - // exposeContainer(); - exposeContainer::run("PathPlannerContainer"); - exposeContainer >::run( - "PathOptimizerContainer"); -} -} // namespace core -} // namespace pyhpp diff --git a/src/pyhpp/core/problem-target.cc b/src/pyhpp/core/problem-target.cc index 4b514d69..8256081a 100644 --- a/src/pyhpp/core/problem-target.cc +++ b/src/pyhpp/core/problem-target.cc @@ -31,6 +31,8 @@ #include #include +// DocNamespace(hpp::core) + using namespace boost::python; namespace pyhpp { @@ -39,6 +41,7 @@ namespace core { using namespace hpp::core; void exposeProblemTarget() { + // DocClass(ProblemTarget) class_("ProblemTarget", no_init) diff --git a/src/pyhpp/core/problem.cc b/src/pyhpp/core/problem.cc index 2b1f4045..e6dcb0f8 100644 --- a/src/pyhpp/core/problem.cc +++ b/src/pyhpp/core/problem.cc @@ -30,25 +30,41 @@ #include "pyhpp/core/problem.hh" +// DocNamespace(hpp::core) + #include +#include +#include +#include +#include #include +#include #include #include #include #include #include #include +#include #include #include #include +#include +#include #include + namespace pyhpp { namespace core { using namespace boost::python; Problem::Problem(const DevicePtr_t& robot) - : obj(hpp::core::Problem::create(robot)) {} + : obj(hpp::core::Problem::create(robot)), + errorThreshold_(1e-4), + maxIterProjection_(20) { + constraints_ = + hpp::core::ConstraintSet::create(robot, "Default constraint set"); +} const DevicePtr_t& Problem::robot() const { return obj->robot(); } @@ -140,6 +156,365 @@ void Problem::addGoalConfig(ConfigurationIn_t config) { void Problem::resetGoalConfigs() { obj->resetGoalConfigs(); } +void Problem::addPartialCom(const std::string& name, + boost::python::list pyjointNames) { + try { + hpp::pinocchio::CenterOfMassComputationPtr_t comc = + hpp::pinocchio::CenterOfMassComputation::create(robot()); + auto jointNames = extract_vector(pyjointNames); + + for (long unsigned int i = 0; i < jointNames.size(); ++i) { + std::string name(jointNames[i]); + hpp::pinocchio::JointPtr_t j = robot()->getJointByName(name); + if (!j) throw std::logic_error("One joint not found."); + comc->add(j); + } + centerOfMassComputations[name] = comc; + } catch (const std::exception& exc) { + throw std::logic_error(exc.what()); + } +} + +hpp::pinocchio::vector3_t Problem::getPartialCom(const std::string& name) { + try { + if (!centerOfMassComputations[name]) { + throw std::logic_error("Partial COM " + name + " not found."); + } + hpp::pinocchio::CenterOfMassComputationPtr_t comc = + centerOfMassComputations[name]; + comc->compute(hpp::pinocchio::COM); + return comc->com(); + } catch (const std::exception& exc) { + throw std::logic_error(exc.what()); + } +} + +static int numberOfTrue(const std::vector& mask) { + std::size_t res = 0; + for (std::size_t i = 0; i < mask.size(); ++i) + if (mask[i]) ++res; + return (int)res; +} + +hpp::constraints::ImplicitPtr_t Problem::createRelativeComConstraint( + const char* constraintName, const char* comName, const char* jointName, + hpp::pinocchio::vector3_t point, boost::python::list mask) { + hpp::pinocchio::JointPtr_t joint; + hpp::pinocchio::CenterOfMassComputationPtr_t comc; + + try { + joint = robot()->getJointByName(jointName); + auto m = extract_vector(mask); + // Test whether joint1 is world frame + std::string name(constraintName), comN(comName); + if (comN.compare("") == 0) { + return hpp::constraints::Implicit::create( + hpp::constraints::RelativeCom::create(name, robot(), joint, point, m), + numberOfTrue(m) * hpp::constraints::EqualToZero); + } else { + if (!centerOfMassComputations[comN]) + throw std::logic_error("Partial COM not found."); + comc = centerOfMassComputations[comN]; + return hpp::constraints::Implicit::create( + hpp::constraints::RelativeCom::create(name, robot(), comc, joint, + point, m), + numberOfTrue(m) * hpp::constraints::EqualToZero); + } + } catch (const std::exception& exc) { + throw std::logic_error(exc.what()); + } +} + +hpp::constraints::ImplicitPtr_t Problem::createTransformationConstraint( + const char* constraintName, const char* joint1Name, const char* joint2Name, + const hpp::pinocchio::Transform3s& M, boost::python::list m) { + try { + if (!robot()) throw std::logic_error("No robot loaded"); + using hpp::constraints::GenericTransformation; + using hpp::constraints::Implicit; + using hpp::constraints::OrientationBit; + using hpp::constraints::PositionBit; + + std::string name(constraintName); + hpp::pinocchio::Frame f2 = robot()->getFrameByName(joint2Name); + auto mask = extract_vector(m); + const hpp::pinocchio::Transform3s ref2 = f2.positionInParentJoint() * M; + if (joint1Name && std::strlen(joint1Name) > 0) { + hpp::pinocchio::Frame f1 = robot()->getFrameByName(joint1Name); + const hpp::pinocchio::Transform3s ref1 = f1.positionInParentJoint(); + auto func = GenericTransformation< + PositionBit | OrientationBit | + hpp::constraints::RelativeBit>::create(name, robot(), f1.joint(), + f2.joint(), ref1, ref2, mask); + return Implicit::create( + func, numberOfTrue(mask) * hpp::constraints::EqualToZero); + } else { + const hpp::pinocchio::Transform3s Id = + hpp::pinocchio::Transform3s::Identity(); + auto func = GenericTransformation::create( + name, robot(), f2.joint(), ref2, Id, mask); + return Implicit::create( + func, numberOfTrue(mask) * hpp::constraints::EqualToZero); + } + } catch (const std::exception& exc) { + throw std::logic_error(exc.what()); + } +} + +hpp::constraints::ImplicitPtr_t Problem::createTransformationConstraint2( + const char* constraintName, const char* joint1Name, const char* joint2Name, + const hpp::pinocchio::Transform3s& M1, + const hpp::pinocchio::Transform3s& M2, const boost::python::list m) { + try { + if (!robot()) throw std::logic_error("No robot loaded"); + + using hpp::constraints::GenericTransformation; + using hpp::constraints::Implicit; + using hpp::constraints::OrientationBit; + using hpp::constraints::PositionBit; + auto mask = extract_vector(m); + + std::string name(constraintName); + + hpp::pinocchio::Frame f2 = robot()->getFrameByName(joint2Name); + const hpp::pinocchio::Transform3s ref2 = f2.positionInParentJoint() * M2; + + if (joint1Name && std::strlen(joint1Name) > 0) { + hpp::pinocchio::Frame f1 = robot()->getFrameByName(joint1Name); + const hpp::pinocchio::Transform3s ref1 = f1.positionInParentJoint() * M1; + auto func = GenericTransformation< + PositionBit | OrientationBit | + hpp::constraints::RelativeBit>::create(name, robot(), f1.joint(), + f2.joint(), ref1, ref2, mask); + return Implicit::create( + func, numberOfTrue(mask) * hpp::constraints::EqualToZero); + } else { + auto func = GenericTransformation::create( + name, robot(), f2.joint(), ref2, M1, mask); + return Implicit::create( + func, numberOfTrue(mask) * hpp::constraints::EqualToZero); + } + } catch (const std::exception& exc) { + throw std::logic_error(exc.what()); + } +} + +void Problem::setConstantRightHandSide( + hpp::constraints::ImplicitPtr_t constraint, bool constant) { + try { + if (constant) { + hpp::constraints::ComparisonTypes_t eqtypes( + constraint->function().outputDerivativeSize(), + hpp::constraints::EqualToZero); + constraint->comparisonType(eqtypes); + } else { + hpp::constraints::ComparisonTypes_t eqtypes( + constraint->function().outputDerivativeSize(), + hpp::constraints::Equality); + constraint->comparisonType(eqtypes); + } + } catch (const std::exception& exc) { + throw std::logic_error(exc.what()); + } +} + +boost::python::tuple Problem::applyConstraints(ConfigurationIn_t config) { + bool success = false; + double residualError = 0.0; + try { + Configuration_t output = config; + success = constraints_->apply(output); + if (hpp::core::ConfigProjectorPtr_t configProjector = + constraints_->configProjector()) { + residualError = configProjector->residualError(); + } + return boost::python::make_tuple(success, output, residualError); + } catch (const std::exception& exc) { + throw std::logic_error(exc.what()); + } +} + +boost::python::tuple Problem::isConfigValid(ConfigurationIn_t dofArray) { + try { + Configuration_t config = dofArray; + hpp::core::ValidationReportPtr_t validationReport; + bool validity = + obj->configValidations()->validate(config, validationReport); + + std::string report; + if (validity) { + report = ""; + } else { + std::ostringstream oss; + oss << *validationReport; + report = oss.str(); + } + return boost::python::make_tuple(validity, report); + } catch (const std::exception& exc) { + throw std::logic_error(exc.what()); + } +} + +void Problem::setConstraints(hpp::core::ConstraintSetPtr_t constraints) { + try { + constraints_ = + hpp::core::ConstraintSet::create(robot(), "Default constraint set"); + constraints_->addConstraint(constraints); + obj->constraints(constraints_); + } catch (const std::exception& exc) { + throw std::logic_error(exc.what()); + } +} + +void Problem::setRightHandSideFromConfig(ConfigurationIn_t configIn) { + try { + hpp::core::ConfigProjectorPtr_t configProjector( + constraints_->configProjector()); + if (!configProjector) { + throw std::runtime_error("No constraint has been set."); + } + Configuration_t q = configIn; + configProjector->rightHandSideFromConfig(q); + } catch (const std::exception& exc) { + throw std::logic_error(exc.what()); + } +} + +void Problem::addNumericalConstraintsToConfigProjector1( + const char* configProjName, boost::python::list constraints, + boost::python::list priorities) { + try { + auto constraintsVec = + extract_vector(constraints); + auto prioritiesVec = extract_vector(priorities); + hpp::core::ConfigProjectorPtr_t configProjector = + constraints_->configProjector(); + for (unsigned int i = 0; i < constraintsVec.size(); ++i) { + if (!configProjector) { + configProjector = hpp::core::ConfigProjector::create( + robot(), configProjName, errorThreshold_, maxIterProjection_); + constraints_->addConstraint(configProjector); + } + configProjector->add(constraintsVec[i], prioritiesVec[i]); + } + } catch (const std::exception& exc) { + throw std::logic_error(exc.what()); + } +} + +void Problem::addNumericalConstraintsToConfigProjector2( + const char* configProjName, boost::python::list constraints) { + try { + auto constraintsVec = + extract_vector(constraints); + hpp::core::ConfigProjectorPtr_t configProjector = + constraints_->configProjector(); + for (unsigned int i = 0; i < constraintsVec.size(); ++i) { + if (!configProjector) { + configProjector = hpp::core::ConfigProjector::create( + robot(), configProjName, errorThreshold_, maxIterProjection_); + constraints_->addConstraint(configProjector); + } + configProjector->add(constraintsVec[i], 0); + } + } catch (const std::exception& exc) { + throw std::logic_error(exc.what()); + } +} + +hpp::constraints::ImplicitPtr_t Problem::createComBetweenFeet( + const char* constraintName, const char* comName, const char* jointLName, + const char* jointRName, const hpp::pinocchio::vector3_t& pointL, + const hpp::pinocchio::vector3_t& pointR, const char* jointRefName, + const hpp::pinocchio::vector3_t& pointRef, boost::python::list mask) { + if (!robot()) throw std::logic_error("No robot loaded"); + + try { + hpp::pinocchio::JointPtr_t jointL, jointR, jointRef; + hpp::pinocchio::CenterOfMassComputationPtr_t comc; + + auto m = extract_vector(mask); + + jointL = robot()->getJointByName(jointLName); + jointR = robot()->getJointByName(jointRName); + + if (std::string(jointRefName) == std::string("")) + jointRef = robot()->rootJoint(); + else + jointRef = robot()->getJointByName(jointRefName); + + std::string name(constraintName), comN(comName); + + hpp::constraints::ComparisonTypes_t comps(2, hpp::constraints::EqualToZero); + comps.push_back(hpp::constraints::Superior); + comps.push_back(hpp::constraints::Inferior); + + if (comN.compare("") == 0) { + return hpp::constraints::Implicit::create( + hpp::constraints::ComBetweenFeet::create(name, robot(), jointL, + jointR, pointL, pointR, + jointRef, pointRef, m), + comps); + } else { + if (!centerOfMassComputations[comN]) + throw std::logic_error("Partial COM " + comN + " not found."); + comc = centerOfMassComputations[comN]; + return hpp::constraints::Implicit::create( + hpp::constraints::ComBetweenFeet::create(name, robot(), comc, jointL, + jointR, pointL, pointR, + jointRef, pointRef, m), + comps); + } + } catch (const std::exception& exc) { + throw std::logic_error(exc.what()); + } +} + +boost::python::tuple Problem::directPath(ConfigurationIn_t start, + ConfigurationIn_t end, bool validate) { + std::string report = ""; + + if (!obj) throw std::runtime_error("The problem is not defined."); + + SteeringMethodPtr_t sm(obj->steeringMethod()); + PathPtr_t dp = (*sm)(start, end); + if (!dp) { + report = "Steering method failed to build a path."; + return boost::python::make_tuple(false, nullptr, report); + } + + PathPtr_t dp1, dp2; + hpp::core::PathValidationReportPtr_t r; + bool projValid = true, projected = true, pathValid = true; + + if (validate) { + PathProjectorPtr_t proj(obj->pathProjector()); + if (proj) { + projected = proj->apply(dp, dp1); + } else { + dp1 = dp; + } + projValid = obj->pathValidation()->validate(dp1, false, dp2, r); + pathValid = projValid && projected; + if (!projValid) { + if (r) { + std::ostringstream oss; + oss << *r; + report = oss.str(); + } else { + report = "No path validation report."; + } + } + } else { + dp2 = dp; + } + + hpp::core::PathVectorPtr_t path(hpp::core::PathVector::create( + dp2->outputSize(), dp2->outputDerivativeSize())); + path->appendPath(dp2); + return boost::python::make_tuple(pathValid, path, report); +} + typedef PyWSteeringMethodPtr_t (Problem::*GetSteeringMethod)() const; typedef void (Problem::*SetSteeringMethod)(const PyWSteeringMethodPtr_t&); @@ -220,12 +595,16 @@ void register_problem_converters() { void exposeProblem() { class_("CppCoreProblem", no_init); + // DocClass(Problem) class_("Problem", init()) - .PYHPP_DEFINE_METHOD_CONST_REF_BY_VALUE(Problem, robot) - .PYHPP_DEFINE_METHOD(Problem, setParameter) + .def("robot", &Problem::robot, + return_value_policy(), DocClassMethod(robot)) + .def("setParameter", &Problem::setParameter, DocClassMethod(setParameter)) .def("setParameter", &Problem::setParameterFloat) .def("setParameter", &Problem::setParameterInt) - .PYHPP_DEFINE_METHOD_CONST_REF_BY_VALUE(Problem, getParameter) + .def("getParameter", &Problem::getParameter, + return_value_policy(), + DocClassMethod(getParameter)) .def("steeringMethod", static_cast( &Problem::steeringMethod)) @@ -240,8 +619,10 @@ void exposeProblem() { static_cast(&Problem::configValidation), (arg("configValidation"))) - .def("addConfigValidation", &Problem::addConfigValidation) - .def("clearConfigValidations", &Problem::clearConfigValidations) + .def("addConfigValidation", &Problem::addConfigValidation, + DocClassMethod(addConfigValidation)) + .def("clearConfigValidations", &Problem::clearConfigValidations, + DocClassMethod(clearConfigValidations)) .def("pathValidation", static_cast(&Problem::pathValidation)) @@ -267,9 +648,32 @@ void exposeProblem() { .def("configurationShooter", static_cast(&Problem::configurationShooter), (arg("configurationShooter"))) - .PYHPP_DEFINE_METHOD(Problem, initConfig) - .PYHPP_DEFINE_METHOD(Problem, addGoalConfig) - .PYHPP_DEFINE_METHOD(Problem, resetGoalConfigs); + .def("initConfig", &Problem::initConfig, DocClassMethod(initConfig)) + .def("addGoalConfig", &Problem::addGoalConfig, + DocClassMethod(addGoalConfig)) + .def("resetGoalConfigs", &Problem::resetGoalConfigs, + DocClassMethod(resetGoalConfigs)) + .PYHPP_DEFINE_METHOD(Problem, addPartialCom) + .PYHPP_DEFINE_METHOD(Problem, getPartialCom) + .PYHPP_DEFINE_METHOD(Problem, createRelativeComConstraint) + .def("createTransformationConstraint", + &Problem::createTransformationConstraint) + .def("createTransformationConstraint", + &Problem::createTransformationConstraint2) + .PYHPP_DEFINE_METHOD(Problem, setConstantRightHandSide) + .PYHPP_DEFINE_METHOD(Problem, applyConstraints) + .PYHPP_DEFINE_METHOD(Problem, isConfigValid) + .PYHPP_DEFINE_METHOD(Problem, setConstraints) + .PYHPP_DEFINE_METHOD(Problem, setRightHandSideFromConfig) + .def("addNumericalConstraintsToConfigProjector", + &Problem::addNumericalConstraintsToConfigProjector1) + .def("addNumericalConstraintsToConfigProjector", + &Problem::addNumericalConstraintsToConfigProjector2) + .def_readwrite("errorThreshold", &Problem::errorThreshold_) + .def_readwrite("maxIterProjection", &Problem::maxIterProjection_) + .PYHPP_DEFINE_METHOD(Problem, createComBetweenFeet) + .PYHPP_DEFINE_METHOD(Problem, directPath); + register_problem_converters(); } diff --git a/src/pyhpp/core/reports.cc b/src/pyhpp/core/reports.cc index 8ee49c50..b12e1a42 100644 --- a/src/pyhpp/core/reports.cc +++ b/src/pyhpp/core/reports.cc @@ -37,6 +37,8 @@ #include #include +// DocNamespace(hpp::core) + using namespace boost::python; namespace pyhpp { @@ -44,16 +46,19 @@ namespace core { using namespace hpp::core; void exposeReports() { + // DocClass(ValidationReport) class_( "ValidationReport", no_init) .def("__str__", &to_str); + // DocClass(CollisionValidationReport) class_ >("CollisionValidationReport", no_init) .def_readonly("object1", &CollisionValidationReport::object1) .def_readonly("object2", &CollisionValidationReport::object2) .def_readonly("result", &CollisionValidationReport::result); + // DocClass(JointBoundValidationReport) class_ >("JointBoundValidationReport", no_init) .def_readonly("joint_", &JointBoundValidationReport::joint_) @@ -62,12 +67,14 @@ void exposeReports() { .def_readonly("upperBound_", &JointBoundValidationReport::upperBound_) .def_readonly("value_", &JointBoundValidationReport::value_); + // DocClass(PathValidationReport) class_ >("PathValidationReport", no_init) .def_readwrite("parameter", &PathValidationReport::parameter) .def_readwrite("configurationReport", &PathValidationReport::configurationReport); + // DocClass(CollisionPathValidationReport) class_ >("CollisionPathValidationReport", no_init) diff --git a/src/pyhpp/core/roadmap.cc b/src/pyhpp/core/roadmap.cc index f2151b0d..3e846343 100644 --- a/src/pyhpp/core/roadmap.cc +++ b/src/pyhpp/core/roadmap.cc @@ -28,10 +28,13 @@ // OF THE POSSIBILITY OF SUCH DAMAGE. #include +#include #include #include #include +// DocNamespace(hpp::core) + using namespace boost::python; namespace pyhpp { @@ -47,14 +50,42 @@ using namespace hpp::core; struct RWrapper { static void addNodeAndEdges(Roadmap& roadmap, const ConfigurationIn_t from, ConfigurationIn_t to, const PathPtr_t path) { - NodePtr_t nodeFrom = roadmap.addNode(from); + value_type d; + NodePtr_t nodeFrom = roadmap.nearestNode(from, d); + if (d > 0) { + std::ostringstream os; + os << "Roadmap::addNodeAndEdge: initial configuration (" << from + << ") not in the roadmap"; + throw std::logic_error(os.str().c_str()); + } roadmap.addNodeAndEdges(nodeFrom, to, path); } + static ConnectedComponentPtr_t connectedComponentOfNode( + Roadmap& roadmap, const ConfigurationIn_t q) { + value_type d; + NodePtr_t node = roadmap.nearestNode(q, d); + if (d > 0) { + std::ostringstream os; + os << "Roadmap::connectedComponentOfNode: input configuration (" << q + << ") not in the roadmap"; + throw std::logic_error(os.str().c_str()); + } + return node->connectedComponent(); + } + static void addNodeAndEdge(Roadmap& roadmap, const ConfigurationIn_t from, ConfigurationIn_t to, const PathPtr_t path) { - NodePtr_t nodeFrom = roadmap.addNode(from); - roadmap.addNodeAndEdge(nodeFrom, to, path); + value_type d; + NodePtr_t nodeFrom = roadmap.nearestNode(from, d); + if (d > 0) { + std::ostringstream os; + os << "Roadmap::addNodeAndEdge: initial configuration (" << from + << ") not in the roadmap"; + throw std::logic_error(os.str().c_str()); + } + NodePtr_t nodeTo = roadmap.addNode(to); + roadmap.addEdge(nodeFrom, nodeTo, path); } static void addNode(Roadmap& roadmap, const ConfigurationIn_t config) { @@ -69,6 +100,20 @@ struct RWrapper { return; } + static void addEdge2(Roadmap& roadmap, const ConfigurationIn_t from, + ConfigurationIn_t to, const PathPtr_t& path, + bool bothEdges) { + NodePtr_t nodeFrom = roadmap.addNode(from); + NodePtr_t nodeTo = roadmap.addNode(to); + if (bothEdges) { + roadmap.addEdge(nodeFrom, nodeTo, path); + roadmap.addEdge(nodeTo, nodeFrom, path->reverse()); + return; + } + roadmap.addEdge(nodeFrom, nodeTo, path); + return; + } + static boost::python::tuple nearestNode1(Roadmap& roadmap, ConfigurationIn_t configuration, bool reverse) { @@ -140,50 +185,87 @@ struct RWrapper { } return to_python_list(res); } + + static boost::python::list nodesConnectedComponent(Roadmap& roadmap, + int connectedComponentId) { + try { + const ConnectedComponents_t& connectedComponents = + roadmap.connectedComponents(); + + if ((std::size_t)connectedComponentId >= connectedComponents.size()) { + std::ostringstream oss; + oss << "connectedComponentId=" << connectedComponentId + << " out of range [0," << connectedComponents.size() - 1 << "]."; + throw std::runtime_error(oss.str()); + } + + ConnectedComponents_t::const_iterator itcc = connectedComponents.begin(); + std::advance(itcc, connectedComponentId); + + const NodeVector_t& nodes = (*itcc)->nodes(); + + Configurations_t res; + res.reserve(nodes.size()); + + for (const auto& node : nodes) { + res.push_back(node->configuration()); + } + + return to_python_list(res); + + } catch (const std::exception& exc) { + throw std::runtime_error(exc.what()); + } + } }; void exposeRoadmap() { + // DocClass(Roadmap) class_("Roadmap", no_init) - .def("create", &Roadmap::create) - .staticmethod("create") + .def("__init__", make_constructor(&Roadmap::create)) .def("__str__", &to_str) - .PYHPP_DEFINE_METHOD(Roadmap, clear) - .PYHPP_DEFINE_METHOD1(RWrapper, addNode, - return_value_policy()) + .def("clear", &Roadmap::clear, DocClassMethod(clear)) + .def("addNode", &RWrapper::addNode, + return_value_policy()) .def("nearestNode", &RWrapper::nearestNode1) .def("nearestNode", &RWrapper::nearestNode2) .def("nearestNode", &RWrapper::nearestNode3) .def("nearestNode", &RWrapper::nearestNode4) .def("nearestNodes", &RWrapper::nearestNodes1) .def("nearestNodes", &RWrapper::nearestNodes2) - .PYHPP_DEFINE_METHOD(Roadmap, nodesWithinBall) - .PYHPP_DEFINE_METHOD1(RWrapper, addNodeAndEdges, - return_value_policy()) - .PYHPP_DEFINE_METHOD1(RWrapper, addNodeAndEdge, - return_value_policy()) - .PYHPP_DEFINE_METHOD(RWrapper, addEdge) - .PYHPP_DEFINE_METHOD(Roadmap, addEdges) + .def("nodesWithinBall", &Roadmap::nodesWithinBall, + DocClassMethod(nodesWithinBall)) + .def("addNodeAndEdges", &RWrapper::addNodeAndEdges, + return_value_policy()) + .def("addNodeAndEdge", &RWrapper::addNodeAndEdge, + return_value_policy()) + .def("addEdge", &RWrapper::addEdge) + .def("addEdge", &RWrapper::addEdge2) + .def("addEdges", &Roadmap::addEdges, DocClassMethod(addEdges)) .def("merge", - static_cast(&Roadmap::merge)) - .PYHPP_DEFINE_METHOD(Roadmap, insertPathVector) - .PYHPP_DEFINE_METHOD1(Roadmap, addGoalNode, - return_value_policy()) - .PYHPP_DEFINE_METHOD(Roadmap, resetGoalNodes) - .PYHPP_DEFINE_METHOD(Roadmap, pathExists) + static_cast(&Roadmap::merge), + DocClassMethod(merge)) + .def("insertPathVector", &Roadmap::insertPathVector, + DocClassMethod(insertPathVector)) + .def("addGoalNode", &Roadmap::addGoalNode, + return_value_policy(), + DocClassMethod(addGoalNode)) + .def("resetGoalNodes", &Roadmap::resetGoalNodes, + DocClassMethod(resetGoalNodes)) + .def("pathExists", &Roadmap::pathExists, DocClassMethod(pathExists)) .def("nodes", &RWrapper::nodes) + .def("nodesConnectedComponent", &RWrapper::nodesConnectedComponent) .def("initNode", &RWrapper::initNode1) .def("initNode", &RWrapper::initNode2, return_value_policy()) - .PYHPP_DEFINE_METHOD_INTERNAL_REF(Roadmap, goalNodes) + .def("goalNodes", &Roadmap::goalNodes, return_internal_reference<>(), + DocClassMethod(goalNodes)) .def("connectedComponents", &RWrapper::connectedComponents) - .PYHPP_DEFINE_METHOD_INTERNAL_REF(Roadmap, distance) + .def("distance", &Roadmap::distance, return_internal_reference<>(), + DocClassMethod(distance)) .def("numberConnectedComponents", &RWrapper::numberConnectedComponents) .def("getConnectedComponent", &RWrapper::getConnectedComponent) - // .def("cost", &RWrapper::cost1) - // .def("cost", &RWrapper::cost2, - // return_value_policy()) - - ; + .def("connectedComponentOfNode", &RWrapper::connectedComponentOfNode); } } // namespace core } // namespace pyhpp diff --git a/src/pyhpp/core/static_stability_constraint_factory.py b/src/pyhpp/core/static_stability_constraint_factory.py new file mode 100644 index 00000000..cc9010bd --- /dev/null +++ b/src/pyhpp/core/static_stability_constraint_factory.py @@ -0,0 +1,215 @@ +#!/usr/bin/env python +# +# Copyright (c) 2025 CNRS +# Author: Paul Sardin +# + +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are +# met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in the +# documentation and/or other materials provided with the distribution. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR +# A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT +# HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, +# SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT +# LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, +# DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY +# THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +# (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +# OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH +# DAMAGE. + +import numpy as np +from hpp import Transform +from pinocchio import SE3 + + +# # This class provides tools to create static stability constraints +class StaticStabilityConstraintsFactory: + def __init__(self, problem, robot): + self.problem = problem + self.robot = robot + + def _getCOM(self, com): + if com == "": + return np.array(self.robot.getCenterOfMass()) + else: + return np.array(self.problem.getPartialCom(com)) + + # # Create static stability constraints where the robot slides on the ground, + # # \param prefix prefix of the names of the constraint + # # \param comName name of the PartialCOM. Put "" for + # # a full COM computations. + # # \param leftAnkle, rightAnkle: names of the ankle joints. + # # \param q0 input configuration for computing constraint reference, + # # \return a list of the names of the created constraints + + def createSlidingStabilityConstraint( + self, prefix, comName, leftAnkle, rightAnkle, q0 + ): + created_constraints = dict() + + _tfs = self.robot.getJointsPosition(q0, [leftAnkle, rightAnkle]) + Ml = Transform(_tfs[0]) + Mr = Transform(_tfs[1]) + self.robot.currentConfiguration(q0) + x = self._getCOM(comName) + result = [] + # COM wrt left ankle frame + xloc = Ml.inverse().transform(x) + result.append(prefix + "relative-com") + constraint = self.problem.createRelativeComConstraint( + result[-1], comName, leftAnkle, xloc, [True] * 3 + ) + created_constraints[result[-1]] = constraint + + result.append(prefix + "relative-pose") + rel_transform = Mr.inverse() * Ml + constraint = self.problem.createTransformationConstraint( + result[-1], + leftAnkle, + rightAnkle, + SE3.Identity(), + SE3(rel_transform.quaternion.toRotationMatrix(), rel_transform.translation), + [True] * 6, + ) + created_constraints[result[-1]] = constraint + + result.append(prefix + "pose-left-foot") + constraint = self.problem.createTransformationConstraint( + result[-1], + "", + leftAnkle, + SE3(Ml.quaternion.toRotationMatrix(), Ml.translation), + SE3.Identity(), + [False, False, True, True, True, False], + ) + created_constraints[result[-1]] = constraint + + result.append(prefix + "pose-left-foot-complement") + constraint = self.problem.createTransformationConstraint( + result[-1], + "", + leftAnkle, + SE3(Ml.quaternion.toRotationMatrix(), Ml.translation), + SE3.Identity(), + [True, True, False, False, False, True], + ) + self.problem.setConstantRightHandSide(constraint, False) + created_constraints[result[-1]] = constraint + + return created_constraints + + # # # Create static stability constraints where the feet are fixed on the ground, + def createStaticStabilityConstraint( + self, prefix, comName, leftAnkle, rightAnkle, q0, maskCom=[True] * 3 + ): + created_constraints = dict() + + _tfs = self.robot.getJointsPosition(q0, [leftAnkle, rightAnkle]) + Ml = Transform(_tfs[0]) + Mr = Transform(_tfs[1]) + self.robot.currentConfiguration(q0) + x = self._getCOM(comName) + result = [] + + # COM wrt left ankle frame + xloc = Ml.inverse().transform(x) + result.append(prefix + "relative-com") + constraint = self.problem.createRelativeComConstraint( + result[-1], comName, leftAnkle, xloc, maskCom + ) + created_constraints[result[-1]] = constraint + + # Pose of the left foot + result.append(prefix + "pose-left-foot") + constraint = self.problem.createTransformationConstraint( + result[-1], + "", + leftAnkle, + SE3(Ml.quaternion.toRotationMatrix(), Ml.translation), + SE3.Identity(), + [True] * 6, + ) + created_constraints[result[-1]] = constraint + + # Pose of the right foot + result.append(prefix + "pose-right-foot") + constraint = self.problem.createTransformationConstraint( + result[-1], + "", + rightAnkle, + SE3(Mr.quaternion.toRotationMatrix(), Mr.translation), + SE3.Identity(), + [True] * 6, + ) + created_constraints[result[-1]] = constraint + + return created_constraints + + def createAlignedCOMStabilityConstraint( + self, prefix, comName, leftAnkle, rightAnkle, q0, sliding + ): + created_constraints = dict() + + _tfs = self.robot.getJointsPosition(q0, [leftAnkle, rightAnkle]) + Ml = Transform(_tfs[0]) + Mr = Transform(_tfs[1]) + self.robot.currentConfiguration(q0) + x = self._getCOM(comName) + result = [] + + # COM between feet + result.append(prefix + "com-between-feet") + constraint = self.problem.createComBetweenFeet( + result[-1], + comName, + leftAnkle, + rightAnkle, + np.zeros(3), + np.zeros(3), + "", + x, + [True] * 4, + ) + created_constraints[result[-1]] = constraint + + if sliding: + mask = [False, False, True, True, True, False] + else: + mask = [True] * 6 + + # Pose of the right foot + result.append(prefix + "pose-right-foot") + constraint = self.problem.createTransformationConstraint( + result[-1], + "", + rightAnkle, + SE3(Mr.quaternion.toRotationMatrix(), Mr.translation), + SE3.Identity(), + mask, + ) + created_constraints[result[-1]] = constraint + + # Pose of the left foot + result.append(prefix + "pose-left-foot") + constraint = self.problem.createTransformationConstraint( + result[-1], + "", + leftAnkle, + SE3(Ml.quaternion.toRotationMatrix(), Ml.translation), + SE3.Identity(), + mask, + ) + created_constraints[result[-1]] = constraint + + return created_constraints diff --git a/src/pyhpp/core/steering-method.cc b/src/pyhpp/core/steering-method.cc index 8f46935b..e2e3532d 100644 --- a/src/pyhpp/core/steering-method.cc +++ b/src/pyhpp/core/steering-method.cc @@ -39,6 +39,8 @@ #include #include +// DocNamespace(hpp::core) + namespace pyhpp { namespace core { @@ -140,13 +142,15 @@ const ConstraintSetPtr_t& SteeringMethod::constraints() const { void exposeSteeringMethod() { register_ptr_to_python>(); + // DocClass(SteeringMethod) class_("SteeringMethod", no_init) .def("__call__", &SteeringMethod::operator()) - .def("steer", &SteeringMethod::steer) - .def("problem", &SteeringMethod::problem) + .def("steer", &SteeringMethod::steer, DocClassMethod(steer)) + .def("problem", &SteeringMethod::problem, DocClassMethod(problem)) .def("constraints", static_cast( - &SteeringMethod::constraints)) + &SteeringMethod::constraints), + DocClassMethod(constraints)) .def("constraints", static_cast( &SteeringMethod::constraints), diff --git a/src/pyhpp/manipulation/bindings.cc b/src/pyhpp/manipulation/bindings.cc index f42bb4f4..9d280791 100644 --- a/src/pyhpp/manipulation/bindings.cc +++ b/src/pyhpp/manipulation/bindings.cc @@ -36,11 +36,13 @@ BOOST_PYTHON_MODULE(bindings) { boost::python::import("pyhpp.pinocchio"); boost::python::import("pyhpp.constraints"); + boost::python::import("pyhpp.core"); pyhpp::manipulation::exposeHandle(); pyhpp::manipulation::exposeProblem(); pyhpp::manipulation::exposeDevice(); pyhpp::manipulation::exposeGraph(); pyhpp::manipulation::exposePathProjector(); - pyhpp::manipulation::exposeGraphSteeringMethod(); + pyhpp::manipulation::exposeManipSteeringMethod(); pyhpp::manipulation::exposePathPlanners(); + pyhpp::manipulation::exposePathOptimizers(); } diff --git a/src/pyhpp/manipulation/constraint_graph_factory.py b/src/pyhpp/manipulation/constraint_graph_factory.py index 6847d779..1e537dbe 100644 --- a/src/pyhpp/manipulation/constraint_graph_factory.py +++ b/src/pyhpp/manipulation/constraint_graph_factory.py @@ -57,7 +57,7 @@ def __init__(self, grasps=[], pregrasps=[], numConstraints=[], lockedJoints=[]): warn( "argument lockedJoints in constructor of class " - + "hpp.corbaserver.manipulation.constraints.Constraints " + + "pyhpp.manipulation.constraint_graph_factory.Constraints " + "is deprecated. Locked joints are handled as numerical " + "constraints." ) @@ -769,9 +769,7 @@ def buildPlacement(self, o): ljs.append(n) q = self.graph.robot.getJointConfig(n) self.registerConstraint( - LockedJoint.create( - self.graph.robot.asPinDevice(), n, np.array(q) - ), + LockedJoint(self.graph.robot, n, np.array(q)), n, ) return dict( diff --git a/src/pyhpp/manipulation/device.cc b/src/pyhpp/manipulation/device.cc index 9c67a3c2..b07ce75c 100644 --- a/src/pyhpp/manipulation/device.cc +++ b/src/pyhpp/manipulation/device.cc @@ -32,58 +32,28 @@ #include #include +// DocNamespace(hpp::manipulation) + namespace pyhpp { namespace manipulation { using namespace boost::python; -Device::Device(const hpp::manipulation::DevicePtr_t& object) { obj = object; } Device::Device(const std::string& name) - : obj(hpp::manipulation::Device::create(name)) {} -// Methods from hpp::pinocchio::Device -const std::string& Device::name() const { return obj->name(); } -const LiegroupSpacePtr_t& Device::configSpace() const { - return obj->configSpace(); -} -Model& Device::model() { return obj->model(); } -Data& Device::data() { return obj->data(); } -GeomData& Device::geomData() { return obj->geomData(); } -GeomModel& Device::geomModel() { return obj->geomModel(); } -GeomModel& Device::visualModel() { return obj->visualModel(); } - -size_type Device::configSize() const { return obj->configSize(); } -size_type Device::numberDof() const { return obj->numberDof(); } -const Configuration_t& Device::currentConfiguration() const { - return obj->currentConfiguration(); -} -bool Device::currentConfiguration(ConfigurationIn_t configuration) { - return obj->currentConfiguration(configuration); -} -void Device::computeForwardKinematics(int flag) { - return obj->computeForwardKinematics(flag); -} -void Device::computeFramesForwardKinematics() { - return obj->computeFramesForwardKinematics(); -} -void Device::updateGeometryPlacements() { - return obj->updateGeometryPlacements(); -} -// Methods for hpp::manipulation::Device + : pyhpp::pinocchio::Device(hpp::manipulation::Device::create(name)) {} + void Device::setRobotRootPosition(const std::string& robotName, const Transform3s& positionWRTParentJoint) { - return obj->setRobotRootPosition(robotName, positionWRTParentJoint); + std::dynamic_pointer_cast(obj) + ->setRobotRootPosition(robotName, positionWRTParentJoint); +} + +std::map Device::handles() { + return std::dynamic_pointer_cast(obj)->handles.map; } -BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(getFrameId_overload, Model::getFrameId, - 1, 2) -BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(existFrame_overload, Model::existFrame, - 1, 2) -BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(addJointFrame_overload, - Model::addJointFrame, 1, 2) - -PinDevicePtr_t Device::asPinDevice() { - hpp::pinocchio::DevicePtr_t pinDevice = - std::dynamic_pointer_cast(obj); - return pinDevice; +std::map Device::grippers() { + return std::dynamic_pointer_cast(obj) + ->grippers.map; } void Device::setJointBounds(const char* jointName, @@ -114,13 +84,127 @@ void Device::setJointBounds(const char* jointName, joint->upperBound(i, vMax); // Set upper bound for DOF i } } + +boost::python::list Device::contactSurfaceNames() { + auto manipDevice = std::dynamic_pointer_cast(obj); + boost::python::list result; + for (const auto& entry : manipDevice->jointAndShapes.map) { + result.append(entry.first); + } + return result; +} + +boost::python::dict Device::contactSurfaces() { + auto manipDevice = std::dynamic_pointer_cast(obj); + boost::python::dict result; + + for (const auto& entry : manipDevice->jointAndShapes.map) { + const std::string& name = entry.first; + const auto& jointAndShapes = entry.second; + boost::python::list surfacesList; + + for (const auto& jas : jointAndShapes) { + boost::python::dict surfaceDict; + // jas.first is JointPtr_t, jas.second is Shape_t (vector) + std::string jointName = jas.first ? jas.first->name() : "universe"; + surfaceDict["joint"] = jointName; + + boost::python::list points; + for (const auto& pt : jas.second) { + boost::python::list point; + point.append(pt[0]); + point.append(pt[1]); + point.append(pt[2]); + points.append(point); + } + surfaceDict["points"] = points; + surfacesList.append(surfaceDict); + } + result[name] = surfacesList; + } + return result; +} + +void Device::addHandle(const std::string& linkName, + const std::string& handleName, const Transform3s& pose, + value_type clearance, const std::vector& mask) { + hpp::manipulation::DevicePtr_t robot = + HPP_DYNAMIC_PTR_CAST(hpp::manipulation::Device, this->obj); + if (!robot) { + throw std::logic_error( + "Device.addHandle expects a manipulation device (imported from pyhpp." + "manipulation)"); + } + if (mask.size() != 6) { + throw std::logic_error("mask should be of size 6"); + } + JointPtr_t joint = robot->getJointByBodyName(linkName); + + const ::pinocchio::Frame& linkFrame = + robot->model().frames[robot->model().getFrameId(std::string(linkName))]; + assert(linkFrame.type == ::pinocchio::BODY); + + hpp::pinocchio::JointIndex index(0); + std::string jointName("universe"); + if (joint) { + index = joint->index(); + jointName = joint->name(); + } + HandlePtr_t handle = + Handle::create(handleName, linkFrame.placement * pose, robot, joint); + handle->clearance(clearance); + handle->mask(mask); + robot->handles.add(handleName, handle); + assert(robot->model().existFrame(jointName)); + FrameIndex previousFrame(robot->model().getFrameId(jointName)); + robot->model().addFrame(::pinocchio::Frame(handleName, index, previousFrame, + linkFrame.placement * pose, + ::pinocchio::OP_FRAME)); + // Recreate pinocchio data after modifying model + robot->createData(); +} + +void Device::addGripper(const std::string& linkName, + const std::string& gripperName, const Transform3s& pose, + value_type clearance) { + hpp::manipulation::DevicePtr_t robot = + HPP_DYNAMIC_PTR_CAST(hpp::manipulation::Device, this->obj); + if (!robot) { + throw std::logic_error( + "Device.addHandle expects a manipulation device (imported from pyhpp." + "manipulation)"); + } + JointPtr_t joint = robot->getJointByBodyName(linkName); + + const ::pinocchio::Frame& linkFrame = + robot->model().frames[robot->model().getFrameId(std::string(linkName))]; + assert(linkFrame.type == ::pinocchio::BODY); + + hpp::pinocchio::JointIndex index(0); + std::string jointName("universe"); + if (joint) { + index = joint->index(); + jointName = joint->name(); + } + assert(robot->model().existFrame(jointName)); + FrameIndex previousFrame(robot->model().getFrameId(jointName)); + robot->model().addFrame(::pinocchio::Frame(gripperName, index, previousFrame, + linkFrame.placement * pose, + ::pinocchio::OP_FRAME)); + // Recreate pinocchio data after modifying model + robot->createData(); + GripperPtr_t gripper = Gripper::create(gripperName, robot); + gripper->clearance(clearance); + robot->grippers.add(gripperName, gripper); +} + boost::python::list Device::getJointConfig(const char* jointName) { try { Frame frame = obj->getFrameByName(jointName); if (frame.isFixed()) return boost::python::list(); JointPtr_t joint = frame.joint(); if (!joint) return boost::python::list(); - hpp::core::vector_t config = obj->currentConfiguration(); + vector_t config = obj->currentConfiguration(); size_type ric = joint->rankInConfiguration(); size_type dim = joint->configSize(); auto segment = config.segment(ric, dim); @@ -140,30 +224,6 @@ boost::python::list Device::getJointNames() { } } -bool Device_currentConfiguration(Device& d, const Configuration_t& c) { - return d.currentConfiguration(c); -} - -Transform3s getObjectPositionInJoint(const GripperPtr_t& gripper) { - Transform3s res(gripper->objectPositionInJoint()); - return res; -} - -bool check(const Device& device) { - for (auto const& g : device.obj->grippers.map) { - std::cout << g.first << std::endl; - } - return true; -} - -std::map getDeviceHandles(const Device& device) { - return device.obj->handles.map; -} - -std::map getDeviceGrippers(const Device& device) { - return device.obj->grippers.map; -} - std::string getHandleName(const HandlePtr_t& handle) { return handle->name(); } void setHandleName(const HandlePtr_t& handle, const std::string& name) { @@ -195,10 +255,23 @@ void setHandleMaskComp(const HandlePtr_t& handle, handle->maskComp(mask); } +vector3_t getApproachingDirection(const HandlePtr_t& handle) { + return handle->approachingDirection(); +} + +void setApproachingDirection(const HandlePtr_t& handle, const vector3_t& dir) { + handle->approachingDirection(dir); +} + +static JointIndex getParentJointId(const HandlePtr_t& handle) { + assert(handle->robot()); + Model model(handle->robot()->model()); + return model.frames[model.getFrameId(handle->name())].parentJoint; +} + void exposeHandle() { + // DocClass(Handle) class_("Handle", no_init) - .def("create", &Handle::create) - .staticmethod("create") .add_property("name", &getHandleName, &setHandleName) .add_property("localPosition", &getHandleLocalPosition, &setHandleLocalPosition) @@ -208,51 +281,65 @@ void exposeHandle() { "clearance", static_cast(&Handle::clearance), static_cast(&Handle::clearance)) - .def("createGrasp", &Handle::createGrasp) - .def("createPreGrasp", &Handle::createPreGrasp) - .def("createGraspComplement", &Handle::createGraspComplement) - .def("createGraspAndComplement", &Handle::createGraspAndComplement); + .add_property("approachingDirection", &getApproachingDirection, + &setApproachingDirection) + .def("createGrasp", &Handle::createGrasp, DocClassMethod(createGrasp)) + .def("getParentJointId", &getParentJointId, + "Get index of the joint the handle is attached to" + " in pinocchio Model") + .def("createPreGrasp", &Handle::createPreGrasp, + DocClassMethod(createPreGrasp)) + .def("createGraspComplement", &Handle::createGraspComplement, + DocClassMethod(createGraspComplement)) + .def("createGraspAndComplement", &Handle::createGraspAndComplement, + DocClassMethod(createGraspAndComplement)); class_ >("HandleMap") .def(boost::python::map_indexing_suite, true>()); } +object asPinDevice(object self) { + PyErr_WarnEx(PyExc_DeprecationWarning, + "asPinDevice() is deprecated: manipulation.Device already " + "inherits from pinocchio.Device, use the object directly", + 1); + return self; +} + void exposeDevice() { - void (Device::*cfk)(int) = &Device::computeForwardKinematics; - // Enable automatic conversion of std::map - class_("Device", init()) - .def("name", &Device::name, return_value_policy()) - .def("configSpace", &Device::configSpace, - return_value_policy()) - .def("model", static_cast(&Device::model), - return_internal_reference<>()) - .def("data", static_cast(&Device::data), - return_internal_reference<>()) - .def("geomData", static_cast(&Device::geomData), - return_internal_reference<>()) - .def("geomModel", - static_cast(&Device::geomModel), - return_internal_reference<>()) - .def("visualModel", - static_cast(&Device::visualModel), - return_internal_reference<>()) - .PYHPP_DEFINE_METHOD(Device, configSize) - .PYHPP_DEFINE_METHOD(Device, numberDof) - .def("currentConfiguration", - static_cast( - &Device::currentConfiguration), - return_value_policy()) - .def("currentConfiguration", Device_currentConfiguration) - .def("computeForwardKinematics", cfk) - .def("computeFramesForwardKinematics", - &Device::computeFramesForwardKinematics) - .def("updateGeometryPlacements", &Device::updateGeometryPlacements) + // DocClass(Device) + class_, boost::shared_ptr, + boost::noncopyable>("Device", init()) .def("setRobotRootPosition", &Device::setRobotRootPosition) - .def("handles", &getDeviceHandles) - .def("grippers", &getDeviceGrippers) - .def("asPinDevice", &Device::asPinDevice) + .def("handles", &Device::handles) + .def("grippers", &Device::grippers) + .def("asPinDevice", &asPinDevice) .def("getJointNames", &Device::getJointNames) .def("getJointConfig", &Device::getJointConfig) - .def("setJointBounds", &Device::setJointBounds); + .def("setJointBounds", &Device::setJointBounds) + .def("contactSurfaceNames", &Device::contactSurfaceNames, + "Return list of contact surface names registered on device") + .def("contactSurfaces", &Device::contactSurfaces, + "Return dict mapping surface names to list of {joint, points}") + .def("addHandle", &Device::addHandle, + "Add a handle to the kinematic chain\n\n" + " input\n" + " linkName: name of the link the handle is attached to,\n" + " handleName: name of the handle,\n" + " pose: pose of the handle in the link frame (SE3),\n" + " clearance: clearance of the handle, the sum of handle and " + "gripper clearances\n" + " defines the distance between pregrasp and grasp,\n" + " mask: list of 6 Boolean use to define symmetries in the grasp " + "constraint.") + .def("addGripper", &Device::addGripper, + "Add a gripper to the kinematic chain\n\n" + " input\n" + " linkName: name of the link the handle is attached to,\n" + " gripperName: name of the gripper,\n" + " pose: pose of the gripper in the link frame (SE3),\n" + " clearance: clearance of the gripper, the sum of handle and " + "gripper clearances\n" + " defines the distance between pregrasp and grasp."); } } // namespace manipulation } // namespace pyhpp diff --git a/src/pyhpp/manipulation/device.hh b/src/pyhpp/manipulation/device.hh index ae89fbd3..f1417e2f 100644 --- a/src/pyhpp/manipulation/device.hh +++ b/src/pyhpp/manipulation/device.hh @@ -30,6 +30,7 @@ #ifndef PYHPP_MANIPULATION_DEVICE_HH #define PYHPP_MANIPULATION_DEVICE_HH +#include <../src/pyhpp/pinocchio/device.hh> #include #include #include @@ -53,6 +54,8 @@ typedef hpp::pinocchio::LiegroupSpacePtr_t LiegroupSpacePtr_t; typedef hpp::pinocchio::GeomModel GeomModel; typedef hpp::pinocchio::Configuration_t Configuration_t; typedef hpp::pinocchio::ConfigurationIn_t ConfigurationIn_t; +typedef hpp::pinocchio::vector_t vector_t; +typedef hpp::pinocchio::vector3_t vector3_t; typedef hpp::pinocchio::size_type size_type; typedef hpp::pinocchio::value_type value_type; typedef hpp::pinocchio::Transform3s Transform3s; @@ -64,7 +67,9 @@ typedef hpp::manipulation::Handle Handle; typedef hpp::manipulation::DevicePtr_t DevicePtr_t; typedef hpp::pinocchio::DevicePtr_t PinDevicePtr_t; typedef hpp::pinocchio::Frame Frame; +typedef hpp::pinocchio::FrameIndex FrameIndex; typedef hpp::pinocchio::Joint Joint; +typedef hpp::pinocchio::JointIndex JointIndex; typedef hpp::pinocchio::JointPtr_t JointPtr_t; // Wrapper class to hpp::manipulation::Device @@ -72,32 +77,23 @@ typedef hpp::pinocchio::JointPtr_t JointPtr_t; // Boost python does not correctly handle weak pointers. To overcome this // limitation, we create a wrapper class and bind this class with boost python // instead of the original class. -struct Device { - DevicePtr_t obj; - Device(const DevicePtr_t& object); +struct Device : public pyhpp::pinocchio::Device { Device(const std::string& name); - // Methods from hpp::pinocchio::Device - const std::string& name() const; - const LiegroupSpacePtr_t& configSpace() const; - Model& model(); - Data& data(); - GeomData& geomData(); - GeomModel& geomModel(); - GeomModel& visualModel(); - size_type configSize() const; - size_type numberDof() const; - const Configuration_t& currentConfiguration() const; - bool currentConfiguration(ConfigurationIn_t configuration); - void computeForwardKinematics(int flag); - void computeFramesForwardKinematics(); - void updateGeometryPlacements(); - // Methods for hpp::manipulation::Device + void setRobotRootPosition(const std::string& robotName, const Transform3s& positionWRTParentJoint); - PinDevicePtr_t asPinDevice(); + std::map handles(); + std::map grippers(); boost::python::list getJointConfig(const char* jointName); boost::python::list getJointNames(); void setJointBounds(const char* jointName, boost::python::list jointBounds); + boost::python::list contactSurfaceNames(); + boost::python::dict contactSurfaces(); + void addHandle(const std::string& linkName, const std::string& handleName, + const Transform3s& pose, value_type clearance, + const std::vector& mask); + void addGripper(const std::string& linkName, const std::string& gripperName, + const Transform3s& pose, value_type clearance); }; // struct Device } // namespace manipulation } // namespace pyhpp diff --git a/src/pyhpp/manipulation/graph.cc b/src/pyhpp/manipulation/graph.cc index eb5c3694..bb682de2 100644 --- a/src/pyhpp/manipulation/graph.cc +++ b/src/pyhpp/manipulation/graph.cc @@ -44,6 +44,9 @@ #include #include "hpp/manipulation/constraint-set.hh" +#include "pyhpp/core/problem.hh" + +// DocNamespace(hpp::manipulation::graph) namespace { @@ -154,19 +157,19 @@ const char* DOC_GETCONFIGERRORFORTRANSITIONTARGET = const char* DOC_APPLYSTATECONSTRAINTS = "Apply constraints to a configuration. " - "Returns ConstraintResult with success flag, output configuration, and " + "Returns tuple with success flag, output configuration, and " "error norm."; const char* DOC_APPLYLEAFCONSTRAINTS = "Apply transition constraints to a configuration. " - "Returns ConstraintResult with success flag, output configuration, and " + "Returns tuple with success flag, output configuration, and " "error norm. " "If success, the output configuration is reachable from q_rhs along the " "transition."; const char* DOC_GENERATETARGETCONFIG = "Generate configuration in destination state on a given leaf. " - "Returns ConstraintResult with success flag, output configuration, and " + "Returns tuple with success flag, output configuration, and " "error norm. " "Computes a configuration in the destination state of the transition, " "reachable from q_rhs."; @@ -233,16 +236,16 @@ namespace manipulation { // Utility functions // ============================================================================= -std::vector> matrixToVectorVector( +std::vector> matrixToVectorVector( const Eigen::Ref>& input) { - std::vector> result; + std::vector> result; result.reserve(input.rows()); for (int i = 0; i < input.rows(); ++i) { - std::vector row; + std::vector row; row.reserve(input.cols()); for (int j = 0; j < input.cols(); ++j) { - row.push_back(static_cast(input(i, j))); + row.push_back(input(i, j)); } result.push_back(std::move(row)); } @@ -265,7 +268,7 @@ PyWGraph::PyWGraph(const hpp::manipulation::graph::GraphPtr_t& object) PyWGraph::PyWGraph(const std::string& name, const PyWDevicePtr_t& d, const PyWProblemPtr_t& problem) : obj(hpp::manipulation::graph::Graph::create( - name, d->obj, problem->asManipulationProblem())), + name, d->asManipulationDevice(), problem->asManipulationProblem())), robot(d), problem(problem) {} @@ -406,12 +409,12 @@ bool PyWGraph::isShort(PyWEdgePtr_t edge) { } } -void PyWGraph::getNodesConnectedByTransition(PyWEdgePtr_t edge, - std::string& nodeFrom, - std::string& nodeTo) { +boost::python::tuple PyWGraph::getNodesConnectedByTransition( + PyWEdgePtr_t edge) { try { - nodeFrom = edge->obj->stateFrom()->name(); - nodeTo = edge->obj->stateTo()->name(); + std::string nodeFrom = edge->obj->stateFrom()->name(); + std::string nodeTo = edge->obj->stateTo()->name(); + return boost::python::make_tuple(nodeFrom, nodeTo); } catch (const std::exception& exc) { throw std::logic_error(exc.what()); } @@ -729,8 +732,8 @@ boost::python::tuple PyWGraph::getConfigErrorForTransitionTarget( // Constraint application // ============================================================================= -ConstraintResult PyWGraph::applyStateConstraints(PyWStatePtr_t state, - ConfigurationIn_t input) { +boost::python::tuple PyWGraph::applyStateConstraints(PyWStatePtr_t state, + ConfigurationIn_t input) { using namespace hpp::manipulation; ConstraintSetPtr_t constraint(state->obj->configConstraint()); value_type residualError(std::numeric_limits::quiet_NaN()); @@ -741,12 +744,12 @@ ConstraintResult PyWGraph::applyStateConstraints(PyWStatePtr_t state, constraint->configProjector()) { residualError = configProjector->residualError(); } - return ConstraintResult(success, output, residualError); + return boost::python::make_tuple(success, output, residualError); } -ConstraintResult PyWGraph::applyLeafConstraints(PyWEdgePtr_t transition, - ConfigurationIn_t q_rhs, - ConfigurationIn_t input) { +boost::python::tuple PyWGraph::applyLeafConstraints(PyWEdgePtr_t transition, + ConfigurationIn_t q_rhs, + ConfigurationIn_t input) { using namespace hpp::manipulation; ConstraintSetPtr_t constraint(transition->obj->pathConstraint()); value_type residualError(std::numeric_limits::quiet_NaN()); @@ -758,12 +761,12 @@ ConstraintResult PyWGraph::applyLeafConstraints(PyWEdgePtr_t transition, success = constraint->apply(output); residualError = constraint->configProjector()->residualError(); } - return ConstraintResult(success, output, residualError); + return boost::python::make_tuple(success, output, residualError); } -ConstraintResult PyWGraph::generateTargetConfig(PyWEdgePtr_t transition, - ConfigurationIn_t q_rhs, - ConfigurationIn_t input) { +boost::python::tuple PyWGraph::generateTargetConfig(PyWEdgePtr_t transition, + ConfigurationIn_t q_rhs, + ConfigurationIn_t input) { using namespace hpp::manipulation; ConstraintSetPtr_t constraint(transition->obj->targetConstraint()); value_type residualError(std::numeric_limits::quiet_NaN()); @@ -778,7 +781,7 @@ ConstraintResult PyWGraph::generateTargetConfig(PyWEdgePtr_t transition, residualError = configProjector->residualError(); } - return ConstraintResult(success, output, residualError); + return boost::python::make_tuple(success, output, residualError); } // ============================================================================= @@ -816,7 +819,7 @@ void PyWGraph::addLevelSetFoliation(PyWEdgePtr_t edge, boost::python::list PyWGraph::getSecurityMarginMatrixForTransition( PyWEdgePtr_t edge) { try { - std::vector> matrix = + std::vector> matrix = matrixToVectorVector(edge->obj->securityMargins()); boost::python::list result; @@ -894,6 +897,10 @@ void PyWGraph::removeCollisionPairFromTransition(PyWEdgePtr_t edge, } } +PathValidationPtr_t PyWEdge::pathValidation() const { + return obj->pathValidation(); +} + // ============================================================================= // Subgraph management // ============================================================================= @@ -1012,14 +1019,15 @@ boost::python::tuple PyWGraph::createPlacementConstraint( auto surface2 = extract_vector(py_surface2); bool explicit_(true); - if (!robot->obj) throw std::runtime_error("No robot loaded"); + if (!robot->asManipulationDevice()) + throw std::runtime_error("No robot loaded"); JointAndShapes_t floorSurfaces, objectSurfaces, l; for (std::vector::const_iterator it1 = surface1.begin(); it1 != surface1.end(); ++it1) { - if (!robot->obj->jointAndShapes.has(*it1)) + if (!robot->asManipulationDevice()->jointAndShapes.has(*it1)) throw std::runtime_error("First list of triangles not found."); - l = robot->obj->jointAndShapes.get(*it1); + l = robot->asManipulationDevice()->jointAndShapes.get(*it1); objectSurfaces.insert(objectSurfaces.end(), l.begin(), l.end()); for (auto js : l) { @@ -1036,8 +1044,8 @@ boost::python::tuple PyWGraph::createPlacementConstraint( for (std::vector::const_iterator it2 = surface2.begin(); it2 != surface2.end(); ++it2) { - if (robot->obj->jointAndShapes.has(*it2)) { - l = robot->obj->jointAndShapes.get(*it2); + if (robot->asManipulationDevice()->jointAndShapes.has(*it2)) { + l = robot->asManipulationDevice()->jointAndShapes.get(*it2); } else if (problem->jointAndShapes.has(*it2)) { l = problem->jointAndShapes.get(*it2); } else { @@ -1050,7 +1058,8 @@ boost::python::tuple PyWGraph::createPlacementConstraint( typedef hpp::constraints::explicit_::ConvexShapeContact Constraint_t; Constraint_t::Constraints_t constraints( Constraint_t::createConstraintAndComplement( - name, robot->obj, floorSurfaces, objectSurfaces, margin)); + name, robot->asManipulationDevice(), floorSurfaces, + objectSurfaces, margin)); assert(hpp::dynamic_pointer_cast( std::get<0>(constraints)->functionPtr())); @@ -1076,7 +1085,8 @@ boost::python::tuple PyWGraph::createPlacementConstraint( std::pair functions(hpp::constraints::ConvexShapeContactComplement::createPair( - name, robot->obj, floorSurfaces, objectSurfaces)); + name, robot->asManipulationDevice(), floorSurfaces, + objectSurfaces)); functions.first->setNormalMargin(margin); @@ -1113,21 +1123,22 @@ ImplicitPtr_t PyWGraph::createPrePlacementConstraint( auto surface1 = extract_vector(py_surface1); auto surface2 = extract_vector(py_surface2); - if (!robot->obj) throw std::runtime_error("No robot loaded"); + if (!robot->asManipulationDevice()) + throw std::runtime_error("No robot loaded"); JointAndShapes_t floorSurfaces, objectSurfaces, l; for (std::vector::const_iterator it1 = surface1.begin(); it1 != surface1.end(); ++it1) { - if (!robot->obj->jointAndShapes.has(*it1)) + if (!robot->asManipulationDevice()->jointAndShapes.has(*it1)) throw std::runtime_error("First list of triangles not found."); - l = robot->obj->jointAndShapes.get(*it1); + l = robot->asManipulationDevice()->jointAndShapes.get(*it1); objectSurfaces.insert(objectSurfaces.end(), l.begin(), l.end()); } for (std::vector::const_iterator it2 = surface2.begin(); it2 != surface2.end(); ++it2) { - if (robot->obj->jointAndShapes.has(*it2)) { - l = robot->obj->jointAndShapes.get(*it2); + if (robot->asManipulationDevice()->jointAndShapes.has(*it2)) { + l = robot->asManipulationDevice()->jointAndShapes.get(*it2); } else if (problem->jointAndShapes.has(*it2)) { l = problem->jointAndShapes.get(*it2); } else { @@ -1138,7 +1149,8 @@ ImplicitPtr_t PyWGraph::createPrePlacementConstraint( hpp::constraints::ConvexShapeContactPtr_t cvxShape( hpp::constraints::ConvexShapeContact::create( - name, robot->obj, floorSurfaces, objectSurfaces)); + name, robot->asManipulationDevice(), floorSurfaces, + objectSurfaces)); cvxShape->setNormalMargin(margin + width); return hpp::constraints::Implicit::create( @@ -1166,9 +1178,11 @@ boost::python::list PyWGraph::createGraspConstraint(const std::string& name, const std::string& handle) { boost::python::list result; - GripperPtr_t g = robot->obj->grippers.get(gripper, GripperPtr_t()); + GripperPtr_t g = + robot->asManipulationDevice()->grippers.get(gripper, GripperPtr_t()); if (!g) throw std::runtime_error("No gripper with name " + gripper + "."); - HandlePtr_t h = robot->obj->handles.get(handle, HandlePtr_t()); + HandlePtr_t h = + robot->asManipulationDevice()->handles.get(handle, HandlePtr_t()); if (!h) throw std::runtime_error("No handle with name " + handle + "."); const std::string cname = name + "/complement"; const std::string bname = name + "/hold"; @@ -1189,15 +1203,34 @@ boost::python::list PyWGraph::createGraspConstraint(const std::string& name, ImplicitPtr_t PyWGraph::createPreGraspConstraint(const std::string& name, const std::string& gripper, const std::string& handle) { - GripperPtr_t g = robot->obj->grippers.get(gripper, GripperPtr_t()); + GripperPtr_t g = + robot->asManipulationDevice()->grippers.get(gripper, GripperPtr_t()); if (!g) throw std::runtime_error("No gripper with name " + gripper + "."); - HandlePtr_t h = robot->obj->handles.get(handle, HandlePtr_t()); + HandlePtr_t h = + robot->asManipulationDevice()->handles.get(handle, HandlePtr_t()); if (!h) throw std::runtime_error("No handle with name " + handle + "."); value_type c = h->clearance() + g->clearance(); ImplicitPtr_t constraint = h->createPreGrasp(g, c, name); return constraint; } + +static void graphCapsuleDestructor(PyObject* capsule) { + auto* ptr = static_cast( + PyCapsule_GetPointer(capsule, "hpp.manipulation.graph.GraphPtr")); + if (ptr) { + delete ptr; + } +} + +/// Create a PyCapsule containing a copy of the GraphPtr_t shared_ptr +/// This allows external modules to extract the native C++ pointer +PyObject* getGraphCapsule(const PyWGraph& self) { + auto* ptr = new GraphPtr_t(self.obj); + return PyCapsule_New(ptr, "hpp.manipulation.graph.GraphPtr", + graphCapsuleDestructor); +} + // ============================================================================= // Boost.Python bindings // ============================================================================= @@ -1205,16 +1238,22 @@ ImplicitPtr_t PyWGraph::createPreGraspConstraint(const std::string& name, using namespace boost::python; void exposeGraph() { + // DocClass(State) class_("State", no_init) - .def("name", &PyWState::name); + .def("name", &PyWState::name, DocClassMethod(name)); + // DocClass(Edge) class_("Transition", no_init) - .def("name", &PyWEdge::name); + .def("name", &PyWEdge::name, DocClassMethod(name)) + .def("pathValidation", &PyWEdge::pathValidation, + DocClassMethod(pathValidation)); - class_( + // DocClass(Graph) + class_( "Graph", init()) + .def("_get_native_graph", &getGraphCapsule) .def_readwrite("robot", &PyWGraph::robot) // Configuration methods .PYHPP_DEFINE_GETTER_SETTER(PyWGraph, maxIterations, @@ -1330,11 +1369,6 @@ void exposeGraph() { // Initialization .PYHPP_DEFINE_METHOD1(PyWGraph, initialize, DOC_INITIALIZE); - - class_("ConstraintResult") - .def_readwrite("success", &ConstraintResult::success) - .def_readwrite("configuration", &ConstraintResult::configuration) - .def_readwrite("error", &ConstraintResult::error); } } // namespace manipulation diff --git a/src/pyhpp/manipulation/graph.hh b/src/pyhpp/manipulation/graph.hh index baa42be8..6840a038 100644 --- a/src/pyhpp/manipulation/graph.hh +++ b/src/pyhpp/manipulation/graph.hh @@ -30,12 +30,14 @@ #ifndef PYHPP_GRAPH_HH #define PYHPP_GRAPH_HH +#include #include #include namespace pyhpp { namespace manipulation { +typedef hpp::core::PathValidationPtr_t PathValidationPtr_t; typedef hpp::manipulation::Configuration_t Configuration_t; typedef hpp::manipulation::ConfigurationIn_t ConfigurationIn_t; typedef hpp::manipulation::size_type size_type; @@ -52,17 +54,6 @@ typedef hpp::manipulation::ConstraintsAndComplements_t ConstraintsAndComplements_t; typedef hpp::manipulation::ConstraintAndComplement_t ConstraintAndComplement_t; -/// Result structure for constraint operations -struct ConstraintResult { - bool success; - Configuration_t configuration; - value_type error; - - ConstraintResult() : success(false), configuration(), error(0.0) {} - ConstraintResult(bool s, const Configuration_t& config, value_type err) - : success(s), configuration(config), error(err) {} -}; - /// Python wrapper for State struct PyWState { StatePtr_t obj; @@ -76,6 +67,7 @@ struct PyWEdge { EdgePtr_t obj; PyWEdge(const EdgePtr_t& object); std::string name() const; + PathValidationPtr_t pathValidation() const; }; typedef std::shared_ptr PyWEdgePtr_t; @@ -125,8 +117,7 @@ struct PyWGraph { std::string getContainingNode(PyWEdgePtr_t edge); void setShort(PyWEdgePtr_t edge, bool isShort); bool isShort(PyWEdgePtr_t edge); - void getNodesConnectedByTransition(PyWEdgePtr_t edge, std::string& nodeFrom, - std::string& nodeTo); + boost::python::tuple getNodesConnectedByTransition(PyWEdgePtr_t edge); void setWeight(PyWEdgePtr_t edge, int weight); size_t getWeight(PyWEdgePtr_t edge); void setWaypoint(PyWEdgePtr_t waypointEdge, int index, PyWEdgePtr_t edge, @@ -201,16 +192,6 @@ struct PyWGraph { const PyWEdgePtr_t& edge, ConfigurationIn_t leafConfig, ConfigurationIn_t config) const; - // Constraint application - ConstraintResult applyStateConstraints(PyWStatePtr_t state, - ConfigurationIn_t input); - ConstraintResult applyLeafConstraints(PyWEdgePtr_t transition, - ConfigurationIn_t q_rhs, - ConfigurationIn_t input); - ConstraintResult generateTargetConfig(PyWEdgePtr_t transition, - ConfigurationIn_t q_rhs, - ConfigurationIn_t input); - // Level set edges void addLevelSetFoliation(PyWEdgePtr_t edge, const boost::python::list& condNC, @@ -224,6 +205,14 @@ struct PyWGraph { void removeCollisionPairFromTransition(PyWEdgePtr_t edge, const char* joint1, const char* joint2); + boost::python::tuple applyStateConstraints(PyWStatePtr_t state, + ConfigurationIn_t input); + boost::python::tuple applyLeafConstraints(PyWEdgePtr_t transition, + ConfigurationIn_t q_rhs, + ConfigurationIn_t input); + boost::python::tuple generateTargetConfig(PyWEdgePtr_t transition, + ConfigurationIn_t q_rhs, + ConfigurationIn_t input); // Subgraph management void createSubGraph(const char* subgraphName, hpp::core::RoadmapPtr_t roadmap); @@ -238,6 +227,7 @@ struct PyWGraph { // Initialization void initialize(); }; +typedef std::shared_ptr PyWGraphPtr_t; } // namespace manipulation } // namespace pyhpp diff --git a/src/pyhpp/manipulation/path-optimizer.cc b/src/pyhpp/manipulation/path-optimizer.cc new file mode 100644 index 00000000..79df1863 --- /dev/null +++ b/src/pyhpp/manipulation/path-optimizer.cc @@ -0,0 +1,78 @@ +// +// Copyright (c) 2025, CNRS +// Authors: Paul Sardin +// +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions +// are met: + +// 1. Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. + +// 2. Redistributions in binary form must reproduce the above +// copyright notice, this list of conditions and the following +// disclaimer in the documentation and/or other materials provided +// with the distribution. + +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +// COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, +// INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +// SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) +// HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, +// STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED +// OF THE POSSIBILITY OF SUCH DAMAGE. + +#include +#include +#include +#include +#include +#include +#include +#include + +// DocNamespace(hpp::manipulation) + +using namespace boost::python; + +namespace pyhpp { +namespace manipulation { +using namespace hpp::manipulation; + +template +hpp::core::PathOptimizerPtr_t createGraphOptimizer( + const hpp::core::ProblemConstPtr_t& problem) { + return GraphOptimizer::create(problem); +} + +void exposePathOptimizers() { + class_, + bases, boost::noncopyable>("RandomShortcut", + no_init) + .def("__init__", + make_constructor(&pathOptimization::RandomShortcut::create)); + + class_, + bases, boost::noncopyable>( + "EnforceTransitionSemantic", no_init) + .def("__init__", + make_constructor( + &pathOptimization::EnforceTransitionSemantic::create)); + + def("GraphRandomShortcut", + &createGraphOptimizer); + + def("GraphPartialShortcut", + &createGraphOptimizer); +} + +} // namespace manipulation +} // namespace pyhpp diff --git a/src/pyhpp/manipulation/path-planner.cc b/src/pyhpp/manipulation/path-planner.cc index c8762e98..fd5720e8 100644 --- a/src/pyhpp/manipulation/path-planner.cc +++ b/src/pyhpp/manipulation/path-planner.cc @@ -30,12 +30,16 @@ #include <../src/pyhpp/manipulation/graph.hh> #include <../src/pyhpp/manipulation/path-planner.hh> #include +#include +#include #include #include #include #include #include +// DocNamespace(hpp::manipulation::pathPlanner) + namespace pyhpp { namespace manipulation { @@ -50,35 +54,19 @@ struct ManipulationPlanner : public pyhpp::core::PathPlanner { } }; -void exposePathPlanners() { - boost::python::class_>( - "TransitionPlanner", boost::python::init()) - .def("innerPlanner", - static_cast( - &TransitionPlanner::innerPlanner)) - .def("innerPlanner", static_cast( - &TransitionPlanner::innerPlanner)) - .def("innerProblem", &TransitionPlanner::innerProblem) - .def("planPath", &TransitionPlanner::planPath) - .def("directPath", &TransitionPlanner::directPath) - .def("validateConfiguration", &TransitionPlanner::validateConfiguration) - .def("optimizePath", &TransitionPlanner::optimizePath) - .def("timeParameterization", &TransitionPlanner::timeParameterization) - .def("setEdge", &TransitionPlanner::setEdge) - .def("setReedsAndSheppSteeringMethod", - &TransitionPlanner::setReedsAndSheppSteeringMethod) - .def("pathProjector", &TransitionPlanner::pathProjector) - .def("clearPathOptimizers", &TransitionPlanner::clearPathOptimizers) - .def("addPathOptimizer", &TransitionPlanner::addPathOptimizer); - - boost::python::class_>( - "ManipulationPlanner", - boost::python::init()); -} +struct StatesPathFinder : public pyhpp::core::PathPlanner { + StatesPathFinder(const pyhpp::core::Problem& problem) { + hpp::manipulation::RoadmapPtr_t roadmap = + hpp::manipulation::Roadmap::create(problem.obj->distance(), + problem.obj->robot()); + obj = hpp::manipulation::pathPlanner::StatesPathFinder::createWithRoadmap( + problem.obj, roadmap); + roadmap->constraintGraph( + problem.asManipulationProblem()->constraintGraph()); + } +}; +// TransitionPlanner implementation TransitionPlanner::TransitionPlanner(const pyhpp::core::Problem& problem) { obj = hpp::manipulation::pathPlanner::TransitionPlanner::createWithRoadmap( problem.obj, hpp::core::Roadmap::create(problem.obj->distance(), @@ -102,6 +90,7 @@ pyhpp::core::PathPlanner TransitionPlanner::innerPlanner() const { void TransitionPlanner::innerPlanner(const pyhpp::core::PathPlanner& planner) { trObj()->innerPlanner(planner.obj); } + pyhpp::core::Problem TransitionPlanner::innerProblem() const { return pyhpp::core::Problem(trObj()->innerProblem()); } @@ -109,7 +98,6 @@ pyhpp::core::Problem TransitionPlanner::innerProblem() const { PathVectorPtr_t TransitionPlanner::planPath(ConfigurationIn_t qInit, matrixIn_t qGoals, bool resetRoadmap) { - // Check sizes of initial and goal configurations if (qInit.rows() != obj->problem()->robot()->configSize()) { std::ostringstream os; os << "qInit = " << hpp::pinocchio::displayConfig(qInit) @@ -132,7 +120,6 @@ PathVectorPtr_t TransitionPlanner::planPath(ConfigurationIn_t qInit, tuple TransitionPlanner::directPath(ConfigurationIn_t q1, ConfigurationIn_t q2, bool validate) { - // Check sizes of initial and goal configurations if (q1.rows() != obj->problem()->robot()->configSize()) { std::ostringstream os; os << "q1 = " << hpp::pinocchio::displayConfig(q1) << "should be of size " @@ -188,5 +175,125 @@ void TransitionPlanner::addPathOptimizer( trObj()->addPathOptimizer(pathOptimizer); } +// EndEffectorTrajectory implementation +EndEffectorTrajectory::EndEffectorTrajectory( + const pyhpp::core::Problem& problem) { + obj = + hpp::manipulation::pathPlanner::EndEffectorTrajectory::createWithRoadmap( + problem.obj, hpp::core::Roadmap::create(problem.obj->distance(), + problem.obj->robot())); +} + +EndEffectorTrajectory::EndEffectorTrajectory( + const pyhpp::core::Problem& problem, + const hpp::core::RoadmapPtr_t& roadmap) { + obj = + hpp::manipulation::pathPlanner::EndEffectorTrajectory::createWithRoadmap( + problem.obj, roadmap); +} + +hpp::manipulation::pathPlanner::EndEffectorTrajectoryPtr_t +EndEffectorTrajectory::eetObj() const { + assert(HPP_DYNAMIC_PTR_CAST( + hpp::manipulation::pathPlanner::EndEffectorTrajectory, obj)); + return HPP_STATIC_PTR_CAST( + hpp::manipulation::pathPlanner::EndEffectorTrajectory, obj); +} + +int EndEffectorTrajectory::nRandomConfig() const { + return eetObj()->nRandomConfig(); +} + +void EndEffectorTrajectory::nRandomConfig(int n) { eetObj()->nRandomConfig(n); } + +int EndEffectorTrajectory::nDiscreteSteps() const { + return eetObj()->nDiscreteSteps(); +} + +void EndEffectorTrajectory::nDiscreteSteps(int n) { + eetObj()->nDiscreteSteps(n); +} + +void EndEffectorTrajectory::checkFeasibilityOnly(bool enable) { + eetObj()->checkFeasibilityOnly(enable); +} + +bool EndEffectorTrajectory::checkFeasibilityOnly() const { + return eetObj()->checkFeasibilityOnly(); +} + +// void +// EndEffectorTrajectory::ikSolverInitialization(IkSolverInitializationPtr_t +// solver) { +// eetObj()->ikSolverInitialization(solver); +// } + +void exposePathPlanners() { + // DocClass(TransitionPlanner) + boost::python::class_>( + "TransitionPlanner", boost::python::init()) + .def("innerPlanner", + static_cast( + &TransitionPlanner::innerPlanner)) + .def("innerPlanner", static_cast( + &TransitionPlanner::innerPlanner)) + .def("innerProblem", &TransitionPlanner::innerProblem, + DocClassMethod(innerProblem)) + .def("planPath", &TransitionPlanner::planPath, DocClassMethod(planPath)) + .def("directPath", &TransitionPlanner::directPath) + .def("validateConfiguration", &TransitionPlanner::validateConfiguration) + .def("optimizePath", &TransitionPlanner::optimizePath, + DocClassMethod(optimizePath)) + .def("timeParameterization", &TransitionPlanner::timeParameterization, + DocClassMethod(timeParameterization)) + .def("setEdge", &TransitionPlanner::setEdge, DocClassMethod(setEdge)) + .def("setReedsAndSheppSteeringMethod", + &TransitionPlanner::setReedsAndSheppSteeringMethod, + DocClassMethod(setReedsAndSheppSteeringMethod)) + .def("pathProjector", &TransitionPlanner::pathProjector, + DocClassMethod(pathProjector)) + .def("clearPathOptimizers", &TransitionPlanner::clearPathOptimizers, + DocClassMethod(clearPathOptimizers)) + .def("addPathOptimizer", &TransitionPlanner::addPathOptimizer, + DocClassMethod(addPathOptimizer)); + + boost::python::class_>( + "ManipulationPlanner", + boost::python::init()); + + boost::python::class_>( + "StatesPathFinder", boost::python::init()); + + // DocClass(EndEffectorTrajectory) + boost::python::class_>( + "EndEffectorTrajectory", + boost::python::init()) + .def(boost::python::init()) + .def("nRandomConfig", static_cast( + &EndEffectorTrajectory::nRandomConfig)) + .def("nRandomConfig", static_cast( + &EndEffectorTrajectory::nRandomConfig)) + .def("nDiscreteSteps", + static_cast( + &EndEffectorTrajectory::nDiscreteSteps)) + .def("nDiscreteSteps", static_cast( + &EndEffectorTrajectory::nDiscreteSteps)) + .def("checkFeasibilityOnly", + static_cast( + &EndEffectorTrajectory::checkFeasibilityOnly)) + .def("checkFeasibilityOnly", + static_cast( + &EndEffectorTrajectory::checkFeasibilityOnly)) + // .def("ikSolverInitialization", + // &EndEffectorTrajectory::ikSolverInitialization) + ; +} + } // namespace manipulation } // namespace pyhpp diff --git a/src/pyhpp/manipulation/path-planner.hh b/src/pyhpp/manipulation/path-planner.hh index c35a7b30..5ca00a8a 100644 --- a/src/pyhpp/manipulation/path-planner.hh +++ b/src/pyhpp/manipulation/path-planner.hh @@ -1,6 +1,6 @@ // // Copyright (c) 2025, CNRS -// Authors: Florent Lamiraux +// Authors: Florent Lamiraux, Paul Sardin // // Redistribution and use in source and binary forms, with or without // modification, are permitted provided that the following conditions @@ -44,6 +44,8 @@ typedef hpp::core::PathPtr_t PathPtr_t; typedef hpp::core::PathVectorPtr_t PathVectorPtr_t; typedef hpp::core::RoadmapPtr_t RoadmapPtr_t; typedef hpp::manipulation::matrixIn_t matrixIn_t; +// typedef hpp::manipulation::pathPlanner::IkSolverInitializationPtr_t +// IkSolverInitializationPtr_t; struct TransitionPlanner : public pyhpp::core::PathPlanner { // Dynamic cast pointer into TransitionPlanner @@ -63,7 +65,24 @@ struct TransitionPlanner : public pyhpp::core::PathPlanner { void pathProjector(const PathProjectorPtr_t pathProjector); void clearPathOptimizers(); void addPathOptimizer(const PathOptimizerPtr_t& pathOptimizer); -}; // struct TransitionPlanner +}; + +struct EndEffectorTrajectory : public pyhpp::core::PathPlanner { + EndEffectorTrajectory(const pyhpp::core::Problem& problem); + EndEffectorTrajectory(const pyhpp::core::Problem& problem, + const RoadmapPtr_t& roadmap); + hpp::manipulation::pathPlanner::EndEffectorTrajectoryPtr_t eetObj() const; + int nRandomConfig() const; + void nRandomConfig(int n); + int nDiscreteSteps() const; + void nDiscreteSteps(int n); + void checkFeasibilityOnly(bool enable); + bool checkFeasibilityOnly() const; + // void ikSolverInitialization(IkSolverInitializationPtr_t solver); +}; + +void exposePathPlanners(); + } // namespace manipulation } // namespace pyhpp #endif // PYHPP_MANIPULATION_PATH_PLANNER_HH diff --git a/src/pyhpp/manipulation/path-projector.cc b/src/pyhpp/manipulation/path-projector.cc index 418f2e7e..61b92bb7 100644 --- a/src/pyhpp/manipulation/path-projector.cc +++ b/src/pyhpp/manipulation/path-projector.cc @@ -39,6 +39,8 @@ #include #include +// DocNamespace(hpp::core) + using namespace boost::python; namespace pyhpp { @@ -49,11 +51,11 @@ typedef pyhpp::core::PyWSteeringMethodPtr_t PyWSteeringMethodPtr_t; using namespace boost::python; void exposePathProjector() { def( - "createNoneProjector", + "NoneProjector", +[]() -> PathProjectorPtr_t { return PathProjectorPtr_t(); }); def( - "createProgressiveProjector", + "ProgressiveProjector", +[](const DistancePtr_t& distance, const PyWSteeringMethodPtr_t& steeringMethodWrapper, const value_type& step) { @@ -62,7 +64,7 @@ void exposePathProjector() { }, (arg("distance"), arg("steeringMethod"), arg("step"))); def( - "createProgressiveProjector", + "ProgressiveProjector", +[](const DistancePtr_t& distance, const PyWGraphSteeringMethodPtr_t& steeringMethodWrapper, const value_type& step) { @@ -72,7 +74,7 @@ void exposePathProjector() { (arg("distance"), arg("steeringMethod"), arg("step"))); def( - "createDichotomyProjector", + "DichotomyProjector", +[](const DistancePtr_t& distance, const PyWSteeringMethodPtr_t& steeringMethodWrapper, const value_type& step) { @@ -81,7 +83,7 @@ void exposePathProjector() { }, (arg("distance"), arg("steeringMethod"), arg("step"))); def( - "createDichotomyProjector", + "DichotomyProjector", +[](const DistancePtr_t& distance, const PyWGraphSteeringMethodPtr_t& steeringMethodWrapper, const value_type& step) { @@ -91,7 +93,7 @@ void exposePathProjector() { (arg("distance"), arg("steeringMethod"), arg("step"))); def( - "createGlobalProjector", + "GlobalProjector", +[](const DistancePtr_t& distance, const PyWSteeringMethodPtr_t& steeringMethodWrapper, const value_type& step) { @@ -100,7 +102,7 @@ void exposePathProjector() { }, (arg("distance"), arg("steeringMethod"), arg("step"))); def( - "createGlobalProjector", + "GlobalProjector", +[](const DistancePtr_t& distance, const PyWGraphSteeringMethodPtr_t& steeringMethodWrapper, const value_type& step) { @@ -110,7 +112,7 @@ void exposePathProjector() { (arg("distance"), arg("steeringMethod"), arg("step"))); def( - "createRecursiveHermiteProjector", + "RecursiveHermiteProjector", +[](const DistancePtr_t& distance, const PyWSteeringMethodPtr_t& steeringMethodWrapper, const value_type& step) { @@ -119,7 +121,7 @@ void exposePathProjector() { }, (arg("distance"), arg("steeringMethod"), arg("step"))); def( - "createRecursiveHermiteProjector", + "RecursiveHermiteProjector", +[](const DistancePtr_t& distance, const PyWGraphSteeringMethodPtr_t& steeringMethodWrapper, const value_type& step) { diff --git a/src/pyhpp/manipulation/problem.cc b/src/pyhpp/manipulation/problem.cc index c2dec1cd..498daf70 100644 --- a/src/pyhpp/manipulation/problem.cc +++ b/src/pyhpp/manipulation/problem.cc @@ -39,26 +39,26 @@ #include #include +// DocNamespace(hpp::manipulation) + using namespace boost::python; namespace pyhpp { namespace manipulation { Problem::Problem(const PyWDevicePtr_t& robot) - : pyhpp::core::Problem(hpp::manipulation::Problem::create(robot->obj)) {} + : pyhpp::core::Problem( + hpp::manipulation::Problem::create(robot->asManipulationDevice())) {} Problem::Problem(const hpp::manipulation::ProblemPtr_t& object) : pyhpp::core::Problem(object) {} void Problem::constraintGraph(const PyWGraphPtr_t& graph) { asManipulationProblem()->constraintGraph(graph->obj); + graph_ = graph; } -PyWGraphPtr_t Problem::constraintGraph() const { - pyhpp::manipulation::PyWGraph* graph = - new PyWGraph(asManipulationProblem()->constraintGraph()); - return std::shared_ptr(graph); -} +PyWGraphPtr_t Problem::constraintGraph() const { return graph_; } void Problem::checkProblem() const { asManipulationProblem()->checkProblem(); } @@ -120,12 +120,12 @@ void Problem::graphSteeringMethod( // } void exposeProblem() { - boost::python::import("pyhpp.core"); + // DocClass(Problem) class_>("Problem", init()) .PYHPP_DEFINE_GETTER_SETTER_CONST_REF(Problem, constraintGraph, PyWGraphPtr_t) - .PYHPP_DEFINE_METHOD(Problem, checkProblem) + .def("checkProblem", &Problem::checkProblem, DocClassMethod(checkProblem)) .def( "steeringMethod", static_cast( diff --git a/src/pyhpp/manipulation/problem.hh b/src/pyhpp/manipulation/problem.hh index ae8df708..e04c165c 100644 --- a/src/pyhpp/manipulation/problem.hh +++ b/src/pyhpp/manipulation/problem.hh @@ -49,6 +49,7 @@ namespace manipulation { // Wrapper class for hpp::manipulation::Problem struct Problem : public pyhpp::core::Problem { hpp::core::Container jointAndShapes; + PyWGraphPtr_t graph_; Problem(const PyWDevicePtr_t& robot); Problem(const hpp::manipulation::ProblemPtr_t& object); diff --git a/src/pyhpp/manipulation/security_margins.py b/src/pyhpp/manipulation/security_margins.py new file mode 100644 index 00000000..8fcc22e9 --- /dev/null +++ b/src/pyhpp/manipulation/security_margins.py @@ -0,0 +1,219 @@ +#!/usr/bin/env python +# +# Copyright (c) 2020 CNRS, Airbus SAS +# Author: Florent Lamiraux +# + +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are +# met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in the +# documentation and/or other materials provided with the distribution. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR +# A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT +# HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, +# SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT +# LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, +# DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY +# THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +# (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +# OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH +# DAMAGE. + + +class SecurityMargins: + defaultMargin = 0 + separators = ["/"] + + def __init__(self, problem, factory, robotsAndObjects, robot): + self.problem = problem + self.robot = robot + self.factory = factory + self.robotsAndObjects = robotsAndObjects + self.marginMatrix = dict() + self.computeJoints() + self.computeGrippers() + self.computePossibleContacts() + + def computeJoints(self): + self.robotToJoints = dict() + model = self.robot.model() + jointNames = list(model.names) + for ro in self.robotsAndObjects: + le = len(ro) + self.robotToJoints[ro] = list( + filter( + lambda n: n[:le] == ro and n[le] in self.separators, + jointNames, + ) + ) + self.robotToJoints["universe"] = ["universe"] + self.jointToRobot = dict() + for ro, joints in self.robotToJoints.items(): + for j in joints: + self.jointToRobot[j] = ro + + def _getChildJoints(self, jointName): + model = self.robot.model() + try: + jointId = model.getJointId(jointName) + except Exception: + return [] + + childNames = [] + if jointId < len(model.children): + for childId in model.children[jointId]: + if childId < len(model.names): + childNames.append(model.names[childId]) + return childNames + + def _getGripperJoint(self, gripperName): + model = self.robot.model() + if model.existFrame(gripperName): + frameId = model.getFrameId(gripperName) + frame = model.frames[frameId] + parentJointId = frame.parentJoint + if parentJointId < len(model.names): + return model.names[parentJointId] + return None + + def computeGrippers(self): + self.gripperToRobot = dict() + self.gripperToJoints = dict() + for g in self.factory.grippers: + j = self._getGripperJoint(g) + if j is not None and j in self.jointToRobot: + self.gripperToRobot[g] = self.jointToRobot[j] + childJoints = self._getChildJoints(j) + self.gripperToJoints[g] = [j] + childJoints + else: + for joint_name in self.robot.model().names: + if g.startswith(joint_name) and joint_name in self.jointToRobot: + self.gripperToRobot[g] = self.jointToRobot[joint_name] + childJoints = self._getChildJoints(joint_name) + self.gripperToJoints[g] = [joint_name] + childJoints + break + + def computePossibleContacts(self): + self.contactSurfaces = dict() + for k in self.robotToJoints.keys(): + self.contactSurfaces[k] = list() + + for io, obj_name in enumerate(self.factory.objects): + if io < len(self.factory.contactsPerObjects): + for surface_name in self.factory.contactsPerObjects[io]: + if obj_name in self.contactSurfaces: + self.contactSurfaces[obj_name].append(surface_name) + + for surface_name in self.factory.envContacts: + found = False + for ro in list(self.robotsAndObjects) + ["universe"]: + for sep in self.separators: + prefix = ro + sep + if surface_name.startswith(prefix): + if ro in self.contactSurfaces: + self.contactSurfaces[ro].append(surface_name) + found = True + break + if found: + break + if not found: + self.contactSurfaces["universe"].append(surface_name) + + self.possibleContacts = list() + for o1, l1 in self.contactSurfaces.items(): + for o2, l2 in self.contactSurfaces.items(): + if o1 != o2 and len(l1) > 0 and len(l2) > 0: + self.possibleContacts.append((o1, o2)) + + def setSecurityMarginBetween(self, obj1, obj2, margin): + self.marginMatrix[frozenset([obj1, obj2])] = margin + + def getSecurityMarginBetween(self, obj1, obj2): + key = frozenset([obj1, obj2]) + return self.marginMatrix.get(key, self.defaultMargin) + + def getActiveConstraintsAlongEdge(self, edge): + factory = self.factory + graph = factory.graph + + result = graph.getNodesConnectedByTransition(edge) + from_name, to_name = result if isinstance(result, tuple) else (result, result) + + state_from = graph.getState(from_name) + state_to = graph.getState(to_name) + + constraints_from = graph.getNumericalConstraintsForState(state_from) + constraints_graph = graph.getNumericalConstraintsForGraph() + constraints_to = graph.getNumericalConstraintsForState(state_to) + + all_constraint_objects = set( + constraints_from + constraints_graph + constraints_to + ) + + active_names = [] + if hasattr(factory, "constraints") and hasattr( + factory.constraints, "available_constraints" + ): + constraint_dict = factory.constraints.available_constraints + for name, constraint_obj in constraint_dict.items(): + if constraint_obj in all_constraint_objects: + active_names.append(name) + + res = {"place": [], "grasp": []} + + for c in active_names: + for o in factory.objects: + if c == "place_" + o: + res["place"].append(o) + for g in factory.grippers: + for o, handle_indices in zip( + factory.objects, factory.handlesPerObjects + ): + for h_idx in handle_indices: + h = factory.handles[h_idx] + if c == g + " grasps " + h: + res["grasp"].append((g, o)) + + return res + + def apply(self): + factory = self.factory + graph = factory.graph + edges = graph.getTransitions() + + for edge in edges: + for i1, ro1 in enumerate([*self.robotsAndObjects, "universe"]): + for i2, ro2 in enumerate([*self.robotsAndObjects, "universe"]): + if i2 < i1: + continue + margin = self.getSecurityMarginBetween(ro1, ro2) + for j1 in self.robotToJoints[ro1]: + for j2 in self.robotToJoints[ro2]: + if j1 != j2: + graph.setSecurityMarginForTransition( + edge, j1, j2, margin + ) + + constraints = self.getActiveConstraintsAlongEdge(edge) + + for g, ro1 in constraints["grasp"]: + if g in self.gripperToJoints and ro1 in self.robotToJoints: + for j1 in self.robotToJoints[ro1]: + for j2 in self.gripperToJoints[g]: + graph.setSecurityMarginForTransition(edge, j1, j2, 0) + + for o1 in constraints["place"]: + for o2, o3 in self.possibleContacts: + if o1 == o2: + for j1 in self.robotToJoints[o1]: + for j2 in self.robotToJoints[o3]: + graph.setSecurityMarginForTransition(edge, j1, j2, 0) diff --git a/src/pyhpp/manipulation/steering-method.cc b/src/pyhpp/manipulation/steering-method.cc index d64a2e17..ecd88083 100644 --- a/src/pyhpp/manipulation/steering-method.cc +++ b/src/pyhpp/manipulation/steering-method.cc @@ -29,8 +29,12 @@ #include <../src/pyhpp/manipulation/steering-method.hh> #include +#include +#include #include +// DocNamespace(hpp::manipulation) + namespace pyhpp { namespace manipulation { @@ -43,10 +47,43 @@ GraphSteeringMethod::GraphSteeringMethod( obj = graph; } -void exposeGraphSteeringMethod() { +hpp::manipulation::steeringMethod::EndEffectorTrajectoryPtr_t +EndEffectorTrajectorySteeringMethod::eetObj() const { + assert(HPP_DYNAMIC_PTR_CAST( + hpp::manipulation::steeringMethod::EndEffectorTrajectory, obj)); + return HPP_STATIC_PTR_CAST( + hpp::manipulation::steeringMethod::EndEffectorTrajectory, obj); +} + +EndEffectorTrajectorySteeringMethod::EndEffectorTrajectorySteeringMethod( + const hpp::core::ProblemConstPtr_t& problem) + : SteeringMethod( + hpp::manipulation::steeringMethod::EndEffectorTrajectory::create( + problem)) {} + +void EndEffectorTrajectorySteeringMethod::setTrajectoryConstraint( + const hpp::constraints::ImplicitPtr_t& ic) { + eetObj()->trajectoryConstraint(ic); +} + +void EndEffectorTrajectorySteeringMethod::setTrajectory( + const hpp::core::PathPtr_t& eeTraj, bool se3Output) { + eetObj()->trajectory(eeTraj, se3Output); +} + +void exposeManipSteeringMethod() { class_( "GraphSteeringMethod", boost::python::init()); + + class_>( + "EndEffectorTrajectorySteeringMethod", + boost::python::init()) + .def("setTrajectoryConstraint", + &EndEffectorTrajectorySteeringMethod::setTrajectoryConstraint) + .def("setTrajectory", + &EndEffectorTrajectorySteeringMethod::setTrajectory); } } // namespace manipulation diff --git a/src/pyhpp/manipulation/steering-method.hh b/src/pyhpp/manipulation/steering-method.hh index 70a3b515..ffb4a1c9 100644 --- a/src/pyhpp/manipulation/steering-method.hh +++ b/src/pyhpp/manipulation/steering-method.hh @@ -19,9 +19,13 @@ #ifndef PYHPP_MANIPULATION_STEERING_METHOD_HH #define PYHPP_MANIPULATION_STEERING_METHOD_HH +#include +#include #include +#include #include #include +#include namespace pyhpp { namespace manipulation { @@ -31,11 +35,16 @@ typedef pyhpp::core::PyWSteeringMethodPtr_t PyWSteeringMethodPtr_t; struct GraphSteeringMethod { hpp::manipulation::steeringMethod::GraphPtr_t obj; - GraphSteeringMethod(const PyWSteeringMethodPtr_t& steeringMethodWrapper); }; +struct EndEffectorTrajectorySteeringMethod : pyhpp::core::SteeringMethod { + hpp::manipulation::steeringMethod::EndEffectorTrajectoryPtr_t eetObj() const; + EndEffectorTrajectorySteeringMethod( + const hpp::core::ProblemConstPtr_t& problem); + void setTrajectoryConstraint(const hpp::constraints::ImplicitPtr_t& ic); + void setTrajectory(const hpp::core::PathPtr_t& eeTraj, bool se3Output); +}; } // namespace manipulation } // namespace pyhpp - #endif // PYHPP_MANIPULATION_STEERING_METHOD_HH diff --git a/src/pyhpp/manipulation/urdf/util.cc b/src/pyhpp/manipulation/urdf/util.cc index e6d42219..60c8561e 100644 --- a/src/pyhpp/manipulation/urdf/util.cc +++ b/src/pyhpp/manipulation/urdf/util.cc @@ -51,7 +51,8 @@ void loadModel(const Device& robot, const FrameIndex& baseFrame, hpp::pinocchio::urdf::loadModel(robot.obj, baseFrame, prefix, rootType, urdfPath, srdfPath, bMr); if (!std::string(srdfPath).empty()) { - hpp::manipulation::srdf::loadModelFromFile(robot.obj, prefix, srdfPath); + hpp::manipulation::srdf::loadModelFromFile(robot.asManipulationDevice(), + prefix, srdfPath); } } @@ -61,7 +62,8 @@ void loadModelFromString(const Device& robot, const FrameIndex& baseFrame, const std::string& srdfString, const SE3& bMr) { hpp::pinocchio::urdf::loadModelFromString( robot.obj, baseFrame, prefix, rootType, urdfString, srdfString, bMr); - hpp::manipulation::srdf::loadModelFromXML(robot.obj, prefix, srdfString); + hpp::manipulation::srdf::loadModelFromXML(robot.asManipulationDevice(), + prefix, srdfString); } void exposeUtil() { diff --git a/src/pyhpp/pinocchio/device.cc b/src/pyhpp/pinocchio/device.cc index 5c1fc1cb..24797888 100644 --- a/src/pyhpp/pinocchio/device.cc +++ b/src/pyhpp/pinocchio/device.cc @@ -27,6 +27,7 @@ // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED // OF THE POSSIBILITY OF SUCH DAMAGE. +#include <../src/pyhpp/pinocchio/device.hh> #include #include #include @@ -41,6 +42,8 @@ #include #include +// DocNamespace(hpp::pinocchio) + // #include // #include @@ -56,22 +59,13 @@ namespace pyhpp { namespace pinocchio { namespace bp = boost::python; -typedef hpp::pinocchio::Model Model; -typedef hpp::pinocchio::Data Data; -typedef hpp::pinocchio::ModelPtr_t ModelPtr_t; -typedef hpp::pinocchio::GeomData GeomData; -typedef hpp::pinocchio::LiegroupSpacePtr_t LiegroupSpacePtr_t; -typedef hpp::pinocchio::GeomModel GeomModel; -typedef hpp::pinocchio::Configuration_t Configuration_t; -typedef hpp::pinocchio::ConfigurationIn_t ConfigurationIn_t; -typedef hpp::pinocchio::size_type size_type; -typedef hpp::pinocchio::Transform3s Transform3s; -typedef hpp::pinocchio::Device Device; typedef hpp::pinocchio::DevicePtr_t DevicePtr_t; typedef hpp::pinocchio::GripperPtr_t GripperPtr_t; typedef hpp::pinocchio::Gripper Gripper; -typedef hpp::pinocchio::Computation_t Computation_t; -typedef hpp::pinocchio::value_type value_type; +typedef hpp::pinocchio::FrameIndex FrameIndex; +typedef hpp::pinocchio::SE3 SE3; +typedef hpp::pinocchio::Frame Frame; +typedef hpp::pinocchio::JointPtr_t JointPtr_t; using namespace boost::python; @@ -91,12 +85,18 @@ Transform3s getObjectPositionInJoint(const GripperPtr_t& gripper) { return res; } -typedef hpp::pinocchio::Frame Frame; -typedef hpp::pinocchio::JointPtr_t JointPtr_t; +static hpp::pinocchio::vector3_t getCenterOfMass(Device& d) { + try { + d.computeForwardKinematics(hpp::pinocchio::COM); + return d.obj->positionCenterOfMass(); + } catch (const std::exception& exc) { + throw std::logic_error(exc.what()); + } +} static void setJointBounds(Device& device, const char* jointName, boost::python::list py_jointBounds) { - Frame frame = device.getFrameByName(jointName); + Frame frame = device.obj->getFrameByName(jointName); JointPtr_t joint = frame.joint(); auto jointBounds = extract_vector(py_jointBounds); @@ -122,21 +122,186 @@ static void setJointBounds(Device& device, const char* jointName, } } +static boost::python::list getJointPosition(Device& device, + const std::string& jointName) { + try { + const Model& model(device.model()); + const Data& data(device.data()); + + if (!model.existFrame(jointName)) { + throw std::logic_error("Robot has no frame with name " + jointName); + } + + FrameIndex joint = model.getFrameId(jointName); + if (model.frames.size() <= (std::size_t)joint) + throw std::logic_error("Frame index of joint " + jointName + + " out of bounds: " + std::to_string(joint)); + + const SE3& M = data.oMf[joint]; + Transform3s::Quaternion q(M.rotation()); + + boost::python::list t; + t.append(M.translation()[0]); + t.append(M.translation()[1]); + t.append(M.translation()[2]); + t.append(q.x()); + t.append(q.y()); + t.append(q.z()); + t.append(q.w()); + + return t; + } catch (const std::exception& exc) { + throw std::logic_error(exc.what()); + } +} + +static boost::python::dict rankInConfiguration(Device& device) { + boost::python::dict rank_dict; + try { + auto joint_names = device.model().names; + for (const auto& joint_name : joint_names) { + Frame frame = device.obj->getFrameByName(joint_name.c_str()); + if (!frame.isFixed()) { + JointPtr_t joint = frame.joint(); + if (joint) { + rank_dict[joint_name] = joint->rankInConfiguration(); + } + } + } + } catch (const std::exception& exc) { + throw std::logic_error(exc.what()); + } + return rank_dict; +} + +static boost::python::list getJointsPosition( + Device& device, const Configuration_t& dofArray, + const boost::python::list& jointNames) { + try { + device.currentConfiguration(dofArray); + device.computeForwardKinematics(hpp::pinocchio::JOINT_POSITION); + device.computeFramesForwardKinematics(); + + const Model& model(device.model()); + const Data& data(device.data()); + boost::python::list transforms; + for (Py_ssize_t i = 0; + i < static_cast(boost::python::len(jointNames)); ++i) { + std::string n = boost::python::extract(jointNames[i]); + if (!model.existFrame(n)) { + throw std::logic_error("Robot has no frame with name " + n); + } + FrameIndex joint = model.getFrameId(n); + if (model.frames.size() <= (std::size_t)joint) + throw std::logic_error("Frame index of joint " + n + + " out of bounds: " + std::to_string(joint)); + const SE3& M = data.oMf[joint]; + Transform3s::Quaternion q(M.rotation()); + boost::python::list t; + t.append(M.translation()[0]); + t.append(M.translation()[1]); + t.append(M.translation()[2]); + t.append(q.x()); + t.append(q.y()); + t.append(q.z()); + t.append(q.w()); + transforms.append(t); + } + return transforms; + } catch (const std::exception& exc) { + throw std::logic_error(exc.what()); + } +} + +static std::string getGripperName(const GripperPtr_t& gripper) { + return gripper->name(); +} + +static JointIndex getParentJointId(const GripperPtr_t& gripper) { + assert(gripper->joint()); + assert(gripper->joint()->robot()); + Model model(gripper->joint()->robot()->model()); + return model.frames[model.getFrameId(gripper->name())].parentJoint; +} + void exposeGripper() { + // DocClass(Gripper) class_("Gripper", no_init) - .def("create", &Gripper::create) - .staticmethod("create") .add_property("localPosition", &getObjectPositionInJoint) .add_property( "clearance", static_cast(&Gripper::clearance), static_cast( - &Gripper::clearance)); + &Gripper::clearance)) + .def("name", &getGripperName) + .def("getParentJointId", &getParentJointId, + "Get index of the joint the handle is attached to" + " in pinocchio Model"); class_ >("GripperMap") .def( boost::python::map_indexing_suite, true>()); } + +static boost::shared_ptr createDevice(const std::string& name) { + return boost::make_shared(hpp::pinocchio::Device::create(name)); +} + +struct DeviceWrapperConverter { + static void* convertible(PyObject* obj_ptr) { + boost::python::object obj(boost::python::borrowed(obj_ptr)); + boost::python::extract extractor(obj); + return extractor.check() ? obj_ptr : nullptr; + } + + static void construct_shared_ptr( + PyObject* obj_ptr, + boost::python::converter::rvalue_from_python_stage1_data* data) { + boost::python::object obj(boost::python::borrowed(obj_ptr)); + boost::python::extract extractor(obj); + + const Device& wrapper = extractor(); + typedef std::shared_ptr PinDevicePtr_t; + typedef boost::python::converter::rvalue_from_python_storage + StorageType; + + void* storage = ((StorageType*)data)->storage.bytes; + new (storage) PinDevicePtr_t(wrapper.obj); + data->convertible = storage; + } + + static void construct_const_shared_ptr( + PyObject* obj_ptr, + boost::python::converter::rvalue_from_python_stage1_data* data) { + boost::python::object obj(boost::python::borrowed(obj_ptr)); + boost::python::extract extractor(obj); + + const Device& wrapper = extractor(); + typedef std::shared_ptr ConstPinDevicePtr_t; + typedef boost::python::converter::rvalue_from_python_storage< + ConstPinDevicePtr_t> + StorageType; + + void* storage = ((StorageType*)data)->storage.bytes; + new (storage) ConstPinDevicePtr_t(wrapper.obj); + data->convertible = storage; + } +}; + +void register_device_converters() { + typedef std::shared_ptr PinDevicePtr_t; + typedef std::shared_ptr ConstPinDevicePtr_t; + + boost::python::converter::registry::push_back( + &DeviceWrapperConverter::convertible, + &DeviceWrapperConverter::construct_shared_ptr, + boost::python::type_id()); + + boost::python::converter::registry::push_back( + &DeviceWrapperConverter::convertible, + &DeviceWrapperConverter::construct_const_shared_ptr, + boost::python::type_id()); +} void exposeDevice() { enum_("ComputationFlag") .value("JOINT_POSITION", hpp::pinocchio::JOINT_POSITION) @@ -146,38 +311,44 @@ void exposeDevice() { .value("COM", hpp::pinocchio::COM) .value("COMPUTE_ALL", hpp::pinocchio::COMPUTE_ALL); void (Device::*cfk)(int) = &Device::computeForwardKinematics; - class_("Device", no_init) - .def("name", &Device::name, return_value_policy()) - .def("create", &Device::create) - .staticmethod("create") + + // DocClass(Device) + class_, boost::noncopyable>("Device", + no_init) + .def("__init__", make_constructor(&createDevice)) + .def("name", &Device::name, return_value_policy(), + DocClassMethod(name)) .def("configSpace", &Device::configSpace, - return_value_policy()) - .def("model", static_cast(&Device::model), - return_internal_reference<>()) - .def("data", static_cast(&Device::data), - return_internal_reference<>()) - .def("geomData", static_cast(&Device::geomData), - return_internal_reference<>()) - .def("geomModel", - static_cast(&Device::geomModel), - return_internal_reference<>()) - .def("visualModel", - static_cast(&Device::visualModel), - return_internal_reference<>()) - .PYHPP_DEFINE_METHOD(Device, configSize) - .PYHPP_DEFINE_METHOD(Device, numberDof) + return_value_policy(), DocClassMethod(configSpace)) + .def("model", &Device::model, return_internal_reference<>()) + .def("data", &Device::data, return_internal_reference<>()) + .def("geomData", &Device::geomData, return_internal_reference<>()) + .def("geomModel", &Device::geomModel, return_internal_reference<>()) + .def("configSize", &Device::configSize, DocClassMethod(configSize)) + .def("numberDof", &Device::numberDof, DocClassMethod(numberDof)) + .def("visualModel", &Device::visualModel, return_internal_reference<>()) .def("currentConfiguration", static_cast( &Device::currentConfiguration), - return_value_policy()) + return_value_policy(), + DocClassMethod(currentConfiguration)) .def("currentConfiguration", Device_currentConfiguration) - .def("computeForwardKinematics", cfk) + .def("computeForwardKinematics", cfk, + DocClassMethod(computeForwardKinematics)) .def("computeFramesForwardKinematics", - &Device::computeFramesForwardKinematics) - .def("updateGeometryPlacements", &Device::updateGeometryPlacements) - .def("setJointBounds", &setJointBounds); + &Device::computeFramesForwardKinematics, + DocClassMethod(computeFramesForwardKinematics)) + .def("updateGeometryPlacements", &Device::updateGeometryPlacements, + DocClassMethod(updateGeometryPlacements)) + .add_property("rankInConfiguration", &rankInConfiguration) + .def("setJointBounds", &setJointBounds) + .def("getCenterOfMass", &getCenterOfMass) + .def("getJointPosition", &getJointPosition) + .def("getJointsPosition", &getJointsPosition) + .def("removeJoints", &Device::removeJoints); + register_device_converters(); } } // namespace pinocchio } // namespace pyhpp diff --git a/src/pyhpp/pinocchio/device.hh b/src/pyhpp/pinocchio/device.hh new file mode 100644 index 00000000..f695767c --- /dev/null +++ b/src/pyhpp/pinocchio/device.hh @@ -0,0 +1,74 @@ +// +// Copyright (c) 2025, CNRS +// Authors: Paul Sardin +// + +#ifndef PYHPP_PINOCCHIO_DEVICE_HH +#define PYHPP_PINOCCHIO_DEVICE_HH + +#include +#include +#include + +namespace pyhpp { +namespace pinocchio { + +typedef hpp::pinocchio::Model Model; +typedef hpp::pinocchio::Data Data; +typedef hpp::pinocchio::ModelPtr_t ModelPtr_t; +typedef hpp::pinocchio::GeomData GeomData; +typedef hpp::pinocchio::LiegroupSpacePtr_t LiegroupSpacePtr_t; +typedef hpp::pinocchio::GeomModel GeomModel; +typedef hpp::pinocchio::Configuration_t Configuration_t; +typedef hpp::pinocchio::ConfigurationIn_t ConfigurationIn_t; +typedef hpp::pinocchio::JointIndex JointIndex; +typedef hpp::pinocchio::size_type size_type; +typedef hpp::pinocchio::Transform3s Transform3s; +typedef hpp::pinocchio::DevicePtr_t DevicePtr_t; +typedef hpp::pinocchio::Computation_t Computation_t; +typedef hpp::pinocchio::value_type value_type; + +struct Device { + DevicePtr_t obj; + + Device(const DevicePtr_t& object) : obj(object) {} + + const std::string& name() const { return obj->name(); } + const LiegroupSpacePtr_t& configSpace() const { return obj->configSpace(); } + Model& model() { return obj->model(); } + Data& data() { return obj->data(); } + GeomData& geomData() { return obj->geomData(); } + GeomModel& geomModel() { return obj->geomModel(); } + GeomModel& visualModel() { return obj->visualModel(); } + size_type configSize() const { return obj->configSize(); } + size_type numberDof() const { return obj->numberDof(); } + const Configuration_t& currentConfiguration() const { + return obj->currentConfiguration(); + } + bool currentConfiguration(ConfigurationIn_t configuration) { + return obj->currentConfiguration(configuration); + } + void computeForwardKinematics(int flag) { + obj->computeForwardKinematics(flag); + } + void computeFramesForwardKinematics() { + obj->computeFramesForwardKinematics(); + } + void updateGeometryPlacements() { obj->updateGeometryPlacements(); } + void removeJoints(const std::vector& jointNames, + Configuration_t q) { + obj->removeJoints(jointNames, q); + } + hpp::manipulation::DevicePtr_t asManipulationDevice() const { + auto manipDevice = HPP_DYNAMIC_PTR_CAST(hpp::manipulation::Device, obj); + if (!manipDevice) { + throw std::runtime_error("Not a manipulation device²"); + } + return manipDevice; + } +}; + +} // namespace pinocchio +} // namespace pyhpp + +#endif // PYHPP_PINOCCHIO_DEVICE_HH diff --git a/src/pyhpp/pinocchio/liegroup.cc b/src/pyhpp/pinocchio/liegroup.cc index e51984e1..1c973878 100644 --- a/src/pyhpp/pinocchio/liegroup.cc +++ b/src/pyhpp/pinocchio/liegroup.cc @@ -36,6 +36,8 @@ #include #include +// DocNamespace(hpp::pinocchio) + using namespace boost::python; namespace pyhpp { @@ -154,39 +156,43 @@ struct LgERWrapper { }; void exposeLiegroup() { + // DocClass(LiegroupSpace) class_("LiegroupSpace", no_init) .def("__str__", &to_str_from_operator) - .def("name", &LiegroupSpace::name, return_value_policy()) - .PYHPP_DEFINE_METHOD(LiegroupSpace, Rn) + .def("name", &LiegroupSpace::name, return_value_policy(), + DocClassMethod(name)) + .def("Rn", &LiegroupSpace::Rn, DocClassMethod(Rn)) .staticmethod("Rn") - .PYHPP_DEFINE_METHOD(LiegroupSpace, R1) + .def("R1", &LiegroupSpace::R1, DocClassMethod(R1)) .staticmethod("R1") - .PYHPP_DEFINE_METHOD(LiegroupSpace, R2) + .def("R2", &LiegroupSpace::R2, DocClassMethod(R2)) .staticmethod("R2") - .PYHPP_DEFINE_METHOD(LiegroupSpace, R3) + .def("R3", &LiegroupSpace::R3, DocClassMethod(R3)) .staticmethod("R3") - .PYHPP_DEFINE_METHOD(LiegroupSpace, SE2) + .def("SE2", &LiegroupSpace::SE2, DocClassMethod(SE2)) .staticmethod("SE2") - .PYHPP_DEFINE_METHOD(LiegroupSpace, SE3) + .def("SE3", &LiegroupSpace::SE3, DocClassMethod(SE3)) .staticmethod("SE3") - .PYHPP_DEFINE_METHOD(LiegroupSpace, R2xSO2) + .def("R2xSO2", &LiegroupSpace::R2xSO2, DocClassMethod(R2xSO2)) .staticmethod("R2xSO2") - .PYHPP_DEFINE_METHOD(LiegroupSpace, R3xSO3) + .def("R3xSO3", &LiegroupSpace::R3xSO3, DocClassMethod(R3xSO3)) .staticmethod("R3xSO3") - .PYHPP_DEFINE_METHOD(LiegroupSpace, empty) + .def("empty", &LiegroupSpace::empty, DocClassMethod(empty)) .staticmethod("empty") - .PYHPP_DEFINE_METHOD(LiegroupSpace, mergeVectorSpaces) - .PYHPP_DEFINE_METHOD(LgSWrapper, dIntegrate_dq) - .PYHPP_DEFINE_METHOD(LgSWrapper, dIntegrate_dv) - .PYHPP_DEFINE_METHOD(LgSWrapper, dDifference_dq0) - .PYHPP_DEFINE_METHOD(LgSWrapper, dDifference_dq1) + .def("mergeVectorSpaces", &LiegroupSpace::mergeVectorSpaces, + DocClassMethod(mergeVectorSpaces)) + .def("dIntegrate_dq", &LgSWrapper::dIntegrate_dq) + .def("dIntegrate_dv", &LgSWrapper::dIntegrate_dv) + .def("dDifference_dq0", &LgSWrapper::dDifference_dq0) + .def("dDifference_dq1", &LgSWrapper::dDifference_dq1) .def(self == self) .def(self != self) // Operation on shared pointers... .def("__imul__", &LgSWrapper::itimes) .def("__mul__", &LgSWrapper::times); + // DocClass(LiegroupElement) class_("LiegroupElement", init()) .def(init()) @@ -199,12 +205,13 @@ void exposeLiegroup() { .def("vector", static_cast( &LiegroupElement::vector), - return_value_policy()) + return_value_policy(), DocClassMethod(vector)) .def("space", &LiegroupElement::space, - return_value_policy()) + return_value_policy(), DocClassMethod(space)) .def(self - self) .def(self + vector_t()); + // DocClass(LiegroupElementRef) class_("LiegroupElementRef", init()) // Pythonic API @@ -216,9 +223,9 @@ void exposeLiegroup() { .def("vector", static_cast( &LiegroupElementRef::vector), - return_value_policy()) + return_value_policy(), DocClassMethod(vector)) .def("space", &LiegroupElementRef::space, - return_value_policy()) + return_value_policy(), DocClassMethod(space)) .def(self - self) .def(self + vector_t()); } diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index be168d4b..eb6e472b 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -12,24 +12,29 @@ # details. You should have received a copy of the GNU Lesser General Public # License along with hpp-python If not, see . -set(PYTHON_UNIT_TESTS - imports - liegroup - load_ur3 - differentiable-function - path-planner - rrt - path - constraint_graph - graph_factory2) +# ============================================================================= +# unittest-based unit tests (uses Python's built-in unittest framework) +# ============================================================================= -foreach(UNIT_TEST ${PYTHON_UNIT_TESTS}) - add_python_unit_test(${UNIT_TEST} tests/${UNIT_TEST}.py src) -endforeach() +# Compute Python path for unit tests +compute_pythonpath(UNITTEST_ENV_VARIABLES src) -set_tests_properties( - ${PYTHON_UNIT_TESTS} - PROPERTIES - ENVIRONMENT_MODIFICATION - "ROS_PACKAGE_PATH=path_list_prepend:${example-robot-data_INCLUDE_DIRS}/../share;ROS_PACKAGE_PATH=path_list_prepend:${hpp-environments_INCLUDE_DIRS}/../share;" -) +# Discover and add individual test files +file(GLOB UNITTEST_TEST_FILES "${CMAKE_CURRENT_SOURCE_DIR}/unit/test_*.py") +foreach(TEST_FILE ${UNITTEST_TEST_FILES}) + get_filename_component(TEST_NAME ${TEST_FILE} NAME_WE) + add_test( + NAME unittest_${TEST_NAME} + COMMAND ${PYTHON_EXECUTABLE} -m unittest discover -s + ${CMAKE_CURRENT_SOURCE_DIR} -p "${TEST_NAME}.py" -v + WORKING_DIRECTORY ${PROJECT_SOURCE_DIR}) + set_tests_properties( + unittest_${TEST_NAME} + PROPERTIES + ENVIRONMENT + "${UNITTEST_ENV_VARIABLES}" + ENVIRONMENT_MODIFICATION + "ROS_PACKAGE_PATH=path_list_prepend:${example-robot-data_INCLUDE_DIRS}/../share;ROS_PACKAGE_PATH=path_list_prepend:${hpp-environments_INCLUDE_DIRS}/../share;" + LABELS + "python;unit") +endforeach() diff --git a/tests/constraint_graph.py b/tests/constraint_graph.py deleted file mode 100644 index edc5b47c..00000000 --- a/tests/constraint_graph.py +++ /dev/null @@ -1,168 +0,0 @@ -from pyhpp.manipulation import Device, urdf, Graph, Problem -from pyhpp.core import ConfigurationShooter # noqa: F401 -import numpy as np -from pinocchio import SE3, StdVec_Bool as Mask, Quaternion - - -from pyhpp.constraints import ( - RelativeTransformation, - Transformation, - ComparisonTypes, - ComparisonType, - BySubstitution, - Implicit, -) - -urdfFilename = ( - "package://example-robot-data/robots/ur_description/urdf/ur5_gripper.urdf" -) -srdfFilename = ( - "package://example-robot-data/robots/ur_description/srdf/ur5_gripper.srdf" -) - -urdfFilenameBall = "package://hpp_environments/urdf/ur_benchmark/pokeball.urdf" -srdfFilenameBall = "package://hpp_environments/srdf/ur_benchmark/pokeball.srdf" - -r0_pose = SE3(rotation=np.identity(3), translation=np.array([-0.25, 0, 0])) -r1_pose = SE3(rotation=np.identity(3), translation=np.array([1, 0, 0])) - -robot = Device("bot") - -urdf.loadModel(robot, 0, "ur5", "anchor", urdfFilename, srdfFilename, r0_pose) -urdf.loadModel( - robot, 0, "pokeball", "freeflyer", urdfFilenameBall, srdfFilenameBall, r1_pose -) - - -robot.setJointBounds( - "pokeball/root_joint", - [ - -0.4, - 0.4, - -0.4, - 0.4, - -0.1, - 1.0, - -1.0001, - 1.0001, - -1.0001, - 1.0001, - -1.0001, - 1.0001, - -1.0001, - 1.0001, - ], -) - -problem = Problem(robot) - -graph = Graph("graph", robot, problem) - -# Create states -state_placement = graph.createState("placement", False, 0) -state_grasp_placement = graph.createState("grasp-placement", False, 0) -state_gripper_above_ball = graph.createState("gripper-above-ball", False, 0) -state_ball_above_ground = graph.createState("ball-above-ground", False, 0) -state_grasp = graph.createState("grasp", False, 0) - -# Create transitions -transitions_transit = graph.createTransition( - state_placement, state_placement, "transit", 1, state_placement -) - -transitions_approach_ball = graph.createTransition( - state_placement, state_gripper_above_ball, "approach-ball", 1, state_placement -) -transitions_move_gripper_away = graph.createTransition( - state_gripper_above_ball, state_placement, "move-gripper-away", 1, state_placement -) -transitions_grasp_ball = graph.createTransition( - state_gripper_above_ball, state_grasp_placement, "grasp-ball", 1, state_placement -) -transitions_move_gripper_up = graph.createTransition( - state_grasp_placement, - state_gripper_above_ball, - "move-gripper-up", - 1, - state_placement, -) -transitions_take_ball_up = graph.createTransition( - state_grasp_placement, state_ball_above_ground, "take-ball-up", 1, state_grasp -) -transitions_put_ball_down = graph.createTransition( - state_ball_above_ground, state_grasp_placement, "put-ball-down", 1, state_grasp -) -transitions_take_ball_away = graph.createTransition( - state_ball_above_ground, state_grasp, "take-ball-away", 1, state_grasp -) -transitions_approach_ground = graph.createTransition( - state_grasp, state_ball_above_ground, "approach-ground", 1, state_grasp -) -transitions_transfer = graph.createTransition( - state_grasp, state_grasp, "transfer", 1, state_grasp -) - -joint2 = robot.model().getJointId("pokeball/root_joint") -joint1 = robot.model().getJointId("ur5/wrist_3_joint") -Id = SE3.Identity() - -m = [ - False, - False, - True, - True, - True, - False, -] -q = Quaternion(0, 0, 0, 1) -ballGround = SE3(q, np.array([0, 0, 0.15])) -pc = Transformation.create( - "placementConstraint", robot.asPinDevice(), joint2, Id, ballGround, m -) -cts = ComparisonTypes() -cts[:] = ( - ComparisonType.EqualToZero, - ComparisonType.EqualToZero, - ComparisonType.EqualToZero, -) -implicit_mask = [True, True, True] -implicitPlacementConstraint = Implicit.create(pc, cts, implicit_mask) - - -q = Quaternion(0.5, 0.5, -0.5, 0.5) -ballInGripper = SE3(q, np.array([0, 0.137, 0])) -m = Mask() -m[:] = (True,) * 6 -pc = RelativeTransformation.create( - "grasp", robot.asPinDevice(), joint1, joint2, ballInGripper, Id, m -) -cts = ComparisonTypes() -cts[:] = ( - ComparisonType.EqualToZero, - ComparisonType.EqualToZero, - ComparisonType.EqualToZero, - ComparisonType.EqualToZero, - ComparisonType.EqualToZero, - ComparisonType.EqualToZero, -) -implicitGraspConstraint = Implicit.create(pc, cts, m) - - -# Set constraints of nodes and edges -graph.addNumericalConstraint(state_placement, implicitPlacementConstraint) -graph.addNumericalConstraint(state_grasp, implicitGraspConstraint) - -graph.maxIterations(100) -graph.errorThreshold(0.00001) - -graph.initialize() - -configurationShooter = problem.configurationShooter() -solver = BySubstitution(robot.configSpace()) - -for i in range(100): - q = configurationShooter.shoot() - res = graph.applyStateConstraints(state_placement, q) - if res.success: - print("after applying constraints: ", res.configuration) - break diff --git a/tests/construction_set.py b/tests/construction_set.py deleted file mode 100644 index 35463043..00000000 --- a/tests/construction_set.py +++ /dev/null @@ -1,96 +0,0 @@ -from math import pi -import numpy as np -from pinocchio import SE3 -from pyhpp.manipulation import Device, urdf - -# Load two UR3 robots -urdfFilename = ( - "package://example-robot-data/robots/ur_description/urdf/ur3_gripper.urdf" -) -srdfFilename = ( - "package://example-robot-data/robots/ur_description/srdf/ur3_gripper.srdf" -) - -r0_pose = SE3(rotation=np.identity(3), translation=np.array([-0.25, 0, 0])) -r1_pose = SE3(rotation=np.identity(3), translation=np.array([0.25, 0, 0])) -robot = Device("construction-set") -urdf.loadModel(robot, 0, "r0", "anchor", urdfFilename, srdfFilename, r0_pose) -urdf.loadModel(robot, 0, "r1", "anchor", urdfFilename, srdfFilename, r1_pose) - -# Load environment -urdfFilename = "package://hpp_environments/urdf/construction_set/ground.urdf" -srdfFilename = "package://hpp_environments/srdf/construction_set/ground.srdf" -urdf.loadModel(robot, 0, "ground", "anchor", urdfFilename, srdfFilename, SE3.Identity()) - -# Load spheres -nSphere = 2 -nCylinder = 2 - -objects = list() -urdfFilename = "package://hpp_environments/urdf/construction_set/sphere.urdf" -srdfFilename = "package://hpp_environments/srdf/construction_set/sphere.srdf" -for i in range(nSphere): - urdf.loadModel( - robot, 0, f"sphere{i}", "freeflyer", urdfFilename, srdfFilename, SE3.Identity() - ) - objects.append(f"sphere{i}") - -# Load cylinders -urdfFilename = "package://hpp_environments/urdf/construction_set/cylinder_08.urdf" -srdfFilename = "package://hpp_environments/srdf/construction_set/cylinder_08.srdf" -for i in range(nCylinder): - urdf.loadModel( - robot, - 0, - f"cylinder{i}", - "freeflyer", - urdfFilename, - srdfFilename, - SE3.Identity(), - ) - objects.append(f"cylinder{i}") - -# Set joint bounds -# Set robot joint bounds -jointBounds = { - "r0/shoulder_pan_joint": [-pi, 4], - "r1/shoulder_pan_joint": [-pi, 4], - "r0/shoulder_lift_joint": [-pi, 0], - "r1/shoulder_lift_joint": [-pi, 0], - "r0/elbow_joint": [-2.6, 2.6], - "r1/elbow_joint": [-2.6, 2.6], -} -m = robot.model() -lowerLimit = m.lowerPositionLimit -upperLimit = m.upperPositionLimit -for jn, [lower, upper] in jointBounds.items(): - lowerLimit[m.getJointId(jn)] = lower - upperLimit[m.getJointId(jn)] = upper - -for i in range(nSphere): - ij = m.getJointId(f"sphere{i}/root_joint") - iq = m.idx_qs[ij] - nq = m.nqs[ij] - lowerLimit[iq : iq + nq] = [-1.0, -1.0, -0.1, -1.0001, -1.0001, -1.0001, -1.0001] - upperLimit[iq : iq + nq] = [1.0, 1.0, 0.1, 1.0001, 1.0001, 1.0001, 1.0001] - -for i in range(nCylinder): - ij = m.getJointId(f"cylinder{i}/root_joint") - iq = m.idx_qs[ij] - nq = m.nqs[ij] - lowerLimit[iq : iq + nq] = [-1.0, -1.0, -0.1, -1.0001, -1.0001, -1.0001, -1.0001] - upperLimit[iq : iq + nq] = [1.0, 1.0, 0.1, 1.0001, 1.0001, 1.0001, 1.0001] - -m.lowerPositionLimit = lowerLimit -m.upperPositionLimit = upperLimit - -grippers = robot.grippers() -grippers["r0/gripper"].localPosition - -handles = robot.handles() -handles["sphere0/magnet"].localPosition - -h = handles["sphere1/magnet"] -g = grippers["cylinder0/magnet1"] - -c = h.createGrasp(g, "sphere1/magnet grasps cylinder0/magnet1") diff --git a/tests/corbaserver-client.py b/tests/corbaserver-client.py deleted file mode 100644 index c9ad45da..00000000 --- a/tests/corbaserver-client.py +++ /dev/null @@ -1,4 +0,0 @@ -from hpp.corbaserver import Client - -cl = Client() -cl._tools.shutdown() diff --git a/tests/corbaserver.py b/tests/corbaserver.py deleted file mode 100644 index a6190d10..00000000 --- a/tests/corbaserver.py +++ /dev/null @@ -1,12 +0,0 @@ -from pyhpp.core import ProblemSolver -from pyhpp.corbaserver import Server - -ps = ProblemSolver.create() - -for i in range(3): - print(i) - server = Server(ps, False) - server.initialize() - server.startCorbaServer() - server.processRequest(True) - del server diff --git a/tests/differentiable-function.py b/tests/differentiable-function.py deleted file mode 100644 index f76c53a6..00000000 --- a/tests/differentiable-function.py +++ /dev/null @@ -1,35 +0,0 @@ -from pyhpp.pinocchio import LiegroupElement - -from pyhpp.constraints import DifferentiableFunction -import numpy as np - - -class Function(DifferentiableFunction): - def __init__(self): - super(Function, self).__init__(2, 2, 2, "Test") - - def impl_compute(self, res, arg): - res.v = 2 * arg - - def impl_jacobian(self, res, arg): - res = 2 * np.eye(2) - return res - - -function = Function() -print(function) - -q = np.ones((function.ni, 1)) - -# C++ API -v = LiegroupElement(function.outputSpace()) -function.value(v, q) -print(v) - -J = np.zeros((function.ndo, function.ndi)) -function.jacobian(J, q) -print(J) - -# Pythonic API -v = function(q) -J = function.J(q) diff --git a/tests/graph_factory.py b/tests/graph_factory.py deleted file mode 100644 index 0a85675d..00000000 --- a/tests/graph_factory.py +++ /dev/null @@ -1,104 +0,0 @@ -from math import sqrt -import numpy as np - -from pyhpp.manipulation.constraint_graph_factory import Rule, ConstraintGraphFactory -from pyhpp.manipulation import Device, urdf, Graph, Problem -from pyhpp.constraints import LockedJoint -from pinocchio import SE3 - -# Robot and environment file paths -pr2_urdf = "package://example-robot-data/robots/pr2_description/urdf/pr2.urdf" -pr2_srdf = ( - "package://example-robot-data/robots/pr2_description/srdf/pr2_manipulation.srdf" -) -box_urdf = "package://hpp_tutorial/urdf/box.urdf" -box_srdf = "package://hpp_tutorial/srdf/box.srdf" -kitchen_urdf = "package://hpp_tutorial/urdf/kitchen_area.urdf" -kitchen_srdf = "package://hpp_tutorial/srdf/kitchen_area.srdf" - -robot = Device("pr2-box") - -# Load PR2 robot -pr2_pose = SE3(rotation=np.identity(3), translation=np.array([0, 0, 0])) -urdf.loadModel(robot, 0, "pr2", "freeflyer", pr2_urdf, pr2_srdf, pr2_pose) - -# Load box to be manipulated -box_pose = SE3(rotation=np.identity(3), translation=np.array([-2.5, -4, 0.746])) -urdf.loadModel(robot, 0, "box", "freeflyer", box_urdf, box_srdf, box_pose) - -# Load kitchen environment -kitchen_pose = SE3(rotation=np.identity(3), translation=np.array([0, 0, 0])) -urdf.loadModel( - robot, 0, "kitchen_area", "anchor", kitchen_urdf, kitchen_srdf, kitchen_pose -) - -model = robot.model() - -robot.setJointBounds("pr2/root_joint", [-5, -2, -5.2, -2.7]) -robot.setJointBounds("box/root_joint", [-5.1, -2, -5.2, -2.7, 0, 1.5]) - -# Create problem -problem = Problem(robot) - -# Generate initial and goal configuration. -rankInConfiguration = dict() -current_rank = 0 -for joint_id in range(1, model.njoints): - joint_name = model.names[joint_id] - joint = model.joints[joint_id] - rankInConfiguration[joint_name[0:]] = current_rank - current_rank += joint.nq - - -q_init = robot.currentConfiguration() -rank = rankInConfiguration["pr2/l_gripper_l_finger_joint"] -q_init[rank] = 0.5 -rank = rankInConfiguration["pr2/l_gripper_r_finger_joint"] -q_init[rank] = 0.5 -q_init[0:2] = [-3.2, -4] -rank = rankInConfiguration["pr2/torso_lift_joint"] -q_init[rank] = 0.2 -rank = rankInConfiguration["box/root_joint"] -q_init[rank : rank + 3] = [-2.5, -4, 0.746] -q_init[rank + 3 : rank + 7] = [0, -sqrt(2) / 2, 0, sqrt(2) / 2] - -q_goal = q_init[::] -q_goal[0:2] = [-3.2, -4] -rank = rankInConfiguration["box/root_joint"] -q_goal[rank : rank + 3] = [-2.5, -4.5, 0.746] - -# Create constraints -ll = LockedJoint.create( - robot.asPinDevice(), "pr2/l_gripper_l_finger_joint", np.array([0.5]) -) -lr = LockedJoint.create( - robot.asPinDevice(), "pr2/l_gripper_r_finger_joint", np.array([0.5]) -) - -# Create the constraint graph -grippers = ["pr2/l_gripper"] -objects = ["box"] -handlesPerObject = [["box/handle2"]] -contactSurfacesPerObject = [["box/box_surface"]] -envContactSurfaces = ["kitchen_area/pancake_table_table_top"] -rules = [Rule([".*"], [".*"], True)] - -cg = Graph("graph", robot, problem) -cg.maxIterations(100) -cg.errorThreshold(0.0001) -factory = ConstraintGraphFactory(cg) - -factory.setGrippers(grippers) -factory.environmentContacts(envContactSurfaces) -factory.setObjects(objects, handlesPerObject, contactSurfacesPerObject) -factory.setRules(rules) -factory.generate() - -cg.addNumericalConstraintsToGraph([ll, lr]) - -cg.initialize() - -problem.initConfig(q_init) -problem.addGoalConfig(q_goal) - -print("Script completed!") diff --git a/tests/graph_factory2.py b/tests/graph_factory2.py deleted file mode 100644 index e90f9fd9..00000000 --- a/tests/graph_factory2.py +++ /dev/null @@ -1,234 +0,0 @@ -from math import pi -import numpy as np - -from pyhpp.manipulation.constraint_graph_factory import ConstraintGraphFactory -from pyhpp.manipulation import Device, urdf, Graph, Problem -from pyhpp.constraints import ( - Transformation, - ComparisonTypes, - ComparisonType, - Implicit, - LockedJoint, -) -from pinocchio import SE3, Quaternion - -# based on /hpp_benchmark/2025/04-01/ur3-spheres/script.py - -# Robot and environment file paths -ur3_urdf = "package://example-robot-data/robots/ur_description/urdf/ur3_gripper.urdf" -ur3_srdf = "package://example-robot-data/robots/ur_description/srdf/ur3_gripper.srdf" -sphere_urdf = "package://hpp_environments/urdf/construction_set/sphere.urdf" -sphere_srdf = "package://hpp_environments/srdf/construction_set/sphere.srdf" -ground_urdf = "package://hpp_environments/urdf/construction_set/ground.urdf" -ground_srdf = "package://hpp_environments/srdf/construction_set/ground.srdf" - -robot = Device("ur3-spheres") - -# Load UR3 robot -ur3_pose = SE3(rotation=np.identity(3), translation=np.array([0, 0, 0])) -urdf.loadModel(robot, 0, "ur3", "anchor", ur3_urdf, ur3_srdf, ur3_pose) - -# Load sphere to be manipulated -objects = list() -nSphere = 2 -sphere_pose = SE3(rotation=np.identity(3), translation=np.array([-2.5, -4, 0.746])) -for i in range(nSphere): - urdf.loadModel( - robot, - 0, - "sphere{0}".format(i), - "freeflyer", - sphere_urdf, - sphere_srdf, - sphere_pose, - ) - robot.setJointBounds( - "sphere{0}/root_joint".format(i), - [ - -1.0, - 1.0, - -1.0, - 1.0, - -0.1, - 1.0, - -1.0001, - 1.0001, - -1.0001, - 1.0001, - -1.0001, - 1.0001, - -1.0001, - 1.0001, - ], - ) - objects.append("sphere{0}".format(i)) - -# Load kitchen environment -kitchen_pose = SE3(rotation=np.identity(3), translation=np.array([0, 0, 0])) -urdf.loadModel( - robot, 0, "kitchen_area", "anchor", ground_urdf, ground_srdf, kitchen_pose -) - -model = robot.model() -robot.setJointBounds("ur3/shoulder_pan_joint", [-pi, 4]) -robot.setJointBounds("ur3/shoulder_lift_joint", [-pi, 0]) -robot.setJointBounds("ur3/elbow_joint", [-2.6, 2.6]) - -problem = Problem(robot) -cg = Graph("graph", robot, problem) - -constraints = dict() - -for i in range(nSphere): - o = objects[i] - h = robot.handles()[o + "/handle"] - h.mask = [True, True, True, False, True, True] - # placement constraint - placementName = "place_sphere{0}".format(i) - Id = SE3.Identity() - q = Quaternion(0, 0, 0, 1) - ballPlacement = SE3(q, np.array([0, 0, 0.02])) - joint = robot.model().getJointId("sphere{0}/root_joint".format(i)) - pc = Transformation.create( - placementName, - robot.asPinDevice(), - joint, - ballPlacement, - Id, - [True, True, False, False, False, True], - ) - cts = ComparisonTypes() - cts[:] = ( - ComparisonType.EqualToZero, - ComparisonType.EqualToZero, - ComparisonType.EqualToZero, - ) - implicit_mask = [True, True, True] - implicitPlacementConstraint = Implicit.create(pc, cts, implicit_mask) - constraints[placementName] = implicitPlacementConstraint - # placement complement constraint - pc = Transformation.create( - placementName + "/complement", - robot.asPinDevice(), - joint, - ballPlacement, - Id, - [False, False, True, True, True, False], - ) - cts[:] = ( - ComparisonType.Equality, - ComparisonType.Equality, - ComparisonType.Equality, - ) - implicit_mask = [True, True, True] - implicitPlacementComplementConstraint = Implicit.create(pc, cts, implicit_mask) - constraints[placementName + "/complement"] = implicitPlacementComplementConstraint - - # combination of placement and complement - cts[:] = ( - ComparisonType.Equality, - ComparisonType.Equality, - ComparisonType.EqualToZero, - ComparisonType.EqualToZero, - ComparisonType.EqualToZero, - ComparisonType.Equality, - ) - ll = LockedJoint.createWithComp( - robot.asPinDevice(), - "sphere{0}/root_joint".format(i), - np.array([0, 0, 0.02, 0, 0, 0, 1]), - cts, - ) - constraints[placementName + "/hold"] = ll - cg.registerConstraints( - constraints[placementName], - constraints[placementName + "/complement"], - constraints[placementName + "/hold"], - ) - - preplacementName = "preplace_sphere{0}".format(i) - Id = SE3.Identity() - q = Quaternion(0, 0, 0, 1) - ballPrePlacement = SE3(q, np.array([0, 0, 0.1])) - joint = robot.model().getJointId("sphere{0}/root_joint".format(i)) - pc = Transformation.create( - preplacementName, - robot.asPinDevice(), - joint, - ballPrePlacement, - Id, - [False, False, True, True, True, False], - ) - cts[:] = ( - ComparisonType.EqualToZero, - ComparisonType.EqualToZero, - ComparisonType.EqualToZero, - ) - implicit_mask = [True, True, True] - implicitPrePlacementConstraint = Implicit.create(pc, cts, implicit_mask) - constraints[preplacementName] = implicitPrePlacementConstraint - -q_init = [ - pi / 6, - -pi / 2, - pi / 2, - 0, - 0, - 0, - 0.2, - 0, - 0.02, - 0, - 0, - 0, - 1, - 0.3, - 0, - 0.02, - 0, - 0, - 0, - 1, -] -q_goal = [ - pi / 6, - -pi / 2, - pi / 2, - 0, - 0, - 0, - 0.3, - 0, - 0.02, - 0, - 0, - 0, - 1, - 0.2, - 0, - 0.02, - 0, - 0, - 0, - 1, -] - - -grippers = ["ur3/gripper"] -handlesPerObject = [["sphere{0}/handle".format(i)] for i in range(nSphere)] -contactsPerObject = [[] for i in range(nSphere)] - -cg.maxIterations(100) -cg.errorThreshold(0.0001) -factory = ConstraintGraphFactory(cg, constraints) - -factory.setGrippers(grippers) -factory.setObjects(objects, handlesPerObject, contactsPerObject) -factory.generate() - -cg.initialize() -# cg.display("./temp.dot") -# problem.initConfig(q_init) -# problem.addGoalConfig(q_goal) - -print("Script completed!") diff --git a/tests/imports.py b/tests/imports.py deleted file mode 100644 index e69de29b..00000000 diff --git a/tests/integration/benchmark_utils.py b/tests/integration/benchmark_utils.py new file mode 100644 index 00000000..670b45c4 --- /dev/null +++ b/tests/integration/benchmark_utils.py @@ -0,0 +1,239 @@ +#!/usr/bin/env python +# +# Copyright (c) 2025 CNRS +# Author: Paul Sardin +# +# Shared utilities for benchmark scripts. + +import datetime as dt +from argparse import ArgumentParser +from dataclasses import dataclass, field +from typing import Callable, Optional, Any + + +def create_benchmark_parser(description: str = "HPP Benchmark") -> ArgumentParser: + """Create a standard argument parser for benchmarks.""" + parser = ArgumentParser(description=description) + parser.add_argument( + "-N", + default=0, + type=int, + help="Number of benchmark iterations to run", + ) + parser.add_argument( + "--display", + action="store_true", + help="Display visualization (if available)", + ) + parser.add_argument( + "--run", + action="store_true", + help="Run path playback after solving", + ) + return parser + + +@dataclass +class BenchmarkResult: + """Results from a single benchmark iteration.""" + + success: bool + time_elapsed: dt.timedelta = field(default_factory=lambda: dt.timedelta(0)) + num_nodes: int = 0 + path_length: float = 0.0 + error_message: str = "" + + +@dataclass +class BenchmarkStats: + """Aggregated statistics from benchmark runs.""" + + total_iterations: int = 0 + num_successes: int = 0 + total_time: dt.timedelta = field(default_factory=lambda: dt.timedelta(0)) + total_nodes: int = 0 + total_path_length: float = 0.0 + + @property + def success_rate(self) -> float: + if self.total_iterations == 0: + return 0.0 + return self.num_successes / self.total_iterations * 100 + + @property + def avg_time_per_success(self) -> float: + if self.num_successes == 0: + return 0.0 + return self.total_time.total_seconds() / self.num_successes + + @property + def avg_nodes_per_success(self) -> float: + if self.num_successes == 0: + return 0.0 + return self.total_nodes / self.num_successes + + @property + def avg_path_length_per_success(self) -> float: + if self.num_successes == 0: + return 0.0 + return self.total_path_length / self.num_successes + + +class BenchmarkRunner: + """ + Runs benchmark iterations and collects statistics. + + Usage: + runner = BenchmarkRunner( + solve_func=lambda: planner.solve(), + reset_func=lambda: planner.roadmap().clear(), + get_nodes_func=lambda: len(planner.roadmap().nodes()), + ) + stats = runner.run(num_iterations=10) + runner.print_stats() + """ + + def __init__( + self, + solve_func: Callable[[], Any], + reset_func: Optional[Callable[[], None]] = None, + get_nodes_func: Optional[Callable[[], int]] = None, + get_path_length_func: Optional[Callable[[], float]] = None, + ): + self.solve_func = solve_func + self.reset_func = reset_func + self.get_nodes_func = get_nodes_func + self.get_path_length_func = get_path_length_func + self.stats = BenchmarkStats() + self.results: list = [] + + def _run_single(self) -> BenchmarkResult: + """Run a single benchmark iteration.""" + result = BenchmarkResult(success=False) + + if self.reset_func: + self.reset_func() + + try: + t1 = dt.datetime.now() + path = self.solve_func() + t2 = dt.datetime.now() + + result.success = True + result.time_elapsed = t2 - t1 + + if self.get_nodes_func: + result.num_nodes = self.get_nodes_func() + + if self.get_path_length_func and path is not None: + result.path_length = self.get_path_length_func() + + except Exception as e: + result.error_message = str(e) + print(f"Failed to plan path: {e}") + + return result + + def run(self, num_iterations: int) -> BenchmarkStats: + """Run the benchmark for num_iterations.""" + self.stats = BenchmarkStats(total_iterations=num_iterations) + self.results = [] + + for i in range(num_iterations): + result = self._run_single() + self.results.append(result) + + if result.success: + self.stats.num_successes += 1 + self.stats.total_time += result.time_elapsed + self.stats.total_nodes += result.num_nodes + self.stats.total_path_length += result.path_length + print(result.time_elapsed) + if result.num_nodes > 0: + print(f"Number nodes: {result.num_nodes}") + + return self.stats + + def print_stats(self) -> None: + """Print benchmark statistics.""" + if self.stats.total_iterations == 0: + return + + print("#" * 20) + print(f"Number of rounds: {self.stats.total_iterations}") + print(f"Number of successes: {self.stats.num_successes}") + print(f"Success rate: {self.stats.success_rate}%") + + if self.stats.num_successes > 0: + print(f"Average time per success: {self.stats.avg_time_per_success:.4f}s") + print( + f"Average number nodes per success: {self.stats.avg_nodes_per_success:.1f}" + ) + if self.stats.total_path_length > 0: + print( + f"Average path length per success: {self.stats.avg_path_length_per_success:.4f}" + ) + + def verify_results(self) -> bool: + """ + Verify that benchmark produced valid results. + Returns True if all assertions pass. + """ + if self.stats.total_iterations == 0: + return True + + success = True + + if self.stats.num_successes > 0: + for result in self.results: + if result.success: + if result.time_elapsed.total_seconds() <= 0: + print("ASSERTION FAILED: time_elapsed should be positive") + success = False + + if self.get_nodes_func and result.num_nodes <= 0: + print("ASSERTION FAILED: num_nodes should be positive") + success = False + + return success + + +def run_benchmark_main( + planner: Any, + problem: Any, + q_init: Any, + q_goal: Any, + num_iterations: int, +) -> BenchmarkStats: + """ + Convenience function to run a standard benchmark. + + Args: + planner: The planner object (must have solve() and roadmap().clear()) + problem: The problem object (must have initConfig(), addGoalConfig(), resetGoalConfigs()) + q_init: Initial configuration + q_goal: Goal configuration + num_iterations: Number of iterations to run + + Returns: + BenchmarkStats with results + """ + + def reset(): + planner.roadmap().clear() + problem.resetGoalConfigs() + problem.initConfig(q_init) + problem.addGoalConfig(q_goal) + + runner = BenchmarkRunner( + solve_func=planner.solve, + reset_func=reset, + get_nodes_func=lambda: len(planner.roadmap().nodes()), + ) + + stats = runner.run(num_iterations) + runner.print_stats() + + assert runner.verify_results(), "Benchmark verification failed!" + + return stats diff --git a/tests/integration/construction-set-m-rrt.py b/tests/integration/construction-set-m-rrt.py new file mode 100644 index 00000000..c2f55b6b --- /dev/null +++ b/tests/integration/construction-set-m-rrt.py @@ -0,0 +1,532 @@ +#!/usr/bin/env python + +from math import pi, sqrt +import numpy as np +import re +import typing as T + +from pyhpp.manipulation.constraint_graph_factory import ConstraintGraphFactory, Rule +from pyhpp.manipulation import Device, Graph, Problem, urdf, ManipulationPlanner +from pyhpp.core import Straight, Progressive, ProgressiveProjector +from pyhpp.constraints import ( + Transformation, + ComparisonTypes, + ComparisonType, + Implicit, + LockedJoint, +) +from pinocchio import SE3, Quaternion + +from pyhpp.manipulation.security_margins import SecurityMargins + +from benchmark_utils import create_benchmark_parser, run_benchmark_main + +parser = create_benchmark_parser("Construction Set M-RRT Benchmark") +parser.add_argument( + "--bigGraph", + action="store_true", + help="Whether constraint graph is generated with all the possible states. " + "If unspecified, constraint graph only has a few states traversed by " + "one safe path.", +) +args = parser.parse_args() + + +class StateName(object): + noGrasp = "free" + + def __init__(self, grasps): + if isinstance(grasps, set): + self.grasps = grasps.copy() + elif isinstance(grasps, str): + if grasps == self.noGrasp: + self.grasps = set() + else: + g1 = [s.strip(" ") for s in grasps.split(":")] + self.grasps = set([tuple(s.split(" grasps ")) for s in g1]) + else: + raise TypeError("expecting a set of pairs (gripper, handle) or a string") + + def __str__(self): + if self.grasps == set(): + return "free" + res = "" + for g in self.grasps: + res += g[0] + " grasps " + g[1] + " : " + return res[:-3] + + def __eq__(self, other): + return self.grasps == other.grasps + + def __ne__(self, other): + return not self.__eq__(other) + + +ur3_urdf = "package://example-robot-data/robots/ur_description/urdf/ur3_gripper.urdf" +ur3_srdf = "package://example-robot-data/robots/ur_description/srdf/ur3_gripper.srdf" +cylinder_08_urdf = "package://hpp_environments/urdf/construction_set/cylinder_08.urdf" +cylinder_08_srdf = "package://hpp_environments/srdf/construction_set/cylinder_08.srdf" +sphere_urdf = "package://hpp_environments/urdf/construction_set/sphere.urdf" +sphere_srdf = "package://hpp_environments/srdf/construction_set/sphere.srdf" +ground_urdf = "package://hpp_environments/urdf/construction_set/ground.urdf" +ground_srdf = "package://hpp_environments/srdf/construction_set/ground.srdf" + +nSphere = 2 +nCylinder = 2 + +robot = Device("2ur5-sphere") + +r0_pose = SE3(rotation=np.identity(3), translation=np.array([-0.25, 0, 0])) +urdf.loadModel(robot, 0, "r0", "anchor", ur3_urdf, ur3_srdf, r0_pose) + +r1_pose = SE3(Quaternion(0, 0, 0, 1), np.array([0.25, 0, 0])) +urdf.loadModel(robot, 0, "r1", "anchor", ur3_urdf, ur3_srdf, r1_pose) + +robot.setJointBounds("r0/shoulder_pan_joint", [-pi, 4]) +robot.setJointBounds("r1/shoulder_pan_joint", [-pi, 4]) +robot.setJointBounds("r0/shoulder_lift_joint", [-pi, 0]) +robot.setJointBounds("r1/shoulder_lift_joint", [-pi, 0]) +robot.setJointBounds("r0/elbow_joint", [-2.6, 2.6]) +robot.setJointBounds("r1/elbow_joint", [-2.6, 2.6]) + +ground_pose = SE3(rotation=np.identity(3), translation=np.array([0, 0, 0])) +urdf.loadModel(robot, 0, "ground", "anchor", ground_urdf, ground_srdf, ground_pose) + +objects = list() +for i in range(nSphere): + sphere_pose = SE3(rotation=np.identity(3), translation=np.array([0, 0, 0])) + urdf.loadModel( + robot, 0, f"sphere{i}", "freeflyer", sphere_urdf, sphere_srdf, sphere_pose + ) + robot.setJointBounds( + f"sphere{i}/root_joint", + [ + -1.0, + 1.0, + -1.0, + 1.0, + -0.1, + 1.0, + -1.0001, + 1.0001, + -1.0001, + 1.0001, + -1.0001, + 1.0001, + -1.0001, + 1.0001, + ], + ) + objects.append(f"sphere{i}") + +for i in range(nCylinder): + cylinder_pose = SE3(rotation=np.identity(3), translation=np.array([0, 0, 0])) + urdf.loadModel( + robot, + 0, + f"cylinder{i}", + "freeflyer", + cylinder_08_urdf, + cylinder_08_srdf, + cylinder_pose, + ) + robot.setJointBounds( + f"cylinder{i}/root_joint", + [ + -1.0, + 1.0, + -1.0, + 1.0, + -0.1, + 1.0, + -1.0001, + 1.0001, + -1.0001, + 1.0001, + -1.0001, + 1.0001, + -1.0001, + 1.0001, + ], + ) + objects.append(f"cylinder{i}") + +model = robot.model() + +problem = Problem(robot) +problem.clearConfigValidations() +problem.addConfigValidation("CollisionValidation") + +cg = Graph("assembly", robot, problem) +cg.errorThreshold(1e-4) +cg.maxIterations(40) + +constraints = dict() + +for i in range(nSphere): + placementName = f"place_sphere{i}" + Id = SE3.Identity() + spherePlacement = SE3(Quaternion(1, 0, 0, 0), np.array([0, 0, 0.025])) + joint = model.getJointId(f"sphere{i}/root_joint") + + pc = Transformation( + placementName, + robot, + joint, + Id, + spherePlacement, + [False, False, True, True, True, False], + ) + cts = ComparisonTypes() + cts[:] = ( + ComparisonType.EqualToZero, + ComparisonType.EqualToZero, + ComparisonType.EqualToZero, + ) + constraints[placementName] = Implicit(pc, cts, [True, True, True]) + + pc_complement = Transformation( + placementName + "/complement", + robot, + joint, + Id, + spherePlacement, + [True, True, False, False, False, True], + ) + cts_complement = ComparisonTypes() + cts_complement[:] = ( + ComparisonType.Equality, + ComparisonType.Equality, + ComparisonType.Equality, + ) + constraints[placementName + "/complement"] = Implicit( + pc_complement, cts_complement, [True, True, True] + ) + + cts_hold = ComparisonTypes() + cts_hold[:] = ( + ComparisonType.Equality, + ComparisonType.Equality, + ComparisonType.EqualToZero, + ComparisonType.EqualToZero, + ComparisonType.EqualToZero, + ComparisonType.Equality, + ) + constraints[placementName + "/hold"] = LockedJoint( + robot, f"sphere{i}/root_joint", np.array([0, 0, 0.025, 0, 0, 0, 1]), cts_hold + ) + + cg.registerConstraints( + constraints[placementName], + constraints[placementName + "/complement"], + constraints[placementName + "/hold"], + ) + + preplacementName = f"preplace_sphere{i}" + spherePrePlacement = SE3(Quaternion(1, 0, 0, 0), np.array([0, 0, 0.075])) + pc_pre = Transformation( + preplacementName, + robot, + joint, + Id, + spherePrePlacement, + [False, False, True, True, True, False], + ) + constraints[preplacementName] = Implicit(pc_pre, cts, [True, True, True]) + +for i in range(nCylinder): + placementName = f"place_cylinder{i}" + Id = SE3.Identity() + cylinderPlacement = SE3(Quaternion(1, 0, 0, 0), np.array([0, 0, 0.025])) + joint = model.getJointId(f"cylinder{i}/root_joint") + + pc = Transformation( + placementName, + robot, + joint, + Id, + cylinderPlacement, + [False, False, True, True, True, False], + ) + cts = ComparisonTypes() + cts[:] = ( + ComparisonType.EqualToZero, + ComparisonType.EqualToZero, + ComparisonType.EqualToZero, + ) + constraints[placementName] = Implicit(pc, cts, [True, True, True]) + + pc_complement = Transformation( + placementName + "/complement", + robot, + joint, + Id, + cylinderPlacement, + [True, True, False, False, False, True], + ) + cts_complement = ComparisonTypes() + cts_complement[:] = ( + ComparisonType.Equality, + ComparisonType.Equality, + ComparisonType.Equality, + ) + constraints[placementName + "/complement"] = Implicit( + pc_complement, cts_complement, [True, True, True] + ) + + cts_hold = ComparisonTypes() + cts_hold[:] = ( + ComparisonType.Equality, + ComparisonType.Equality, + ComparisonType.EqualToZero, + ComparisonType.EqualToZero, + ComparisonType.EqualToZero, + ComparisonType.Equality, + ) + constraints[placementName + "/hold"] = LockedJoint( + robot, f"cylinder{i}/root_joint", np.array([0, 0, 0.025, 0, 0, 0, 1]), cts_hold + ) + + cg.registerConstraints( + constraints[placementName], + constraints[placementName + "/complement"], + constraints[placementName + "/hold"], + ) + + preplacementName = f"preplace_cylinder{i}" + cylinderPrePlacement = SE3(Quaternion(1, 0, 0, 0), np.array([0, 0, 0.075])) + pc_pre = Transformation( + preplacementName, + robot, + joint, + Id, + cylinderPrePlacement, + [False, False, True, True, True, False], + ) + constraints[preplacementName] = Implicit(pc_pre, cts, [True, True, True]) + +grippers = [f"cylinder{i}/magnet0" for i in range(nCylinder)] +grippers += [f"cylinder{i}/magnet1" for i in range(nCylinder)] +grippers += [f"r{i}/gripper" for i in range(2)] + +handlesPerObjects = [[f"sphere{i}/handle", f"sphere{i}/magnet"] for i in range(nSphere)] +handlesPerObjects += [[f"cylinder{i}/handle"] for i in range(nCylinder)] + +shapesPerObject = [[] for o in objects] + +allHandles = [handle for objHandles in handlesPerObjects for handle in objHandles] + + +def makeRule(grasps): + _grippers = list() + _handles = list() + for g, h in grasps: + _grippers.append(g) + _handles.append(h) + for g in grippers: + if g not in _grippers: + _grippers.append(g) + _handles += (len(_grippers) - len(_handles)) * ["^$"] + return Rule(grippers=_grippers, handles=_handles, link=True) + + +def forbidExcept(g: str, h: T.List[str]) -> T.List[Rule]: + gRegex = re.compile(g) + hRegex = [re.compile(handle) for handle in h] + forbiddenList: T.List[Rule] = list() + idForbidGrippers = [i for i in range(len(grippers)) if gRegex.match(grippers[i])] + forbidHandles = [ + handle + for handle in allHandles + if not any([handlePattern.match(handle) for handlePattern in hRegex]) + ] + for id in idForbidGrippers: + for handle in forbidHandles: + _handles = [handle if i == id else ".*" for i in range(len(grippers))] + forbiddenList.append(Rule(grippers=grippers, handles=_handles, link=False)) + return forbiddenList + + +q0_r0 = [pi / 6, -pi / 2, pi / 2, 0, 0, 0] +q0_r1 = q0_r0[::] + +q0_spheres = list() +i = 0 +y = 0.04 +while i < nSphere: + q0_spheres.append([-0.1 * (i / 2), -0.12 + y, 0.025, 0, 0, 0, 1]) + i += 1 + y = -y + +q0_cylinders = list() +i = 0 +y = -0.04 +while i < nCylinder: + q0_cylinders.append([0.45 + 0.1 * (i / 2), -0.12 + y, 0.025, 0, 0, 0, 1]) + i += 1 + y = -y + +q0 = np.array(q0_r0 + q0_r1 + sum(q0_spheres, []) + sum(q0_cylinders, [])) +if not args.bigGraph: + nodes = list() + rules = list() + grasps = set() + nodes.append(StateName(grasps)) + rules.append(makeRule(grasps=grasps)) + + grasps.add(("r0/gripper", "sphere0/handle")) + nodes.append(StateName(grasps)) + rules.append(makeRule(grasps=grasps)) + + grasps.add(("r1/gripper", "cylinder0/handle")) + nodes.append(StateName(grasps)) + rules.append(makeRule(grasps=grasps)) + + grasps.add(("cylinder0/magnet0", "sphere0/magnet")) + nodes.append(StateName(grasps)) + rules.append(makeRule(grasps=grasps)) + + grasps.remove(("r0/gripper", "sphere0/handle")) + nodes.append(StateName(grasps)) + rules.append(makeRule(grasps=grasps)) + + grasps.add(("r0/gripper", "sphere1/handle")) + nodes.append(StateName(grasps)) + rules.append(makeRule(grasps=grasps)) + + grasps.add(("cylinder0/magnet1", "sphere1/magnet")) + nodes.append(StateName(grasps)) + rules.append(makeRule(grasps=grasps)) + + grasps.remove(("r0/gripper", "sphere1/handle")) + nodes.append(StateName(grasps)) + rules.append(makeRule(grasps=grasps)) + + grasps.remove(("r1/gripper", "cylinder0/handle")) + nodes.append(StateName(grasps)) + rules.append(makeRule(grasps=grasps)) + + problem.steeringMethod = Straight(problem) + problem.pathValidation = Progressive(robot, 0.02) + problem.pathProjector = ProgressiveProjector( + problem.distance(), problem.steeringMethod, 0.05 + ) + + factory = ConstraintGraphFactory(cg, constraints) + factory.setGrippers(grippers) + factory.setObjects(objects, handlesPerObjects, shapesPerObject) + factory.setRules(rules) + factory.generate() + + sm = SecurityMargins( + problem, + factory, + ["r0", "r1", "sphere0", "sphere1", "cylinder0", "cylinder1"], + robot, + ) + sm.defaultMargin = 0.02 + sm.apply() + + cg.initialize() + + problem.pathProjector = ProgressiveProjector( + problem.distance(), problem.steeringMethod, 0.05 + ) + + nodes.append(nodes[-1]) + +else: + rules = list() + + rules.extend(forbidExcept("^r0/gripper$", ["^sphere\\d*/handle$"])) + rules.extend(forbidExcept("^r1/gripper$", ["^cylinder0*/handle$"])) + rules.extend(forbidExcept("^cylinder0/magnet\\d*$", ["^sphere\\d*/magnet$"])) + rules.extend(forbidExcept("^cylinder[1-9][0-9]*.*$", ["^$"])) + + rules.append(Rule(grippers=grippers, handles=[".*"] * len(grippers), link=True)) + + problem.steeringMethod = Straight(problem) + problem.pathValidation = Progressive(robot, 0.02) + problem.pathProjector = ProgressiveProjector( + problem.distance(), problem.steeringMethod, 0.05 + ) + + factory = ConstraintGraphFactory(cg, constraints) + factory.setGrippers(grippers) + factory.setObjects(objects, handlesPerObjects, shapesPerObject) + factory.setRules(rules) + factory.generate() + + sm = SecurityMargins( + problem, + factory, + ["r0", "r1", "sphere0", "sphere1", "cylinder0", "cylinder1"], + robot, + ) + sm.defaultMargin = 0.02 + sm.apply() + + cg.initialize() + + problem.pathProjector = ProgressiveProjector( + problem.distance(), problem.steeringMethod, 0.05 + ) + +assert nCylinder == 2 and nSphere == 2 +c = sqrt(2) / 2 +q_goal = np.array( + q0_r0 + + q0_r1 + + [ + -0.06202136144745322, + -0.15, + 0.025, + c, + 0, + -c, + 0, + 0.06202136144745322, + -0.15, + 0.025, + c, + 0, + c, + 0, + 0, + -0.15, + 0.025, + 0, + 0, + 0, + 1, + 0.5, + -0.08, + 0.025, + 0, + 0, + 0, + 1, + ] +) + +problem.initConfig(q0) +problem.addGoalConfig(q_goal) +problem.constraintGraph(cg) + +if args.display: + from pyhpp_plot import show_graph + + show_graph(cg) + +manipulationPlanner = ManipulationPlanner(problem) +manipulationPlanner.maxIterations(5000) + +# Run benchmark using shared utilities +if args.N > 0: + run_benchmark_main( + planner=manipulationPlanner, + problem=problem, + q_init=q0, + q_goal=q_goal, + num_iterations=args.N, + ) diff --git a/tests/integration/pr2-in-iai-kitchen.py b/tests/integration/pr2-in-iai-kitchen.py new file mode 100644 index 00000000..9897b49f --- /dev/null +++ b/tests/integration/pr2-in-iai-kitchen.py @@ -0,0 +1,64 @@ +#!/usr/bin/env python + +import numpy as np + +from pyhpp.core import Problem, DiffusingPlanner, Dichotomy, Straight +from pyhpp.pinocchio import Device, urdf +from pinocchio import SE3 + +from benchmark_utils import create_benchmark_parser, run_benchmark_main + +parser = create_benchmark_parser("PR2 Navigation in IAI Kitchen Benchmark") +args = parser.parse_args() + +pr2_urdf = "package://example-robot-data/robots/pr2_description/urdf/pr2.urdf" +pr2_srdf = "package://example-robot-data/robots/pr2_description/srdf/pr2.srdf" +kitchen_urdf = "package://hpp_tutorial/urdf/kitchen_area_obstacle.urdf" + +robot = Device("pr2") + +pr2_pose = SE3(rotation=np.identity(3), translation=np.array([0, 0, 0])) +urdf.loadModel(robot, 0, "pr2", "planar", pr2_urdf, pr2_srdf, pr2_pose) + +robot.setJointBounds("pr2/root_joint", [-4, -3, -5, -3, -2, 2, -2, 2]) + +kitchen_pose = SE3(rotation=np.identity(3), translation=np.array([0, 0, 0])) +urdf.loadModel(robot, 0, "kitchen", "anchor", kitchen_urdf, "", kitchen_pose) + +model = robot.model() + +q_init = robot.currentConfiguration() +q_goal = q_init.copy() + +q_init[0:2] = [-3.2, -4] +rank = model.idx_qs[model.getJointId("pr2/torso_lift_joint")] +q_init[rank] = 0.2 + +q_goal[0:2] = [-3.2, -4] +rank = model.idx_qs[model.getJointId("pr2/l_shoulder_lift_joint")] +q_goal[rank] = 0.5 +rank = model.idx_qs[model.getJointId("pr2/r_shoulder_lift_joint")] +q_goal[rank] = 0.5 +rank = model.idx_qs[model.getJointId("pr2/l_elbow_flex_joint")] +q_goal[rank] = -0.5 +rank = model.idx_qs[model.getJointId("pr2/r_elbow_flex_joint")] +q_goal[rank] = -0.5 + +problem = Problem(robot) +problem.pathValidation = Dichotomy(robot, 0.0) +problem.steeringMethod = Straight(problem) + +problem.initConfig(q_init) +problem.addGoalConfig(q_goal) +planner = DiffusingPlanner(problem) +planner.maxIterations(5000) + +# Run benchmark using shared utilities +if args.N > 0: + run_benchmark_main( + planner=planner, + problem=problem, + q_init=q_init, + q_goal=q_goal, + num_iterations=args.N, + ) diff --git a/tests/integration/romeo-placard.py b/tests/integration/romeo-placard.py new file mode 100644 index 00000000..cc481f7e --- /dev/null +++ b/tests/integration/romeo-placard.py @@ -0,0 +1,276 @@ +#!/usr/bin/env python + +import numpy as np + +from pyhpp.manipulation.constraint_graph_factory import ConstraintGraphFactory, Rule +from pyhpp.manipulation import Device, Graph, Problem, urdf, ManipulationPlanner +from pyhpp.core import Dichotomy, Straight, ProgressiveProjector +from pyhpp.constraints import LockedJoint +from pyhpp.core.static_stability_constraint_factory import ( + StaticStabilityConstraintsFactory, +) +from pinocchio import SE3 + +from benchmark_utils import create_benchmark_parser, run_benchmark_main + +parser = create_benchmark_parser("Romeo Placard Humanoid Manipulation Benchmark") +args = parser.parse_args() + +# Robot and object file paths +romeo_urdf = "package://example-robot-data/robots/romeo_description/urdf/romeo.urdf" +romeo_srdf_moveit = ( + "package://example-robot-data/robots/romeo_description/srdf/romeo_moveit.srdf" +) +placard_urdf = "package://hpp_environments/urdf/placard.urdf" +placard_srdf = "package://hpp_environments/srdf/placard.srdf" + +robot = Device("romeo-placard") + +# Load Romeo robot +romeo_pose = SE3(rotation=np.identity(3), translation=np.array([0, 0, 0])) +urdf.loadModel( + robot, 0, "romeo", "freeflyer", romeo_urdf, romeo_srdf_moveit, romeo_pose +) + +robot.setJointBounds( + "romeo/root_joint", [-1, 1, -1, 1, 0, 2, -2.0, 2, -2.0, 2, -2.0, 2, -2.0, 2] +) + +# Load placard +placard_pose = SE3(rotation=np.identity(3), translation=np.array([0, 0, 0])) +urdf.loadModel( + robot, 0, "placard", "freeflyer", placard_urdf, placard_srdf, placard_pose +) + +robot.setJointBounds( + "placard/root_joint", [-1, 1, -1, 1, 0, 1.5, -2.0, 2, -2.0, 2, -2.0, 2, -2.0, 2] +) + +model = robot.model() + +problem = Problem(robot) + +problem.clearConfigValidations() +problem.addConfigValidation("CollisionValidation") + +cg = Graph("graph", robot, problem) +cg.errorThreshold(2e-4) +cg.maxIterations(40) + +constraints = dict() +graphConstraints = dict() + +leftHandOpen = { + "LHand": 1, + "LFinger12": 1.06, + "LFinger13": 1.06, + "LFinger21": 1.06, + "LFinger22": 1.06, + "LFinger23": 1.06, + "LFinger31": 1.06, + "LFinger32": 1.06, + "LFinger33": 1.06, + "LThumb1": 0, + "LThumb2": 1.06, + "LThumb3": 1.06, +} + +rightHandOpen = { + "RHand": 1, + "RFinger12": 1.06, + "RFinger13": 1.06, + "RFinger21": 1.06, + "RFinger22": 1.06, + "RFinger23": 1.06, + "RFinger31": 1.06, + "RFinger32": 1.06, + "RFinger33": 1.06, + "RThumb1": 0, + "RThumb2": 1.06, + "RThumb3": 1.06, +} + + +# Lock left hand +locklhand = list() +for j, v in leftHandOpen.items(): + joint_name = "romeo/" + j + locklhand.append(joint_name) + if type(v) is float or type(v) is int: + val = np.array([v]) + else: + val = np.array(v) + cs = LockedJoint(robot, joint_name, val) + constraints[joint_name] = cs + graphConstraints[joint_name] = cs + +# Lock right hand +lockrhand = list() +for j, v in rightHandOpen.items(): + joint_name = "romeo/" + j + lockrhand.append(joint_name) + if type(v) is float or type(v) is int: + val = np.array([v]) + else: + val = np.array(v) + cs = LockedJoint(robot, joint_name, val) + constraints[joint_name] = cs + graphConstraints[joint_name] = cs + +lockHands = lockrhand + locklhand + +# Create static stability constraint +q = robot.currentConfiguration() +placard_rank = model.idx_qs[model.getJointId("placard/root_joint")] +q[placard_rank : placard_rank + 3] = [0.4, 0, 1.2] + +problem.addPartialCom("romeo", ["romeo/root_joint"]) + +leftAnkle = "romeo/LAnkleRoll" +rightAnkle = "romeo/RAnkleRoll" + +factory = StaticStabilityConstraintsFactory(problem, robot) +balanceConstraintsDict = factory.createStaticStabilityConstraint( + "balance/", "romeo", leftAnkle, rightAnkle, q +) + +balanceConstraints = [ + balanceConstraintsDict.get("balance/pose-left-foot"), + balanceConstraintsDict.get("balance/pose-right-foot"), + balanceConstraintsDict.get("balance/relative-com"), +] +balanceConstraints = [c for c in balanceConstraints if c is not None] + +for name, constraint in balanceConstraintsDict.items(): + if constraint not in balanceConstraints: + constraints[name] = constraint + graphConstraints[name] = constraint + +# Build graph +grippers = ["romeo/r_hand", "romeo/l_hand"] +handlesPerObjects = [["placard/low", "placard/high"]] + +rules = [ + Rule(["romeo/l_hand", "romeo/r_hand"], ["placard/low", ""], True), + Rule(["romeo/l_hand", "romeo/r_hand"], ["", "placard/high"], True), + Rule(["romeo/l_hand", "romeo/r_hand"], ["placard/low", "placard/high"], True), +] + +factory_cg = ConstraintGraphFactory(cg, constraints) +factory_cg.setGrippers(grippers) +factory_cg.setObjects(["placard"], handlesPerObjects, [[]]) +factory_cg.setRules(rules) +factory_cg.generate() + +# Add balance constraints and locked hands to graph +all_graph_constraints = list(graphConstraints.values()) + balanceConstraints +cg.addNumericalConstraintsToGraph(all_graph_constraints) +cg.initialize() + +# Define initial and final configurations +q_goal = np.array( + [ + -0.003429678026293006, + 7.761615492429529e-05, + 0.8333148411182841, + -0.08000440760954532, + 0.06905332841243099, + -0.09070086400314036, + 0.9902546570793265, + 0.2097693637044623, + 0.19739743868699455, + -0.6079135018296973, + 0.8508704420155889, + -0.39897628829947995, + -0.05274298289004072, + 0.20772797293264825, + 0.1846394290733244, + -0.49824886682709824, + 0.5042013065348324, + -0.16158420369261683, + -0.039828502509861335, + -0.3827070014985058, + -0.24118425356319423, + 1.0157846623463191, + 0.5637424355124602, + -1.3378817283780955, + -1.3151786907256797, + -0.392409481224193, + 0.11332560818107676, + 1.06, + 1.06, + 1.06, + 1.06, + 1.06, + 1.06, + 1.0, + 1.06, + 1.06, + -1.06, + 1.06, + 1.06, + 0.35936687035487364, + -0.32595302056157444, + -0.33115291290191723, + 0.20387672048126043, + 0.9007626913161502, + -0.39038645767349395, + 0.31725226129015516, + 1.5475253831101246, + -0.0104572058777634, + 0.32681856374063933, + 0.24476959944940427, + 1.06, + 1.06, + 1.06, + 1.06, + 1.06, + 1.06, + 1.0, + 1.06, + 1.06, + -1.06, + 1.06, + 1.06, + 0.412075621240969, + 0.020809907186176854, + 1.056724788359247, + 0.0, + 0.0, + 0.0, + 1.0, + ] +) +q_init = q_goal.copy() +q_init[placard_rank + 3 : placard_rank + 7] = [0, 0, 1, 0] + +n = "romeo/l_hand grasps placard/low" +state = cg.getState(n) +res, q_init_proj, err = cg.applyStateConstraints(state, q_init) +if not res: + raise RuntimeError("Failed to project initial configuration.") +res, q_goal_proj, err = cg.applyStateConstraints(state, q_goal) +if not res: + raise RuntimeError("Failed to project goal configuration.") + +problem.steeringMethod = Straight(problem) +problem.pathValidation = Dichotomy(robot, 0) +problem.pathProjector = ProgressiveProjector( + problem.distance(), problem.steeringMethod, 0.05 +) + +problem.initConfig(q_init_proj) +problem.addGoalConfig(q_goal_proj) +problem.constraintGraph(cg) + +manipulationPlanner = ManipulationPlanner(problem) + +# Run benchmark using shared utilities +if args.N > 0: + run_benchmark_main( + planner=manipulationPlanner, + problem=problem, + q_init=q_init_proj, + q_goal=q_goal_proj, + num_iterations=args.N, + ) diff --git a/tests/integration/test_benchmarks.py b/tests/integration/test_benchmarks.py new file mode 100644 index 00000000..84ee36b0 --- /dev/null +++ b/tests/integration/test_benchmarks.py @@ -0,0 +1,54 @@ +#!/usr/bin/env python3 +# +# Copyright (c) 2025 CNRS +# Author: Paul Sardin +# +# Benchmark test runner - runs all benchmarks with N=1 to verify they work. + +import subprocess +import sys +from pathlib import Path + + +def main(): + folder = Path(__file__).parent + + # List of benchmark files to run (excluding this file and utilities) + excluded = {Path(__file__).name, "benchmark_utils.py", "__init__.py"} + benchmarks = sorted( + script for script in folder.glob("*.py") if script.name not in excluded + ) + + print(f"Running {len(benchmarks)} benchmarks...") + + failed = [] + for script in benchmarks: + print(f"\n{'=' * 60}") + print(f"Running: {script.name}") + print("=" * 60) + + result = subprocess.run( + [sys.executable, str(script), "-N", "1"], + cwd=str(folder), + ) + + if result.returncode != 0: + failed.append(script.name) + print(f"FAILED: {script.name} (exit code {result.returncode})") + + print(f"\n{'=' * 60}") + print("SUMMARY") + print("=" * 60) + print(f"Total benchmarks: {len(benchmarks)}") + print(f"Passed: {len(benchmarks) - len(failed)}") + print(f"Failed: {len(failed)}") + + if failed: + print(f"\nFailed benchmarks: {', '.join(failed)}") + sys.exit(1) + else: + print("\nAll benchmarks passed!") + + +if __name__ == "__main__": + main() diff --git a/tests/benchmarks/ur3-spheres-spf.py b/tests/integration/ur3-spheres-spf.py similarity index 79% rename from tests/benchmarks/ur3-spheres-spf.py rename to tests/integration/ur3-spheres-spf.py index e0a516b9..1df22205 100644 --- a/tests/benchmarks/ur3-spheres-spf.py +++ b/tests/integration/ur3-spheres-spf.py @@ -7,19 +7,17 @@ from math import pi import numpy as np -import datetime as dt -from argparse import ArgumentParser from pyhpp.manipulation.constraint_graph_factory import ConstraintGraphFactory from pyhpp.manipulation import ( Device, Graph, Problem, - createProgressiveProjector, + ProgressiveProjector, urdf, StatesPathFinder, ) -from pyhpp.core import createDichotomy, Straight +from pyhpp.core import Dichotomy, Straight from pyhpp.constraints import ( Transformation, @@ -30,8 +28,9 @@ ) from pinocchio import SE3, Quaternion -parser = ArgumentParser() -parser.add_argument("-N", default=20, type=int) +from benchmark_utils import create_benchmark_parser, run_benchmark_main + +parser = create_benchmark_parser("UR3 Spheres StatesPathFinder Benchmark") args = parser.parse_args() # Robot and environment file paths @@ -130,9 +129,9 @@ q = Quaternion(1, 0, 0, 0) ballPlacement = SE3(q, np.array([0, 0, 0.02])) joint = robot.model().getJointId("sphere{0}/root_joint".format(i)) - pc = Transformation.create( + pc = Transformation( placementName, - robot.asPinDevice(), + robot, joint, Id, ballPlacement, @@ -145,13 +144,13 @@ ComparisonType.EqualToZero, ) implicit_mask = [True, True, True] - implicitPlacementConstraint = Implicit.create(pc, cts, implicit_mask) + implicitPlacementConstraint = Implicit(pc, cts, implicit_mask) constraints[placementName] = implicitPlacementConstraint # placement complement constraint - pc = Transformation.create( + pc = Transformation( placementName + "/complement", - robot.asPinDevice(), + robot, joint, Id, ballPlacement, @@ -163,7 +162,7 @@ ComparisonType.Equality, ) implicit_mask = [True, True, True] - implicitPlacementComplementConstraint = Implicit.create(pc, cts, implicit_mask) + implicitPlacementComplementConstraint = Implicit(pc, cts, implicit_mask) constraints[placementName + "/complement"] = implicitPlacementComplementConstraint # combination of placement and complement @@ -175,8 +174,8 @@ ComparisonType.EqualToZero, ComparisonType.Equality, ) - ll = LockedJoint.createWithComp( - robot.asPinDevice(), + ll = LockedJoint( + robot, "sphere{0}/root_joint".format(i), np.array([0, 0, 0.02, 0, 0, 0, 1]), cts, @@ -194,9 +193,9 @@ q = Quaternion(1, 0, 0, 0) ballPrePlacement = SE3(q, np.array([0, 0, 0.1])) joint = robot.model().getJointId("sphere{0}/root_joint".format(i)) - pc = Transformation.create( + pc = Transformation( preplacementName, - robot.asPinDevice(), + robot, joint, Id, ballPrePlacement, @@ -208,7 +207,7 @@ ComparisonType.EqualToZero, ) implicit_mask = [True, True, True] - implicitPrePlacementConstraint = Implicit.create(pc, cts, implicit_mask) + implicitPrePlacementConstraint = Implicit(pc, cts, implicit_mask) constraints[preplacementName] = implicitPrePlacementConstraint q_init = [ @@ -281,17 +280,20 @@ ) problem.steeringMethod = Straight(problem) -problem.pathValidation = createDichotomy(robot.asPinDevice(), 0) +problem.pathValidation = Dichotomy(robot, 0) # need to set path projector due to implicit constraints added above -problem.pathProjector = createProgressiveProjector( +problem.pathProjector = ProgressiveProjector( problem.distance(), problem.steeringMethod, 0.01 ) cg.initialize() -problem.initConfig(np.array(q_init)) -problem.addGoalConfig(np.array(q_goal)) +q_init_arr = np.array(q_init) +q_goal_arr = np.array(q_goal) + +problem.initConfig(q_init_arr) +problem.addGoalConfig(q_goal_arr) problem.constraintGraph(cg) planner = StatesPathFinder(problem) @@ -301,34 +303,12 @@ problem.setParameter("StatesPathFinder/innerPlannerMaxIterations", 100) problem.setParameter("StatesPathFinder/nTriesUntilBacktrack", 3) -# Run benchmark -# -totalTime = dt.timedelta(0) -totalNumberNodes = 0 -success = 0 -for i in range(args.N): - try: - planner.roadmap().clear() - # TODO: Verify if resetGoalConfigs equivalent is needed - # In old API: ps.resetGoalConfigs() - t1 = dt.datetime.now() - planner.solve() - t2 = dt.datetime.now() - except Exception as e: - print(f"Failed to plan path: {e}") - else: - success += 1 - totalTime += t2 - t1 - print(t2 - t1) - n = len(planner.roadmap().nodes()) - totalNumberNodes += n - print("Number nodes: " + str(n)) - -if args.N != 0: - print("#" * 20) - print(f"Number of rounds: {args.N}") - print(f"Number of successes: {success}") - print(f"Success rate: {success / args.N * 100}%") - if success > 0: - print(f"Average time per success: {totalTime.total_seconds() / success}") - print(f"Average number nodes per success: {totalNumberNodes / success}") +# Run benchmark using shared utilities +if args.N > 0: + run_benchmark_main( + planner=planner, + problem=problem, + q_init=q_init_arr, + q_goal=q_goal_arr, + num_iterations=args.N, + ) diff --git a/tests/benchmarks/ur3-spheres.py b/tests/integration/ur3-spheres.py similarity index 77% rename from tests/benchmarks/ur3-spheres.py rename to tests/integration/ur3-spheres.py index f52e9b48..9efa9ec0 100644 --- a/tests/benchmarks/ur3-spheres.py +++ b/tests/integration/ur3-spheres.py @@ -1,17 +1,16 @@ from math import pi import numpy as np -import datetime as dt from pyhpp.manipulation.constraint_graph_factory import ConstraintGraphFactory from pyhpp.manipulation import ( Device, Graph, Problem, - createProgressiveProjector, + ProgressiveProjector, urdf, ManipulationPlanner, ) -from pyhpp.core import createDichotomy, Straight +from pyhpp.core import Dichotomy, Straight from pyhpp.constraints import ( Transformation, @@ -22,11 +21,9 @@ ) from pinocchio import SE3, Quaternion -# based on /hpp_benchmark/2025/04-01/ur3-spheres/script.py -from argparse import ArgumentParser +from benchmark_utils import create_benchmark_parser, run_benchmark_main -parser = ArgumentParser() -parser.add_argument("-N", default=20, type=int) +parser = create_benchmark_parser("UR3 Spheres Manipulation Benchmark") args = parser.parse_args() # Robot and environment file paths ur3_urdf = "package://example-robot-data/robots/ur_description/urdf/ur3_gripper.urdf" @@ -107,9 +104,9 @@ q = Quaternion(1, 0, 0, 0) ballPlacement = SE3(q, np.array([0, 0, 0.02])) joint = robot.model().getJointId("sphere{0}/root_joint".format(i)) - pc = Transformation.create( + pc = Transformation( placementName, - robot.asPinDevice(), + robot, joint, Id, ballPlacement, @@ -122,12 +119,12 @@ ComparisonType.EqualToZero, ) implicit_mask = [True, True, True] - implicitPlacementConstraint = Implicit.create(pc, cts, implicit_mask) + implicitPlacementConstraint = Implicit(pc, cts, implicit_mask) constraints[placementName] = implicitPlacementConstraint # placement complement constraint - pc = Transformation.create( + pc = Transformation( placementName + "/complement", - robot.asPinDevice(), + robot, joint, Id, ballPlacement, @@ -139,7 +136,7 @@ ComparisonType.Equality, ) implicit_mask = [True, True, True] - implicitPlacementComplementConstraint = Implicit.create(pc, cts, implicit_mask) + implicitPlacementComplementConstraint = Implicit(pc, cts, implicit_mask) constraints[placementName + "/complement"] = implicitPlacementComplementConstraint # combination of placement and complement @@ -151,8 +148,8 @@ ComparisonType.EqualToZero, ComparisonType.Equality, ) - ll = LockedJoint.createWithComp( - robot.asPinDevice(), + ll = LockedJoint( + robot, "sphere{0}/root_joint".format(i), np.array([0, 0, 0.02, 0, 0, 0, 1]), cts, @@ -169,9 +166,9 @@ q = Quaternion(1, 0, 0, 0) ballPrePlacement = SE3(q, np.array([0, 0, 0.1])) joint = robot.model().getJointId("sphere{0}/root_joint".format(i)) - pc = Transformation.create( + pc = Transformation( preplacementName, - robot.asPinDevice(), + robot, joint, Id, ballPrePlacement, @@ -183,7 +180,7 @@ ComparisonType.EqualToZero, ) implicit_mask = [True, True, True] - implicitPrePlacementConstraint = Implicit.create(pc, cts, implicit_mask) + implicitPrePlacementConstraint = Implicit(pc, cts, implicit_mask) constraints[preplacementName] = implicitPrePlacementConstraint q_init = [ @@ -262,46 +259,28 @@ ) problem.steeringMethod = Straight(problem) -problem.pathValidation = createDichotomy(robot.asPinDevice(), 0) -problem.pathProjector = createProgressiveProjector( +problem.pathValidation = Dichotomy(robot, 0) +problem.pathProjector = ProgressiveProjector( problem.distance(), problem.steeringMethod, 0.01 ) cg.initialize() -problem.initConfig(np.array(q_init)) -problem.addGoalConfig(np.array(q_goal)) +q_init_arr = np.array(q_init) +q_goal_arr = np.array(q_goal) + +problem.initConfig(q_init_arr) +problem.addGoalConfig(q_goal_arr) problem.constraintGraph(cg) manipulationPlanner = ManipulationPlanner(problem) manipulationPlanner.maxIterations(5000) -# Run benchmark -# - -totalTime = dt.timedelta(0) -totalNumberNodes = 0 -success = 0 -for i in range(args.N): - try: - manipulationPlanner.roadmap().clear() - t1 = dt.datetime.now() - manipulationPlanner.solve() - t2 = dt.datetime.now() - except Exception as e: - print(f"Failed to plan path: {e}") - break - else: - success += 1 - totalTime += t2 - t1 - print(t2 - t1) - n = len(manipulationPlanner.roadmap().nodes()) - totalNumberNodes += n - print("Number nodes: " + str(n)) -if args.N != 0: - print("#" * 20) - print(f"Number of rounds: {args.N}") - print(f"Number of successes: {success}") - print(f"Success rate: {success / args.N * 100}%") - if success > 0: - print(f"Average time per success: {totalTime.total_seconds() / success}") - print(f"Average number nodes per success: {totalNumberNodes / success}") +# Run benchmark using shared utilities +if args.N > 0: + run_benchmark_main( + planner=manipulationPlanner, + problem=problem, + q_init=q_init_arr, + q_goal=q_goal_arr, + num_iterations=args.N, + ) diff --git a/tests/liegroup.py b/tests/liegroup.py deleted file mode 100644 index 1a0875ae..00000000 --- a/tests/liegroup.py +++ /dev/null @@ -1,25 +0,0 @@ -from pyhpp.pinocchio import LiegroupSpace, LiegroupElement -import numpy as np - -space = LiegroupSpace.R2() -space *= LiegroupSpace.R1(False) -assert str(space) == "R^3" - -space2 = space * space -space2.mergeVectorSpaces() -assert str(space2) == "R^6" - -el1 = LiegroupElement(np.array([0.0, 1.0, 2.0]), space) -el2 = LiegroupElement(np.array([1.0, 2.0, 3.0]), space) -print(f"el1\n->{el1}") -print(f"el2\n->{el2}") -v = el2 - el1 -print(f"v = el2 - el1\n->{v.T}") - -el3 = el1 + v -print(f"el3 = el1 + v\n->{el3}") - -# __eq__ for LiegroupElement not defined -assert el2.space() == el3.space() -assert not el2.space() != el3.space() -assert all(el2.v == el3.v) diff --git a/tests/load_ur3.py b/tests/load_ur3.py deleted file mode 100644 index b27059b2..00000000 --- a/tests/load_ur3.py +++ /dev/null @@ -1,42 +0,0 @@ -import numpy as np -from pinocchio import SE3 -from pinocchio import StdVec_Bool as Mask -from pyhpp.constraints import Position -from pyhpp.pinocchio import Device, LiegroupElement, urdf - -urdfFilename = ( - "package://example-robot-data/robots/ur_description/urdf/ur3_gripper.urdf" -) -srdfFilename = ( - "package://example-robot-data/robots/ur_description/srdf/ur3_gripper.srdf" -) - -robot = Device.create("ur3") -urdf.loadModel(robot, 0, "", "anchor", urdfFilename, srdfFilename, SE3.Identity()) - -q = robot.currentConfiguration() -robot.currentConfiguration(q) - - -m = Mask() -m[:] = (True,) * 3 -Id = SE3.Identity() -pc = Position.create( - "position", robot, robot.model().getJointId("wrist_3_joint"), Id, Id, m -) -print(pc) - -qa = np.zeros((pc.ni, 1)) - -# C++ API -v = LiegroupElement(pc.outputSpace()) -pc.value(v, qa) -print(f"{v.space().name()}: {v.vector().T}") - -J = np.zeros((pc.ndo, pc.ndi)) -pc.jacobian(J, q) -print(J[:, 0:4]) - -# Pythonic API -v = pc(qa) -J = pc.J(q) diff --git a/tests/motion_planner.py b/tests/motion_planner.py deleted file mode 100644 index 4e88bf04..00000000 --- a/tests/motion_planner.py +++ /dev/null @@ -1,67 +0,0 @@ -class MotionPlanner: - def __init__(self, robot, ps): - self.robot = robot - self.ps = ps - - def solveBiRRT(self, maxIter=float("inf")): - self.ps.prepareSolveStepByStep() - finished = False - - # In the framework of the course, - # we restrict ourselves to 2 connected components. - nbCC = self.ps.numberConnectedComponents() - if nbCC != 2: - raise Exception("There should be 2 connected components.") - - iter = 0 - ps = self.ps - robot = self.robot - while True: - # RRT begin - # Extend - newNodes = list() - newEdges = list() - q_rand = robot.shootRandomConfig() - for i in range(ps.numberConnectedComponents()): - q_near, d = ps.getNearestConfig(q_rand, i) - res, pid, msg = ps.directPath(q_near, q_rand, True) - if res: - q_new = q_rand - else: - q_new = ps.configAtParam(pid, ps.pathLength(pid)) - newNodes.append(q_new) - newEdges.append((q_near, q_new, pid)) - for q in newNodes: - ps.addConfigToRoadmap(q) - for q1, q2, pid in newEdges: - ps.addEdgeToRoadmap(q1, q2, pid, True) - # connect - for q_new in newNodes: - for i in range(ps.numberConnectedComponents()): - q_near, d = ps.getNearestConfig(q_new, i) - # if q_near == q_new, q_new is in this connected component - if q_near != q_new: - res, pid, msg = ps.directPath(q_new, q_near, True) - if res: - ps.addEdgeToRoadmap(q_new, q_near, pid, True) - print("finished") - break - # RRT end - # Check if the problem is solved. - nbCC = self.ps.numberConnectedComponents() - if nbCC == 1: - # Problem solved - finished = True - break - iter = iter + 1 - if iter > maxIter: - break - if finished: - self.ps.finishSolveStepByStep() - return self.ps.numberPaths() - 1 - - def solvePRM(self): - self.ps.prepareSolveStepByStep() - # PRM begin - # PRM end - self.ps.finishSolveStepByStep() diff --git a/tests/non_linear_spline_gradient_based.py b/tests/non_linear_spline_gradient_based.py deleted file mode 100644 index 929b63b3..00000000 --- a/tests/non_linear_spline_gradient_based.py +++ /dev/null @@ -1,313 +0,0 @@ -import numpy as np -from pyhpp.constraints import ComparisonType -from pyhpp.core.path import SplineB3 as Spline -from pyhpp.core.path_optimization import LinearConstraint, SplineGradientBasedAbstractB3 - - -class CostFunction: - def __init__(self, robot, order): - self.order = order - self.robot = robot - - def value(self, splines): - return sum(s.squaredNormIntegral(self.order) for s in splines) - - def jacobian(self, splines): - r = splines[0].NbCoeffs * self.robot.numberDof() - J = np.empty((splines.size() * r, 1)) - n = 0 - for s in splines: - s.squaredNormIntegralDerivative(self.order, J[n : n + r, :]) - n += r - return J.T - - def hessian(self, splines): - r = splines[0].NbCoeffs * self.robot.numberDof() - Ic = np.empty((splines[0].NbCoeffs, splines[0].NbCoeffs)) - H = np.zeros((r * splines.size(), r * splines.size())) - - for k, s in enumerate(splines): - s.squaredNormBasisFunctionIntegral(self.order, Ic) - Ic *= 2 - paramSize = s.parameterSize() - shift = k * s.NbCoeffs * paramSize - for i in range(s.NbCoeffs): - ic = shift + i * paramSize - for j in range(s.NbCoeffs): - jc = shift + j * paramSize - H[ic : ic + paramSize, jc : jc + paramSize] = Ic[i, j] * np.eye( - paramSize - ) - return H - - -class NonLinearSplineGradientBasedB3(SplineGradientBasedAbstractB3): - def __init__(self, problem): - super(NonLinearSplineGradientBasedB3, self).__init__(problem) - self.problem = problem - - def optimize(self, path): - print("NonLinearSplineGradientBasedB3::optimize") - checkJointBound = True - - robot = self.problem.robot() - rDof = robot.numberDof() - - # 1 - splines = SplineGradientBasedAbstractB3.Splines() - self.appendEquivalentSpline(path, splines) - if splines.size() == 1: - return self.buildPathVector(splines) - - self.initializePathValidation(splines) - - # 2 - nParameters = len(splines) * Spline.NbCoeffs - # Order 1 -> max continuity: 0 - # Order 3 -> max continuity: 1 - SplineOrder = 3 - MaxContinuityOrder = int((SplineOrder - 1) / 2) - # For the sake of the example, use C^0 continuity, not C^1 - orderContinuity = MaxContinuityOrder - # orderContinuity = 0 - - constraint = LinearConstraint(nParameters * rDof, 0) - solvers = self.SplineOptimizationDatas( - splines.size(), self.SplineOptimizationData(rDof) - ) - - # TODO - # addProblemConstraints (input, splines, constraint, solvers); - self.addContinuityConstraints(splines, orderContinuity, solvers, constraint) - - validations, parameterizations = self.createConstraints(splines) - value, jacobian = self.initValueAndJacobian( - splines, validations, parameterizations - ) - linearizedConstraint = LinearConstraint(jacobian.shape[1], jacobian.shape[0]) - lcReduced = LinearConstraint(0, 0) - # self.computeValue (value, splines, validations, parameterizations) - # self.computeJacobian (jacobian, splines, validations, parameterizations) - # self.computeLinearizedConstraint (linearizedConstraint, value, jacobian, splines, validations, parameterizations) - - # 3 - collision = LinearConstraint(nParameters * rDof, 0) - # collisionFunctions = CollisionFunctions (); - - # 4 - # TODO: Initialize cost function. - cost = CostFunction(robot, 1) - print("Current {cost.value(splines)}") - - # 5 - _feasible = constraint.decompose(True, True) - - self.checkConstraint(splines, constraint, msg="continuity") - - collisionReduced = LinearConstraint(constraint.PK.shape[0], 0) - constraint.reduceConstraint(collision, collisionReduced) - - boundConstraint = LinearConstraint(nParameters * rDof, 0) - if checkJointBound: - self.jointBoundConstraint(splines, boundConstraint) - self.checkConstraint(splines, boundConstraint, msg="bounds", ineq=True) - # if (!this->validateBounds(splines, boundConstraint).empty()) - # throw std::invalid_argument("Input path does not satisfy joint bounds"); - boundConstraintReduced = LinearConstraint(boundConstraint.PK.shape[0], 0) - constraint.reduceConstraint(boundConstraint, boundConstraintReduced, False) - - costLinearized = LinearConstraint(nParameters * rDof, 1) - clReduced = LinearConstraint(0, 1) - constraint.reduceConstraint(costLinearized, clReduced) - - finalLinearProblem = LinearConstraint(0, 1) - ok = False - iter = 0 - while not ok: - v = cost.value(splines) - print("Iter {iter}, cost: {v}") - - # Compute linearized cost - parameters = np.empty((nParameters * rDof, 1)) - self.updateParameters(parameters, splines) - costLinearized.J = cost.jacobian(splines) - costLinearized.v = costLinearized.J.dot(parameters) - np.matrix([[v]]) - constraint.reduceConstraint(costLinearized, clReduced) - - # Compute decomposition. - self.computeLinearizedConstraint( - linearizedConstraint, - value, - jacobian, - splines, - validations, - parameterizations, - ) - print("Linearized constraints") - constraint.reduceConstraint(linearizedConstraint, lcReduced, True) - print("reduction") - lcReduced.decompose(True, True) - print("decompose") - - lcReduced.reduceConstraint(clReduced, finalLinearProblem, False) - print("reduction") - finalLinearProblem.decompose(True, True) # TODO need only xStar, not PK - - # Compute descent direction. - lcReduced.computeSolution(finalLinearProblem.xStar) - print("computeSolution") - constraint.computeSolution(lcReduced.xSol) - - self.integrate(splines, constraint.xSol) - - iter += 1 - if iter == 5: - ok = True - - return self.buildPathVector(splines) - - def integrate(self, splines, dir): - alpha = 0.5 - row = 0 - for s in splines: - n = s.parameterSize() * s.NbCoeffs - s.parameterIntegrate(alpha * dir[row : row + n]) - row += n - - def createConstraints(self, splines): - # Functions of the form f(v_k) == 0 - validations = [[] for s in splines] - # Functions of the form g(v_k) == g(v_{k+1}) - parameterizations = [[] for s in splines] - for k, s in enumerate(splines): - # Extend function to - c = s.constraints() - if c: - cp = c.configProjector() - if cp: - ncs = cp.numericalConstraints() - ljs = cp.lockedJoints() - for nc in ncs: - if ComparisonType.Equality in nc.comparisonType(): - parameterizations[k].append(nc) - else: - validations[k].append(nc) - for nc in ljs: - if ComparisonType.Equality in nc.comparisonType(): - parameterizations[k].append(nc) - else: - validations[k].append(nc) - else: - print(f"{k} no config projector") - else: - print(f"{k} no constraints") - return validations, parameterizations - - def initValueAndJacobian(self, splines, validations, parameterizations): - vRows = 0 - JRows = JCols = 0 - for k, (s, v, p) in enumerate(zip(splines, validations, parameterizations)): - for f in v: - vRows += f.function().outputSize() * 2 - JRows += f.function().outputDerivativeSize() * 2 - for f in p: - vRows += f.function().outputDerivativeSize() - JRows += f.function().outputDerivativeSize() - - JCols += s.parameterSize() * s.NbCoeffs - - return np.zeros((vRows, 1)), np.zeros((JRows, JCols)) - - def computeValue(self, value, splines, validations, parameterizations): - i = 0 - for k, (s, v, p) in enumerate(zip(splines, validations, parameterizations)): - qinit = s.initial() - qend = s.end() - - for f in v: - e = i + f.function().outputSize() - value[i:e] = f.function()(qinit).vector() - i = e - - e = i + f.function().outputSize() - value[i:e] = f.function()(qend).vector() - i = e - - for f in p: - e = i + f.function().outputDerivativeSize() - value[i:e] = f.function()(qinit) - f.function()(qend) - i = e - - def computeJacobian(self, J, splines, validations, parameterizations): - ir = ic = 0 - for k, (s, v, p) in enumerate(zip(splines, validations, parameterizations)): - qinit = s.initial() - qend = s.end() - - ps = s.parameterSize() - _ec = ic + s.NbCoeffs * ps - ics = [ic + i * ps for i in range(s.NbCoeffs + 1)] - - # Only the endpoints and not the other points ? - for f in v: - er = ir + f.function().outputDerivativeSize() - Jfunction = f.function().J(qinit) - paramDerInit = s.parameterDerivativeCoefficients(s.paramRange().first) - for c in range(s.NbCoeffs): - J[ir:er, ics[c] : ics[c + 1]] = Jfunction * paramDerInit[c, 0] - ir = er - - er = ir + f.function().outputDerivativeSize() - Jfunction = f.function().J(qend) - paramDerEnd = s.parameterDerivativeCoefficients(s.paramRange().second) - for c in range(s.NbCoeffs): - J[ir:er, ics[c] : ics[c + 1]] = Jfunction * paramDerEnd[c, 0] - ir = er - - for f in p: - er = ir + f.function().outputDerivativeSize() - JfunctionInit = f.function().J(qinit) - JfunctionEnd = f.function().J(qend) - paramDerInit = s.parameterDerivativeCoefficients(s.paramRange().first) - paramDerEnd = s.parameterDerivativeCoefficients(s.paramRange().second) - # J[ir:er, ics[0]:ics[1]] = f.function().J(s.end()) * s.parameterDerivativeCoefficients(s.paramRange().second) - for c in range(s.NbCoeffs): - J[ir:er, ics[c] : ics[c + 1]] = ( - JfunctionInit * paramDerInit[c, 0] - - JfunctionEnd * paramDerEnd[c, 0] - ) - ir = er - - return J - - def computeLinearizedConstraint( - self, constraint, value, J, splines, validations, parameterizations - ): - self.computeValue(value, splines, validations, parameterizations) - self.computeJacobian(J, splines, validations, parameterizations) - - s = splines[0] - parameters = np.empty((splines.size() * s.parameterSize() * s.NbCoeffs, 1)) - self.updateParameters(parameters, splines) - - constraint.b = J.dot(parameters) - value - constraint.J = J - constraint.decompose(False, False) - - def checkConstraint(self, splines, constraints, ineq=False, msg=""): - x = np.empty((0)) - for s in splines: - x = np.concatenate((x, s.rowParameters())) - res = constraints.J.dot(x) - constraints.b - if ineq: - ok = all(res >= 0) - else: - ok = np.linalg.norm(res) < 1e-3 - if not ok: - print(f"Constraint {msg} not satisfied: {res.T}") - return ok, res - - @staticmethod - def create(problem): - print("NonLinearSplineGradientBasedB3.create called!") - return NonLinearSplineGradientBasedB3(problem) diff --git a/tests/path-optimizer.py b/tests/path-optimizer.py deleted file mode 100644 index 58cca6f8..00000000 --- a/tests/path-optimizer.py +++ /dev/null @@ -1,60 +0,0 @@ -from pyhpp.pinocchio import urdf -from pyhpp.core import ProblemSolver -import pyhpp.core.path -import numpy as np - -from non_linear_spline_gradient_based import NonLinearSplineGradientBasedB3 - -ps = ProblemSolver.create() -robot = ps.createRobot("ur3") -urdf.loadRobotModel( - robot, - "anchor", - "example-robot-data/robots/ur_description", - "ur3", - "_gripper", - "_gripper", -) -ps.robot(robot) - -ps.pathOptimizers.add( - "NonLinearSplineGradientBasedB3", NonLinearSplineGradientBasedB3.create -) -ps.addPathOptimizer("NonLinearSplineGradientBasedB3") -ps.createPathOptimizers() -ps.clearPathOptimizers() - -qinit = np.zeros((robot.model().nq, 1)) -qgoal = qinit.copy() -qgoal[0, 0] = 1 - -ps.initConfig(qinit) -ps.addGoalConfig(qgoal) -ps.solve() - -paths = ps.paths() -path = paths[0] - -problem = ps.problem() -po = NonLinearSplineGradientBasedB3(problem) -optPath = po.optimize(path) - -steer = problem.steeringMethod() -shooter = problem.configurationShooter() -for _ in range(10): - qs = shooter.shoot() # start - qi = shooter.shoot() # intermediate - qe = shooter.shoot() # end - p1 = steer(qs, qi) - p2 = steer(qi, qe) - # valid = problem.pathValidation().validate (path, False, validPart, report) - valid1, validPart, report = problem.pathValidation().validate(p1, False) - valid2, validPart, report = problem.pathValidation().validate(p2, False) - if not valid1 or not valid2: - pathVector = pyhpp.core.path.Vector.create( - robot.configSize(), robot.numberDof() - ) - pathVector.appendPath(p1) - pathVector.appendPath(p2) - print(pathVector.numberPaths()) - po.optimize(pathVector) diff --git a/tests/path-planner.py b/tests/path-planner.py deleted file mode 100644 index b0c7d9c6..00000000 --- a/tests/path-planner.py +++ /dev/null @@ -1,102 +0,0 @@ -import numpy as np -from pinocchio import SE3 -from pyhpp.pinocchio import Device, urdf -from pyhpp.core import ( - Problem, - DiffusingPlanner, - BiRRTPlanner, - VisibilityPrmPlanner, - BiRrtStar, - kPrmStar, -) - -urdfFilename = "package://example-robot-data/robots/pr2_description/urdf/pr2.urdf" -srdfFilename = "package://example-robot-data/robots/pr2_description/srdf/pr2.srdf" -rootJointType = "planar" - -# Initialize robot and viewer -robot = Device.create("ur2") -urdf.loadModel( - robot, 0, "r0", rootJointType, urdfFilename, srdfFilename, SE3.Identity() -) - -# Define initial and goal configurations -q_init = np.array(robot.currentConfiguration()) -q_goal = np.array(q_init[::].copy()) -model = robot.model() - -# Set root_joint bounds only -lowerLimit = model.lowerPositionLimit -upperLimit = model.upperPositionLimit - -# Set root_joint bounds specifically -root_joint_bounds = [-4, -3, -5, -3] # [x_lower, x_upper, y_lower, y_upper] -ij = model.getJointId("r0/root_joint") -iq = model.idx_qs[ij] - -# Apply bounds (assuming first 2 DOF are x,y position) -lowerLimit[iq] = -4 -upperLimit[iq] = -3 -lowerLimit[iq + 1] = -5 -upperLimit[iq + 1] = -3 - -rankInConfiguration = dict() -current_rank = 0 -for joint_id in range(1, model.njoints): - joint_name = model.names[joint_id] - joint = model.joints[joint_id] - rankInConfiguration[joint_name[3:]] = current_rank - - current_rank += joint.nq - -q_init[0:2] = [-3.2, -4] - -rank = rankInConfiguration["torso_lift_joint"] -q_init[rank] = 0.2 -q_goal[0:2] = [-3.2, -4] -rank = rankInConfiguration["l_shoulder_lift_joint"] -q_goal[rank] = 0.5 -rank = rankInConfiguration["l_elbow_flex_joint"] -q_goal[rank] = -0.5 -rank = rankInConfiguration["r_shoulder_lift_joint"] -q_goal[rank] = 0.5 -rank = rankInConfiguration["r_elbow_flex_joint"] -q_goal[rank] = -0.5 - -# urdf.loadModel(robot, 0, "kitchen", "anchor", "package://hpp_environments/urdf/kitchen_area_obstacle.urdf", "", SE3.Identity()) - -problem = Problem(robot) -problem.initConfig(q_init) -problem.addGoalConfig(q_goal) - -diffusingPlanner = DiffusingPlanner(problem) -biRRTPlanner = BiRRTPlanner(problem) -visibilityPrmPlanner = VisibilityPrmPlanner(problem) -biRrtStar = BiRrtStar(problem) -kPrmStar_inst = kPrmStar(problem) - -print(diffusingPlanner.maxIterations()) - -path = biRRTPlanner.solve() -# viewer.displayPath(path, 0.001, 50) - -# path = diffusingPlanner.solve() -# viewer.displayPath(path, 0.001, 50) - -# path = visibilityPrmPlanner.solve() infinite search -# viewer.displayPath(path, 0.001, 50) - -# path = biRrtStar.solve() -# viewer.displayPath(path, 0.001, 50) - -# path = kPrmStar_inst.solve() infinite search -# viewer.displayPath(path, 0.001, 50) - -# roadmap = Roadmap.create(problem.distance(), robot) -# roadmap.initNode(q_init) -# roadmap.addGoalNode(q_goal) -# searchInRoadmap = SearchInRoadmap(problem, roadmap) -# searchInRoadmap.solve() SearchInRoadmap: no goal configuration in the connected componentof initial configuration. A* fails - -# planAndOptimize = PlanAndOptimize(diffusingPlanner) -# viewer.displayPath(planAndOptimize.solve(), 0.001, 50) diff --git a/tests/path.py b/tests/path.py deleted file mode 100644 index c5b465d6..00000000 --- a/tests/path.py +++ /dev/null @@ -1,118 +0,0 @@ -import unittest - -import numpy as np -import pyhpp.core - - -class PythonStraightPath(pyhpp.core.PathWrap): - def __init__(self, a: float, b: float, v: float): - duration = (b - a) / v - super().__init__(pyhpp.core.interval(0, duration), 1, 1) - self.a = a - self.b = b - self.v = v - self.initPtr(self) - self._counters = {} - - def _inc_counter(self, key): - self._counters[key] = self._counters.get(key, 0) + 1 - - def copy(self): - self._inc_counter("copy") - p = PythonStraightPath(self.a, self.b, self.v) - return p - - def initial(self): - self._inc_counter("initial") - return self.a - - def end(self): - self._inc_counter("end") - return self.b - - def impl_compute(self, param): - self._inc_counter("impl_compute") - return ( - np.array( - [ - self.a + param * self.v, - ] - ), - True, - ) - - def impl_derivative(self, param, order): - self._inc_counter("impl_derivative") - if order == 1: - return np.array( - [ - self.v, - ] - ) - else: - return np.array( - [ - 0.0, - ] - ) - - def clear_counter(self, key=None): - if key is None: - self._counters.clear() - else: - self._counters["key"] = 0 - - def assert_called(self, key): - assert self._counters.get(key, 0) > 0 - - -class TestPath(unittest.TestCase): - def test_path_api(self): - space = pyhpp.pinocchio.LiegroupSpace.R1(False) - path = pyhpp.core.StraightPath.create( - space, - np.array((0.0,)), - np.array((1.0,)), - pyhpp.core.interval(1.0, 2.0), - None, - ) - self.assertEqual(path.initial(), 0.0) - self.assertEqual(path.end(), 1.0) - self.assertEqual(path.length(), 1.0) - - self.assertEqual(path.eval(1.5), (0.5, True)) - self.assertEqual(path.derivative(1.5, 1), 1.0) - self.assertEqual(path.derivative(1.5, 2), 0.0) - - extracted_path = path.extract(1.5, 2.0) - self.assertTrue(isinstance(extracted_path, pyhpp.core.StraightPath)) - self.assertEqual(extracted_path.length(), 0.5) - - def test_path_inheritance(self): - path = PythonStraightPath(2.0, 4.0, 1.0) - - # path.initial() - res, ok = path.eval(1.0) - self.assertTrue(ok) - self.assertEqual(res, 3.0) - path.assert_called("impl_compute") - - res = path.derivative(1.0, 1) - self.assertEqual(res, 1.0) - path.assert_called("impl_derivative") - - copy = path.copy() - self.assertEqual(copy.initial(), path.initial()) - self.assertEqual(copy.end(), path.end()) - - # Check that it can be used as a path - pv = pyhpp.core.path.Vector.create(1, 1) - pv.appendPath(path) - - path.clear_counter("copy") - _path_0 = pv.pathAtRank(0) - path.assert_called("copy") - - -if __name__ == "__main__": - unittest.main() diff --git a/tests/problem-solver.py b/tests/problem-solver.py deleted file mode 100644 index 6bf61717..00000000 --- a/tests/problem-solver.py +++ /dev/null @@ -1,35 +0,0 @@ -from pyhpp.pinocchio import urdf -from pyhpp.core import ProblemSolver -import numpy as np - -ps = ProblemSolver.create() -robot = ps.createRobot("ur3") -urdf.loadRobotModel( - robot, - "anchor", - "example-robot-data/robots/ur_description", - "ur3", - "_gripper", - "_gripper", -) -ps.robot(robot) - -qinit = np.zeros((robot.model().nq, 1)) -qgoal = qinit.copy() -qgoal[0, 0] = 1 - -ps.initConfig(qinit) -ps.addGoalConfig(qgoal) -ps.solve() - -paths = ps.paths() -path = paths[0] - -assert (path.initial() == qinit).all() - -q, success = path(0.3) -assert success - -qq = np.zeros_like(qinit) -success = path(qq, path.length()) -assert success diff --git a/tests/rrt.py b/tests/rrt.py deleted file mode 100644 index 6edf2447..00000000 --- a/tests/rrt.py +++ /dev/null @@ -1,101 +0,0 @@ -import numpy as np -from pinocchio import SE3 -from pyhpp.pinocchio import Device, urdf -from pyhpp.core import Problem, Roadmap, WeighedDistance - -# Robot configuration -urdfFilename = "package://example-robot-data/robots/ur_description/urdf/ur5_joint_limited_robot.urdf" -srdfFilename = "package://example-robot-data/robots/ur_description/srdf/ur5_joint_limited_robot.srdf" - -# Initialize robot and viewer -robot = Device.create("ur5") - -# Add robot and obstacles to scene -urdf.loadModel(robot, 0, "r0", "anchor", urdfFilename, srdfFilename, SE3.Identity()) - - -# urdf.loadModel(robot, 0, "table", "anchor", "package://hpp_environments/urdf/ur_benchmark/table.urdf", "", SE3.Identity()) -# urdf.loadModel(robot, 0, "wall", "anchor", "package://hpp_environments/urdf/ur_benchmark/wall.urdf", "", SE3.Identity()) -# urdf.loadModel(robot, 0, "obstacles", "anchor", "package://hpp_environments/urdf/ur_benchmark/obstacles.urdf", "", SE3.Identity()) - - -# Define initial and goal configurations -qInit = np.array([0.2, -1.57, -1.8, 0, 0.8, 0]) -qGoal = np.array([1.57, -1.57, -1.8, 0, 0.8, 0]) - -# Setup problem and RRT components -problem = Problem(robot) -configurationShooter = problem.configurationShooter() -steer = problem.steeringMethod() -weighedDistance = WeighedDistance.create(robot) -distance = weighedDistance.asDistancePtr_t() - -# Initialize roadmap -roadmap = Roadmap.create(distance, robot) -roadmap.initNode(qInit) -roadmap.addGoalNode(qGoal) - -# RRT algorithm parameters -finished = False -iter = 0 -maxIter = 1000 - -# Main RRT loop -while not finished and iter < maxIter: - # Extend phase - newNodes = [] - newEdges = [] - q_rand = configurationShooter.shoot() - iter += 1 - - # Try to extend from each connected component - for i in range(roadmap.numberConnectedComponents()): - cc = roadmap.getConnectedComponent(i) - q_near, d = roadmap.nearestNode(q_rand, cc) - directpath = steer(q_near, q_rand) - res, validPart, report = problem.pathValidation().validate(directpath, False) - - if res: - q_new = q_rand - else: - q_new = validPart.end() - - newNodes.append(q_new) - newEdges.append((q_near, q_new, validPart)) - - # Add new nodes to roadmap - for q in newNodes: - roadmap.addNode(q) - - # Add new edges to roadmap - for q1, q2, path in newEdges: - roadmap.addEdge(q1, q2, path) - - # Connect phase - for q_new in newNodes: - for i in range(roadmap.numberConnectedComponents()): - cc = roadmap.getConnectedComponent(i) - q_near, d = roadmap.nearestNode(q_new, cc) - - if (q_near != q_new).all(): - directpath = steer(q_new, q_near) - res, validPart, report = problem.pathValidation().validate( - directpath, False - ) - - if res: - roadmap.addEdge(q_new, q_near, validPart) - break - - # Check if problem is solved - nbCC = roadmap.numberConnectedComponents() - if nbCC == 1: - print("Problem solved!") - finished = True - -# Compute and display final path -if finished: - path = problem.target().computePath(roadmap) - print(path) -else: - print(f"Maximum iterations ({maxIter}) reached without finding solution") diff --git a/tests/solver.py b/tests/solver.py deleted file mode 100644 index a7d7baed..00000000 --- a/tests/solver.py +++ /dev/null @@ -1,66 +0,0 @@ -from pyhpp.pinocchio import Device, urdf - -from pyhpp.constraints import ( - Position, - ComparisonTypes, - ComparisonType, - BySubstitution, - segment, - Implicit, - Explicit, -) -from pinocchio import SE3, StdVec_Bool as Mask - -robot = Device.create("ur3") -urdf.loadRobotModel( - robot, - "anchor", - "example-robot-data/robots/ur_description", - "ur3", - "_gripper", - "_gripper", -) - -m = Mask() -m[:] = (True,) * 3 -Id = SE3.Identity() -pc = Position.create( - "position", robot, robot.model().getJointId("wrist_3_joint"), Id, Id, m -) - -solver = BySubstitution(robot.configSpace()) -cts = ComparisonTypes() -cts[:] = ( - ComparisonType.EqualToZero, - ComparisonType.EqualToZero, - ComparisonType.Equality, -) -solver.add(Implicit.create(pc, cts, [True, True, True]), 0) - -print(solver) - -# This only tests the call to add. -# The inputs are not valid so the solver -# will not be able to solve anything. -solver.explicitConstraintSet().add( - Explicit.create( - robot.configSpace(), - pc, - [ - segment(0, 2), - ], - [ - segment(2, 4), - ], - [ - segment(0, 2), - ], - [ - segment(2, 4), - ], - cts, - ) -) -solver.explicitConstraintSetHasChanged() - -print(solver) diff --git a/tests/test_non_linear_optimization.py b/tests/test_non_linear_optimization.py deleted file mode 100644 index 70d11782..00000000 --- a/tests/test_non_linear_optimization.py +++ /dev/null @@ -1,14 +0,0 @@ -from pyhpp.core import ProblemSolver -from pyhpp.corbaserver import Server - -from non_linear_spline_gradient_based import NonLinearSplineGradientBasedB3 - -ps = ProblemSolver.create() -ps.pathOptimizers.add( - "NonLinearSplineGradientBasedB3", NonLinearSplineGradientBasedB3.create -) - -server = Server(ps, False) -server.startCorbaServer() -server.processRequest(True) -del server diff --git a/tests/unit/__init__.py b/tests/unit/__init__.py new file mode 100644 index 00000000..b9e59ae1 --- /dev/null +++ b/tests/unit/__init__.py @@ -0,0 +1 @@ +# Unit tests for hpp-python diff --git a/tests/unit/conftest.py b/tests/unit/conftest.py new file mode 100644 index 00000000..293535c7 --- /dev/null +++ b/tests/unit/conftest.py @@ -0,0 +1,263 @@ +#!/usr/bin/env python +# +# Copyright (c) 2025 CNRS +# Author: Paul Sardin +# +# Test fixtures for hpp-python unit tests. +# Provides factory functions that create test objects. + +import numpy as np +from math import pi +from pinocchio import SE3, Quaternion + +from pyhpp.pinocchio import Device as CoreDevice, urdf as core_urdf +from pyhpp.core import Problem as CoreProblem +from pyhpp.manipulation import Device, urdf, Graph, Problem +from pyhpp.manipulation.constraint_graph_factory import ConstraintGraphFactory +from pyhpp.manipulation.security_margins import SecurityMargins +from pyhpp.constraints import ( + Transformation, + ComparisonTypes, + ComparisonType, + Implicit, + LockedJoint, +) + + +UR5_URDF = "package://example-robot-data/robots/ur_description/urdf/ur5_joint_limited_robot.urdf" +UR5_SRDF = "package://example-robot-data/robots/ur_description/srdf/ur5_joint_limited_robot.srdf" + + +def create_ur5_problem(): + """Create a UR5 robot and core Problem for non-manipulation tests.""" + robot = CoreDevice("ur5") + core_urdf.loadModel(robot, 0, "ur5", "anchor", UR5_URDF, UR5_SRDF, SE3.Identity()) + problem = CoreProblem(robot) + return problem, robot + + +# Robot URDF/SRDF paths +UR3_URDF = "package://example-robot-data/robots/ur_description/urdf/ur3_gripper.urdf" +UR3_SRDF = "package://example-robot-data/robots/ur_description/srdf/ur3_gripper.srdf" +SPHERE_URDF = "package://hpp_environments/urdf/construction_set/sphere.urdf" +SPHERE_SRDF = "package://hpp_environments/srdf/construction_set/sphere.srdf" +GROUND_URDF = "package://hpp_environments/urdf/construction_set/ground.urdf" +GROUND_SRDF = "package://hpp_environments/srdf/construction_set/ground.srdf" + + +def create_ur3_robot(): + """Load a basic UR3 robot without manipulation objects.""" + robot = Device("ur3-test") + ur3_pose = SE3(rotation=np.identity(3), translation=np.array([0, 0, 0])) + urdf.loadModel(robot, 0, "ur3", "anchor", UR3_URDF, UR3_SRDF, ur3_pose) + robot.setJointBounds("ur3/shoulder_pan_joint", [-pi, 4]) + robot.setJointBounds("ur3/shoulder_lift_joint", [-pi, 0]) + robot.setJointBounds("ur3/elbow_joint", [-2.6, 2.6]) + return robot + + +def _create_placement_constraints(robot, objects): + """Helper to create placement constraints for objects.""" + constraints = {} + model = robot.model() + + for obj in objects: + placement_name = f"place_{obj}" + Id = SE3.Identity() + q = Quaternion(1, 0, 0, 0) + ball_placement = SE3(q, np.array([0, 0, 0.02])) + joint = model.getJointId(f"{obj}/root_joint") + + # Placement constraint + pc = Transformation( + placement_name, + robot, + joint, + Id, + ball_placement, + [False, False, True, True, True, False], + ) + cts = ComparisonTypes() + cts[:] = ( + ComparisonType.EqualToZero, + ComparisonType.EqualToZero, + ComparisonType.EqualToZero, + ) + constraints[placement_name] = Implicit(pc, cts, [True, True, True]) + + # Placement complement + pc_comp = Transformation( + placement_name + "/complement", + robot, + joint, + Id, + ball_placement, + [True, True, False, False, False, True], + ) + cts_comp = ComparisonTypes() + cts_comp[:] = ( + ComparisonType.Equality, + ComparisonType.Equality, + ComparisonType.Equality, + ) + constraints[placement_name + "/complement"] = Implicit( + pc_comp, cts_comp, [True, True, True] + ) + + # Hold constraint (LockedJoint) + cts_hold = ComparisonTypes() + cts_hold[:] = ( + ComparisonType.Equality, + ComparisonType.Equality, + ComparisonType.EqualToZero, + ComparisonType.EqualToZero, + ComparisonType.EqualToZero, + ComparisonType.Equality, + ) + ll = LockedJoint( + robot, + f"{obj}/root_joint", + np.array([0, 0, 0.02, 0, 0, 0, 1]), + cts_hold, + ) + constraints[placement_name + "/hold"] = ll + + # Pre-placement constraint + preplace_name = f"preplace_{obj}" + ball_preplace = SE3(q, np.array([0, 0, 0.1])) + pc_pre = Transformation( + preplace_name, + robot, + joint, + Id, + ball_preplace, + [False, False, True, True, True, False], + ) + cts_pre = ComparisonTypes() + cts_pre[:] = ( + ComparisonType.EqualToZero, + ComparisonType.EqualToZero, + ComparisonType.EqualToZero, + ) + constraints[preplace_name] = Implicit(pc_pre, cts_pre, [True, True, True]) + + return constraints + + +def create_ur3_with_spheres(): + """ + Load UR3 with two spheres for manipulation testing. + Returns (robot, objects, constraints) tuple. + """ + robot = Device("ur3-spheres-test") + + # Load UR3 + ur3_pose = SE3(rotation=np.identity(3), translation=np.array([0, 0, 0])) + urdf.loadModel(robot, 0, "ur3", "anchor", UR3_URDF, UR3_SRDF, ur3_pose) + + # Load spheres + n_spheres = 2 + objects = [] + for i in range(n_spheres): + sphere_pose = SE3(rotation=np.identity(3), translation=np.array([0, 0, 0])) + urdf.loadModel( + robot, + 0, + f"sphere{i}", + "freeflyer", + SPHERE_URDF, + SPHERE_SRDF, + sphere_pose, + ) + robot.setJointBounds( + f"sphere{i}/root_joint", + [ + -1.0, + 1.0, + -1.0, + 1.0, + -0.1, + 1.0, + -1.0001, + 1.0001, + -1.0001, + 1.0001, + -1.0001, + 1.0001, + -1.0001, + 1.0001, + ], + ) + objects.append(f"sphere{i}") + + # Load ground + ground_pose = SE3(rotation=np.identity(3), translation=np.array([0, 0, 0])) + urdf.loadModel( + robot, 0, "kitchen_area", "anchor", GROUND_URDF, GROUND_SRDF, ground_pose + ) + + # Set robot joint bounds + robot.setJointBounds("ur3/shoulder_pan_joint", [-pi, 4]) + robot.setJointBounds("ur3/shoulder_lift_joint", [-pi, 0]) + robot.setJointBounds("ur3/elbow_joint", [-2.6, 2.6]) + + # Create constraints + constraints = _create_placement_constraints(robot, objects) + + return robot, objects, constraints + + +def create_constraint_graph_setup(): + """ + Create a complete constraint graph setup with factory. + Returns (problem, graph, factory, robot, objects) tuple. + """ + robot, objects, constraints = create_ur3_with_spheres() + + problem = Problem(robot) + graph = Graph("test-graph", robot, problem) + graph.maxIterations(40) + graph.errorThreshold(0.0001) + + # Register constraints with graph + for obj in objects: + graph.registerConstraints( + constraints[f"place_{obj}"], + constraints[f"place_{obj}/complement"], + constraints[f"place_{obj}/hold"], + ) + + # Setup handle masks (similar to graph_factory2.py pattern) + handles = robot.handles() + for obj in objects: + handle_name = f"{obj}/handle" + if handle_name in handles: + h = handles[handle_name] + h.mask = [True, True, True, False, True, True] + + # Create factory + factory = ConstraintGraphFactory(graph, constraints) + grippers = ["ur3/gripper"] + handles_per_object = [[f"{obj}/handle"] for obj in objects] + contacts_per_object = [[] for _ in objects] + + factory.setGrippers(grippers) + factory.setObjects(objects, handles_per_object, contacts_per_object) + factory.generate() + + graph.initialize() + + return problem, graph, factory, robot, objects + + +def create_security_margins_instance(): + """ + Create a SecurityMargins instance with basic configuration. + Returns (security_margins, problem, graph, factory, robot, objects) tuple. + """ + problem, graph, factory, robot, objects = create_constraint_graph_setup() + robots_and_objects = ["ur3"] + objects + + sm = SecurityMargins(problem, factory, robots_and_objects, robot) + + return sm, problem, graph, factory, robot, objects diff --git a/tests/unit/test_configuration_shooter.py b/tests/unit/test_configuration_shooter.py new file mode 100644 index 00000000..fdfd4cf3 --- /dev/null +++ b/tests/unit/test_configuration_shooter.py @@ -0,0 +1,46 @@ +#!/usr/bin/env python +# +# Copyright (c) 2025 CNRS +# Author: Paul Sardin +# + +import unittest +import numpy as np +from unit.conftest import create_ur5_problem + + +class TestConfigurationShooter(unittest.TestCase): + @classmethod + def setUpClass(cls): + cls.problem, cls.robot = create_ur5_problem() + + def test_returns_valid_configuration(self): + shooter = self.problem.configurationShooter() + + q = shooter.shoot() + + self.assertEqual(len(q), self.robot.configSize()) + + +class TestConfigurationShooterEdgeCases(unittest.TestCase): + """Edge case tests for ConfigurationShooter.""" + + @classmethod + def setUpClass(cls): + cls.problem, cls.robot = create_ur5_problem() + + def test_shot_respects_joint_bounds(self): + """Shot configurations should respect joint bounds.""" + shooter = self.problem.configurationShooter() + + for _ in range(10): + q = shooter.shoot() + for i in range(len(q)): + self.assertFalse(np.isnan(q[i]), f"Configuration element {i} is NaN") + self.assertFalse( + np.isinf(q[i]), f"Configuration element {i} is infinite" + ) + + +if __name__ == "__main__": + unittest.main() diff --git a/tests/unit/test_constraint_factory.py b/tests/unit/test_constraint_factory.py new file mode 100644 index 00000000..420805ff --- /dev/null +++ b/tests/unit/test_constraint_factory.py @@ -0,0 +1,142 @@ +#!/usr/bin/env python +# +# Copyright (c) 2025 CNRS +# Author: Paul Sardin +# + +import unittest +from unit.conftest import create_constraint_graph_setup +from pyhpp.manipulation.constraint_graph_factory import ( + Constraints, +) + + +class TestConstraintFactoryBuildGrasp(unittest.TestCase): + @classmethod + def setUpClass(cls): + cls.problem, cls.graph, cls.factory, cls.robot, cls.objects = ( + create_constraint_graph_setup() + ) + + def test_build_grasp_returns_dict(self): + result = self.factory.constraints.buildGrasp("ur3/gripper", "sphere0/handle") + + self.assertIsInstance(result, dict) + self.assertIn("grasp", result) + self.assertIn("graspComplement", result) + self.assertIn("preGrasp", result) + + def test_build_grasp_constraint_values_are_constraints(self): + result = self.factory.constraints.buildGrasp("ur3/gripper", "sphere0/handle") + + self.assertIsInstance(result["grasp"], Constraints) + self.assertIsInstance(result["graspComplement"], Constraints) + self.assertIsInstance(result["preGrasp"], Constraints) + + def test_build_grasp_caches_constraints(self): + cf = self.factory.constraints + + result1 = cf.buildGrasp("ur3/gripper", "sphere1/handle") + result2 = cf.buildGrasp("ur3/gripper", "sphere1/handle") + + self.assertEqual( + result1["grasp"].numConstraints, result2["grasp"].numConstraints + ) + + +class TestConstraintFactoryBuildPlacement(unittest.TestCase): + @classmethod + def setUpClass(cls): + cls.problem, cls.graph, cls.factory, cls.robot, cls.objects = ( + create_constraint_graph_setup() + ) + + def test_build_placement_returns_dict(self): + result = self.factory.constraints.buildPlacement("sphere0") + + self.assertIsInstance(result, dict) + self.assertIn("placement", result) + self.assertIn("placementComplement", result) + self.assertIn("prePlacement", result) + + def test_build_placement_constraint_values_are_constraints(self): + result = self.factory.constraints.buildPlacement("sphere0") + + self.assertIsInstance(result["placement"], Constraints) + self.assertIsInstance(result["placementComplement"], Constraints) + self.assertIsInstance(result["prePlacement"], Constraints) + + +class TestConstraintFactoryRegistration(unittest.TestCase): + @classmethod + def setUpClass(cls): + cls.problem, cls.graph, cls.factory, cls.robot, cls.objects = ( + create_constraint_graph_setup() + ) + + def test_constraints_registered_after_build(self): + cf = self.factory.constraints + + cf.buildGrasp("ur3/gripper", "sphere0/handle") + + name = "ur3/gripper grasps sphere0/handle" + self.assertIn(name, cf.available_constraints) + + def test_pregrasp_registered_after_build(self): + cf = self.factory.constraints + + cf.buildGrasp("ur3/gripper", "sphere0/handle") + + name = "ur3/gripper pregrasps sphere0/handle" + self.assertIn(name, cf.available_constraints) + + +class TestConstraintFactoryNegativeCases(unittest.TestCase): + """Negative test cases for ConstraintFactory.""" + + @classmethod + def setUpClass(cls): + cls.problem, cls.graph, cls.factory, cls.robot, cls.objects = ( + create_constraint_graph_setup() + ) + + def test_build_grasp_invalid_gripper_raises(self): + """Building grasp with non-existent gripper should raise.""" + cf = self.factory.constraints + + with self.assertRaises(Exception): + cf.buildGrasp("nonexistent/gripper", "sphere0/handle") + + def test_build_grasp_invalid_handle_raises(self): + """Building grasp with non-existent handle should raise.""" + cf = self.factory.constraints + + with self.assertRaises(Exception): + cf.buildGrasp("ur3/gripper", "nonexistent/handle") + + def test_build_placement_invalid_object_raises(self): + """Building placement with non-existent object should raise.""" + cf = self.factory.constraints + + with self.assertRaises(Exception): + cf.buildPlacement("nonexistent_object") + + def test_grasp_constraints_not_empty(self): + """Built grasp constraints should not be empty.""" + cf = self.factory.constraints + + result = cf.buildGrasp("ur3/gripper", "sphere0/handle") + + self.assertFalse(result["grasp"].empty()) + + def test_placement_constraints_not_empty(self): + """Built placement constraints should not be empty.""" + cf = self.factory.constraints + + result = cf.buildPlacement("sphere0") + + self.assertFalse(result["placement"].empty()) + + +if __name__ == "__main__": + unittest.main() diff --git a/tests/unit/test_constraint_graph_factory.py b/tests/unit/test_constraint_graph_factory.py new file mode 100644 index 00000000..456f1afd --- /dev/null +++ b/tests/unit/test_constraint_graph_factory.py @@ -0,0 +1,99 @@ +#!/usr/bin/env python +# +# Copyright (c) 2025 CNRS +# Author: Paul Sardin +# + +import unittest +from pyhpp.manipulation.constraint_graph_factory import ( + PossibleGrasps, + GraspIsAllowed, + Rules, + Rule, +) + + +class TestPossibleGrasps(unittest.TestCase): + def test_allowed_grasp_returns_true(self): + grippers = ["gripper1", "gripper2"] + handles = ["handle1", "handle2", "handle3"] + grasps = {"gripper1": ["handle1", "handle2"], "gripper2": ["handle3"]} + validator = PossibleGrasps(grippers, handles, grasps) + + self.assertTrue(validator((0, 2))) # gripper1->handle1, gripper2->handle3 + self.assertTrue(validator((1, 2))) # gripper1->handle2, gripper2->handle3 + + def test_forbidden_grasp_returns_false(self): + grippers = ["gripper1", "gripper2"] + handles = ["handle1", "handle2", "handle3"] + grasps = {"gripper1": ["handle1"], "gripper2": ["handle3"]} + validator = PossibleGrasps(grippers, handles, grasps) + + self.assertFalse(validator((1, 2))) # gripper1->handle2 not allowed + + def test_none_grasp_allowed(self): + grippers = ["gripper1"] + handles = ["handle1"] + grasps = {"gripper1": ["handle1"]} + validator = PossibleGrasps(grippers, handles, grasps) + + self.assertTrue(validator((None,))) + + +class TestGraspIsAllowed(unittest.TestCase): + def test_empty_validator_allows_all(self): + validator = GraspIsAllowed() + self.assertTrue(validator((0, 1, 2))) + self.assertTrue(validator((None, None))) + + def test_appended_validator_is_called(self): + validator = GraspIsAllowed() + validator.append(lambda g: g[0] == 0) + + self.assertTrue(validator((0,))) + self.assertFalse(validator((1,))) + + def test_all_validators_must_pass(self): + validator = GraspIsAllowed() + validator.append(lambda g: g[0] == 0) + validator.append(lambda g: len(g) > 1) + + self.assertFalse(validator((0,))) # fails second check + self.assertTrue(validator((0, 1))) + + +class TestRules(unittest.TestCase): + def test_simple_rule_allows(self): + grippers = ["gripper1"] + handles = ["handle1", "handle2"] + rules = [Rule(grippers=["gripper1"], handles=["handle1"], link=True)] + + validator = Rules(grippers, handles, rules) + + self.assertTrue(validator((0,))) # gripper1 -> handle1 + + def test_simple_rule_forbids(self): + grippers = ["gripper1"] + handles = ["handle1", "handle2"] + rules = [Rule(grippers=["gripper1"], handles=["handle1"], link=False)] + + validator = Rules(grippers, handles, rules) + + self.assertFalse(validator((0,))) # gripper1 -> handle1 forbidden + + def test_regex_pattern_match(self): + """Regex patterns in rules should match gripper/handle names.""" + grippers = ["left_gripper", "right_gripper"] + handles = ["box_handle", "cup_handle"] + # Rule: left_.* can grasp box_.* (regex matching) + rules = [Rule(grippers=["left_.*"], handles=["box_.*"], link=True)] + + validator = Rules(grippers, handles, rules) + + # Tuple format: (handle_idx_for_gripper0, handle_idx_for_gripper1) + # (0, None) = left_gripper grasps box_handle, right_gripper grasps nothing + self.assertTrue(validator((0, None))) + + +if __name__ == "__main__": + unittest.main() diff --git a/tests/unit/test_device.py b/tests/unit/test_device.py new file mode 100644 index 00000000..d823798e --- /dev/null +++ b/tests/unit/test_device.py @@ -0,0 +1,184 @@ +#!/usr/bin/env python +# +# Copyright (c) 2025 CNRS +# Author: Paul Sardin +# + +import unittest +import numpy as np +from pinocchio import SE3 +from pyhpp.pinocchio import Device, urdf + + +UR5_URDF = "package://example-robot-data/robots/ur_description/urdf/ur5_joint_limited_robot.urdf" +UR5_SRDF = "package://example-robot-data/robots/ur_description/srdf/ur5_joint_limited_robot.srdf" + + +def create_ur5_device(): + robot = Device("ur5") + urdf.loadModel(robot, 0, "ur5", "anchor", UR5_URDF, UR5_SRDF, SE3.Identity()) + return robot + + +class TestDeviceInstantiation(unittest.TestCase): + def test_device_creates_with_name(self): + robot = Device("test_robot") + + self.assertEqual(robot.name(), "test_robot") + + def test_device_loads_urdf(self): + robot = create_ur5_device() + + self.assertEqual(robot.name(), "ur5") + self.assertGreater(robot.configSize(), 0) + + +class TestDeviceConfiguration(unittest.TestCase): + def test_config_size_returns_dof_count(self): + robot = create_ur5_device() + + size = robot.configSize() + + self.assertEqual(size, 6) + + def test_number_dof_returns_velocity_size(self): + robot = create_ur5_device() + + ndof = robot.numberDof() + + self.assertEqual(ndof, 6) + + def test_current_configuration_settable(self): + robot = create_ur5_device() + q = np.array([0.0, -1.57, -1.8, 0.0, 0.8, 0.0]) + + result = robot.currentConfiguration(q) + + self.assertTrue(result) + + def test_current_configuration_readable(self): + robot = create_ur5_device() + q = np.array([0.1, -1.57, -1.8, 0.0, 0.8, 0.0]) + robot.currentConfiguration(q) + + current_q = robot.currentConfiguration() + + np.testing.assert_array_almost_equal(current_q, q) + + +class TestDeviceJointBounds(unittest.TestCase): + def test_set_joint_bounds(self): + robot = create_ur5_device() + + robot.setJointBounds("ur5/shoulder_pan_joint", [-1.0, 1.0]) + + def test_set_joint_bounds_multiple_dof(self): + robot = create_ur5_device() + + robot.setJointBounds("ur5/shoulder_pan_joint", [-2.0, 2.0]) + + def test_set_joint_bounds_invalid_size_raises(self): + robot = create_ur5_device() + + with self.assertRaises(Exception): + robot.setJointBounds("ur5/shoulder_pan_joint", [-1.0]) + + +class TestDeviceForwardKinematics(unittest.TestCase): + def test_compute_forward_kinematics_joint_position(self): + robot = create_ur5_device() + q = np.array([0.0, -1.57, -1.8, 0.0, 0.8, 0.0]) + robot.currentConfiguration(q) + + robot.computeForwardKinematics(1) + + def test_compute_frames_forward_kinematics(self): + robot = create_ur5_device() + q = np.array([0.0, -1.57, -1.8, 0.0, 0.8, 0.0]) + robot.currentConfiguration(q) + robot.computeForwardKinematics(1) + + robot.computeFramesForwardKinematics() + + def test_get_joint_position_returns_7_elements(self): + robot = create_ur5_device() + q = np.array([0.0, -1.57, -1.8, 0.0, 0.8, 0.0]) + robot.currentConfiguration(q) + robot.computeForwardKinematics(1) + robot.computeFramesForwardKinematics() + + position = robot.getJointPosition("ur5/ee_fixed_joint") + + self.assertEqual(len(position), 7) + + def test_joint_position_format(self): + """Joint position should be [x, y, z, qx, qy, qz, qw].""" + robot = create_ur5_device() + q = np.array([0.0, -1.57, -1.8, 0.0, 0.8, 0.0]) + robot.currentConfiguration(q) + robot.computeForwardKinematics(1) + robot.computeFramesForwardKinematics() + + position = robot.getJointPosition("ur5/ee_fixed_joint") + + quat_norm = sum(x**2 for x in position[3:7]) + self.assertAlmostEqual(quat_norm, 1.0, places=5) + + def test_get_joints_position_batch(self): + robot = create_ur5_device() + q = np.array([0.0, -1.57, -1.8, 0.0, 0.8, 0.0]) + joints = ["ur5/shoulder_link", "ur5/ee_fixed_joint"] + + positions = robot.getJointsPosition(q, joints) + + self.assertEqual(len(positions), 2) + for pos in positions: + self.assertEqual(len(pos), 7) + + +class TestDeviceRankInConfiguration(unittest.TestCase): + def test_rank_in_configuration_returns_dict(self): + robot = create_ur5_device() + + ranks = robot.rankInConfiguration + + self.assertIsInstance(ranks, dict) + + def test_rank_in_configuration_has_joints(self): + robot = create_ur5_device() + + ranks = robot.rankInConfiguration + + self.assertIn("ur5/shoulder_pan_joint", ranks) + self.assertIn("ur5/shoulder_lift_joint", ranks) + + def test_rank_values_are_valid_indices(self): + robot = create_ur5_device() + + ranks = robot.rankInConfiguration + + for joint_name, rank in ranks.items(): + self.assertGreaterEqual(rank, 0) + self.assertLess(rank, robot.configSize()) + + +class TestDeviceNegativeCases(unittest.TestCase): + def test_get_joint_position_invalid_frame_raises(self): + robot = create_ur5_device() + q = np.array([0.0, -1.57, -1.8, 0.0, 0.8, 0.0]) + robot.currentConfiguration(q) + robot.computeForwardKinematics(1) + robot.computeFramesForwardKinematics() + + with self.assertRaises(Exception): + robot.getJointPosition("nonexistent_joint") + + def test_set_joint_bounds_invalid_joint_raises(self): + robot = create_ur5_device() + + with self.assertRaises(Exception): + robot.setJointBounds("nonexistent_joint", [-1.0, 1.0]) + + +if __name__ == "__main__": + unittest.main() diff --git a/tests/unit/test_differentiable_function.py b/tests/unit/test_differentiable_function.py new file mode 100644 index 00000000..6259ca4e --- /dev/null +++ b/tests/unit/test_differentiable_function.py @@ -0,0 +1,82 @@ +#!/usr/bin/env python +# +# Copyright (c) 2025 CNRS +# Author: Paul Sardin +# + +import unittest +import numpy as np +from pyhpp.pinocchio import LiegroupElement +from pyhpp.constraints import DifferentiableFunction + + +class DoubleFunction(DifferentiableFunction): + """Simple function that doubles the input.""" + + def __init__(self): + super().__init__(2, 2, 2, "Double") + + def impl_compute(self, res, arg): + res.v = 2 * arg + + def impl_jacobian(self, res, arg): + res = 2 * np.eye(2) + return res + + +class TestDifferentiableFunctionInheritance(unittest.TestCase): + def test_create_subclass(self): + func = DoubleFunction() + + self.assertIsNotNone(func) + + def test_function_name(self): + func = DoubleFunction() + + self.assertIn("Double", str(func)) + + def test_input_output_sizes(self): + func = DoubleFunction() + + self.assertEqual(func.ni, 2) + self.assertEqual(func.ndo, 2) + self.assertEqual(func.ndi, 2) + + def test_value(self): + func = DoubleFunction() + q = np.ones((func.ni, 1)) + + v = LiegroupElement(func.outputSpace()) + func.value(v, q) + + np.testing.assert_array_almost_equal(v.vector(), [2.0, 2.0]) + + def test_jacobian(self): + func = DoubleFunction() + q = np.ones((func.ni, 1)) + + J = np.zeros((func.ndo, func.ndi)) + func.jacobian(J, q) + + expected = 2 * np.eye(2) + np.testing.assert_array_almost_equal(J, expected) + + def test_call_operator(self): + func = DoubleFunction() + q = np.ones((func.ni, 1)) + + v = func(q) + + self.assertIsNotNone(v) + + def test_jacobian_shorthand(self): + func = DoubleFunction() + q = np.ones((func.ni, 1)) + + J = func.J(q) + + self.assertEqual(J.shape, (func.ndo, func.ndi)) + + +if __name__ == "__main__": + unittest.main() diff --git a/tests/unit/test_handles_grippers.py b/tests/unit/test_handles_grippers.py new file mode 100644 index 00000000..c1e2073c --- /dev/null +++ b/tests/unit/test_handles_grippers.py @@ -0,0 +1,107 @@ +#!/usr/bin/env python +# +# Copyright (c) 2025 CNRS +# Author: Paul Sardin +# + +import unittest +from pinocchio import SE3 +from pyhpp.manipulation import Device, urdf + + +UR3_URDF = "package://example-robot-data/robots/ur_description/urdf/ur3_gripper.urdf" +UR3_SRDF = "package://example-robot-data/robots/ur_description/srdf/ur3_gripper.srdf" +SPHERE_URDF = "package://hpp_environments/urdf/construction_set/sphere.urdf" +SPHERE_SRDF = "package://hpp_environments/srdf/construction_set/sphere.srdf" + + +def create_manipulation_device(): + robot = Device("ur3-sphere") + urdf.loadModel(robot, 0, "ur3", "anchor", UR3_URDF, UR3_SRDF, SE3.Identity()) + urdf.loadModel( + robot, 0, "sphere", "freeflyer", SPHERE_URDF, SPHERE_SRDF, SE3.Identity() + ) + return robot + + +class TestGrippers(unittest.TestCase): + def test_grippers_returns_map(self): + robot = create_manipulation_device() + + grippers = robot.grippers() + + self.assertIsNotNone(grippers) + + def test_gripper_exists(self): + robot = create_manipulation_device() + + grippers = robot.grippers() + + self.assertIn("ur3/gripper", grippers) + + def test_gripper_has_local_position(self): + robot = create_manipulation_device() + grippers = robot.grippers() + + gripper = grippers["ur3/gripper"] + + self.assertTrue(hasattr(gripper, "localPosition")) + + +class TestHandles(unittest.TestCase): + def test_handles_returns_map(self): + robot = create_manipulation_device() + + handles = robot.handles() + + self.assertIsNotNone(handles) + + def test_handle_exists(self): + robot = create_manipulation_device() + + handles = robot.handles() + + self.assertIn("sphere/handle", handles) + + def test_handle_has_local_position(self): + robot = create_manipulation_device() + handles = robot.handles() + + handle = handles["sphere/handle"] + + self.assertTrue(hasattr(handle, "localPosition")) + + def test_handle_has_mask(self): + robot = create_manipulation_device() + handles = robot.handles() + + handle = handles["sphere/handle"] + + self.assertTrue(hasattr(handle, "mask")) + + def test_handle_mask_modifiable(self): + robot = create_manipulation_device() + handles = robot.handles() + handle = handles["sphere/handle"] + + handle.mask = [True, True, True, False, True, True] + + self.assertEqual(handle.mask[3], False) + + +class TestGraspCreation(unittest.TestCase): + def test_create_grasp_constraint(self): + robot = create_manipulation_device() + handles = robot.handles() + grippers = robot.grippers() + + handle = handles["sphere/handle"] + gripper = grippers["ur3/gripper"] + + constraint = handle.createGrasp(gripper, "test_grasp") + + self.assertIsNotNone(constraint) + + +if __name__ == "__main__": + unittest.main() diff --git a/tests/unit/test_liegroup.py b/tests/unit/test_liegroup.py new file mode 100644 index 00000000..2e8821c1 --- /dev/null +++ b/tests/unit/test_liegroup.py @@ -0,0 +1,95 @@ +#!/usr/bin/env python +# +# Copyright (c) 2025 CNRS +# Author: Paul Sardin +# + +import unittest +import numpy as np +from pyhpp.pinocchio import LiegroupSpace, LiegroupElement + + +class TestLiegroupSpace(unittest.TestCase): + def test_create_r1(self): + space = LiegroupSpace.R1(False) + + self.assertIsNotNone(space) + self.assertEqual(str(space), "R^1") + + def test_create_r2(self): + space = LiegroupSpace.R2() + + self.assertEqual(str(space), "R^2") + + def test_create_r3(self): + space = LiegroupSpace.R3() + + self.assertEqual(str(space), "R^3") + + def test_multiply_spaces(self): + space1 = LiegroupSpace.R2() + space2 = LiegroupSpace.R1(False) + + combined = space1 * space2 + + self.assertEqual(str(combined), "R^3") + + def test_inplace_multiply(self): + space = LiegroupSpace.R2() + space *= LiegroupSpace.R1(False) + + self.assertEqual(str(space), "R^3") + + def test_merge_vector_spaces(self): + space = LiegroupSpace.R2() * LiegroupSpace.R2() + space.mergeVectorSpaces() + + self.assertEqual(str(space), "R^4") + + +class TestLiegroupElement(unittest.TestCase): + def test_create_element(self): + space = LiegroupSpace.R3() + v = np.array([1.0, 2.0, 3.0]) + + el = LiegroupElement(v, space) + + self.assertIsNotNone(el) + + def test_element_vector(self): + space = LiegroupSpace.R3() + v = np.array([1.0, 2.0, 3.0]) + + el = LiegroupElement(v, space) + + np.testing.assert_array_equal(el.vector(), v) + + def test_element_space(self): + space = LiegroupSpace.R3() + v = np.array([1.0, 2.0, 3.0]) + + el = LiegroupElement(v, space) + + self.assertEqual(el.space(), space) + + def test_element_subtraction(self): + space = LiegroupSpace.R3() + el1 = LiegroupElement(np.array([0.0, 1.0, 2.0]), space) + el2 = LiegroupElement(np.array([1.0, 2.0, 3.0]), space) + + diff = el2 - el1 + + np.testing.assert_array_almost_equal(diff.flatten(), [1.0, 1.0, 1.0]) + + def test_element_addition(self): + space = LiegroupSpace.R3() + el1 = LiegroupElement(np.array([0.0, 1.0, 2.0]), space) + v = np.array([1.0, 1.0, 1.0]) + + el2 = el1 + v + + np.testing.assert_array_almost_equal(el2.vector(), [1.0, 2.0, 3.0]) + + +if __name__ == "__main__": + unittest.main() diff --git a/tests/unit/test_path.py b/tests/unit/test_path.py new file mode 100644 index 00000000..9fa863ed --- /dev/null +++ b/tests/unit/test_path.py @@ -0,0 +1,222 @@ +#!/usr/bin/env python +# +# Copyright (c) 2025 CNRS +# Author: Paul Sardin +# + +import unittest +import numpy as np +from unit.conftest import create_ur5_problem +import pyhpp.core.path + + +class TestPathMethods(unittest.TestCase): + @classmethod + def setUpClass(cls): + cls.problem, cls.robot = create_ur5_problem() + + def test_extract_returns_subpath(self): + """Path.extract should return a portion of the original path.""" + q1 = np.array([0.0, -1.57, -1.8, 0.0, 0.8, 0.0]) + q2 = np.array([1.0, -1.57, -1.8, 0.0, 0.8, 0.0]) + + steer = self.problem.steeringMethod() + path = steer(q1, q2) + original_length = path.length() + + # Extract middle half + t_start = original_length * 0.25 + t_end = original_length * 0.75 + extracted = path.extract(t_start, t_end) + + self.assertIsNotNone(extracted) + self.assertAlmostEqual(extracted.length(), original_length * 0.5, places=5) + + def test_copy_creates_independent_path(self): + """Path.copy should create an independent copy.""" + q1 = np.array([0.0, -1.57, -1.8, 0.0, 0.8, 0.0]) + q2 = np.array([0.5, -1.57, -1.8, 0.0, 0.8, 0.0]) + + steer = self.problem.steeringMethod() + path = steer(q1, q2) + + copied = path.copy() + + self.assertIsNotNone(copied) + self.assertEqual(copied.length(), path.length()) + np.testing.assert_array_almost_equal(copied.initial(), path.initial()) + np.testing.assert_array_almost_equal(copied.end(), path.end()) + + def test_derivative_returns_velocity(self): + """Path.derivative should return velocity at given parameter.""" + q1 = np.array([0.0, -1.57, -1.8, 0.0, 0.8, 0.0]) + q2 = np.array([1.0, -1.57, -1.8, 0.0, 0.8, 0.0]) + + steer = self.problem.steeringMethod() + path = steer(q1, q2) + + t_mid = path.length() / 2.0 + deriv = path.derivative(t_mid, 1) + + self.assertEqual(len(deriv), self.robot.numberDof()) + # For straight path, first joint derivative should be non-zero + self.assertNotEqual(deriv[0], 0.0) + + def test_length_is_non_negative(self): + """Path.length should be non-negative for valid paths.""" + q1 = np.array([0.0, -1.57, -1.8, 0.0, 0.8, 0.0]) + q2 = np.array([0.5, -1.57, -1.8, 0.0, 0.8, 0.0]) + + steer = self.problem.steeringMethod() + path = steer(q1, q2) + + self.assertGreaterEqual(path.length(), 0.0) + + +class TestPathNegativeCases(unittest.TestCase): + """Negative test cases for Path operations.""" + + @classmethod + def setUpClass(cls): + cls.problem, cls.robot = create_ur5_problem() + + def test_extract_valid_range(self): + """Extracting with valid range should succeed.""" + q1 = np.array([0.0, -1.57, -1.8, 0.0, 0.8, 0.0]) + q2 = np.array([1.0, -1.57, -1.8, 0.0, 0.8, 0.0]) + + steer = self.problem.steeringMethod() + path = steer(q1, q2) + length = path.length() + + # Valid extraction + extracted = path.extract(0.2 * length, 0.8 * length) + self.assertIsNotNone(extracted) + self.assertAlmostEqual(extracted.length(), 0.6 * length, places=5) + + def test_initial_and_end_accessors(self): + """Path.initial() and path.end() should return correct configs.""" + q1 = np.array([0.0, -1.57, -1.8, 0.0, 0.8, 0.0]) + q2 = np.array([0.5, -1.57, -1.8, 0.0, 0.8, 0.0]) + + steer = self.problem.steeringMethod() + path = steer(q1, q2) + + # Check initial config + np.testing.assert_array_almost_equal(path.initial(), q1) + # Check end config + np.testing.assert_array_almost_equal(path.end(), q2) + + def test_call_operator_at_midpoint(self): + """Calling path(t) should return interpolated config and success status.""" + q1 = np.array([0.0, -1.57, -1.8, 0.0, 0.8, 0.0]) + q2 = np.array([1.0, -1.57, -1.8, 0.0, 0.8, 0.0]) + + steer = self.problem.steeringMethod() + path = steer(q1, q2) + + # Path call operator returns (config, success) tuple + q_mid, success = path(path.length() / 2.0) + self.assertTrue(success) + # First joint should be midway + self.assertAlmostEqual(q_mid[0], 0.5, places=5) + + def test_derivative_at_order_one(self): + """Derivative at order 1 returns velocity.""" + q1 = np.array([0.0, -1.57, -1.8, 0.0, 0.8, 0.0]) + q2 = np.array([1.0, -1.57, -1.8, 0.0, 0.8, 0.0]) + + steer = self.problem.steeringMethod() + path = steer(q1, q2) + + # Order 1 derivative is velocity + deriv1 = path.derivative(path.length() / 2.0, 1) + self.assertEqual(len(deriv1), self.robot.numberDof()) + + +class TestPathVector(unittest.TestCase): + @classmethod + def setUpClass(cls): + cls.problem, cls.robot = create_ur5_problem() + + def test_create_empty_path_vector(self): + """PathVector.create should create an empty vector.""" + pv = pyhpp.core.path.Vector.create( + self.robot.configSize(), self.robot.numberDof() + ) + + self.assertIsNotNone(pv) + self.assertEqual(pv.numberPaths(), 0) + + def test_append_path_increases_count(self): + """Appending paths should increase numberPaths.""" + pv = pyhpp.core.path.Vector.create( + self.robot.configSize(), self.robot.numberDof() + ) + + q1 = np.array([0.0, -1.57, -1.8, 0.0, 0.8, 0.0]) + q2 = np.array([0.5, -1.57, -1.8, 0.0, 0.8, 0.0]) + + steer = self.problem.steeringMethod() + path = steer(q1, q2) + + pv.appendPath(path) + + self.assertEqual(pv.numberPaths(), 1) + + def test_path_vector_length_is_sum(self): + """PathVector length should be sum of contained paths.""" + pv = pyhpp.core.path.Vector.create( + self.robot.configSize(), self.robot.numberDof() + ) + + q1 = np.array([0.0, -1.57, -1.8, 0.0, 0.8, 0.0]) + q2 = np.array([0.5, -1.57, -1.8, 0.0, 0.8, 0.0]) + q3 = np.array([1.0, -1.57, -1.8, 0.0, 0.8, 0.0]) + + steer = self.problem.steeringMethod() + path1 = steer(q1, q2) + path2 = steer(q2, q3) + + pv.appendPath(path1) + pv.appendPath(path2) + + expected_length = path1.length() + path2.length() + self.assertAlmostEqual(pv.length(), expected_length, places=10) + + +class TestPathVectorNegativeCases(unittest.TestCase): + """Negative test cases for PathVector operations.""" + + @classmethod + def setUpClass(cls): + cls.problem, cls.robot = create_ur5_problem() + + def test_path_at_valid_index_works(self): + """Accessing path at valid index should work.""" + pv = pyhpp.core.path.Vector.create( + self.robot.configSize(), self.robot.numberDof() + ) + + q1 = np.array([0.0, -1.57, -1.8, 0.0, 0.8, 0.0]) + q2 = np.array([0.5, -1.57, -1.8, 0.0, 0.8, 0.0]) + + steer = self.problem.steeringMethod() + path = steer(q1, q2) + pv.appendPath(path) + + retrieved = pv.pathAtRank(0) + self.assertIsNotNone(retrieved) + self.assertAlmostEqual(retrieved.length(), path.length(), places=10) + + def test_empty_path_vector_has_zero_length(self): + """Empty PathVector should have zero length.""" + pv = pyhpp.core.path.Vector.create( + self.robot.configSize(), self.robot.numberDof() + ) + + self.assertEqual(pv.length(), 0.0) + + +if __name__ == "__main__": + unittest.main() diff --git a/tests/unit/test_path_optimizer.py b/tests/unit/test_path_optimizer.py new file mode 100644 index 00000000..588f8834 --- /dev/null +++ b/tests/unit/test_path_optimizer.py @@ -0,0 +1,144 @@ +#!/usr/bin/env python +# +# Copyright (c) 2025 CNRS +# Author: Paul Sardin +# + +import unittest +import numpy as np +from unit.conftest import create_ur5_problem +from pyhpp.core import RandomShortcut, SimpleShortcut +import pyhpp.core.path + + +class TestPathOptimizerInstantiation(unittest.TestCase): + @classmethod + def setUpClass(cls): + cls.problem, cls.robot = create_ur5_problem() + + def test_random_shortcut_creates(self): + optimizer = RandomShortcut(self.problem) + + self.assertIsNotNone(optimizer) + + def test_simple_shortcut_creates(self): + optimizer = SimpleShortcut(self.problem) + + self.assertIsNotNone(optimizer) + + +class TestPathOptimizerOptimize(unittest.TestCase): + def test_optimize_returns_path(self): + """PathOptimizer.optimize should return a valid path.""" + problem, robot = create_ur5_problem() + + q1 = np.array([0.0, -1.57, -1.8, 0.0, 0.8, 0.0]) + q2 = np.array([0.3, -1.57, -1.8, 0.0, 0.8, 0.0]) + q3 = np.array([0.6, -1.57, -1.8, 0.0, 0.8, 0.0]) + + steer = problem.steeringMethod() + path1 = steer(q1, q2) + path2 = steer(q2, q3) + + pv = pyhpp.core.path.Vector.create(robot.configSize(), robot.numberDof()) + pv.appendPath(path1) + pv.appendPath(path2) + + optimizer = RandomShortcut(problem) + optimizer.maxIterations(10) + + optimized = optimizer.optimize(pv) + + self.assertIsNotNone(optimized) + # Optimized path should preserve start and end + np.testing.assert_array_almost_equal(optimized.initial(), q1) + np.testing.assert_array_almost_equal(optimized.end(), q3) + + def test_max_iterations_settable(self): + """PathOptimizer.maxIterations should be settable.""" + problem, robot = create_ur5_problem() + + optimizer = RandomShortcut(problem) + optimizer.maxIterations(50) + + # Verify it doesn't crash and actually limits iterations + # by running optimization on a simple path + q1 = np.array([0.0, -1.57, -1.8, 0.0, 0.8, 0.0]) + q2 = np.array([0.5, -1.57, -1.8, 0.0, 0.8, 0.0]) + steer = problem.steeringMethod() + path = steer(q1, q2) + pv = pyhpp.core.path.Vector.create(robot.configSize(), robot.numberDof()) + pv.appendPath(path) + + optimized = optimizer.optimize(pv) + self.assertIsNotNone(optimized) + + +class TestPathOptimizerNegativeCases(unittest.TestCase): + """Negative test cases for PathOptimizer.""" + + def test_optimize_zero_length_path(self): + """Optimizing a zero-length path should return zero-length path.""" + problem, robot = create_ur5_problem() + q = np.array([0.0, -1.57, -1.8, 0.0, 0.8, 0.0]) + + steer = problem.steeringMethod() + path = steer(q, q) + + pv = pyhpp.core.path.Vector.create(robot.configSize(), robot.numberDof()) + pv.appendPath(path) + + optimizer = RandomShortcut(problem) + optimizer.maxIterations(10) + + optimized = optimizer.optimize(pv) + + self.assertEqual(optimized.length(), 0.0) + + def test_optimize_preserves_endpoints(self): + """Optimization must always preserve path endpoints.""" + problem, robot = create_ur5_problem() + + q1 = np.array([0.0, -1.57, -1.8, 0.0, 0.8, 0.0]) + q2 = np.array([0.2, -1.4, -1.6, 0.1, 0.7, 0.1]) + q3 = np.array([0.4, -1.2, -1.4, 0.2, 0.6, 0.2]) + q4 = np.array([0.6, -1.0, -1.2, 0.3, 0.5, 0.3]) + + steer = problem.steeringMethod() + pv = pyhpp.core.path.Vector.create(robot.configSize(), robot.numberDof()) + pv.appendPath(steer(q1, q2)) + pv.appendPath(steer(q2, q3)) + pv.appendPath(steer(q3, q4)) + + optimizer = SimpleShortcut(problem) + optimizer.maxIterations(20) + + optimized = optimizer.optimize(pv) + + np.testing.assert_array_almost_equal(optimized.initial(), q1) + np.testing.assert_array_almost_equal(optimized.end(), q4) + + def test_zero_iterations_returns_same_path(self): + """Zero iterations should return path with same length.""" + problem, robot = create_ur5_problem() + + q1 = np.array([0.0, -1.57, -1.8, 0.0, 0.8, 0.0]) + q2 = np.array([0.5, -1.57, -1.8, 0.0, 0.8, 0.0]) + + steer = problem.steeringMethod() + path = steer(q1, q2) + + pv = pyhpp.core.path.Vector.create(robot.configSize(), robot.numberDof()) + pv.appendPath(path) + original_length = pv.length() + + optimizer = RandomShortcut(problem) + optimizer.maxIterations(0) + + optimized = optimizer.optimize(pv) + + self.assertAlmostEqual(optimized.length(), original_length, places=5) + + +if __name__ == "__main__": + unittest.main() diff --git a/tests/unit/test_path_planner.py b/tests/unit/test_path_planner.py new file mode 100644 index 00000000..5acc1fe2 --- /dev/null +++ b/tests/unit/test_path_planner.py @@ -0,0 +1,125 @@ +#!/usr/bin/env python +# +# Copyright (c) 2025 CNRS +# Author: Paul Sardin +# + +import unittest +import numpy as np +from unit.conftest import create_ur5_problem +from pyhpp.core import ( + DiffusingPlanner, + BiRRTPlanner, + VisibilityPrmPlanner, +) + + +class TestPathPlannerInstantiation(unittest.TestCase): + @classmethod + def setUpClass(cls): + cls.problem, cls.robot = create_ur5_problem() + + def test_diffusing_planner_creates(self): + planner = DiffusingPlanner(self.problem) + + self.assertIsNotNone(planner) + + def test_birrt_planner_creates(self): + planner = BiRRTPlanner(self.problem) + + self.assertIsNotNone(planner) + + def test_visibility_prm_planner_creates(self): + planner = VisibilityPrmPlanner(self.problem) + + self.assertIsNotNone(planner) + + +class TestPathPlannerConfiguration(unittest.TestCase): + @classmethod + def setUpClass(cls): + cls.problem, cls.robot = create_ur5_problem() + + def test_max_iterations_settable(self): + planner = DiffusingPlanner(self.problem) + + planner.maxIterations(500) + + self.assertEqual(planner.maxIterations(), 500) + + def test_max_iterations_defaults_to_zero(self): + planner = BiRRTPlanner(self.problem) + + self.assertEqual(planner.maxIterations(), 0) + + +class TestPathPlannerSolve(unittest.TestCase): + def test_birrt_solves_simple_path(self): + """BiRRT should find a path between nearby collision-free configurations.""" + problem, robot = create_ur5_problem() + + q_init = np.array([0.0, -1.57, -1.8, 0.0, 0.8, 0.0]) + q_goal = np.array([0.5, -1.57, -1.8, 0.0, 0.8, 0.0]) + problem.initConfig(q_init) + problem.addGoalConfig(q_goal) + + planner = BiRRTPlanner(problem) + planner.maxIterations(200) + + path = planner.solve() + + self.assertIsNotNone(path) + self.assertGreater(path.length(), 0) + np.testing.assert_array_almost_equal(path.initial(), q_init) + np.testing.assert_array_almost_equal(path.end(), q_goal) + + +class TestPathPlannerNegativeCases(unittest.TestCase): + """Negative test cases for path planners.""" + + def test_solve_without_goal_raises(self): + """Solving without a goal configuration should raise an error.""" + problem, robot = create_ur5_problem() + q_init = np.array([0.0, -1.57, -1.8, 0.0, 0.8, 0.0]) + problem.initConfig(q_init) + + planner = BiRRTPlanner(problem) + planner.maxIterations(10) + + with self.assertRaises(Exception): + planner.solve() + + def test_solve_without_init_raises(self): + """Solving without an initial configuration should raise an error.""" + problem, robot = create_ur5_problem() + q_goal = np.array([0.5, -1.57, -1.8, 0.0, 0.8, 0.0]) + problem.addGoalConfig(q_goal) + + planner = BiRRTPlanner(problem) + planner.maxIterations(10) + + with self.assertRaises(Exception): + planner.solve() + + def test_max_iterations_zero_limits_exploration(self): + """Planner with zero max iterations should do minimal exploration.""" + problem, robot = create_ur5_problem() + q_init = np.array([0.0, -1.57, -1.8, 0.0, 0.8, 0.0]) + q_goal = np.array([3.0, -1.57, -1.8, 0.0, 0.8, 0.0]) + problem.initConfig(q_init) + problem.addGoalConfig(q_goal) + + planner = BiRRTPlanner(problem) + planner.maxIterations(0) + + try: + planner.solve() + except Exception: + pass + + roadmap = planner.roadmap() + self.assertLessEqual(len(roadmap.nodes()), 2) + + +if __name__ == "__main__": + unittest.main() diff --git a/tests/unit/test_path_projector.py b/tests/unit/test_path_projector.py new file mode 100644 index 00000000..d92d1cef --- /dev/null +++ b/tests/unit/test_path_projector.py @@ -0,0 +1,113 @@ +#!/usr/bin/env python +# +# Copyright (c) 2025 CNRS +# Author: Paul Sardin +# + +import unittest +import numpy as np +from unit.conftest import create_ur5_problem +from pyhpp.core import ( + Progressive, + GlobalProjector, +) + + +class TestPathProjectorInstantiation(unittest.TestCase): + @classmethod + def setUpClass(cls): + cls.problem, cls.robot = create_ur5_problem() + + def test_progressive_projector_creates(self): + projector = Progressive(self.robot, 0.1) + + self.assertIsNotNone(projector) + + def test_global_projector_creates(self): + steer = self.problem.steeringMethod() + distance = self.problem.distance() + projector = GlobalProjector(distance, steer, 0.1) + + self.assertIsNotNone(projector) + + +class TestPathProjectorProject(unittest.TestCase): + @classmethod + def setUpClass(cls): + cls.problem, cls.robot = create_ur5_problem() + + def test_global_projector_projects_unconstrained_path(self): + """Without constraints, projection should succeed and preserve endpoints.""" + steer = self.problem.steeringMethod() + distance = self.problem.distance() + projector = GlobalProjector(distance, steer, 0.1) + + q1 = np.array([0.0, -1.57, -1.8, 0.0, 0.8, 0.0]) + q2 = np.array([0.5, -1.57, -1.8, 0.0, 0.8, 0.0]) + path = steer(q1, q2) + + success, projected_path = projector.apply(path) + + self.assertTrue(success) + self.assertIsNotNone(projected_path) + np.testing.assert_array_almost_equal(projected_path.initial(), q1) + np.testing.assert_array_almost_equal(projected_path.end(), q2) + + +class TestPathProjectorNegativeCases(unittest.TestCase): + """Negative test cases for PathProjector.""" + + @classmethod + def setUpClass(cls): + cls.problem, cls.robot = create_ur5_problem() + + def test_zero_step_creates_projector(self): + """Projector with zero step should still be created.""" + steer = self.problem.steeringMethod() + distance = self.problem.distance() + + projector = GlobalProjector(distance, steer, 0.0) + + self.assertIsNotNone(projector) + + def test_negative_step_creates_projector(self): + """Projector with negative step still creates (library accepts it).""" + steer = self.problem.steeringMethod() + distance = self.problem.distance() + + projector = GlobalProjector(distance, steer, -0.1) + + self.assertIsNotNone(projector) + + def test_zero_length_path_projection(self): + """Projection of zero-length path should succeed.""" + steer = self.problem.steeringMethod() + distance = self.problem.distance() + projector = GlobalProjector(distance, steer, 0.1) + + q = np.array([0.0, -1.57, -1.8, 0.0, 0.8, 0.0]) + path = steer(q, q) + + success, projected_path = projector.apply(path) + + self.assertTrue(success) + self.assertEqual(projected_path.length(), 0.0) + + def test_apply_returns_two_values(self): + """Apply should return (success, path) tuple.""" + steer = self.problem.steeringMethod() + distance = self.problem.distance() + projector = GlobalProjector(distance, steer, 0.1) + + q1 = np.array([0.0, -1.57, -1.8, 0.0, 0.8, 0.0]) + q2 = np.array([0.5, -1.57, -1.8, 0.0, 0.8, 0.0]) + path = steer(q1, q2) + + result = projector.apply(path) + + self.assertEqual(len(result), 2) + self.assertIsInstance(result[0], bool) + + +if __name__ == "__main__": + unittest.main() diff --git a/tests/unit/test_path_validation.py b/tests/unit/test_path_validation.py new file mode 100644 index 00000000..e0bfa714 --- /dev/null +++ b/tests/unit/test_path_validation.py @@ -0,0 +1,106 @@ +#!/usr/bin/env python +# +# Copyright (c) 2025 CNRS +# Author: Paul Sardin +# + +import unittest +import numpy as np +from unit.conftest import create_ur5_problem + + +class TestPathValidation(unittest.TestCase): + @classmethod + def setUpClass(cls): + cls.problem, cls.robot = create_ur5_problem() + + def test_collision_free_path_is_valid(self): + q1 = np.array([0.0, -1.57, -1.8, 0.0, 0.8, 0.0]) + q2 = np.array([0.1, -1.57, -1.8, 0.0, 0.8, 0.0]) + + steer = self.problem.steeringMethod() + path = steer(q1, q2) + + is_valid, valid_part, report = self.problem.pathValidation().validate( + path, False + ) + self.assertTrue(is_valid) + + def test_zero_length_path_returns_valid_part(self): + """A zero-length path (same start/end) should still return a valid_part.""" + q1 = np.array([0.0, -1.57, -1.8, 0.0, 0.8, 0.0]) + + steer = self.problem.steeringMethod() + path = steer(q1, q1) + + is_valid, valid_part, report = self.problem.pathValidation().validate( + path, False + ) + self.assertIsNotNone(valid_part) + self.assertEqual(valid_part.length(), 0.0) + + def test_validate_returns_three_values(self): + """Validate always returns (bool, path, report) tuple.""" + q1 = np.array([0.0, -1.57, -1.8, 0.0, 0.8, 0.0]) + q2 = np.array([0.5, -1.57, -1.8, 0.0, 0.8, 0.0]) + + steer = self.problem.steeringMethod() + path = steer(q1, q2) + + result = self.problem.pathValidation().validate(path, False) + self.assertEqual(len(result), 3) + + +class TestPathValidationNegativeCases(unittest.TestCase): + """Negative test cases for PathValidation.""" + + @classmethod + def setUpClass(cls): + cls.problem, cls.robot = create_ur5_problem() + + def test_valid_part_never_none(self): + """Valid part should never be None, even for invalid paths.""" + q1 = np.array([0.0, -1.57, -1.8, 0.0, 0.8, 0.0]) + q2 = np.array([0.5, -1.57, -1.8, 0.0, 0.8, 0.0]) + + steer = self.problem.steeringMethod() + path = steer(q1, q2) + + is_valid, valid_part, report = self.problem.pathValidation().validate( + path, False + ) + + self.assertIsNotNone(valid_part) + + def test_valid_part_length_less_than_or_equal_original(self): + """Valid part length should never exceed original path length.""" + q1 = np.array([0.0, -1.57, -1.8, 0.0, 0.8, 0.0]) + q2 = np.array([1.0, -1.57, -1.8, 0.0, 0.8, 0.0]) + + steer = self.problem.steeringMethod() + path = steer(q1, q2) + + is_valid, valid_part, report = self.problem.pathValidation().validate( + path, False + ) + + self.assertLessEqual(valid_part.length(), path.length()) + + def test_reverse_flag_produces_valid_result(self): + """Validation with reverse=True should still produce valid results.""" + q1 = np.array([0.0, -1.57, -1.8, 0.0, 0.8, 0.0]) + q2 = np.array([0.3, -1.57, -1.8, 0.0, 0.8, 0.0]) + + steer = self.problem.steeringMethod() + path = steer(q1, q2) + + is_valid, valid_part, report = self.problem.pathValidation().validate( + path, True + ) + + self.assertIsNotNone(valid_part) + self.assertIsInstance(is_valid, bool) + + +if __name__ == "__main__": + unittest.main() diff --git a/tests/unit/test_position_constraint.py b/tests/unit/test_position_constraint.py new file mode 100644 index 00000000..2d9d020a --- /dev/null +++ b/tests/unit/test_position_constraint.py @@ -0,0 +1,125 @@ +#!/usr/bin/env python +# +# Copyright (c) 2025 CNRS +# Author: Paul Sardin +# + +import unittest +import numpy as np +from pinocchio import SE3, StdVec_Bool as Mask +from pyhpp.pinocchio import Device, LiegroupElement, urdf +from pyhpp.constraints import Position + + +UR5_URDF = "package://example-robot-data/robots/ur_description/urdf/ur5_joint_limited_robot.urdf" +UR5_SRDF = "package://example-robot-data/robots/ur_description/srdf/ur5_joint_limited_robot.srdf" + + +def create_ur5_device(): + robot = Device("ur5") + urdf.loadModel(robot, 0, "ur5", "anchor", UR5_URDF, UR5_SRDF, SE3.Identity()) + return robot + + +class TestPositionConstraint(unittest.TestCase): + def test_create_position_constraint(self): + robot = create_ur5_device() + joint_id = robot.model().getJointId("ur5/wrist_3_joint") + mask = Mask() + mask[:] = (True, True, True) + + pc = Position( + "test_position", robot, joint_id, SE3.Identity(), SE3.Identity(), mask + ) + + self.assertIsNotNone(pc) + + def test_position_constraint_str(self): + robot = create_ur5_device() + joint_id = robot.model().getJointId("ur5/wrist_3_joint") + mask = Mask() + mask[:] = (True, True, True) + + pc = Position( + "my_position", robot, joint_id, SE3.Identity(), SE3.Identity(), mask + ) + + self.assertIn("my_position", str(pc)) + + def test_position_constraint_dimensions(self): + robot = create_ur5_device() + joint_id = robot.model().getJointId("ur5/wrist_3_joint") + mask = Mask() + mask[:] = (True, True, True) + + pc = Position("position", robot, joint_id, SE3.Identity(), SE3.Identity(), mask) + + self.assertEqual(pc.ndo, 3) + + +class TestPositionConstraintEvaluation(unittest.TestCase): + @classmethod + def setUpClass(cls): + cls.robot = create_ur5_device() + + def test_value_cpp_api(self): + joint_id = self.robot.model().getJointId("ur5/wrist_3_joint") + mask = Mask() + mask[:] = (True, True, True) + + pc = Position( + "position", self.robot, joint_id, SE3.Identity(), SE3.Identity(), mask + ) + + q = np.zeros((pc.ni, 1)) + v = LiegroupElement(pc.outputSpace()) + pc.value(v, q) + + self.assertEqual(len(v.vector()), 3) + + def test_jacobian_cpp_api(self): + joint_id = self.robot.model().getJointId("ur5/wrist_3_joint") + mask = Mask() + mask[:] = (True, True, True) + + pc = Position( + "position", self.robot, joint_id, SE3.Identity(), SE3.Identity(), mask + ) + + q = self.robot.currentConfiguration() + J = np.zeros((pc.ndo, pc.ndi)) + pc.jacobian(J, q) + + self.assertEqual(J.shape, (3, self.robot.numberDof())) + + def test_call_operator(self): + joint_id = self.robot.model().getJointId("ur5/wrist_3_joint") + mask = Mask() + mask[:] = (True, True, True) + + pc = Position( + "position", self.robot, joint_id, SE3.Identity(), SE3.Identity(), mask + ) + + q = np.zeros((pc.ni, 1)) + result = pc(q) + + self.assertIsNotNone(result) + + def test_jacobian_shorthand(self): + joint_id = self.robot.model().getJointId("ur5/wrist_3_joint") + mask = Mask() + mask[:] = (True, True, True) + + pc = Position( + "position", self.robot, joint_id, SE3.Identity(), SE3.Identity(), mask + ) + + q = self.robot.currentConfiguration() + J = pc.J(q) + + self.assertEqual(J.shape[0], pc.ndo) + + +if __name__ == "__main__": + unittest.main() diff --git a/tests/unit/test_problem.py b/tests/unit/test_problem.py new file mode 100644 index 00000000..a56d3165 --- /dev/null +++ b/tests/unit/test_problem.py @@ -0,0 +1,138 @@ +#!/usr/bin/env python +# +# Copyright (c) 2025 CNRS +# Author: Paul Sardin +# + +import unittest +import numpy as np +from pinocchio import SE3 +from unit.conftest import create_ur5_problem + + +class TestProblemAccessors(unittest.TestCase): + def test_steering_method_returns_object(self): + problem, robot = create_ur5_problem() + + sm = problem.steeringMethod() + + self.assertIsNotNone(sm) + + def test_distance_returns_object(self): + problem, robot = create_ur5_problem() + + distance = problem.distance() + + self.assertIsNotNone(distance) + + def test_configuration_shooter_returns_object(self): + problem, robot = create_ur5_problem() + + shooter = problem.configurationShooter() + + self.assertIsNotNone(shooter) + + def test_path_validation_returns_object(self): + problem, robot = create_ur5_problem() + + pv = problem.pathValidation() + + self.assertIsNotNone(pv) + + def test_config_validation_returns_object(self): + problem, robot = create_ur5_problem() + + cv = problem.configValidation() + + self.assertIsNotNone(cv) + + +class TestProblemDirectPath(unittest.TestCase): + def test_direct_path_without_validation(self): + problem, robot = create_ur5_problem() + q_start = np.array([0.0, -1.57, -1.8, 0.0, 0.8, 0.0]) + q_end = np.array([0.1, -1.57, -1.8, 0.0, 0.8, 0.0]) + + result = problem.directPath(q_start, q_end, False) + + self.assertIsInstance(result, tuple) + self.assertEqual(len(result), 3) + valid, path, report = result + self.assertTrue(valid) + self.assertIsNotNone(path) + + def test_direct_path_with_validation(self): + problem, robot = create_ur5_problem() + problem.addConfigValidation("JointBoundValidation") + q_start = np.array([0.0, -1.57, -1.8, 0.0, 0.8, 0.0]) + q_end = np.array([0.1, -1.57, -1.8, 0.0, 0.8, 0.0]) + + valid, path, report = problem.directPath(q_start, q_end, True) + + self.assertIsInstance(valid, bool) + + def test_direct_path_endpoints_match(self): + problem, robot = create_ur5_problem() + q_start = np.array([0.0, -1.57, -1.8, 0.0, 0.8, 0.0]) + q_end = np.array([0.5, -1.57, -1.8, 0.0, 0.8, 0.0]) + + valid, path, report = problem.directPath(q_start, q_end, False) + + np.testing.assert_array_almost_equal(path.initial(), q_start) + np.testing.assert_array_almost_equal(path.end(), q_end) + + +class TestProblemConstraintProjection(unittest.TestCase): + def test_error_threshold_settable(self): + problem, robot = create_ur5_problem() + + problem.errorThreshold = 1e-6 + + self.assertAlmostEqual(problem.errorThreshold, 1e-6) + + def test_max_iter_projection_settable(self): + problem, robot = create_ur5_problem() + + problem.maxIterProjection = 50 + + self.assertEqual(problem.maxIterProjection, 50) + + def test_apply_constraints_returns_tuple(self): + problem, robot = create_ur5_problem() + q = np.array([0.0, -1.57, -1.8, 0.0, 0.8, 0.0]) + + result = problem.applyConstraints(q) + + self.assertIsInstance(result, tuple) + self.assertEqual(len(result), 3) + success, output, residual = result + self.assertIsInstance(success, bool) + self.assertEqual(len(output), len(q)) + + +class TestProblemTransformationConstraints(unittest.TestCase): + def test_create_transformation_constraint(self): + problem, robot = create_ur5_problem() + M = SE3.Identity() + mask = [True, True, True, True, True, True] + + constraint = problem.createTransformationConstraint( + "test_constraint", "", "ur5/ee_fixed_joint", M, mask + ) + + self.assertIsNotNone(constraint) + + def test_create_relative_transformation_constraint(self): + problem, robot = create_ur5_problem() + M = SE3.Identity() + mask = [True, True, True, True, True, True] + + constraint = problem.createTransformationConstraint( + "relative_constraint", "ur5/shoulder_link", "ur5/ee_fixed_joint", M, mask + ) + + self.assertIsNotNone(constraint) + + +if __name__ == "__main__": + unittest.main() diff --git a/tests/unit/test_roadmap.py b/tests/unit/test_roadmap.py new file mode 100644 index 00000000..444075f4 --- /dev/null +++ b/tests/unit/test_roadmap.py @@ -0,0 +1,129 @@ +#!/usr/bin/env python +# +# Copyright (c) 2025 CNRS +# Author: Paul Sardin +# + +import unittest +import numpy as np +from unit.conftest import create_ur5_problem +from pyhpp.core import Roadmap, WeighedDistance + + +class TestRoadmap(unittest.TestCase): + @classmethod + def setUpClass(cls): + cls.problem, cls.robot = create_ur5_problem() + cls.distance = WeighedDistance(cls.robot) + + def test_init_creates_one_component(self): + roadmap = Roadmap(self.distance, self.robot) + q_init = np.array([0.0, -1.57, -1.8, 0.0, 0.8, 0.0]) + roadmap.initNode(q_init) + + self.assertEqual(roadmap.numberConnectedComponents(), 1) + + def test_goal_creates_separate_component(self): + roadmap = Roadmap(self.distance, self.robot) + q_init = np.array([0.0, -1.57, -1.8, 0.0, 0.8, 0.0]) + q_goal = np.array([1.57, -1.57, -1.8, 0.0, 0.8, 0.0]) + + roadmap.initNode(q_init) + roadmap.addGoalNode(q_goal) + + self.assertEqual(roadmap.numberConnectedComponents(), 2) + + def test_nearest_node_returns_closest(self): + roadmap = Roadmap(self.distance, self.robot) + q_init = np.array([0.0, -1.57, -1.8, 0.0, 0.8, 0.0]) + roadmap.initNode(q_init) + + q_query = np.array([0.1, -1.57, -1.8, 0.0, 0.8, 0.0]) + cc = roadmap.getConnectedComponent(0) + q_near, dist = roadmap.nearestNode(q_query, cc) + + np.testing.assert_array_almost_equal(q_near, q_init) + self.assertGreater(dist, 0) + + def test_add_node_increases_component_count(self): + roadmap = Roadmap(self.distance, self.robot) + q_init = np.array([0.0, -1.57, -1.8, 0.0, 0.8, 0.0]) + q_other = np.array([0.5, -1.57, -1.8, 0.0, 0.8, 0.0]) + + roadmap.initNode(q_init) + self.assertEqual(roadmap.numberConnectedComponents(), 1) + + roadmap.addNode(q_other) + self.assertEqual(roadmap.numberConnectedComponents(), 2) + + def test_add_edge_creates_connection(self): + """Adding an edge between nodes should create a connection.""" + roadmap = Roadmap(self.distance, self.robot) + q1 = np.array([0.0, -1.57, -1.8, 0.0, 0.8, 0.0]) + q2 = np.array([0.5, -1.57, -1.8, 0.0, 0.8, 0.0]) + + roadmap.initNode(q1) + roadmap.addNode(q2) + + nodes = roadmap.nodes() + self.assertEqual(len(nodes), 2) + node1, node2 = nodes[0], nodes[1] + + steer = self.problem.steeringMethod() + path = steer(q1, q2) + roadmap.addEdge(node1, node2, path) + + # Edge was added successfully (no exception raised) + self.assertEqual(len(roadmap.nodes()), 2) + + +class TestRoadmapNegativeCases(unittest.TestCase): + """Negative test cases for Roadmap operations.""" + + @classmethod + def setUpClass(cls): + cls.problem, cls.robot = create_ur5_problem() + cls.distance = WeighedDistance(cls.robot) + + def test_empty_roadmap_has_zero_components(self): + """Empty roadmap should have zero connected components.""" + roadmap = Roadmap(self.distance, self.robot) + + self.assertEqual(roadmap.numberConnectedComponents(), 0) + + def test_empty_roadmap_nodes_list_empty(self): + """Empty roadmap should have no nodes.""" + roadmap = Roadmap(self.distance, self.robot) + + self.assertEqual(len(roadmap.nodes()), 0) + + def test_multiple_nodes_tracked(self): + """Adding multiple nodes should increase node count.""" + roadmap = Roadmap(self.distance, self.robot) + q1 = np.array([0.0, -1.57, -1.8, 0.0, 0.8, 0.0]) + q2 = np.array([0.5, -1.57, -1.8, 0.0, 0.8, 0.0]) + q3 = np.array([1.0, -1.57, -1.8, 0.0, 0.8, 0.0]) + + roadmap.initNode(q1) + roadmap.addNode(q2) + roadmap.addNode(q3) + + self.assertEqual(len(roadmap.nodes()), 3) + + def test_clear_resets_roadmap(self): + """Clear should reset the roadmap to empty state.""" + roadmap = Roadmap(self.distance, self.robot) + q1 = np.array([0.0, -1.57, -1.8, 0.0, 0.8, 0.0]) + q2 = np.array([0.5, -1.57, -1.8, 0.0, 0.8, 0.0]) + + roadmap.initNode(q1) + roadmap.addNode(q2) + self.assertEqual(roadmap.numberConnectedComponents(), 2) + + roadmap.clear() + self.assertEqual(roadmap.numberConnectedComponents(), 0) + self.assertEqual(len(roadmap.nodes()), 0) + + +if __name__ == "__main__": + unittest.main() diff --git a/tests/unit/test_security_margins.py b/tests/unit/test_security_margins.py new file mode 100644 index 00000000..77948c8f --- /dev/null +++ b/tests/unit/test_security_margins.py @@ -0,0 +1,222 @@ +#!/usr/bin/env python +# +# Copyright (c) 2025 CNRS +# Author: Paul Sardin +# + +import unittest +from unit.conftest import ( + create_security_margins_instance, + create_constraint_graph_setup, +) +from pyhpp.manipulation.security_margins import SecurityMargins + + +class TestSecurityMarginsCorrectness(unittest.TestCase): + def test_gripper_correctly_mapped_to_robot(self): + sm, *_ = create_security_margins_instance() + + self.assertIn("ur3/gripper", sm.gripperToRobot) + self.assertEqual(sm.gripperToRobot["ur3/gripper"], "ur3") + self.assertIn("ur3/gripper", sm.gripperToJoints) + self.assertGreater(len(sm.gripperToJoints["ur3/gripper"]), 0) + + def test_margins_stored_symmetrically(self): + sm, *_ = create_security_margins_instance() + + sm.setSecurityMarginBetween("ur3", "sphere0", 0.05) + + self.assertEqual(sm.getSecurityMarginBetween("ur3", "sphere0"), 0.05) + self.assertEqual(sm.getSecurityMarginBetween("sphere0", "ur3"), 0.05) + + def test_default_margin_used_for_unset_pairs(self): + sm, *_ = create_security_margins_instance() + + sm.defaultMargin = 0.03 + self.assertEqual(sm.getSecurityMarginBetween("sphere0", "sphere1"), 0.03) + + def test_apply_sets_margins_on_edges(self): + sm, _, graph, *_ = create_security_margins_instance() + + sm.defaultMargin = 0.02 + sm.setSecurityMarginBetween("ur3", "sphere0", 0.05) + sm.apply() + + edges = graph.getTransitions() + self.assertGreater(len(edges), 0) + + for edge in edges: + matrix = graph.getSecurityMarginMatrixForTransition(edge) + self.assertIsNotNone(matrix) + + def test_full_workflow_with_graph(self): + problem, graph, factory, robot, objects = create_constraint_graph_setup() + + sm = SecurityMargins(problem, factory, ["ur3"] + objects, robot) + sm.defaultMargin = 0.02 + sm.setSecurityMarginBetween("ur3", "sphere0", 0.05) + sm.setSecurityMarginBetween("ur3", "sphere1", 0.05) + sm.apply() + + edges = graph.getTransitions() + self.assertGreater(len(edges), 0) + + state = graph.getState("free") + q = robot.currentConfiguration() + result, _, _ = graph.applyStateConstraints(state, q) + self.assertIsInstance(result, bool) + + def test_grasp_edge_has_zero_margin_between_gripper_and_object(self): + """ + Grasp edges must have zero margin between gripper and grasped object, + otherwise collision detection would always reject the grasp. + Free/transit edges keep the configured safety margin. + """ + problem, graph, factory, robot, objects = create_constraint_graph_setup() + + sm = SecurityMargins(problem, factory, ["ur3"] + objects, robot) + configured_margin = 0.05 + sm.defaultMargin = configured_margin + sm.setSecurityMarginBetween("ur3", "sphere0", configured_margin) + sm.apply() + + model = robot.model() + gripper_idx = model.getJointId("ur3/wrist_3_joint") + object_idx = model.getJointId("sphere0/root_joint") + + def get_margin(edge, idx1, idx2): + matrix = graph.getSecurityMarginMatrixForTransition(edge) + if matrix and len(matrix) > idx1 and len(matrix[idx1]) > idx2: + return matrix[idx1][idx2] + return None + + edges = graph.getTransitions() + edge_names = graph.getTransitionNames() + + found_free_loop = False + found_grasp_edge = False + free_margin = None + grasp_margin = None + + for edge, name in zip(edges, edge_names): + margin = get_margin(edge, gripper_idx, object_idx) + if margin is None: + continue + + if name == "Loop | f": + found_free_loop = True + free_margin = margin + self.assertAlmostEqual(margin, configured_margin, places=6) + + elif name == "Loop | 0-0": + found_grasp_edge = True + grasp_margin = margin + self.assertEqual(margin, 0.0) + + self.assertTrue(found_free_loop, "Did not find 'Loop | f' edge") + self.assertTrue( + found_grasp_edge, f"Did not find 'Loop | 0-0' edge. Available: {edge_names}" + ) + + if free_margin is not None and grasp_margin is not None: + self.assertNotEqual(free_margin, grasp_margin) + + def test_joint_to_robot_mapping_correct(self): + sm, *_ = create_security_margins_instance() + + self.assertIn("ur3", sm.robotToJoints) + self.assertIn("sphere0", sm.robotToJoints) + self.assertIn("sphere1", sm.robotToJoints) + + for joint in sm.robotToJoints["ur3"]: + self.assertEqual(sm.jointToRobot[joint], "ur3") + + for joint in sm.robotToJoints["sphere0"]: + self.assertEqual(sm.jointToRobot[joint], "sphere0") + + def test_get_active_constraints_returns_dict(self): + sm, _, graph, *_ = create_security_margins_instance() + + edges = graph.getTransitions() + self.assertGreater(len(edges), 0) + + result = sm.getActiveConstraintsAlongEdge(edges[0]) + + self.assertIsInstance(result, dict) + self.assertIn("place", result) + self.assertIn("grasp", result) + self.assertIsInstance(result["place"], list) + self.assertIsInstance(result["grasp"], list) + + def test_grasp_edge_has_grasp_constraint(self): + problem, graph, factory, robot, objects = create_constraint_graph_setup() + sm = SecurityMargins(problem, factory, ["ur3"] + objects, robot) + + edge_names = graph.getTransitionNames() + edges = graph.getTransitions() + + for edge, name in zip(edges, edge_names): + if "0-0" in name: + result = sm.getActiveConstraintsAlongEdge(edge) + self.assertGreater(len(result["grasp"]), 0) + break + + +class TestSecurityMarginsNegativeCases(unittest.TestCase): + """Negative test cases for SecurityMargins.""" + + def test_get_margin_unknown_robot_returns_default(self): + """Getting margin for unknown robot pair should return default.""" + sm, *_ = create_security_margins_instance() + sm.defaultMargin = 0.01 + + margin = sm.getSecurityMarginBetween("unknown1", "unknown2") + self.assertEqual(margin, 0.01) + + def test_set_negative_margin_allowed(self): + """Negative margins should be allowed (for penetration tolerance).""" + sm, *_ = create_security_margins_instance() + + sm.setSecurityMarginBetween("ur3", "sphere0", -0.01) + + self.assertEqual(sm.getSecurityMarginBetween("ur3", "sphere0"), -0.01) + + def test_zero_default_margin(self): + """Zero default margin should work correctly.""" + sm, *_ = create_security_margins_instance() + sm.defaultMargin = 0.0 + + self.assertEqual(sm.getSecurityMarginBetween("sphere0", "sphere1"), 0.0) + + def test_overwrite_margin(self): + """Setting margin twice should overwrite.""" + sm, *_ = create_security_margins_instance() + + sm.setSecurityMarginBetween("ur3", "sphere0", 0.05) + sm.setSecurityMarginBetween("ur3", "sphere0", 0.10) + + self.assertEqual(sm.getSecurityMarginBetween("ur3", "sphere0"), 0.10) + + def test_apply_multiple_times(self): + """Applying margins multiple times should not raise.""" + sm, _, graph, *_ = create_security_margins_instance() + + sm.defaultMargin = 0.02 + sm.apply() + sm.defaultMargin = 0.03 + sm.apply() + + edges = graph.getTransitions() + self.assertGreater(len(edges), 0) + + def test_large_margin_value(self): + """Very large margin values should be accepted.""" + sm, *_ = create_security_margins_instance() + + sm.setSecurityMarginBetween("ur3", "sphere0", 100.0) + + self.assertEqual(sm.getSecurityMarginBetween("ur3", "sphere0"), 100.0) + + +if __name__ == "__main__": + unittest.main() diff --git a/tests/unit/test_static_stability.py b/tests/unit/test_static_stability.py new file mode 100644 index 00000000..babde7d1 --- /dev/null +++ b/tests/unit/test_static_stability.py @@ -0,0 +1,94 @@ +#!/usr/bin/env python +# +# Copyright (c) 2025 CNRS +# Author: Paul Sardin +# + +import unittest +import numpy as np +from pinocchio import SE3 + +from pyhpp.manipulation import Device, Problem, urdf +from pyhpp.core.static_stability_constraint_factory import ( + StaticStabilityConstraintsFactory, +) + + +def load_romeo(): + romeo_urdf = "package://example-robot-data/robots/romeo_description/urdf/romeo.urdf" + romeo_srdf = ( + "package://example-robot-data/robots/romeo_description/srdf/romeo_moveit.srdf" + ) + + robot = Device("romeo") + romeo_pose = SE3(rotation=np.identity(3), translation=np.array([0, 0, 0])) + urdf.loadModel(robot, 0, "romeo", "freeflyer", romeo_urdf, romeo_srdf, romeo_pose) + + robot.setJointBounds( + "romeo/root_joint", + [-1, 1, -1, 1, 0, 2, -2.0, 2, -2.0, 2, -2.0, 2, -2.0, 2], + ) + + return robot + + +class TestStaticStabilityConstraintsFactory(unittest.TestCase): + @classmethod + def setUpClass(cls): + cls.robot = load_romeo() + cls.problem = Problem(cls.robot) + cls.factory = StaticStabilityConstraintsFactory(cls.problem, cls.robot) + cls.q0 = cls.robot.currentConfiguration() + cls.left_ankle = "romeo/LAnkleRoll" + cls.right_ankle = "romeo/RAnkleRoll" + + def test_create_static_stability_constraint_returns_dict(self): + constraints = self.factory.createStaticStabilityConstraint( + "test_", "", self.left_ankle, self.right_ankle, self.q0 + ) + + self.assertIsInstance(constraints, dict) + self.assertIn("test_relative-com", constraints) + self.assertIn("test_pose-left-foot", constraints) + self.assertIn("test_pose-right-foot", constraints) + self.assertEqual(len(constraints), 3) + + def test_create_sliding_stability_constraint_returns_dict(self): + constraints = self.factory.createSlidingStabilityConstraint( + "slide_", "", self.left_ankle, self.right_ankle, self.q0 + ) + + self.assertIsInstance(constraints, dict) + self.assertIn("slide_relative-com", constraints) + self.assertIn("slide_relative-pose", constraints) + self.assertIn("slide_pose-left-foot", constraints) + self.assertIn("slide_pose-left-foot-complement", constraints) + self.assertEqual(len(constraints), 4) + + def test_create_aligned_com_stability_constraint_returns_dict(self): + constraints = self.factory.createAlignedCOMStabilityConstraint( + "aligned_", "", self.left_ankle, self.right_ankle, self.q0, sliding=False + ) + + self.assertIsInstance(constraints, dict) + self.assertIn("aligned_com-between-feet", constraints) + self.assertIn("aligned_pose-left-foot", constraints) + self.assertIn("aligned_pose-right-foot", constraints) + self.assertEqual(len(constraints), 3) + + def test_create_aligned_com_stability_constraint_sliding(self): + constraints = self.factory.createAlignedCOMStabilityConstraint( + "aligned_slide_", + "", + self.left_ankle, + self.right_ankle, + self.q0, + sliding=True, + ) + + self.assertIsInstance(constraints, dict) + self.assertEqual(len(constraints), 3) + + +if __name__ == "__main__": + unittest.main() diff --git a/tests/unit/test_steering_method.py b/tests/unit/test_steering_method.py new file mode 100644 index 00000000..0842c354 --- /dev/null +++ b/tests/unit/test_steering_method.py @@ -0,0 +1,50 @@ +#!/usr/bin/env python +# +# Copyright (c) 2025 CNRS +# Author: Paul Sardin +# + +import unittest +import numpy as np +from unit.conftest import create_ur5_problem + + +class TestSteeringMethod(unittest.TestCase): + @classmethod + def setUpClass(cls): + cls.problem, cls.robot = create_ur5_problem() + + def test_creates_path_with_correct_endpoints(self): + q1 = np.array([0.0, -1.57, -1.8, 0.0, 0.8, 0.0]) + q2 = np.array([1.57, -1.57, -1.8, 0.0, 0.8, 0.0]) + + steer = self.problem.steeringMethod() + path = steer(q1, q2) + + self.assertIsNotNone(path) + self.assertGreater(path.length(), 0) + np.testing.assert_array_almost_equal(path.initial(), q1) + np.testing.assert_array_almost_equal(path.end(), q2) + + def test_path_length_is_positive(self): + q1 = np.array([0.0, -1.57, -1.8, 0.0, 0.8, 0.0]) + q2 = np.array([0.5, -1.0, -1.5, 0.2, 0.5, 0.1]) + + steer = self.problem.steeringMethod() + path = steer(q1, q2) + + self.assertGreater(path.length(), 0) + + def test_identical_configs_creates_zero_length_path(self): + """Steering between identical configs should create a zero-length path.""" + q = np.array([0.0, -1.57, -1.8, 0.0, 0.8, 0.0]) + + steer = self.problem.steeringMethod() + path = steer(q, q) + + self.assertIsNotNone(path) + self.assertEqual(path.length(), 0.0) + + +if __name__ == "__main__": + unittest.main()