diff --git a/.github/workflows/vcpkg_triplets/arm64-osx-dynamic-release.cmake b/.github/workflows/vcpkg_triplets/arm64-osx-dynamic-release.cmake new file mode 100644 index 000000000..abb8d556c --- /dev/null +++ b/.github/workflows/vcpkg_triplets/arm64-osx-dynamic-release.cmake @@ -0,0 +1,9 @@ +set(VCPKG_TARGET_ARCHITECTURE arm64) +set(VCPKG_CRT_LINKAGE dynamic) +set(VCPKG_LIBRARY_LINKAGE dynamic) + +set(VCPKG_CMAKE_SYSTEM_NAME Darwin) +set(VCPKG_OSX_ARCHITECTURES arm64) + +set(VCPKG_BUILD_TYPE release) +set(VCPKG_OSX_DEPLOYMENT_TARGET 12.0) diff --git a/.github/workflows/wheels.yml b/.github/workflows/wheels.yml index c7cbc5b70..d24b60f70 100644 --- a/.github/workflows/wheels.yml +++ b/.github/workflows/wheels.yml @@ -14,6 +14,8 @@ env: VCPKG_PKGS: >- boost-dll boost-program-options boost-serialization boost-filesystem + boost-stacktrace boost-algorithm + boost-format tinyxml2 console-bridge assimp urdfdom octomap orocos-kdl pcl gtest benchmark flann jsoncpp @@ -30,48 +32,48 @@ jobs: fail-fast: false matrix: config: - - os: ubuntu-20.04 - runs_on: ubuntu-20.04 - container: ubuntu:20.04 - py_platform: manylinux_2_31_x86_64 - python_version: "3.7" - docs: false - docs_cmake: OFF - - os: ubuntu-20.04 - runs_on: ubuntu-20.04 - container: ubuntu:20.04 - py_platform: manylinux_2_31_x86_64 + - os: ubuntu-22.04 + runs_on: ubuntu-22.04 + container: ubuntu:22.04 + py_platform: manylinux_2_35_x86_64 python_version: "3.8" docs: true docs_cmake: ON - - os: ubuntu-20.04 - runs_on: ubuntu-20.04 - container: ubuntu:20.04 - py_platform: manylinux_2_31_x86_64 + - os: ubuntu-22.04 + runs_on: ubuntu-22.04 + container: ubuntu:22.04 + py_platform: manylinux_2_35_x86_64 python_version: "3.9" docs: true docs_cmake: ON - - os: ubuntu-20.04 - runs_on: ubuntu-20.04 - container: ubuntu:20.04 - py_platform: manylinux_2_31_x86_64 + - os: ubuntu-22.04 + runs_on: ubuntu-22.04 + container: ubuntu:22.04 + py_platform: manylinux_2_35_x86_64 python_version: "3.10" docs: true docs_cmake: ON - - os: ubuntu-20.04 - runs_on: ubuntu-20.04 - container: ubuntu:20.04 - py_platform: manylinux_2_31_x86_64 + - os: ubuntu-22.04 + runs_on: ubuntu-22.04 + container: ubuntu:22.04 + py_platform: manylinux_2_35_x86_64 python_version: "3.11.0" docs: true docs_cmake: ON - - os: ubuntu-20.04 - runs_on: ubuntu-20.04 - container: ubuntu:20.04 - py_platform: manylinux_2_31_x86_64 + - os: ubuntu-22.04 + runs_on: ubuntu-22.04 + container: ubuntu:22.04 + py_platform: manylinux_2_35_x86_64 python_version: "3.12.0" docs: true docs_cmake: ON + - os: ubuntu-22.04 + runs_on: ubuntu-22.04 + container: ubuntu:22.04 + py_platform: manylinux_2_35_x86_64 + python_version: "3.13.0" + docs: true + docs_cmake: ON env: DEBIAN_FRONTEND: noninteractive steps: @@ -93,7 +95,7 @@ jobs: liburdfdom-dev liboctomap-dev liborocos-kdl-dev libpcl-dev libflann-dev libjsoncpp-dev libyaml-cpp-dev git cmake ninja-build build-essential autoconf automake libtool bison libpcre2-dev libpcre3-dev - lcov libbullet-dev libbullet-extras-dev python3-venv curl -y -qq + lcov libbullet-dev libbullet-extras-dev python3-venv curl libfcl-dev -y -qq - name: build-swig uses: johnwason/swig-build-action@v1 with: @@ -149,17 +151,17 @@ jobs: export LD_LIBRARY_PATH=$LD_LIBRARY_PATH:$GITHUB_WORKSPACE/ws/install/lib cmake --build . --config Release --target tesseract_python_doc - name: archive wheels - uses: actions/upload-artifact@v2 + uses: actions/upload-artifact@v4 with: name: 'python-wheels-${{ matrix.config.os }}-${{ matrix.config.python_version }}' path: ws/build/tesseract_python/python/* - name: archive docs - uses: actions/upload-artifact@v2 + uses: actions/upload-artifact@v4 with: name: 'python-docs-${{ matrix.config.os }}-${{ matrix.config.python_version }}' path: ws/build/tesseract_python/docs/* build-win: - runs-on: windows-2019 + runs-on: windows-2022 strategy: fail-fast: false matrix: @@ -188,6 +190,10 @@ jobs: vcpkg_triplet: x64-windows-release cmake_arch: x64 python_version: "3.12.0" + - arch: x64 + vcpkg_triplet: x64-windows-release + cmake_arch: x64 + python_version: "3.13.0" steps: - uses: actions/checkout@v2 with: @@ -198,7 +204,7 @@ jobs: python-version: '${{ matrix.config.python_version }}' architecture: ${{ matrix.config.arch }} - name: vcpkg build - uses: johnwason/vcpkg-action@v5 + uses: johnwason/vcpkg-action@v6 with: pkgs: >- ${{ env.VCPKG_PKGS }} @@ -206,7 +212,7 @@ jobs: extra-args: --clean-after-build token: ${{ github.token }} cache-key: win-${{ matrix.config.arch }}-python-${{ matrix.config.python_version }} - revision: master + github-binarycache: true - name: pip3 run: | python -m pip install numpy setuptools wheel pytest delvewheel colcon-common-extensions vcstool @@ -220,7 +226,7 @@ jobs: working-directory: ws shell: cmd run: | - call "C:\Program Files (x86)\Microsoft Visual Studio\2019\Enterprise\Common7\Tools\VsDevCmd.bat" -arch=amd64 -host_arch=amd64 + call "C:\Program Files\Microsoft Visual Studio\2022\Enterprise\Common7\Tools\VsDevCmd.bat" -arch=amd64 -host_arch=amd64 set CXXFLAGS=%CXXFLAGS% -DEIGEN_DONT_ALIGN=1 -DEIGEN_DONT_VECTORIZE=1 set CMAKE_PREFIX_PATH=%GITHUB_WORKSPACE%\vcpkg\installed\${{ matrix.config.vcpkg_triplet }} set PATH=%PATH%;%GITHUB_WORKSPACE%\vcpkg\installed\${{ matrix.config.vcpkg_triplet }}\bin @@ -263,25 +269,44 @@ jobs: if %errorlevel% neq 0 exit /b %errorlevel% - name: archive wheels if: always() - uses: actions/upload-artifact@v2 + uses: actions/upload-artifact@v4 with: name: 'python-wheels-win-${{matrix.config.python_version}}' path: ws/build/tesseract_python/python - name: archive logs if: failure() - uses: actions/upload-artifact@v2 + uses: actions/upload-artifact@v4 with: name: 'build-logs-win-${{ matrix.config.arch }}-python-${{ matrix.config.python_version }}' path: "**/*.log" retention-days: 2 build-macos: - runs-on: macos-latest + runs-on: ${{ matrix.config.runner }} strategy: fail-fast: false matrix: config: - - py_platform: macosx-10.9-x86_64 + - py_platform: macosx-12.0-x86_64 + python_version: "3.12" + arch: x64 + runner: macos-13 + brew_prefix: /usr/local + - py_platform: macosx-12.0-x86_64 + python_version: "3.13" + arch: x64 + runner: macos-13 + brew_prefix: /usr/local + - py_platform: macosx-12.0-arm64 python_version: "3.12" + arch: arm64 + runner: macos-14 + brew_prefix: /opt/homebrew + - py_platform: macosx-12.0-arm64 + python_version: "3.13" + arch: arm64 + runner: macos-14 + brew_prefix: /opt/homebrew + steps: - uses: actions/checkout@v2 with: @@ -292,16 +317,17 @@ jobs: python-version: '${{ matrix.config.python_version }}' - name: brew run: | - brew install libomp cmake automake autoconf libtool gcc ninja + brew install libomp cmake automake autoconf libtool gcc ninja swig - name: vcpkg build - uses: johnwason/vcpkg-action@v5 + uses: johnwason/vcpkg-action@v6 with: pkgs: >- ${{ env.VCPKG_PKGS }} - triplet: x64-osx-dynamic-release + triplet: ${{ matrix.config.arch }}-osx-dynamic-release extra-args: --clean-after-build --overlay-triplets=${{ github.workspace }}/ws/src/tesseract_python/.github/workflows/vcpkg_triplets token: ${{ github.token }} - cache-key: osx-x64-vcpkg + cache-key: osx-${{ matrix.config.arch }}-vcpkg + github-binarycache: true revision: master - name: pip3 run: | @@ -312,8 +338,8 @@ jobs: - name: colcon build working-directory: ws run: | - export DYLD_LIBRARY_PATH=$DYLD_LIBRARY_PATH:$GITHUB_WORKSPACE/vcpkg/installed/x64-osx-dynamic-release/lib:$GITHUB_WORKSPACE/ws/install/lib - export CMAKE_PREFIX_PATH=$GITHUB_WORKSPACE/vcpkg/installed/x64-osx-dynamic-release + export DYLD_LIBRARY_PATH=$DYLD_LIBRARY_PATH:$GITHUB_WORKSPACE/vcpkg/installed/${{ matrix.config.arch }}-osx-dynamic-release/lib:$GITHUB_WORKSPACE/ws/install/lib + export CMAKE_PREFIX_PATH=$GITHUB_WORKSPACE/vcpkg/installed/${{ matrix.config.arch }}-osx-dynamic-release colcon build --merge-install \ --packages-ignore tesseract_examples trajopt_ifopt trajopt_sqp ifopt vhacd \ @@ -324,12 +350,12 @@ jobs: -DBUILD_SHARED_LIBS=ON -DTESSERACT_ENABLE_EXAMPLES=OFF -DTESSERACT_BUILD_TRAJOPT_IFOPT=OFF \ -DVCPKG_APPLOCAL_DEPS=OFF -DTESSERACT_ENABLE_TESTING=ON \ -DCMAKE_OSX_DEPLOYMENT_TARGET=12.0 \ - -DOpenMP_CXX_INCLUDE_DIR=/usr/local/opt/libomp/include \ - -DOpenMP_C_INCLUDE_DIR=/usr/local/opt/libomp/include \ + -DOpenMP_CXX_INCLUDE_DIR=${{ matrix.config.brew_prefix }}/opt/libomp/include \ + -DOpenMP_C_INCLUDE_DIR=${{ matrix.config.brew_prefix }}/opt/libomp/include \ -DOpenMP_CXX_LIB_NAMES=libomp -DOpenMP_CXX_FLAGS="-Xpreprocessor -fopenmp" \ -DOpenMP_C_LIB_NAMES=libomp -DOpenMP_C_FLAGS="-Xpreprocessor -fopenmp" \ - -DOpenMP_libomp_LIBRARY=/usr/local/opt/libomp/lib/libomp.dylib \ - -Dtcmalloc_minimal_LIBRARY=${{ github.workspace }}/vcpkg/installed/x64-osx-dynamic-release/lib/libtcmalloc_minimal.dylib \ + -DOpenMP_libomp_LIBRARY=${{ matrix.config.brew_prefix }}/opt/libomp/lib/libomp.dylib \ + -Dtcmalloc_minimal_LIBRARY=${{ github.workspace }}/vcpkg/installed/${{ matrix.config.arch }}-osx-dynamic-release/lib/libtcmalloc_minimal.dylib \ -DPYTHON_EXECUTABLE="${{ steps.setup-python.outputs.python-path }}" \ -DTESSERACT_PYTHON_WHEEL_PLATFORM=${{ matrix.config.py_platform }} \ -DTESSERACT_PYTHON_BUILD_WHEEL=ON @@ -347,13 +373,13 @@ jobs: pytest -s - name: archive wheels if: always() - uses: actions/upload-artifact@v2 + uses: actions/upload-artifact@v4 with: - name: 'python-macos-win-${{matrix.config.python_version}}' + name: 'python-macos-${{ matrix.config.arch }}-python-${{matrix.config.python_version}}' path: ws/build/tesseract_python/python - name: archive logs if: failure() - uses: actions/upload-artifact@v2 + uses: actions/upload-artifact@v4 with: name: 'build-logs-macos-${{ matrix.config.arch }}-python-${{ matrix.config.python_version }}' path: "**/*.log" @@ -363,17 +389,17 @@ jobs: - build-win - build-ubuntu - build-macos - runs-on: ubuntu-20.04 + runs-on: ubuntu-22.04 steps: - uses: actions/checkout@v2 with: path: robotraconteur - name: Download CI artifacts - uses: actions/download-artifact@v2 + uses: actions/download-artifact@v4 with: path: artifacts/main - name: archive wheels - uses: actions/upload-artifact@v2 + uses: actions/upload-artifact@v4 with: name: 'python-wheels-all' path: artifacts/**/wheelhouse/*.whl diff --git a/README.md b/README.md index 976f42eca..15dbf8838 100644 --- a/README.md +++ b/README.md @@ -1,13 +1,11 @@ # Tesseract Python -[![Python](https://img.shields.io/badge/python-3.7+-blue.svg)](https://github.com/tesseract-robotics/tesseract_python) +[![Python](https://img.shields.io/badge/python-3.8+-blue.svg)](https://github.com/tesseract-robotics/tesseract_python) ![PyPI](https://img.shields.io/pypi/v/tesseract-robotics) Platform | CI Status ---------------------|:--------- -Linux (Focal) | [![Build Status](https://github.com/tesseract-robotics/tesseract_python/workflows/Focal-Build/badge.svg)](https://github.com/tesseract-robotics/tesseract_python/actions) -Windows | [![Build Status](https://github.com/tesseract-robotics/tesseract_python/workflows/Windows-Noetic-Build/badge.svg)](https://github.com/tesseract-robotics/tesseract_python/actions) Wheels | [![Build Status](https://github.com/tesseract-robotics/tesseract_python/actions/workflows/wheels.yml/badge.svg)](https://github.com/tesseract-robotics/tesseract_python/actions) [![Github Issues](https://img.shields.io/github/issues/tesseract-robotics/tesseract_python.svg)](http://github.com/tesseract-robotics/tesseract_python/issues) @@ -23,7 +21,7 @@ checking (Bullet, FCL), kinematics (KDL, OPW, UR), planning (OMPL, Descartes, Tr (tesseract_viewer_python) Standalone packages are provided on PyPi (pip install) for Windows and Linux, containing all the native dependencies -for Python 3.7+. +for Python 3.8+. The Tesseract Python package is developed and maintained by Wason Technology, LLC. @@ -39,18 +37,17 @@ See https://tesseract-robotics.github.io/tesseract_python/ for documentation. Standalone packages are provided on PyPi (pip install) for Windows and Linux, containing Tesseract, Tesseract Planning, andall the native dependencies -for Python 3.7+. These packages have been tested on Windows 10, Ubuntu 20.04, and Ubuntu 22.04, but should work -on any relatively recent x64 Windows or Linux operating system. Packages are available for Python 3.7 - 3.11. +for Python 3.8+. These packages have been tested on Windows 10, and Ubuntu 22.04, but should work +on any relatively recent x64 Windows or Linux operating system. Packages are available for Python 3.8 - 3.13 To install on Windows: ``` python -m pip install tesseract-robotics tesseract-robotics-viewer ``` -To install on Ubuntu 20.04 and Ubuntu 22.04: +To install on Ubuntu 24.04 and Ubuntu 22.04: ``` sudo apt install python3-pip python3-numpy -# The supplied version of pip on Ubuntu 20.04 is too old for manylinux_2_31, upgrade pip python3 -m pip install -U pip python3 -m pip install --user tesseract_robotics tesseract_robotics_viewer ``` @@ -115,7 +112,7 @@ import numpy as np import numpy.testing as nptest from tesseract_robotics.tesseract_common import GeneralResourceLocator -from tesseract_robotics.tesseract_environment import Environment +from tesseract_robotics.tesseract_environment import Environment, AnyPoly_wrap_EnvironmentConst from tesseract_robotics.tesseract_common import FilesystemPath, Isometry3d, Translation3d, Quaterniond, \ ManipulatorInfo, AnyPoly, AnyPoly_wrap_double from tesseract_robotics.tesseract_command_language import CartesianWaypoint, WaypointPoly, \ @@ -125,9 +122,10 @@ from tesseract_robotics.tesseract_command_language import CartesianWaypoint, Way AnyPoly_wrap_CompositeInstruction, DEFAULT_PROFILE_KEY, JointWaypoint, JointWaypointPoly, \ InstructionPoly_as_MoveInstructionPoly, WaypointPoly_as_StateWaypointPoly, \ MoveInstructionPoly_wrap_MoveInstruction, StateWaypointPoly_wrap_StateWaypoint, \ - CartesianWaypointPoly_wrap_CartesianWaypoint, JointWaypointPoly_wrap_JointWaypoint + CartesianWaypointPoly_wrap_CartesianWaypoint, JointWaypointPoly_wrap_JointWaypoint, \ + AnyPoly_wrap_ProfileDictionary -from tesseract_robotics.tesseract_task_composer import TaskComposerPluginFactory, PlanningTaskComposerProblem, \ +from tesseract_robotics.tesseract_task_composer import TaskComposerPluginFactory, \ TaskComposerDataStorage, TaskComposerContext from tesseract_robotics_viewer import TesseractViewer @@ -200,7 +198,8 @@ factory = TaskComposerPluginFactory(config_path) task = factory.createTaskComposerNode("FreespacePipeline") # Get the output keys for the task -output_key = task.getOutputKeys()[0] +output_key = task.getOutputKeys().get("program") +input_key = task.getInputKeys().get("planning_input") # Create a profile dictionary. Profiles can be customized by adding to this dictionary and setting the profiles # in the instructions. @@ -209,18 +208,26 @@ profiles = ProfileDictionary() # Create an AnyPoly containing the program. This explicit step is required because the Python bindings do not # support implicit conversion from the CompositeInstruction to the AnyPoly. program_anypoly = AnyPoly_wrap_CompositeInstruction(program) +environment_anypoly = AnyPoly_wrap_EnvironmentConst(t_env) +profiles_anypoly = AnyPoly_wrap_ProfileDictionary(profiles) -# Create the task problem and input -task_planning_problem = PlanningTaskComposerProblem(t_env, profiles) -task_planning_problem.input = program_anypoly +# Create the task data +task_data = TaskComposerDataStorage() +task_data.setData(input_key, program_anypoly) +task_data.setData("environment", environment_anypoly) +task_data.setData("profiles", profiles_anypoly) # Create an executor to run the task task_executor = factory.createTaskComposerExecutor("TaskflowExecutor") # Run the task and wait for completion -future = task_executor.run(task.get(), task_planning_problem) +future = task_executor.run(task.get(), task_data) future.wait() +if not future.context.isSuccessful(): + print("Planning task failed") + exit(1) + # Retrieve the output, converting the AnyPoly back to a CompositeInstruction results = AnyPoly_as_CompositeInstruction(future.context.data_storage.getData(output_key)) diff --git a/dependencies.rosinstall b/dependencies.rosinstall index da2bdbd0c..33caa7124 100644 --- a/dependencies.rosinstall +++ b/dependencies.rosinstall @@ -1,23 +1,23 @@ - git: local-name: ros_industrial_cmake_boilerplate uri: https://github.com/ros-industrial/ros_industrial_cmake_boilerplate.git - version: 0.5.4 + version: 0.6.3 - git: local-name: tesseract uri: https://github.com/tesseract-robotics/tesseract.git - version: 0.21.5 + version: c0162ef9a30507d0ef750d37aa96d098846a4e38 - git: local-name: tesseract_planning uri: https://github.com/tesseract-robotics/tesseract_planning.git - version: 0.21.6 + version: b8f02cc8382eb86b1a77a2369acccf01f646499a - git: local-name: trajopt uri: https://github.com/tesseract-robotics/trajopt.git - version: 0.7.0 + version: 0840fc737e2fcc4441fa50d036467c5f17f6746b - git: local-name: descartes_light uri: https://github.com/swri-robotics/descartes_light.git - version: 0.4.2 + version: 0.4.3 - git: local-name: opw_kinematics uri: https://github.com/Jmeyer1292/opw_kinematics.git diff --git a/dependencies_with_ext.rosinstall b/dependencies_with_ext.rosinstall index 246e9414c..694e9985e 100644 --- a/dependencies_with_ext.rosinstall +++ b/dependencies_with_ext.rosinstall @@ -1,23 +1,23 @@ - git: local-name: ros_industrial_cmake_boilerplate uri: https://github.com/ros-industrial/ros_industrial_cmake_boilerplate.git - version: 0.5.4 + version: 0.6.3 - git: local-name: tesseract uri: https://github.com/tesseract-robotics/tesseract.git - version: 0.21.5 + version: c0162ef9a30507d0ef750d37aa96d098846a4e38 - git: local-name: tesseract_planning uri: https://github.com/tesseract-robotics/tesseract_planning.git - version: 0.21.6 + version: b8f02cc8382eb86b1a77a2369acccf01f646499a - git: local-name: trajopt uri: https://github.com/tesseract-robotics/trajopt.git - version: 0.7.0 + version: 0840fc737e2fcc4441fa50d036467c5f17f6746b - git: local-name: descartes_light uri: https://github.com/swri-robotics/descartes_light.git - version: 0.4.2 + version: 0.4.3 - git: local-name: opw_kinematics uri: https://github.com/Jmeyer1292/opw_kinematics.git diff --git a/examples/tesseract_planning_example_composer.py b/examples/tesseract_planning_example_composer.py index 9eb327392..be9775f58 100644 --- a/examples/tesseract_planning_example_composer.py +++ b/examples/tesseract_planning_example_composer.py @@ -5,7 +5,7 @@ import numpy.testing as nptest from tesseract_robotics.tesseract_common import GeneralResourceLocator -from tesseract_robotics.tesseract_environment import Environment +from tesseract_robotics.tesseract_environment import Environment, AnyPoly_wrap_EnvironmentConst from tesseract_robotics.tesseract_common import FilesystemPath, Isometry3d, Translation3d, Quaterniond, \ ManipulatorInfo, AnyPoly, AnyPoly_wrap_double from tesseract_robotics.tesseract_command_language import CartesianWaypoint, WaypointPoly, \ @@ -15,9 +15,10 @@ AnyPoly_wrap_CompositeInstruction, DEFAULT_PROFILE_KEY, JointWaypoint, JointWaypointPoly, \ InstructionPoly_as_MoveInstructionPoly, WaypointPoly_as_StateWaypointPoly, \ MoveInstructionPoly_wrap_MoveInstruction, StateWaypointPoly_wrap_StateWaypoint, \ - CartesianWaypointPoly_wrap_CartesianWaypoint, JointWaypointPoly_wrap_JointWaypoint + CartesianWaypointPoly_wrap_CartesianWaypoint, JointWaypointPoly_wrap_JointWaypoint, \ + AnyPoly_wrap_ProfileDictionary -from tesseract_robotics.tesseract_task_composer import TaskComposerPluginFactory, PlanningTaskComposerProblem, \ +from tesseract_robotics.tesseract_task_composer import TaskComposerPluginFactory, \ TaskComposerDataStorage, TaskComposerContext from tesseract_robotics_viewer import TesseractViewer @@ -134,7 +135,8 @@ task = factory.createTaskComposerNode("FreespacePipeline") # Get the output keys for the task -output_key = task.getOutputKeys()[0] +output_key = task.getOutputKeys().get("program") +input_key = task.getInputKeys().get("planning_input") # Create a profile dictionary. Profiles can be customized by adding to this dictionary and setting the profiles # in the instructions. @@ -143,18 +145,26 @@ # Create an AnyPoly containing the program. This explicit step is required because the Python bindings do not # support implicit conversion from the CompositeInstruction to the AnyPoly. program_anypoly = AnyPoly_wrap_CompositeInstruction(program) +environment_anypoly = AnyPoly_wrap_EnvironmentConst(t_env) +profiles_anypoly = AnyPoly_wrap_ProfileDictionary(profiles) -# Create the task problem and input -task_planning_problem = PlanningTaskComposerProblem(t_env, profiles) -task_planning_problem.input = program_anypoly +# Create the task data +task_data = TaskComposerDataStorage() +task_data.setData(input_key, program_anypoly) +task_data.setData("environment", environment_anypoly) +task_data.setData("profiles", profiles_anypoly) # Create an executor to run the task task_executor = factory.createTaskComposerExecutor("TaskflowExecutor") # Run the task and wait for completion -future = task_executor.run(task.get(), task_planning_problem) +future = task_executor.run(task.get(), task_data) future.wait() +if not future.context.isSuccessful(): + print("Planning task failed") + exit(1) + # Retrieve the output, converting the AnyPoly back to a CompositeInstruction results = AnyPoly_as_CompositeInstruction(future.context.data_storage.getData(output_key)) diff --git a/examples/tesseract_planning_example_no_composer.py b/examples/tesseract_planning_example_no_composer.py index 76c3c9cb4..6dfb2fd69 100644 --- a/examples/tesseract_planning_example_no_composer.py +++ b/examples/tesseract_planning_example_no_composer.py @@ -10,13 +10,12 @@ from tesseract_robotics.tesseract_motion_planners import PlannerRequest, PlannerResponse from tesseract_robotics.tesseract_motion_planners_simple import generateInterpolatedProgram -from tesseract_robotics.tesseract_motion_planners_ompl import OMPLDefaultPlanProfile, RRTConnectConfigurator, \ - OMPLProblemGeneratorFn, OMPLMotionPlanner, ProfileDictionary_addProfile_OMPLPlanProfile +from tesseract_robotics.tesseract_motion_planners_ompl import RRTConnectConfigurator, \ + OMPLMotionPlanner from tesseract_robotics.tesseract_time_parameterization import TimeOptimalTrajectoryGeneration, \ InstructionsTrajectory from tesseract_robotics.tesseract_motion_planners_trajopt import TrajOptDefaultPlanProfile, TrajOptDefaultCompositeProfile, \ - TrajOptProblemGeneratorFn, TrajOptMotionPlanner, ProfileDictionary_addProfile_TrajOptPlanProfile, \ - ProfileDictionary_addProfile_TrajOptCompositeProfile + TrajOptMotionPlanner import os import re @@ -118,13 +117,11 @@ profiles = ProfileDictionary() ProfileDictionary_addProfile_OMPLPlanProfile(profiles,OMPL_DEFAULT_NAMESPACE, "TEST_PROFILE", plan_profile) -cur_state = t_env.getState() # Create the planning request and run the planner request = PlannerRequest() request.instructions = program request.env = t_env -request.env_state = cur_state request.profiles = profiles ompl_planner = OMPLMotionPlanner(OMPL_DEFAULT_NAMESPACE) @@ -135,7 +132,7 @@ # The OMPL program does not generate dense waypoints. This function will interpolate the results to generate # a dense set of waypoints. -interpolated_results_instruction = generateInterpolatedProgram(results_instruction, cur_state, t_env, 3.14, 1.0, 3.14, 10) +interpolated_results_instruction = generateInterpolatedProgram(results_instruction, t_env, 3.14, 1.0, 3.14, 10) # Create the TrajOpt planner profile configurations. TrajOpt is used to optimize the random program generated # by OMPL @@ -153,7 +150,6 @@ trajopt_request = PlannerRequest() trajopt_request.instructions = interpolated_results_instruction trajopt_request.env = t_env -trajopt_request.env_state = cur_state trajopt_request.profiles = trajopt_profiles trajopt_response = trajopt_planner.solve(trajopt_request) @@ -167,9 +163,13 @@ # output program since the input is modified. time_parameterization = TimeOptimalTrajectoryGeneration() instructions_trajectory = InstructionsTrajectory(trajopt_results_instruction) -max_velocity = np.array([2.088, 2.082, 3.27, 3.6, 3.3, 3.078],dtype=np.float64) -max_acceleration = np.array([ 1, 1, 1, 1, 1, 1],dtype=np.float64) -assert time_parameterization.computeTimeStamps(instructions_trajectory, max_velocity, max_acceleration) +max_velocity = np.array([[2.088, 2.082, 3.27, 3.6, 3.3, 3.078]],dtype=np.float64) +max_velocity = np.hstack((-max_velocity.T, max_velocity.T)) +max_acceleration = np.array([[ 1, 1, 1, 1, 1, 1]],dtype=np.float64) +max_acceleration = np.hstack((-max_acceleration.T, max_acceleration.T)) +max_jerk = np.array([[ 1, 1, 1, 1, 1, 1]],dtype=np.float64) +max_jerk = np.hstack((-max_jerk.T, max_jerk.T)) +assert time_parameterization.compute(instructions_trajectory, max_velocity, max_acceleration, max_jerk) # Flatten the results into a single list of instructions trajopt_results = trajopt_results_instruction.flatten() diff --git a/tesseract_python/CMakeLists.txt b/tesseract_python/CMakeLists.txt index 9f664a734..d2e500112 100644 --- a/tesseract_python/CMakeLists.txt +++ b/tesseract_python/CMakeLists.txt @@ -40,7 +40,7 @@ find_package(tesseract_kinematics REQUIRED) # find_package(TinyXML2 REQUIRED) # find_package(opw_kinematics REQUIRED) -find_package(tesseract_task_composer REQUIRED) +find_package(tesseract_task_composer REQUIRED COMPONENTS core planning taskflow) if(NOT TinyXML2_INCLUDE_DIRS AND TARGET tinyxml2::tinyxml2) get_target_property(TinyXML2_INCLUDE_DIRS tinyxml2::tinyxml2 INTERFACE_INCLUDE_DIRECTORIES) @@ -49,6 +49,8 @@ endif() tesseract_variables() +find_bullet() + include_directories( ${trajopt_INCLUDE_DIRS} ${tesseract_common_INCLUDE_DIRS} @@ -87,19 +89,29 @@ message(STATUS "Building Python Version: " ${PYTHON_VERSION_MAJOR}) # Find NumPy find_path(NUMPY_INCLUDE_NDARRAYOBJECT_DIR numpy/ndarrayobject.h PATHS ${PYTHON_INCLUDE_DIRS} NO_DEFAULT_PATH) -if (NUMPY_INCLUDE_NDARRAYOBJECT_DIR) -set(NUMPY_INCLUDE_DIR ${NUMPY_INCLUDE_NDARRAYOBJECT_DIR}) +if(NUMPY_INCLUDE_NDARRAYOBJECT_DIR) + set(NUMPY_INCLUDE_DIR ${NUMPY_INCLUDE_NDARRAYOBJECT_DIR}) else() -execute_process(COMMAND ${PYTHON_EXECUTABLE} -c "import numpy; import os; print(os.path.dirname(numpy.__file__) + '/core/include')" RESULT_VARIABLE FIND_NUMPY_RESULT OUTPUT_VARIABLE NUMPY_INCLUDE_DIR ) -string(STRIP "${NUMPY_INCLUDE_DIR}" NUMPY_INCLUDE_DIR) + execute_process(COMMAND ${PYTHON_EXECUTABLE} -c "import numpy; import os; print(os.path.dirname(numpy.__file__))" + RESULT_VARIABLE FIND_NUMPY_RESULT OUTPUT_VARIABLE NUMPY_INCLUDE_DIR1) + string(STRIP "${NUMPY_INCLUDE_DIR1}" NUMPY_INCLUDE_DIR1) + + if(${FIND_NUMPY_RESULT}) + message(FATAL_ERROR "Could not determine NumPy include directory") + endif() + + if(EXISTS "${NUMPY_INCLUDE_DIR1}/core/include/numpy/ndarrayobject.h") + set(NUMPY_INCLUDE_DIR "${NUMPY_INCLUDE_DIR1}/core/include") + else() + if(EXISTS "${NUMPY_INCLUDE_DIR1}/_core/include/numpy/ndarrayobject.h") + set(NUMPY_INCLUDE_DIR "${NUMPY_INCLUDE_DIR1}/_core/include") + endif() + endif() -if(${FIND_NUMPY_RESULT}) -message(FATAL_ERROR "Could not determine NumPy include directory") -endif() endif() -if (NOT EXISTS "${NUMPY_INCLUDE_DIR}/numpy/ndarrayobject.h") -message(FATAL_ERROR "Could not find numpy/ndarrayobject.h include file") +if(NOT EXISTS "${NUMPY_INCLUDE_DIR}/numpy/ndarrayobject.h") + message(FATAL_ERROR "Could not find numpy/ndarrayobject.h include file") endif() message(STATUS "NumPy Include Directory: ${NUMPY_INCLUDE_DIR}") @@ -118,16 +130,55 @@ include(FindSWIG) find_package(SWIG 4.1.1 REQUIRED) include(UseSWIG) +macro(tesseract_python_find_target lib_name search_target) + # Find install directory of search_target + + get_target_property(search_target_location ${search_target} LOCATION) + if (NOT WIN32) + get_filename_component(search_target_location_dir ${search_target_location} DIRECTORY) + else() + get_filename_component(search_target_location_dir1 ${search_target_location} DIRECTORY) + get_filename_component(search_target_location_dir2 ${search_target_location_dir1} DIRECTORY) + set(search_target_location_dir "${search_target_location_dir2}/lib") + endif() + # Find library lib_name + unset(lib_name_location) + find_library(lib_name_location ${lib_name} HINTS ${search_target_location_dir} NO_CACHE) + if (NOT lib_name_location) + message(FATAL_ERROR "Could not find library ${lib_name}") + endif() + # Create import target + add_library(tesseract::${lib_name} SHARED IMPORTED) + if (WIN32) + set_target_properties(tesseract::${lib_name} PROPERTIES IMPORTED_IMPLIB_RELWITHDEBINFO ${lib_name_location}) + set_target_properties(tesseract::${lib_name} PROPERTIES IMPORTED_IMPLIB ${lib_name_location}) + set_target_properties(tesseract::${lib_name} PROPERTIES IMPORTED_LOCATION ${lib_name_location}) + else() + set_target_properties(tesseract::${lib_name} PROPERTIES IMPORTED_LOCATION ${lib_name_location}) + endif() +endmacro() + +tesseract_python_find_target(tesseract_collision_fcl_factories tesseract::tesseract_collision_core) +tesseract_python_find_target(tesseract_collision_bullet_factories tesseract::tesseract_collision_core) +tesseract_python_find_target(tesseract_kinematics_kdl_factories tesseract::tesseract_kinematics_core) +tesseract_python_find_target(tesseract_kinematics_ur_factory tesseract::tesseract_kinematics_core) +tesseract_python_find_target(tesseract_kinematics_opw_factory tesseract::tesseract_kinematics_core) +#tesseract_python_find_target(tesseract_kinematics_core_factories tesseract::tesseract_kinematics_core) +tesseract_python_find_target(tesseract_task_composer_factories tesseract::tesseract_task_composer) +tesseract_python_find_target(tesseract_task_composer_planning_factories tesseract::tesseract_task_composer) +# tesseract_python_find_target(tesseract_task_composer_planning_nodes tesseract::tesseract_task_composer) +tesseract_python_find_target(tesseract_task_composer_taskflow_factories tesseract::tesseract_task_composer) + include(${CMAKE_CURRENT_SOURCE_DIR}/cmake/tesseract_python_module.cmake) tesseract_python_module(tesseract_common_python SWIG_SRCS swig/tesseract_common_python.i PACKAGE tesseract_common LIBS tesseract::tesseract_common ) tesseract_python_module(tesseract_geometry_python SWIG_SRCS swig/tesseract_geometry_python.i PACKAGE tesseract_geometry LIBS tesseract::tesseract_geometry ) -tesseract_python_module(tesseract_collision_python SWIG_SRCS swig/tesseract_collision_python.i PACKAGE tesseract_collision LIBS tesseract::tesseract_collision_core tesseract::tesseract_collision_bullet_factories tesseract::tesseract_collision_fcl_factories ) +tesseract_python_module(tesseract_collision_python SWIG_SRCS swig/tesseract_collision_python.i PACKAGE tesseract_collision LIBS tesseract::tesseract_collision_core Bullet3::Bullet tesseract::tesseract_collision_bullet_factories tesseract::tesseract_collision_fcl_factories ) tesseract_python_module(tesseract_scene_graph_python SWIG_SRCS swig/tesseract_scene_graph_python.i PACKAGE tesseract_scene_graph LIBS tesseract::tesseract_scene_graph ) tesseract_python_module(tesseract_srdf_python SWIG_SRCS swig/tesseract_srdf_python.i PACKAGE tesseract_srdf LIBS tesseract::tesseract_srdf ) tesseract_python_module(tesseract_urdf_python SWIG_SRCS swig/tesseract_urdf_python.i PACKAGE tesseract_urdf LIBS tesseract::tesseract_urdf ) tesseract_python_module(tesseract_state_solver_python SWIG_SRCS swig/tesseract_state_solver_python.i PACKAGE tesseract_state_solver LIBS tesseract::tesseract_state_solver_core tesseract::tesseract_state_solver_ofkt tesseract::tesseract_state_solver_kdl) -tesseract_python_module(tesseract_kinematics_python SWIG_SRCS swig/tesseract_kinematics_python.i PACKAGE tesseract_kinematics LIBS tesseract::tesseract_kinematics_kdl_factories tesseract::tesseract_kinematics_ur_factory tesseract::tesseract_kinematics_opw_factory tesseract::tesseract_kinematics_core_factories tesseract::tesseract_kinematics_core ) +tesseract_python_module(tesseract_kinematics_python SWIG_SRCS swig/tesseract_kinematics_python.i PACKAGE tesseract_kinematics LIBS tesseract::tesseract_kinematics_core tesseract::tesseract_kinematics_kdl_factories tesseract::tesseract_kinematics_ur_factory tesseract::tesseract_kinematics_opw_factory tesseract::tesseract_kinematics_core_factories ) tesseract_python_module(tesseract_environment_python SWIG_SRCS swig/tesseract_environment_python.i PACKAGE tesseract_environment LIBS tesseract::tesseract_environment ) tesseract_python_module(tesseract_visualization_python SWIG_SRCS swig/tesseract_visualization_python.i PACKAGE tesseract_visualization LIBS tesseract::tesseract_visualization) tesseract_python_module(tesseract_command_language_python SWIG_SRCS swig/tesseract_command_language_python.i PACKAGE tesseract_command_language LIBS tesseract::tesseract_command_language) @@ -137,7 +188,7 @@ tesseract_python_module(tesseract_motion_planners_trajopt_python SWIG_SRCS swig/ tesseract_python_module(tesseract_motion_planners_ompl_python SWIG_SRCS swig/tesseract_motion_planners_ompl_python.i PACKAGE tesseract_motion_planners_ompl LIBS tesseract::tesseract_motion_planners_ompl ) tesseract_python_module(tesseract_motion_planners_descartes_python SWIG_SRCS swig/tesseract_motion_planners_descartes_python.i PACKAGE tesseract_motion_planners_descartes LIBS tesseract::tesseract_motion_planners_descartes ) tesseract_python_module(tesseract_time_parameterization_python SWIG_SRCS swig/tesseract_time_parameterization_python.i PACKAGE tesseract_time_parameterization LIBS tesseract::tesseract_time_parameterization_core tesseract::tesseract_time_parameterization_isp tesseract::tesseract_time_parameterization_totg tesseract::tesseract_time_parameterization_ruckig) -tesseract_python_module(tesseract_task_composer_python SWIG_SRCS swig/tesseract_task_composer_python.i PACKAGE tesseract_task_composer LIBS tesseract::tesseract_task_composer tesseract::tesseract_task_composer_nodes tesseract::tesseract_task_composer_taskflow tesseract::tesseract_task_composer_factories tesseract::tesseract_task_composer_planning_factories tesseract::tesseract_task_composer_planning tesseract::tesseract_task_composer_taskflow_factories ) +tesseract_python_module(tesseract_task_composer_python SWIG_SRCS swig/tesseract_task_composer_python.i PACKAGE tesseract_task_composer LIBS tesseract::tesseract_task_composer tesseract::tesseract_task_composer_nodes tesseract::tesseract_task_composer_taskflow tesseract::tesseract_collision_core tesseract::tesseract_task_composer_factories tesseract::tesseract_task_composer_planning_factories tesseract::tesseract_task_composer_planning_nodes tesseract::tesseract_task_composer_taskflow_factories ) set(TESSERACT_PYTHON_MODULE_TARGETS _tesseract_common_python diff --git a/tesseract_python/package.xml b/tesseract_python/package.xml index a7de48773..f56f55fad 100644 --- a/tesseract_python/package.xml +++ b/tesseract_python/package.xml @@ -1,7 +1,7 @@ tesseract_python - 0.4.0 + 0.5.0 The tesseract_python package John Wason Apache 2.0 diff --git a/tesseract_python/swig/boost_filesystem_path.i b/tesseract_python/swig/boost_filesystem_path.i index cb4257af2..ed8a95977 100644 --- a/tesseract_python/swig/boost_filesystem_path.i +++ b/tesseract_python/swig/boost_filesystem_path.i @@ -61,4 +61,6 @@ def __str__(self): %} }; } -} \ No newline at end of file +} + +%template(VectorFilesystemPath) std::vector; diff --git a/tesseract_python/swig/rework_include/tesseract_command_language/composite_instruction.i b/tesseract_python/swig/rework_include/tesseract_command_language/composite_instruction.i index 7da0598a3..1f248fc3a 100644 --- a/tesseract_python/swig/rework_include/tesseract_command_language/composite_instruction.i +++ b/tesseract_python/swig/rework_include/tesseract_command_language/composite_instruction.i @@ -15,8 +15,8 @@ class CompositeInstruction public: CompositeInstruction(std::string profile = DEFAULT_PROFILE_KEY, - CompositeInstructionOrder order = CompositeInstructionOrder::ORDERED, - ManipulatorInfo manipulator_info = ManipulatorInfo()); + tesseract_common::ManipulatorInfo manipulator_info = tesseract_common::ManipulatorInfo(), + CompositeInstructionOrder order = CompositeInstructionOrder::ORDERED); CompositeInstructionOrder getOrder() const; @@ -33,12 +33,12 @@ public: const std::string& getProfile() const; /** @brief Dictionary of profiles that will override named profiles for a specific task*/ - void setProfileOverrides(ProfileDictionary::ConstPtr profile_overrides); - ProfileDictionary::ConstPtr getProfileOverrides() const; + void setProfileOverrides(ProfileOverrides profile_overrides); + ProfileOverrides getProfileOverrides() const; - void setManipulatorInfo(ManipulatorInfo info); - const ManipulatorInfo& getManipulatorInfo() const; - ManipulatorInfo& getManipulatorInfo(); + void setManipulatorInfo(tesseract_common::ManipulatorInfo info); + const tesseract_common::ManipulatorInfo& getManipulatorInfo() const; + tesseract_common::ManipulatorInfo& getManipulatorInfo(); void setInstructions(std::vector instructions); std::vector& getInstructions(); diff --git a/tesseract_python/swig/rework_include/tesseract_command_language/poly/move_instruction_poly.i b/tesseract_python/swig/rework_include/tesseract_command_language/poly/move_instruction_poly.i index fd57479c6..0a1449fd4 100644 --- a/tesseract_python/swig/rework_include/tesseract_command_language/poly/move_instruction_poly.i +++ b/tesseract_python/swig/rework_include/tesseract_command_language/poly/move_instruction_poly.i @@ -40,11 +40,11 @@ struct MoveInstructionPoly void setPathProfile(const std::string& profile); const std::string& getPathProfile() const; - void setProfileOverrides(ProfileDictionary::ConstPtr profile_overrides); - ProfileDictionary::ConstPtr getProfileOverrides() const; + void setProfileOverrides(ProfileOverrides profile_overrides); + const ProfileOverrides getProfileOverrides() const; - void setPathProfileOverrides(ProfileDictionary::ConstPtr profile_overrides); - ProfileDictionary::ConstPtr getPathProfileOverrides() const; + void setPathProfileOverrides(ProfileOverrides profile_overrides); + const ProfileOverrides getPathProfileOverrides() const; void setMoveType(MoveInstructionType move_type); MoveInstructionType getMoveType() const; diff --git a/tesseract_python/swig/tesseract_collision_python.i b/tesseract_python/swig/tesseract_collision_python.i index 9c348aa64..5f468fddb 100644 --- a/tesseract_python/swig/tesseract_collision_python.i +++ b/tesseract_python/swig/tesseract_collision_python.i @@ -93,12 +93,15 @@ class ContactResult; // %ignore ContactResultMap; // tesseract_collision #define TESSERACT_COLLISION_CORE_PUBLIC +%ignore tesseract_collision::ContactTrajectoryResults::trajectoryCollisionResultsTable; +%ignore tesseract_collision::ContactTrajectoryResults::collisionFrequencyPerLink; %include "tesseract_collision/core/types.h" %include "tesseract_collision/core/discrete_contact_manager.h" %include "tesseract_collision/core/continuous_contact_manager.h" %include "tesseract_collision/core/contact_managers_plugin_factory.h" %init %{ +// TODO: fix anchors tesseract_common::PluginLoader::addSymbolLibraryToSearchLibrariesEnv(tesseract_collision::tesseract_collision_bullet::BulletFactoriesAnchor(), "TESSERACT_CONTACT_MANAGERS_PLUGINS"); tesseract_common::PluginLoader::addSymbolLibraryToSearchLibrariesEnv(tesseract_collision::tesseract_collision_fcl::FCLFactoriesAnchor(), "TESSERACT_CONTACT_MANAGERS_PLUGINS"); %} diff --git a/tesseract_python/swig/tesseract_command_language_python.i b/tesseract_python/swig/tesseract_command_language_python.i index 345cd1a65..086a33a0a 100644 --- a/tesseract_python/swig/tesseract_command_language_python.i +++ b/tesseract_python/swig/tesseract_command_language_python.i @@ -36,6 +36,11 @@ %{ +#include + +// tesseract_common +#include + // tesseract_command_language #include #include @@ -57,7 +62,7 @@ #include #include #include -#include +#include #include @@ -65,7 +70,6 @@ #include #include "tesseract_command_language_python_std_functions.h" -#include "tesseract_command_language_python_profile_dictionary_functions.h" #include %} @@ -77,6 +81,10 @@ %include "tesseract_type_erasure_macros.i" +#define TESSERACT_CARTESIAN_WAYPOINT_EXPORT_KEY(a,b) +#define TESSERACT_STATE_WAYPOINT_EXPORT_KEY(a,b) +#define TESSERACT_INSTRUCTION_EXPORT_KEY(a,b) + %define %tesseract_erasure_ctor_planning(source_class_type,dest_class_type) %tesseract_erasure_ctor(source_class_type,tesseract_planning,dest_class_type,tesseract_planning); %enddef @@ -132,11 +140,16 @@ const tesseract_planning::TYPE as_const_ ## TYPE() {return $self->asas; -%template(ProfileDictionary_removeProfileEntry_##TYPE) tesseract_planning::ProfileDictionary_removeProfileEntry; -%template(ProfileDictionary_getProfileEntry_##TYPE) tesseract_planning::ProfileDictionary_getProfileEntry; -%template(ProfileDictionary_addProfile_##TYPE) tesseract_planning::ProfileDictionary_addProfile; -%template(ProfileDictionary_getProfile_##TYPE) tesseract_planning::ProfileDictionary_getProfile; -%template(ProfileDictionary_hasProfile_##TYPE) tesseract_planning::ProfileDictionary_hasProfile; -%template(ProfileDictionary_removeProfile_##TYPE) tesseract_planning::ProfileDictionary_removeProfile; +// %template(ProfileDictionary_hasProfileEntry_##TYPE) tesseract_planning::ProfileDictionary_hasProfileEntry; +// %template(ProfileDictionary_removeProfileEntry_##TYPE) tesseract_planning::ProfileDictionary_removeProfileEntry; +// %template(ProfileDictionary_getProfileEntry_##TYPE) tesseract_planning::ProfileDictionary_getProfileEntry; +// %template(ProfileDictionary_addProfile_##TYPE) tesseract_planning::ProfileDictionary_addProfile; +// %template(ProfileDictionary_getProfile_##TYPE) tesseract_planning::ProfileDictionary_getProfile; +// %template(ProfileDictionary_hasProfile_##TYPE) tesseract_planning::ProfileDictionary_hasProfile; +// %template(ProfileDictionary_removeProfile_##TYPE) tesseract_planning::ProfileDictionary_removeProfile; %enddef %define %tesseract_command_language_add_profile_type2(NAME, TYPE ) -%template(ProfileDictionary_hasProfileEntry_##NAME) tesseract_planning::ProfileDictionary_hasProfileEntry; -%template(ProfileDictionary_removeProfileEntry_##NAME) tesseract_planning::ProfileDictionary_removeProfileEntry; -%template(ProfileDictionary_getProfileEntry_##NAME) tesseract_planning::ProfileDictionary_getProfileEntry; -%template(ProfileDictionary_addProfile_##NAME) tesseract_planning::ProfileDictionary_addProfile; -%template(ProfileDictionary_getProfile_##NAME) tesseract_planning::ProfileDictionary_getProfile; -%template(ProfileDictionary_hasProfile_##NAME) tesseract_planning::ProfileDictionary_hasProfile; -%template(ProfileDictionary_removeProfile_##NAME) tesseract_planning::ProfileDictionary_removeProfile; +// %template(ProfileDictionary_hasProfileEntry_##NAME) tesseract_planning::ProfileDictionary_hasProfileEntry; +// %template(ProfileDictionary_removeProfileEntry_##NAME) tesseract_planning::ProfileDictionary_removeProfileEntry; +// %template(ProfileDictionary_getProfileEntry_##NAME) tesseract_planning::ProfileDictionary_getProfileEntry; +// %template(ProfileDictionary_addProfile_##NAME) tesseract_planning::ProfileDictionary_addProfile; +// %template(ProfileDictionary_getProfile_##NAME) tesseract_planning::ProfileDictionary_getProfile; +// %template(ProfileDictionary_hasProfile_##NAME) tesseract_planning::ProfileDictionary_hasProfile; +// %template(ProfileDictionary_removeProfile_##NAME) tesseract_planning::ProfileDictionary_removeProfile; %enddef -%include "tesseract_command_language/constants.h" \ No newline at end of file +%include "tesseract_command_language/constants.h" + +%tesseract_any_poly_type_shared_ptr(ProfileDictionary,tesseract_planning); diff --git a/tesseract_python/swig/tesseract_command_language_python_profile_dictionary_functions.h b/tesseract_python/swig/tesseract_command_language_python_profile_dictionary_functions.h deleted file mode 100644 index 31a04fff3..000000000 --- a/tesseract_python/swig/tesseract_command_language_python_profile_dictionary_functions.h +++ /dev/null @@ -1,71 +0,0 @@ -/** - * @file tesseract_command_language_python_profile_dictionary_functions.h - * @brief Utility functions for template types in tesseract_planning::ProfileDictionary - * - * @author John Wason - * @date March 21, 2021 - * @version TODO - * @bug No known bugs - * - * @copyright Copyright (c) 2021, Wason Technology, LLC - * - * @par License - * Software License Agreement (Apache License) - * @par - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * http://www.apache.org/licenses/LICENSE-2.0 - * @par - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -namespace tesseract_planning -{ - template - bool ProfileDictionary_hasProfileEntry(std::shared_ptr profile_dictionary, const std::string& ns) - { - return profile_dictionary->hasProfileEntry(ns); - } - - template - void ProfileDictionary_removeProfileEntry(std::shared_ptr profile_dictionary, const std::string& ns) - { - profile_dictionary->removeProfileEntry(ns); - } - - template - std::unordered_map> ProfileDictionary_getProfileEntry(std::shared_ptr profile_dictionary, const std::string& ns) - { - return profile_dictionary->getProfileEntry(ns); - } - - template - void ProfileDictionary_addProfile(std::shared_ptr profile_dictionary, const std::string& ns, const std::string& profile_name, std::shared_ptr profile) - { - profile_dictionary->addProfile(ns, profile_name, profile); - } - - template - bool ProfileDictionary_hasProfile(std::shared_ptr profile_dictionary, const std::string& ns, const std::string& profile_name) - { - return profile_dictionary->hasProfile(ns, profile_name); - } - - template - std::shared_ptr ProfileDictionary_getProfile(std::shared_ptr profile_dictionary, const std::string& ns, const std::string& profile_name) - { - return profile_dictionary->getProfile(ns, profile_name); - } - - template - void ProfileDictionary_removeProfile(std::shared_ptr profile_dictionary, const std::string& ns, const std::string& profile_name) - { - profile_dictionary->removeProfile(ns, profile_name); - } - -} \ No newline at end of file diff --git a/tesseract_python/swig/tesseract_common_python.i b/tesseract_python/swig/tesseract_common_python.i index 7fa8b2465..c51f76186 100644 --- a/tesseract_python/swig/tesseract_common_python.i +++ b/tesseract_python/swig/tesseract_common_python.i @@ -49,6 +49,7 @@ #include #include #include +#include #include "tesseract_common_python_std_functions.h" @@ -105,6 +106,8 @@ %template(name) std::unordered_map,std::equal_to,Eigen::aligned_allocator>>; %enddef +using VectorVector3d = std::vector; + %tesseract_aligned_vector(VectorIsometry3d, Eigen::Isometry3d); %template(VectorVector3d) std::vector; %template(VectorVectorXd) std::vector; @@ -166,6 +169,8 @@ namespace std #define BOOST_SERIALIZATION_ASSUME_ABSTRACT(a) #define BOOST_CONCEPT_ASSERT(a) +#define TESSERACT_ANY_EXPORT_KEY(a,b) + #define EIGEN_MAKE_ALIGNED_OPERATOR_NEW #define TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #define TESSERACT_COMMON_IGNORE_WARNINGS_POP @@ -204,6 +209,7 @@ namespace std %include "tesseract_common/allowed_collision_matrix.h" %include "tesseract_common/kinematic_limits.h" %include "tesseract_common/timer.h" +%include "tesseract_common/filesystem.h" // TODO: ? @@ -250,8 +256,8 @@ namespace std %} } -%template(isWithinPositionLimits) tesseract_common::isWithinPositionLimits; -%template(satisfiesPositionLimits) tesseract_common::satisfiesPositionLimits; +%template(isWithinLimits) tesseract_common::isWithinLimits; +%template(satisfiesLimits) tesseract_common::satisfiesLimits; //%template(enforcePositionLimits) tesseract_common::enforcePositionLimits; %ignore tesseract_common::TypeErasureInterface::clone; diff --git a/tesseract_python/swig/tesseract_environment_python.i b/tesseract_python/swig/tesseract_environment_python.i index 4fd38a3cf..d060ee7de 100644 --- a/tesseract_python/swig/tesseract_environment_python.i +++ b/tesseract_python/swig/tesseract_environment_python.i @@ -54,6 +54,15 @@ #include #include +// tesseract_kinematics +#include +#include + +// tesseract_collision +#include +#include +#include + // tesseract_environment #include #include @@ -68,8 +77,8 @@ %unique_ptr_value_wrapper(tesseract_collision::ContinuousContactManager); %unique_ptr_value_wrapper(tesseract_collision::DiscreteContactManager); %unique_ptr_value_wrapper(tesseract_scene_graph::StateSolver); -%unique_ptr_value_wrapper(tesseract_kinematics::KinematicGroup) -%unique_ptr_value_wrapper(tesseract_kinematics::JointGroup) +// %unique_ptr_value_wrapper(tesseract_kinematics::KinematicGroup) +// %unique_ptr_value_wrapper(tesseract_kinematics::JointGroup) %tesseract_std_function(FindTCPOffsetCallbackFn,tesseract,Eigen::Isometry3d,const tesseract_common::ManipulatorInfo&,a); %tesseract_std_function(EventCallbackFn,tesseract_environment,void,const tesseract_environment::Event&,a); @@ -162,6 +171,8 @@ %shared_ptr(tesseract_environment::Environment) %wrap_unique_ptr(EnvironmentUPtr,tesseract_environment::Environment) +%ignore tesseract_environment::Environment::Environment(std::unique_ptr impl); + %include "tesseract_environment/environment.h" %inline { @@ -180,4 +191,6 @@ { return dynamic_cast(a); } -} \ No newline at end of file +} + +%tesseract_any_poly_type_shared_ptr(Environment, tesseract_environment) diff --git a/tesseract_python/swig/tesseract_geometry_python.i b/tesseract_python/swig/tesseract_geometry_python.i index a8bed139d..4c5876971 100644 --- a/tesseract_python/swig/tesseract_geometry_python.i +++ b/tesseract_python/swig/tesseract_geometry_python.i @@ -145,7 +145,8 @@ $1 = is_array($input); tesseract_geometry::PolygonMesh, tesseract_geometry::Mesh, tesseract_geometry::SDFMesh, - tesseract_geometry::Sphere + tesseract_geometry::Sphere, + tesseract_geometry::CompoundMesh ) @@ -194,6 +195,9 @@ class OcTree {}; %shared_ptr(tesseract_geometry::Sphere) %include +%shared_ptr(tesseract_geometry::CompoundMesh) +%include + %include "tesseract_geometry/geometries.h" %include "tesseract_geometry/utils.h" diff --git a/tesseract_python/swig/tesseract_kinematics_python.i b/tesseract_python/swig/tesseract_kinematics_python.i index 927b735df..ad0a7e381 100644 --- a/tesseract_python/swig/tesseract_kinematics_python.i +++ b/tesseract_python/swig/tesseract_kinematics_python.i @@ -131,6 +131,7 @@ struct KinGroupIKInput; %init %{ +// TODO: fix anchors tesseract_common::PluginLoader::addSymbolLibraryToSearchLibrariesEnv(tesseract_kinematics::REPInvKinFactoriesAnchor(), "TESSERACT_KINEMATICS_PLUGINS"); tesseract_common::PluginLoader::addSymbolLibraryToSearchLibrariesEnv(tesseract_kinematics::ROPInvKinFactoriesAnchor(), "TESSERACT_KINEMATICS_PLUGINS"); tesseract_common::PluginLoader::addSymbolLibraryToSearchLibrariesEnv(tesseract_kinematics::KDLFactoriesAnchor(), "TESSERACT_KINEMATICS_PLUGINS"); diff --git a/tesseract_python/swig/tesseract_motion_planners_descartes_python.i b/tesseract_python/swig/tesseract_motion_planners_descartes_python.i index 90302c7b4..a214649a5 100644 --- a/tesseract_python/swig/tesseract_motion_planners_descartes_python.i +++ b/tesseract_python/swig/tesseract_motion_planners_descartes_python.i @@ -39,12 +39,9 @@ %{ // tesseract_motion_planner_descartes -#include #include #include #include -#include -#include #include @@ -57,8 +54,29 @@ #include #include +// tesseract_kinematics +#include +#include + +// tesseract_environment +#include +#include +#include + +// tesseract_command_language +#include +#include +#include +#include + +// tesseract_motion_planners +#include +#include + +// tesseract_visualization +#include + #include "tesseract_command_language_python_std_functions.h" -#include "tesseract_command_language_python_profile_dictionary_functions.h" #include "tesseract_environment_python_std_functions.h" %} @@ -66,14 +84,15 @@ // tesseract_motion_planner_descartes #define TESSERACT_MOTION_PLANNERS_DESCARTES_PUBLIC +%ignore createWaypointSampler; +%ignore createEdgeEvaluator; +%ignore createStateEvaluator; + %include "tesseract_motion_planners/descartes/descartes_utils.h" %tesseract_std_function_base(PoseSamplerFn,tesseract_planning,tesseract_common::VectorIsometry3d,const Eigen::Isometry3d&,a); %tesseract_std_function(PoseSamplerFn,tesseract_planning,tesseract_common::VectorIsometry3d,const Eigen::Isometry3d&,a); -%shared_ptr(tesseract_planning::DescartesProblem) -%shared_ptr(DescartesProblemD) -%include "tesseract_motion_planners/descartes/descartes_problem.h" -%template(DescartesProblemD) tesseract_planning::DescartesProblem; +%pythondynamic tesseract_planning::DescartesPlanProfileD; %shared_ptr(tesseract_planning::DescartesPlanProfile) %include "tesseract_motion_planners/descartes/profile/descartes_profile.h" %template(DescartesPlanProfileD) std::shared_ptr >; @@ -81,6 +100,7 @@ namespace tesseract_planning {using DescartesPlanProfileMapD = std::unordered_map>>;} %tesseract_command_language_add_profile_type2(DescartesPlanProfileD,DescartesPlanProfile); +%pythondynamic tesseract_planning::DescartesDefaultPlanProfileD; %shared_ptr(tesseract_planning::DescartesDefaultPlanProfile) %ignore tesseract_planning::DescartesDefaultPlanProfile::edge_evaluator; %ignore tesseract_planning::DescartesDefaultPlanProfile::is_valid; @@ -91,11 +111,12 @@ namespace tesseract_planning {using DescartesPlanProfileMapD = std::unordered_ma %pythondynamic tesseract_planning::DescartesMotionPlanner; %shared_ptr(tesseract_planning::DescartesMotionPlannerD); %shared_ptr(tesseract_planning::DescartesMotionPlanner); +%ignore tesseract_planning::DescartesMotionPlanner::clone; %include "tesseract_motion_planners/descartes/descartes_motion_planner.h" %template(DescartesMotionPlannerD) tesseract_planning::DescartesMotionPlanner; %inline { - std::shared_ptr> cast_DescartesPlanProfileD( + std::shared_ptr cast_DescartesPlanProfileD( const std::shared_ptr>& a ) { diff --git a/tesseract_python/swig/tesseract_motion_planners_ompl_python.i b/tesseract_python/swig/tesseract_motion_planners_ompl_python.i index 5458bed17..90e010179 100644 --- a/tesseract_python/swig/tesseract_motion_planners_ompl_python.i +++ b/tesseract_python/swig/tesseract_motion_planners_ompl_python.i @@ -36,13 +36,31 @@ %{ // tesseract_motion_planners_ompl #include -#include #include -#include +#include #include -#include -#include +// tesseract_kinematics +#include +#include + +// tesseract_environment +#include +#include +#include + +// tesseract_command_language +#include +#include +#include +#include + +// tesseract_motion_planners +#include +#include + +// tesseract_visualization +#include #include #include @@ -55,7 +73,6 @@ #include #include "tesseract_command_language_python_std_functions.h" -#include "tesseract_command_language_python_profile_dictionary_functions.h" #include "tesseract_environment_python_std_functions.h" %} @@ -63,6 +80,12 @@ // tesseract_motion_planners_ompl #define TESSERACT_MOTION_PLANNERS_OMPL_PUBLIC +%ignore createSolverConfig; +%ignore createSimpleSetup; +%ignore omplPlanFromXMLString; + +%include "tesseract_motion_planners/ompl/types.h" + %shared_ptr(tesseract_planning::OMPLPlannerConfigurator) %shared_ptr(tesseract_planning::SBLConfigurator) %shared_ptr(tesseract_planning::ESTConfigurator) @@ -99,25 +122,20 @@ tesseract_planning::SPARSConfigurator ) -%shared_ptr(tesseract_planning::OMPLProblem) -%ignore tesseract_planning::OMPLProblem::extractor; -%include "tesseract_motion_planners/ompl/ompl_problem.h" -%template(OMPLProblems) std::vector>; - +%pythondynamic tesseract_planning::OMPLPlanProfile; %shared_ptr(tesseract_planning::OMPLPlanProfile) %include "tesseract_motion_planners/ompl/profile/ompl_profile.h" -%template(OMPLPlanProfileMap) std::unordered_map>; +// %template(OMPLPlanProfileMap) std::unordered_map>; %tesseract_command_language_add_profile_type(OMPLPlanProfile); -%shared_ptr(tesseract_planning::OMPLDefaultPlanProfile) -%ignore tesseract_planning::OMPLDefaultPlanProfile::allocWeightedRealVectorStateSampler; -%include "tesseract_motion_planners/ompl/profile/ompl_default_plan_profile.h" +%pythondynamic tesseract_planning::OMPLRealVectorPlanProfile; +%shared_ptr(tesseract_planning::OMPLRealVectorPlanProfile) +%include "tesseract_motion_planners/ompl/profile/ompl_real_vector_plan_profile.h" -%tesseract_std_function_base(OMPLProblemGeneratorFn,tesseract_planning,std::vector>,const std::string&,a,const tesseract_planning::PlannerRequest&,b,const tesseract_planning::OMPLPlanProfileMap&,c); -%tesseract_std_function(OMPLProblemGeneratorFn,tesseract_planning,std::vector>,const std::string&,a,const tesseract_planning::PlannerRequest&,b,const tesseract_planning::OMPLPlanProfileMap&,c); +// %tesseract_std_function_base(OMPLProblemGeneratorFn,tesseract_planning,std::vector>,const std::string&,a,const tesseract_planning::PlannerRequest&,b,const tesseract_planning::OMPLPlanProfileMap&,c); +// %tesseract_std_function(OMPLProblemGeneratorFn,tesseract_planning,std::vector>,const std::string&,a,const tesseract_planning::PlannerRequest&,b,const tesseract_planning::OMPLPlanProfileMap&,c); %pythondynamic tesseract_planning::OMPLMotionPlanner; %shared_ptr(tesseract_planning::OMPLMotionPlanner) +%ignore tesseract_planning::OMPLMotionPlanner::clone; %include "tesseract_motion_planners/ompl/ompl_motion_planner.h" -%include "tesseract_motion_planners/ompl/serialize.h" -%include "tesseract_motion_planners/ompl/deserialize.h" diff --git a/tesseract_python/swig/tesseract_motion_planners_python.i b/tesseract_python/swig/tesseract_motion_planners_python.i index c7d6e567a..871b53d77 100644 --- a/tesseract_python/swig/tesseract_motion_planners_python.i +++ b/tesseract_python/swig/tesseract_motion_planners_python.i @@ -50,6 +50,19 @@ #include #include +// tesseract_kinematics +#include +#include + +// tesseract_environment +#include +#include +#include + +// tesseract_command_language +#include +#include + #include "tesseract_environment_python_std_functions.h" %} @@ -60,10 +73,10 @@ // tesseract_motion_planners #define TESSERACT_MOTION_PLANNERS_CORE_PUBLIC -%wrap_unique_ptr(PlannerProfileRemappingUPtr,tesseract_planning::PlannerProfileRemapping) %include "tesseract_motion_planners/core/types.h" %shared_ptr(tesseract_planning::MotionPlanner) +%wrap_unique_ptr(MotionPlannerUPtr,tesseract_planning::MotionPlanner) %include "tesseract_motion_planners/core/planner.h" %include "tesseract_motion_planners/core/utils.h" diff --git a/tesseract_python/swig/tesseract_motion_planners_simple_python.i b/tesseract_python/swig/tesseract_motion_planners_simple_python.i index 85e2ec721..20c79786e 100644 --- a/tesseract_python/swig/tesseract_motion_planners_simple_python.i +++ b/tesseract_python/swig/tesseract_motion_planners_simple_python.i @@ -54,9 +54,27 @@ #include #include +// tesseract_kinematics +#include +#include + +// tesseract_environment +#include +#include +#include + +// tesseract_command_language +#include +#include +#include +#include + +// tesseract_motion_planners +#include +#include + #include "tesseract_environment_python_std_functions.h" -#include "tesseract_command_language_python_profile_dictionary_functions.h" %} // tesseract_motion_planners_simple @@ -65,26 +83,33 @@ %ignore tesseract_planning::JointGroupInstructionInfo::getWorkingFrame; %include "tesseract_motion_planners/simple/interpolation.h" +%pythondynamic tesseract_planning::SimplePlannerPlanProfile; +%pythondynamic tesseract_planning::SimplePlannerCompositeProfile; %shared_ptr(tesseract_planning::SimplePlannerPlanProfile) -%shared_ptr(tesseract_planning::SimplePlannerPlanCompositeProfile) +%shared_ptr(tesseract_planning::SimplePlannerCompositeProfile) %include "tesseract_motion_planners/simple/profile/simple_planner_profile.h" %template(SimplePlannerPlanProfileMap) std::unordered_map; %template(SimplePlannerCompositeProfileMap) std::unordered_map; %tesseract_command_language_add_profile_type(SimplePlannerPlanProfile); %tesseract_command_language_add_profile_type(SimplePlannerCompositeProfile); +%pythondynamic tesseract_planning::SimplePlannerLVSPlanProfile; %shared_ptr(tesseract_planning::SimplePlannerLVSPlanProfile) %include "tesseract_motion_planners/simple/profile/simple_planner_lvs_plan_profile.h" +%pythondynamic tesseract_planning::SimplePlannerLVSNoIKPlanProfile; %shared_ptr(tesseract_planning::SimplePlannerLVSNoIKPlanProfile) %include "tesseract_motion_planners/simple/profile/simple_planner_lvs_no_ik_plan_profile.h" +%pythondynamic tesseract_planning::SimplePlannerFixedSizePlanProfile; %shared_ptr(tesseract_planning::SimplePlannerFixedSizePlanProfile) %include "tesseract_motion_planners/simple/profile/simple_planner_fixed_size_plan_profile.h" +%pythondynamic tesseract_planning::SimplePlannerFixedSizeAssignPlanProfile; %shared_ptr(tesseract_planning::SimplePlannerFixedSizeAssignPlanProfile) %include "tesseract_motion_planners/simple/profile/simple_planner_fixed_size_assign_plan_profile.h" %pythondynamic tesseract_planning::SimpleMotionPlanner; %shared_ptr(tesseract_planning::SimpleMotionPlanner) +%ignore tesseract_planning::SimpleMotionPlanner::clone; %include "tesseract_motion_planners/simple/simple_motion_planner.h" diff --git a/tesseract_python/swig/tesseract_motion_planners_trajopt_python.i b/tesseract_python/swig/tesseract_motion_planners_trajopt_python.i index af1f2348f..a077b4c60 100644 --- a/tesseract_python/swig/tesseract_motion_planners_trajopt_python.i +++ b/tesseract_python/swig/tesseract_motion_planners_trajopt_python.i @@ -41,12 +41,10 @@ #include #include #include +#include #include #include #include -#include -#include -#include #include @@ -60,8 +58,29 @@ #include #include +// tesseract_kinematics +#include +#include + +// tesseract_environment +#include +#include +#include + +// tesseract_command_language +#include +#include +#include +#include + +// tesseract_motion_planners +#include +#include + +// tesseract_visualization +#include + #include "tesseract_command_language_python_std_functions.h" -#include "tesseract_command_language_python_profile_dictionary_functions.h" #include "tesseract_environment_python_std_functions.h" @@ -69,34 +88,41 @@ // trajopt +%ignore createSolverConfig; +%ignore createOptimizationParameters; +%ignore createOptimizationCallbacks; + // Including trajopt headers is too noisy, use *.i file instead %include "trajopt/problem_description.i" -%tesseract_std_function_base(TrajOptProblemGeneratorFn,tesseract_planning,std::shared_ptr,const std::string&,a,const tesseract_planning::PlannerRequest&,b,const tesseract_planning::TrajOptPlanProfileMap&,c,const tesseract_planning::TrajOptCompositeProfileMap&,d,const tesseract_planning::TrajOptSolverProfileMap&,e); -%tesseract_std_function(TrajOptProblemGeneratorFn,tesseract_planning,std::shared_ptr,const std::string&,a,const tesseract_planning::PlannerRequest&,b,const tesseract_planning::TrajOptPlanProfileMap&,c,const tesseract_planning::TrajOptCompositeProfileMap&,d,const tesseract_planning::TrajOptSolverProfileMap&,e); +// %tesseract_std_function_base(TrajOptProblemGeneratorFn,tesseract_planning,std::shared_ptr,const std::string&,a,const tesseract_planning::PlannerRequest&,b,const tesseract_planning::TrajOptPlanProfileMap&,c,const tesseract_planning::TrajOptCompositeProfileMap&,d,const tesseract_planning::TrajOptSolverProfileMap&,e); +// %tesseract_std_function(TrajOptProblemGeneratorFn,tesseract_planning,std::shared_ptr,const std::string&,a,const tesseract_planning::PlannerRequest&,b,const tesseract_planning::TrajOptPlanProfileMap&,c,const tesseract_planning::TrajOptCompositeProfileMap&,d,const tesseract_planning::TrajOptSolverProfileMap&,e); // tesseract_motion_planners_trajopt #define TESSERACT_MOTION_PLANNERS_TRAJOPT_PUBLIC %include "tesseract_motion_planners/trajopt/trajopt_collision_config.h" +%include "tesseract_motion_planners/trajopt/trajopt_waypoint_config.h" +%pythondynamic tesseract_planning::TrajOptPlanProfile; +%pythondynamic tesseract_planning::TrajOptCompositeProfile; +%pythondynamic tesseract_planning::TrajOptSolverProfile; %shared_ptr(tesseract_planning::TrajOptPlanProfile) %shared_ptr(tesseract_planning::TrajOptSolverProfile) %shared_ptr(tesseract_planning::TrajOptCompositeProfile) %include "tesseract_motion_planners/trajopt/profile/trajopt_profile.h" -%template(TrajOptSolverProfileMap) std::unordered_map>; -%template(TrajOptCompositeProfileMap) std::unordered_map>; -%template(TrajOptPlanProfileMap) std::unordered_map>; +// %template(TrajOptSolverProfileMap) std::unordered_map>; +// %template(TrajOptCompositeProfileMap) std::unordered_map>; +// %template(TrajOptPlanProfileMap) std::unordered_map>; %tesseract_command_language_add_profile_type(TrajOptSolverProfile); %tesseract_command_language_add_profile_type(TrajOptPlanProfile); %tesseract_command_language_add_profile_type(TrajOptCompositeProfile); +%pythondynamic tesseract_planning::TrajOptDefaultPlanProfile; %shared_ptr(tesseract_planning::TrajOptDefaultPlanProfile) %include "tesseract_motion_planners/trajopt/profile/trajopt_default_plan_profile.h" -%shared_ptr(tesseract_planning::TrajOptDefaultSolverProfile) -%include "tesseract_motion_planners/trajopt/profile/trajopt_default_solver_profile.h" - +%pythondynamic tesseract_planning::TrajOptDefaultCompositeProfile; %shared_ptr(tesseract_planning::TrajOptDefaultCompositeProfile) %include "tesseract_motion_planners/trajopt/profile/trajopt_default_composite_profile.h" @@ -106,9 +132,8 @@ %pythondynamic tesseract_planning::TrajOptMotionPlanner; %shared_ptr(tesseract_planning::TrajOptMotionPlanner) +%ignore tesseract_planning::TrajOptMotionPlanner::clone; %include "tesseract_motion_planners/trajopt/trajopt_motion_planner.h" -%include "tesseract_motion_planners/trajopt/serialize.h" -%include "tesseract_motion_planners/trajopt/deserialize.h" // TODO // %template(TrajOptIfoptCompositeProfileMap) std::unordered_map>; diff --git a/tesseract_python/swig/tesseract_srdf_python.i b/tesseract_python/swig/tesseract_srdf_python.i index b310dfec4..6b54e7d1a 100644 --- a/tesseract_python/swig/tesseract_srdf_python.i +++ b/tesseract_python/swig/tesseract_srdf_python.i @@ -49,6 +49,7 @@ #include #include #include +#include %} diff --git a/tesseract_python/swig/tesseract_task_composer_python.i b/tesseract_python/swig/tesseract_task_composer_python.i index 642c82cba..982563001 100644 --- a/tesseract_python/swig/tesseract_task_composer_python.i +++ b/tesseract_python/swig/tesseract_task_composer_python.i @@ -38,6 +38,29 @@ %{ +// tesseract_common +#include + +// tesseract_kinematics +#include +#include + +// tesseract_environment +#include +#include +#include + +// tesseract_collision +#include +#include +#include + +// tesseract_command_language +#include +#include +#include +#include + // tesseract_motion_planners_simple #include #include @@ -53,27 +76,18 @@ #include #include #include -#include -#include -#include #include // tesseract_motion_planners_ompl #include -#include #include -#include +#include #include -#include -#include // tesseract_motion_planner_descartes -#include #include #include #include -#include -#include // tesseract_time_parameterization #include @@ -84,7 +98,7 @@ // tesseract_task_composer #include #include -#include +//#include #include #include #include @@ -92,8 +106,10 @@ #include #include #include +#include +#include -#include +// #include #include #include @@ -104,7 +120,7 @@ // tesseract_task_composer profiles -#include +// #include #include #include #include @@ -116,7 +132,6 @@ #include - #include #include #include @@ -129,7 +144,6 @@ #include "tesseract_environment_python_std_functions.h" -#include "tesseract_command_language_python_profile_dictionary_functions.h" %} @@ -144,14 +158,22 @@ %unique_ptr_as(source_class_type, tesseract_planning, dest_class_type, tesseract_planning) %enddef +%include "tesseract_task_composer/core/fwd.h" + +// task_composer_keys +%include "tesseract_task_composer/core/task_composer_keys.h" +%template(get) tesseract_planning::TaskComposerKeys::get; + // task_composer_node_info %s_u_ptr(TaskComposerNodeInfo) %s_u_ptr(TaskComposerNodeInfoContainer) -%s_u_ptr(TaskComposerProblem) +// %s_u_ptr(TaskComposerProblem) // TODO: Handle maps containing unique_ptr // %template(MapUuidTaskComposerNodeInfoUPtr) std::map >; %ignore tesseract_planning::TaskComposerNodeInfoContainer::getInfoMap; +%ignore tesseract_planning::TaskComposerNodeInfo::find; +%ignore tesseract_planning::TaskComposerNodeInfoContainer::find; %include "tesseract_task_composer/core/task_composer_node_info.h" // task_composer_data_storage @@ -160,9 +182,11 @@ // TODO: Handle tesseract_common::AnyPoly %include "tesseract_task_composer/core/task_composer_data_storage.h" +%unique_ptr_constructor(tesseract_planning::TaskComposerDataStorage, %arg(), %arg()); + // task_composer_problem -%include "tesseract_task_composer/core/task_composer_problem.h" +// %include "tesseract_task_composer/core/task_composer_problem.h" // task_composer_context %s_u_ptr(TaskComposerContext) @@ -222,6 +246,8 @@ enum class future_status { %shared_ptr(tesseract_planning::TaskComposerNodeFactory) %shared_ptr(tesseract_planning::TaskComposerExecutorFactory) +%shared_ptr(tesseract_planning::TaskComposerPluginFactory) + %include "tesseract_task_composer/core/task_composer_plugin_factory.h" // task_composer_server @@ -229,7 +255,7 @@ enum class future_status { %include "tesseract_task_composer/core/task_composer_server.h" // planning_task_composer_problem -%s_u_ptr(PlanningTaskComposerProblem) +/*%s_u_ptr(PlanningTaskComposerProblem) %include "tesseract_task_composer/planning/planning_task_composer_problem.h" %unique_ptr_as_planning(PlanningTaskComposerProblem, TaskComposerProblem); @@ -260,44 +286,47 @@ enum class future_status { %unique_ptr_constructor(tesseract_planning::PlanningTaskComposerProblem, %arg(tesseract_environment::Environment::ConstPtr env, tesseract_planning::ProfileDictionary::ConstPtr profiles = nullptr, std::string name = "unset"), - %arg(env, profiles, name)); - -// check_input_profile -%shared_ptr(tesseract_planning::CheckInputProfile) -%include "tesseract_task_composer/planning/profiles/check_input_profile.h" -%tesseract_command_language_add_profile_type(CheckInputProfile); + %arg(env, profiles, name));*/ // contact_check_profile +%pythondynamic tesseract_planning::ContactCheckProfile; %shared_ptr(tesseract_planning::ContactCheckProfile) %include "tesseract_task_composer/planning/profiles/contact_check_profile.h" %tesseract_command_language_add_profile_type(ContactCheckProfile); // fix_state_bounds_profile +%pythondynamic tesseract_planning::FixStateBoundsProfile; %shared_ptr(tesseract_planning::FixStateBoundsProfile) %include "tesseract_task_composer/planning/profiles/fix_state_bounds_profile.h" %tesseract_command_language_add_profile_type(FixStateBoundsProfile); // fix_state_collision_profile +%pythondynamic tesseract_planning::FixStateCollisionProfile; %shared_ptr(tesseract_planning::FixStateCollisionProfile) %include "tesseract_task_composer/planning/profiles/fix_state_collision_profile.h" %tesseract_command_language_add_profile_type(FixStateCollisionProfile); // iterative_spline_parameterization_profile +%pythondynamic tesseract_planning::IterativeSplineParameterizationProfile; %shared_ptr(tesseract_planning::IterativeSplineParameterizationProfile) %include "tesseract_task_composer/planning/profiles/iterative_spline_parameterization_profile.h" %tesseract_command_language_add_profile_type(IterativeSplineParameterizationProfile); // min_length_profile +%pythondynamic tesseract_planning::MinLengthProfile; %shared_ptr(tesseract_planning::MinLengthProfile) %include "tesseract_task_composer/planning/profiles/min_length_profile.h" %tesseract_command_language_add_profile_type(MinLengthProfile); // profile_switch_profile +%pythondynamic tesseract_planning::ProfileSwitchProfile; %shared_ptr(tesseract_planning::ProfileSwitchProfile) %include "tesseract_task_composer/planning/profiles/profile_switch_profile.h" %tesseract_command_language_add_profile_type(ProfileSwitchProfile); // ruckig_trajectory_smoothing_profile +%pythondynamic tesseract_planning::RuckigTrajectorySmoothingCompositeProfile; +%pythondynamic tesseract_planning::RuckigTrajectorySmoothingMoveProfile; %shared_ptr(tesseract_planning::RuckigTrajectorySmoothingCompositeProfile) %shared_ptr(tesseract_planning::RuckigTrajectorySmoothingMoveProfile) %include "tesseract_task_composer/planning/profiles/ruckig_trajectory_smoothing_profile.h" @@ -305,16 +334,19 @@ enum class future_status { %tesseract_command_language_add_profile_type(RuckigTrajectorySmoothingMoveProfile); //time_optimal_parameterization_profile +%pythondynamic tesseract_planning::TimeOptimalParameterizationProfile; %shared_ptr(tesseract_planning::TimeOptimalParameterizationProfile) %include "tesseract_task_composer/planning/profiles/time_optimal_parameterization_profile.h" %tesseract_command_language_add_profile_type(TimeOptimalParameterizationProfile); // upsample_trajectory_profile +%pythondynamic tesseract_planning::UpsampleTrajectoryProfile; %shared_ptr(tesseract_planning::UpsampleTrajectoryProfile) %include "tesseract_task_composer/planning/profiles/upsample_trajectory_profile.h" %tesseract_command_language_add_profile_type(UpsampleTrajectoryProfile); %init %{ +// TODO: fix anchors tesseract_common::PluginLoader::addSymbolLibraryToSearchLibrariesEnv(tesseract_planning::TaskComposerPlanningFactoriesAnchor(), "TESSERACT_TASK_COMPOSER_PLUGINS"); tesseract_common::PluginLoader::addSymbolLibraryToSearchLibrariesEnv(tesseract_planning::TaskComposerTaskflowFactoriesAnchor(), "TESSERACT_TASK_COMPOSER_PLUGINS"); tesseract_common::PluginLoader::addSymbolLibraryToSearchLibrariesEnv(tesseract_planning::TaskComposerTaskFactoryAnchor(), "TESSERACT_TASK_COMPOSER_PLUGINS"); diff --git a/tesseract_python/swig/tesseract_type_erasure_macros.i b/tesseract_python/swig/tesseract_type_erasure_macros.i index 5e1a33714..e22b418cc 100644 --- a/tesseract_python/swig/tesseract_type_erasure_macros.i +++ b/tesseract_python/swig/tesseract_type_erasure_macros.i @@ -7,6 +7,20 @@ } %enddef +%define %tesseract_erasure_ctor_shared_ptr(class_type,class_namespace,inner_type,inner_namespace) +%inline { + class_namespace::class_type class_type ## _wrap_ ## inner_type (std::shared_ptr inner_waypoint) + { + return class_namespace::class_type (inner_waypoint); + } + + class_namespace::class_type class_type ## _wrap_ ## inner_type ## Const (std::shared_ptr inner_waypoint) + { + return class_namespace::class_type (inner_waypoint); + } +} +%enddef + %define %tesseract_erasure_ctor2(class_type,class_namespace,inner_type) %inline { class_namespace::class_type class_type ## _wrap_ ## inner_type (inner_type inner_waypoint) @@ -26,6 +40,21 @@ } %enddef +%define %tesseract_erasure_as_shared_ptr(source_class_type,source_class_namespace,dest_class_type,dest_class_namespace) + +%inline { + std::shared_ptr source_class_type ## _as_ ## dest_class_type (source_class_namespace::source_class_type& self) + { + return self.as>(); + } + + std::shared_ptr source_class_type ## _as_ ## dest_class_type ## Const (source_class_namespace::source_class_type& self) + { + return self.as>(); + } +} +%enddef + %define %tesseract_erasure_as2(source_class_type,source_class_namespace,dest_class_type) %inline { @@ -45,6 +74,20 @@ } %enddef +%define %tesseract_erasure_is_shared_ptr(source_class_type,source_class_namespace,dest_class_type,dest_class_namespace) +%inline { + bool source_class_type ## _is_ ## dest_class_type (const source_class_namespace::source_class_type& self) + { + return self.getType() == typeid(std::shared_ptr); + } + + bool source_class_type ## _is_ ## dest_class_type ## Const (const source_class_namespace::source_class_type& self) + { + return self.getType() == typeid(std::shared_ptr); + } +} +%enddef + %define %tesseract_erasure_is2(source_class_type,source_class_namespace,dest_class_type) %inline { bool source_class_type ## _is_ ## dest_class_type (const source_class_namespace::source_class_type& self) @@ -60,6 +103,12 @@ %tesseract_erasure_is(AnyPoly,tesseract_common,TYPE,NAMESPACE) %enddef +%define %tesseract_any_poly_type_shared_ptr(TYPE,NAMESPACE) +%tesseract_erasure_ctor_shared_ptr(AnyPoly,tesseract_common,TYPE,NAMESPACE) +%tesseract_erasure_as_shared_ptr(AnyPoly,tesseract_common,TYPE,NAMESPACE) +%tesseract_erasure_is_shared_ptr(AnyPoly,tesseract_common,TYPE,NAMESPACE) +%enddef + %define %tesseract_any_poly_type2(TYPE) %tesseract_erasure_ctor2(AnyPoly,tesseract_common,TYPE) %tesseract_erasure_as2(AnyPoly,tesseract_common,TYPE) diff --git a/tesseract_python/swig/tesseract_urdf_python.i b/tesseract_python/swig/tesseract_urdf_python.i index 035cd5e69..4a33ee01b 100644 --- a/tesseract_python/swig/tesseract_urdf_python.i +++ b/tesseract_python/swig/tesseract_urdf_python.i @@ -45,6 +45,7 @@ #include #include +#include // tesseract_urdf #include diff --git a/tesseract_python/swig/tesseract_visualization_python.i b/tesseract_python/swig/tesseract_visualization_python.i index af70ccf86..a94f59af1 100644 --- a/tesseract_python/swig/tesseract_visualization_python.i +++ b/tesseract_python/swig/tesseract_visualization_python.i @@ -59,6 +59,15 @@ #include #include +// tesseract_kinematics +#include +#include + +// tesseract_environment +#include +#include +#include + #include "tesseract_environment_python_std_functions.h" %} diff --git a/tesseract_python/swig/trajopt/problem_description.i b/tesseract_python/swig/trajopt/problem_description.i index 395bceabc..00ebb0919 100644 --- a/tesseract_python/swig/trajopt/problem_description.i +++ b/tesseract_python/swig/trajopt/problem_description.i @@ -160,11 +160,12 @@ class TrajOptProb; struct ProblemConstructionInfo; struct TrajOptResult; -enum TermType +enum class TermType : char { - TT_COST = 0x1, - TT_CNT = 0x2, - TT_USE_TIME = 0x4, + TT_INVALID = 0, // 0000 0000 + TT_COST = 0x1, // 0000 0001 + TT_CNT = 0x2, // 0000 0010 + TT_USE_TIME = 0x4, // 0000 0100 }; class TrajOptProb @@ -182,8 +183,8 @@ public: // VarArray& GetVars(); int GetNumSteps(); int GetNumDOF(); - tesseract_kinematics::JointGroup::ConstPtr GetKin(); - tesseract_environment::Environment::ConstPtr GetEnv(); + std::shared_ptr GetKin(); + std::shared_ptr GetEnv(); void SetInitTraj(const TrajArray& x); TrajArray GetInitTraj(); bool GetHasTime(); @@ -214,12 +215,12 @@ struct BasicInfo struct InitInfo { - enum Type + enum Type : std::uint8_t { STATIONARY, JOINT_INTERPOLATED, GIVEN_TRAJ, - }; + }; Type type; TrajArray data; double dt; @@ -234,13 +235,13 @@ public: using Ptr = std::shared_ptr; std::string name; - int term_type; - int getSupportedTypes(); + TermType term_type; + TermType getSupportedTypes(); virtual void fromJson(ProblemConstructionInfo& pci, const Json::Value& v); virtual void hatch(TrajOptProb& prob); static TermInfo::Ptr fromName(const std::string& type); - typedef TermInfo::Ptr (*MakerFunc)(void); + //static void RegisterMaker(const std::string& type, MakerFunc); virtual ~TermInfo(); }; @@ -254,10 +255,10 @@ public: std::vector cnt_infos; InitInfo init_info; - tesseract_environment::Environment::ConstPtr env; - tesseract_kinematics::JointGroup::ConstPtr kin; + std::shared_ptr env; + std::shared_ptr kin; - ProblemConstructionInfo(tesseract_environment::Environment::ConstPtr env); + ProblemConstructionInfo(std::shared_ptr env); void fromJson(const Json::Value& v); }; diff --git a/tesseract_python/tests/tesseract_collision/__init__.py b/tesseract_python/tests/tesseract_collision/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/tesseract_python/tests/tesseract_collision/test_collision_box_cone_unit.py b/tesseract_python/tests/tesseract_collision/test_collision_box_cone_unit.py index df564c136..6425e7f3e 100644 --- a/tesseract_python/tests/tesseract_collision/test_collision_box_cone_unit.py +++ b/tesseract_python/tests/tesseract_collision/test_collision_box_cone_unit.py @@ -4,6 +4,7 @@ from tesseract_robotics import tesseract_common from tesseract_robotics import tesseract_collision import os +from ..tesseract_support_resource_locator import TesseractSupportResourceLocator TESSERACT_SUPPORT_DIR = os.environ["TESSERACT_SUPPORT_DIR"] @@ -123,16 +124,20 @@ def run_test(checker): def get_plugin_factory(): collision_config = tesseract_common.FilesystemPath(TESSERACT_SUPPORT_DIR + "/urdf/" + "contact_manager_plugins.yaml") - return tesseract_collision.ContactManagersPluginFactory(collision_config) + locator = TesseractSupportResourceLocator() + return tesseract_collision.ContactManagersPluginFactory(collision_config, locator), locator -def test_bullet_discrete_simple(): - checker = get_plugin_factory().createDiscreteContactManager("BulletDiscreteSimpleManager") +def test_bullet_discrete_simple(): + factory, locator =get_plugin_factory() + checker= factory.createDiscreteContactManager("BulletDiscreteSimpleManager") run_test(checker) def test_bullet_discrete_bvh(): - checker = get_plugin_factory().createDiscreteContactManager("BulletDiscreteBVHManager") + factory, locator = get_plugin_factory() + checker = factory.createDiscreteContactManager("BulletDiscreteBVHManager") run_test(checker) def __test_fcl_discrete_bvh(): - checker = get_plugin_factory().createDiscreteContactManager("FCLDiscreteBVHManager") + factory, locator = get_plugin_factory() + checker = factory.createDiscreteContactManager("FCLDiscreteBVHManager") run_test(checker) \ No newline at end of file diff --git a/tesseract_python/tests/tesseract_environment/test_mesh_material_loading.py b/tesseract_python/tests/tesseract_environment/test_mesh_material_loading.py index a57f99d85..44aa93f55 100644 --- a/tesseract_python/tests/tesseract_environment/test_mesh_material_loading.py +++ b/tesseract_python/tests/tesseract_environment/test_mesh_material_loading.py @@ -41,66 +41,71 @@ def get_scene_graph(): def test_mesh_material_loading(): scene = get_scene_graph() visual = scene.getLink("mesh_dae_link").visual - assert len(visual) == 4 + assert len(visual) == 1 - mesh0 = visual[1].geometry - mesh1 = visual[2].geometry - mesh2 = visual[3].geometry - mesh3 = visual[0].geometry + print("Geometry type") + print(visual[0].geometry.getType()) - assert mesh0.getFaceCount() == 34 - assert mesh0.getVertexCount() == 68 - assert mesh1.getFaceCount() == 15 - assert mesh1.getVertexCount() == 17 + meshes = visual[0].geometry.getMeshes() + + mesh0 = meshes[0] + mesh1 = meshes[1] + mesh2 = meshes[2] + mesh3 = meshes[3] + + assert mesh0.getFaceCount() == 2 + assert mesh0.getVertexCount() == 4 + assert mesh1.getFaceCount() == 34 + assert mesh1.getVertexCount() == 68 assert mesh2.getFaceCount() == 15 assert mesh2.getVertexCount() == 17 - assert mesh3.getFaceCount() == 2 - assert mesh3.getVertexCount() == 4 + assert mesh3.getFaceCount() == 15 + assert mesh3.getVertexCount() == 17 mesh0_normals = mesh0.getNormals() assert mesh0_normals is not None - assert len(mesh0_normals) == 68 + assert len(mesh0_normals) == 4 mesh1_normals = mesh1.getNormals() assert mesh1_normals is not None - assert len(mesh1_normals) == 17 + assert len(mesh1_normals) == 68 mesh2_normals = mesh2.getNormals() assert mesh2_normals is not None assert len(mesh2_normals) == 17 mesh3_normals = mesh3.getNormals() assert mesh3_normals is not None - assert len(mesh3_normals) == 4 + assert len(mesh3_normals) == 17 mesh0_material = mesh0.getMaterial() - nptest.assert_allclose(mesh0_material.getBaseColorFactor().flatten(),[0.7,0.7,0.7,1], atol=0.01) + nptest.assert_allclose(mesh0_material.getBaseColorFactor().flatten(),[1,1,1,1], atol=0.01) nptest.assert_almost_equal(mesh0_material.getMetallicFactor(), 0.0) nptest.assert_almost_equal(mesh0_material.getRoughnessFactor(), 0.5) nptest.assert_allclose(mesh0_material.getEmissiveFactor().flatten(), [0,0,0,1], atol = 0.01) mesh1_material = mesh1.getMaterial() - nptest.assert_allclose(mesh1_material.getBaseColorFactor().flatten(),[0.8,0.0,0.0,1], atol=0.01) + nptest.assert_allclose(mesh1_material.getBaseColorFactor().flatten(),[0.7,0.7,0.7,1], atol=0.01) nptest.assert_almost_equal(mesh1_material.getMetallicFactor(), 0.0) nptest.assert_almost_equal(mesh1_material.getRoughnessFactor(), 0.5) nptest.assert_allclose(mesh1_material.getEmissiveFactor().flatten(), [0,0,0,1], atol = 0.01) mesh2_material = mesh2.getMaterial() - nptest.assert_allclose(mesh2_material.getBaseColorFactor().flatten(),[0.05,0.8,0.05,1], atol=0.01) + nptest.assert_allclose(mesh2_material.getBaseColorFactor().flatten(),[0.8,0 ,0, 1], atol=0.01) nptest.assert_almost_equal(mesh2_material.getMetallicFactor(), 0.0) nptest.assert_almost_equal(mesh2_material.getRoughnessFactor(), 0.5) - nptest.assert_allclose(mesh2_material.getEmissiveFactor().flatten(), [0.1,0.1,0.5,1], atol = 0.01) + nptest.assert_allclose(mesh2_material.getEmissiveFactor().flatten(), [0,0,0,1], atol = 0.01) mesh3_material = mesh3.getMaterial() - nptest.assert_allclose(mesh3_material.getBaseColorFactor().flatten(),[1,1,1,1], atol=0.01) + nptest.assert_allclose(mesh3_material.getBaseColorFactor().flatten(),[0.05,0.8,0.05,1], atol=0.01) nptest.assert_almost_equal(mesh3_material.getMetallicFactor(), 0.0) nptest.assert_almost_equal(mesh3_material.getRoughnessFactor(), 0.5) - nptest.assert_allclose(mesh3_material.getEmissiveFactor().flatten(), [0,0,0,1], atol = 0.01) + nptest.assert_allclose(mesh3_material.getEmissiveFactor().flatten(), [0.1,0.1,0.5,1], atol = 0.01) - assert mesh0.getTextures() is None assert mesh1.getTextures() is None - assert mesh2.getTextures() is None + assert mesh2.getTextures() is None + assert mesh3.getTextures() is None - assert mesh3.getTextures() is not None - assert len(mesh3.getTextures()) == 1 + assert mesh0.getTextures() is not None + assert len(mesh0.getTextures()) == 1 - texture = mesh3.getTextures()[0] + texture = mesh0.getTextures()[0] assert len(texture.getTextureImage().getResourceContents()) == 38212 assert len(texture.getUVs()) == 4 diff --git a/tesseract_python/tests/tesseract_environment/test_tesseract_environment_kingroup.py b/tesseract_python/tests/tesseract_environment/test_tesseract_environment_kingroup.py index 5b599ad91..a06f27124 100644 --- a/tesseract_python/tests/tesseract_environment/test_tesseract_environment_kingroup.py +++ b/tesseract_python/tests/tesseract_environment/test_tesseract_environment_kingroup.py @@ -28,7 +28,7 @@ def test_kinematic_group(): env, manip_info, joint_names = get_environment() - kin_group = env.getKinematicGroup(manip_info.manipulator).release() + kin_group = env.getKinematicGroup(manip_info.manipulator) joint_vals = np.ones((6,),dtype=np.float64)*0.1 pose_map = kin_group.calcFwdKin(joint_vals) @@ -57,7 +57,7 @@ def test_tesseract_redundant_solutions_tesseract_function(): env, manip_info, joint_names = get_environment() - kin_group = env.getKinematicGroup(manip_info.manipulator).release() + kin_group = env.getKinematicGroup(manip_info.manipulator) limits = kin_group.getLimits() redundancy_indices = list(kin_group.getRedundancyCapableJointIndices()) diff --git a/tesseract_python/tests/tesseract_kinematics/test_kdl_kinematics.py b/tesseract_python/tests/tesseract_kinematics/test_kdl_kinematics.py index 03e070de3..9e172718c 100644 --- a/tesseract_python/tests/tesseract_kinematics/test_kdl_kinematics.py +++ b/tesseract_python/tests/tesseract_kinematics/test_kdl_kinematics.py @@ -22,7 +22,8 @@ def get_scene_graph(): def get_plugin_factory(): support_dir = os.environ["TESSERACT_SUPPORT_DIR"] kin_config = tesseract_common.FilesystemPath(support_dir + "/urdf/" + "lbr_iiwa_14_r820_plugins.yaml") - return tesseract_kinematics.KinematicsPluginFactory(kin_config) + locator = TesseractSupportResourceLocator() + return tesseract_kinematics.KinematicsPluginFactory(kin_config, locator), locator def run_inv_kin_test(inv_kin, fwd_kin): @@ -41,7 +42,7 @@ def run_inv_kin_test(inv_kin, fwd_kin): nptest.assert_almost_equal(pose,result["tool0"].matrix(),decimal=3) def test_kdl_kin_chain_lma_inverse_kinematic(): - plugin_factory = get_plugin_factory() + plugin_factory, p_locator = get_plugin_factory() scene_graph = get_scene_graph() solver = tesseract_state_solver.KDLStateSolver(scene_graph) scene_state1 = solver.getState(np.zeros((7,))) @@ -59,7 +60,7 @@ def test_kdl_kin_chain_lma_inverse_kinematic(): def test_jacobian(): - plugin_factory = get_plugin_factory() + plugin_factory, p_locator = get_plugin_factory() scene_graph = get_scene_graph() solver = tesseract_state_solver.KDLStateSolver(scene_graph) scene_state = solver.getState(np.zeros((7,))) diff --git a/tesseract_python/tests/tesseract_kinematics/test_opw_kinematics.py b/tesseract_python/tests/tesseract_kinematics/test_opw_kinematics.py index cb63e178b..4379342cb 100644 --- a/tesseract_python/tests/tesseract_kinematics/test_opw_kinematics.py +++ b/tesseract_python/tests/tesseract_kinematics/test_opw_kinematics.py @@ -22,7 +22,8 @@ def get_scene_graph(): def get_plugin_factory(): support_dir = os.environ["TESSERACT_SUPPORT_DIR"] kin_config = tesseract_common.FilesystemPath(support_dir + "/urdf/" + "abb_irb2400_plugins.yaml") - return tesseract_kinematics.KinematicsPluginFactory(kin_config) + locator = TesseractSupportResourceLocator() + return tesseract_kinematics.KinematicsPluginFactory(kin_config, locator), locator def run_inv_kin_test(inv_kin, fwd_kin): @@ -41,7 +42,7 @@ def run_inv_kin_test(inv_kin, fwd_kin): nptest.assert_almost_equal(pose,result["tool0"].matrix(),decimal=3) def test_opw_inverse_kinematic(): - plugin_factory = get_plugin_factory() + plugin_factory, p_locator = get_plugin_factory() scene_graph = get_scene_graph() solver = tesseract_state_solver.KDLStateSolver(scene_graph) scene_state1 = solver.getState(np.zeros((6,))) diff --git a/tesseract_python/tests/tesseract_motion_planning/test_descartes_planner.py b/tesseract_python/tests/tesseract_motion_planning/test_descartes_planner.py index 61b979b49..b80a413bc 100644 --- a/tesseract_python/tests/tesseract_motion_planning/test_descartes_planner.py +++ b/tesseract_python/tests/tesseract_motion_planning/test_descartes_planner.py @@ -14,8 +14,7 @@ CartesianWaypointPoly_wrap_CartesianWaypoint, MoveInstructionPoly_wrap_MoveInstruction from tesseract_robotics.tesseract_motion_planners import PlannerRequest, PlannerResponse from tesseract_robotics.tesseract_motion_planners_descartes import DescartesDefaultPlanProfileD, \ - DescartesMotionPlannerD, DescartesPlanProfileD, \ - ProfileDictionary_addProfile_DescartesPlanProfileD, cast_DescartesPlanProfileD + DescartesMotionPlannerD, DescartesPlanProfileD, cast_DescartesPlanProfileD from tesseract_robotics.tesseract_motion_planners_simple import generateInterpolatedProgram from ..tesseract_support_resource_locator import TesseractSupportResourceLocator @@ -42,8 +41,6 @@ def test_descartes_freespace_fixed_poses(): env, manip, joint_names = get_environment() kin_group = env.getKinematicGroup(manip.manipulator,manip.manipulator_ik_solver) - cur_state = env.getState() - wp1 = CartesianWaypoint(Isometry3d.Identity() * Translation3d(0.8,-0.2,0.8) * Quaterniond(0,0,-1.0,0)) wp2 = CartesianWaypoint(Isometry3d.Identity() * Translation3d(0.8,0.2,0.8) * Quaterniond(0,0,-1.0,0)) @@ -55,23 +52,21 @@ def test_descartes_freespace_fixed_poses(): program.appendMoveInstruction(MoveInstructionPoly_wrap_MoveInstruction(start_instruction)) program.appendMoveInstruction(MoveInstructionPoly_wrap_MoveInstruction(plan_f1)) - interpolated_program = generateInterpolatedProgram(program, cur_state, env, 3.14, 1.0, 3.14, 10) + interpolated_program = generateInterpolatedProgram(program, env, 3.14, 1.0, 3.14, 10) plan_profile = DescartesDefaultPlanProfileD() # DescartesDefaultPlanProfileD is not upcasting automatically, use helper function plan_profile1 = cast_DescartesPlanProfileD(plan_profile) profiles = ProfileDictionary() - ProfileDictionary_addProfile_DescartesPlanProfileD(profiles,DESCARTES_DEFAULT_NAMESPACE,"TEST_PROFILE",plan_profile1) + profiles.addProfile(DESCARTES_DEFAULT_NAMESPACE,"TEST_PROFILE",plan_profile1) single_descartes_planner = DescartesMotionPlannerD(DESCARTES_DEFAULT_NAMESPACE) - plan_profile.num_threads = 1 request = PlannerRequest() request.instructions = interpolated_program request.env = env - request.env_state = cur_state request.profiles = profiles response = single_descartes_planner.solve(request) diff --git a/tesseract_python/tests/tesseract_motion_planning/test_iterative_spline.py b/tesseract_python/tests/tesseract_motion_planning/test_iterative_spline.py index e74dc90f3..20e6bceb0 100644 --- a/tesseract_python/tests/tesseract_motion_planning/test_iterative_spline.py +++ b/tesseract_python/tests/tesseract_motion_planning/test_iterative_spline.py @@ -37,9 +37,13 @@ def test_time_parameterization(): program = create_straight_trajectory() traj = InstructionsTrajectory(program) - max_velocity = np.array([2.088, 2.082, 3.27, 3.6, 3.3, 3.078],dtype=np.float64) - max_acceleration = np.array([ 1, 1, 1, 1, 1, 1],dtype=np.float64) - assert time_parameterization.compute(traj, max_velocity, max_acceleration) + max_velocity = np.array([[2.088, 2.082, 3.27, 3.6, 3.3, 3.078]],dtype=np.float64) + max_velocity = np.hstack((-max_velocity.T, max_velocity.T)) + max_acceleration = np.array([[ 1, 1, 1, 1, 1, 1]],dtype=np.float64) + max_acceleration = np.hstack((-max_acceleration.T, max_acceleration.T)) + max_jerk = np.array([[ 1, 1, 1, 1, 1, 1]],dtype=np.float64) + max_jerk = np.hstack((-max_jerk.T, max_jerk.T)) + assert time_parameterization.compute(traj, max_velocity, max_acceleration, max_jerk) res_instr1 = InstructionPoly_as_MoveInstructionPoly(program[-1]) WaypointPoly_as_StateWaypointPoly(res_instr1.getWaypoint()).getTime() > 1.0 res_instr2 = InstructionPoly_as_MoveInstructionPoly(program[-1]) @@ -51,9 +55,13 @@ def test_time_parameterization_vec(): program = create_straight_trajectory() traj = InstructionsTrajectory(program) - max_velocity = np.array([2.088, 2.082, 3.27, 3.6, 3.3, 3.078],dtype=np.float64) - max_acceleration = np.array([ 1, 1, 1, 1, 1, 1],dtype=np.float64) - assert time_parameterization.compute(traj, max_velocity, max_acceleration) + max_velocity = np.array([[2.088, 2.082, 3.27, 3.6, 3.3, 3.078]],dtype=np.float64) + max_velocity = np.hstack((-max_velocity.T, max_velocity.T)) + max_acceleration = np.array([[ 1, 1, 1, 1, 1, 1]],dtype=np.float64) + max_acceleration = np.hstack((-max_acceleration.T, max_acceleration.T)) + max_jerk = np.array([[ 1, 1, 1, 1, 1, 1]],dtype=np.float64) + max_jerk = np.hstack((-max_jerk.T, max_jerk.T)) + assert time_parameterization.compute(traj, max_velocity, max_acceleration, max_jerk) res_instr1 = InstructionPoly_as_MoveInstructionPoly(program[-1]) WaypointPoly_as_StateWaypointPoly(res_instr1.getWaypoint()).getTime() > 1.0 res_instr2 = InstructionPoly_as_MoveInstructionPoly(program[-1]) diff --git a/tesseract_python/tests/tesseract_motion_planning/test_ompl_planner.py b/tesseract_python/tests/tesseract_motion_planning/test_ompl_planner.py index d870674c2..5e3e95e29 100644 --- a/tesseract_python/tests/tesseract_motion_planning/test_ompl_planner.py +++ b/tesseract_python/tests/tesseract_motion_planning/test_ompl_planner.py @@ -15,8 +15,8 @@ JointWaypointPoly_wrap_JointWaypoint, CartesianWaypointPoly_wrap_CartesianWaypoint, \ MoveInstructionPoly_wrap_MoveInstruction from tesseract_robotics.tesseract_motion_planners import PlannerRequest, PlannerResponse -from tesseract_robotics.tesseract_motion_planners_ompl import OMPLDefaultPlanProfile, RRTConnectConfigurator, \ - OMPLProblemGeneratorFn, OMPLMotionPlanner, ProfileDictionary_addProfile_OMPLPlanProfile +from tesseract_robotics.tesseract_motion_planners_ompl import RRTConnectConfigurator, \ + OMPLMotionPlanner, OMPLRealVectorPlanProfile from tesseract_robotics.tesseract_motion_planners_simple import generateInterpolatedProgram from ..tesseract_support_resource_locator import TesseractSupportResourceLocator @@ -46,8 +46,6 @@ def test_ompl_freespace_joint_cart(): env, manip, joint_names = get_environment() kin_group = env.getKinematicGroup(manip.manipulator) - cur_state = env.getState() - wp1 = JointWaypoint(joint_names, start_state) goal = kin_group.calcFwdKin(end_state)[manip.tcp_frame] @@ -61,19 +59,16 @@ def test_ompl_freespace_joint_cart(): program.appendMoveInstruction(MoveInstructionPoly_wrap_MoveInstruction(start_instruction)) program.appendMoveInstruction(MoveInstructionPoly_wrap_MoveInstruction(plan_f1)) - interpolated_program = generateInterpolatedProgram(program, cur_state, env, 3.14, 1.0, 3.14, 10) + interpolated_program = generateInterpolatedProgram(program, env, 3.14, 1.0, 3.14, 10) - plan_profile = OMPLDefaultPlanProfile() - plan_profile.planners.clear() - plan_profile.planners.append(RRTConnectConfigurator()) + plan_profile = OMPLRealVectorPlanProfile() profiles = ProfileDictionary() - ProfileDictionary_addProfile_OMPLPlanProfile(profiles,OMPL_DEFAULT_NAMESPACE, "TEST_PROFILE", plan_profile) + profiles.addProfile(OMPL_DEFAULT_NAMESPACE, "TEST_PROFILE", plan_profile) request = PlannerRequest() request.instructions = interpolated_program request.env = env - request.env_state = cur_state request.profiles = profiles diff --git a/tesseract_python/tests/tesseract_motion_planning/test_simple_planner.py b/tesseract_python/tests/tesseract_motion_planning/test_simple_planner.py index df5c1ba2f..646d360fb 100644 --- a/tesseract_python/tests/tesseract_motion_planning/test_simple_planner.py +++ b/tesseract_python/tests/tesseract_motion_planning/test_simple_planner.py @@ -38,12 +38,9 @@ def test_get_environment(): def test_interpolatestatewaypoint_jointcart_freespace(): env, manip_info, joint_names = get_environment() - request = PlannerRequest() - request.env = env - request.env_state = env.getState() joint_group = env.getJointGroup(manip_info.manipulator) wp1 = JointWaypoint(joint_names, np.zeros((7,),dtype=np.float64)) - wp1_seed = JointWaypoint(joint_names, request.env_state.getJointValues(joint_names)) + wp1_seed = JointWaypoint(joint_names, env.getState().getJointValues(joint_names)) wp2 = CartesianWaypoint(joint_group.calcFwdKin(np.ones((7,),dtype=np.float64))[manip_info.tcp_frame]) instr1 = MoveInstruction(JointWaypointPoly_wrap_JointWaypoint(wp1), MoveInstructionType_FREESPACE, "TEST_PROFILE", manip_info) instr1_seed = MoveInstruction(JointWaypointPoly_wrap_JointWaypoint(wp1), MoveInstructionType_LINEAR, "TEST_PROFILE", manip_info) @@ -54,7 +51,7 @@ def test_interpolatestatewaypoint_jointcart_freespace(): profile = SimplePlannerLVSPlanProfile(3.14,0.5,1.57,5) composite = profile.generate(MoveInstructionPoly_wrap_MoveInstruction(instr1),MoveInstructionPoly_wrap_MoveInstruction(instr1_seed), - MoveInstructionPoly_wrap_MoveInstruction(instr2),instr3,request,manip_info) + MoveInstructionPoly_wrap_MoveInstruction(instr2),instr3,env,manip_info) for c in composite: assert c.getWaypoint().isCartesianWaypoint() or c.getWaypoint().isJointWaypoint() diff --git a/tesseract_python/tests/tesseract_motion_planning/test_time_optimal_trajectory.py b/tesseract_python/tests/tesseract_motion_planning/test_time_optimal_trajectory.py index 069013efd..92453a5b6 100644 --- a/tesseract_python/tests/tesseract_motion_planning/test_time_optimal_trajectory.py +++ b/tesseract_python/tests/tesseract_motion_planning/test_time_optimal_trajectory.py @@ -36,9 +36,13 @@ def test_time_parameterization(): program = create_straight_trajectory() traj = InstructionsTrajectory(program) - max_velocity = np.array([2.088, 2.082, 3.27, 3.6, 3.3, 3.078],dtype=np.float64) - max_acceleration = np.array([ 1, 1, 1, 1, 1, 1],dtype=np.float64) - assert time_parameterization.computeTimeStamps(traj, max_velocity, max_acceleration) + max_velocity = np.array([[2.088, 2.082, 3.27, 3.6, 3.3, 3.078]],dtype=np.float64) + max_velocity = np.hstack((-max_velocity.T, max_velocity.T)) + max_acceleration = np.array([[ 1, 1, 1, 1, 1, 1]],dtype=np.float64) + max_acceleration = np.hstack((-max_acceleration.T, max_acceleration.T)) + max_jerk = np.array([[ 1, 1, 1, 1, 1, 1]],dtype=np.float64) + max_jerk = np.hstack((-max_jerk.T, max_jerk.T)) + assert time_parameterization.compute(traj, max_velocity, max_acceleration, max_jerk) instr1 = program[-1] instr1_1 = InstructionPoly_as_MoveInstructionPoly(instr1) result_wp1 = instr1_1.getWaypoint() diff --git a/tesseract_python/tests/tesseract_motion_planning/test_trajopt_planner.py b/tesseract_python/tests/tesseract_motion_planning/test_trajopt_planner.py index 69e173a08..be05d5ef6 100644 --- a/tesseract_python/tests/tesseract_motion_planning/test_trajopt_planner.py +++ b/tesseract_python/tests/tesseract_motion_planning/test_trajopt_planner.py @@ -16,8 +16,7 @@ MoveInstructionPoly_wrap_MoveInstruction from tesseract_robotics.tesseract_motion_planners import PlannerRequest, PlannerResponse from tesseract_robotics.tesseract_motion_planners_trajopt import TrajOptDefaultPlanProfile, TrajOptDefaultCompositeProfile, \ - TrajOptProblemGeneratorFn, TrajOptMotionPlanner, ProfileDictionary_addProfile_TrajOptPlanProfile, \ - ProfileDictionary_addProfile_TrajOptCompositeProfile + TrajOptMotionPlanner from tesseract_robotics.tesseract_motion_planners_simple import generateInterpolatedProgram from ..tesseract_support_resource_locator import TesseractSupportResourceLocator @@ -44,9 +43,6 @@ def test_trajopt_freespace_joint_cart(): env, manip, joint_names = get_environment() - - cur_state = env.getState() - wp1 = JointWaypoint(joint_names, np.array([0,0,0,-1.57,0,0,0],dtype=np.float64)) wp2 = CartesianWaypoint(Isometry3d.Identity() * Translation3d(-.2,.4,0.2) * Quaterniond(0,0,1.0,0)) @@ -58,14 +54,14 @@ def test_trajopt_freespace_joint_cart(): program.appendMoveInstruction(MoveInstructionPoly_wrap_MoveInstruction(start_instruction)) program.appendMoveInstruction(MoveInstructionPoly_wrap_MoveInstruction(plan_f1)) - interpolated_program = generateInterpolatedProgram(program, cur_state, env, 3.14, 1.0, 3.14, 10) + interpolated_program = generateInterpolatedProgram(program, env, 3.14, 1.0, 3.14, 10) plan_profile = TrajOptDefaultPlanProfile() composite_profile = TrajOptDefaultCompositeProfile() profiles = ProfileDictionary() - ProfileDictionary_addProfile_TrajOptPlanProfile(profiles, TRAJOPT_DEFAULT_NAMESPACE, "TEST_PROFILE", plan_profile) - ProfileDictionary_addProfile_TrajOptCompositeProfile(profiles, TRAJOPT_DEFAULT_NAMESPACE, "TEST_PROFILE", composite_profile) + profiles.addProfile(TRAJOPT_DEFAULT_NAMESPACE, "TEST_PROFILE", plan_profile) + profiles.addProfile(TRAJOPT_DEFAULT_NAMESPACE, "TEST_PROFILE", composite_profile) test_planner = TrajOptMotionPlanner(TRAJOPT_DEFAULT_NAMESPACE) @@ -74,7 +70,6 @@ def test_trajopt_freespace_joint_cart(): request = PlannerRequest() request.instructions = interpolated_program request.env = env - request.env_state = cur_state request.profiles = profiles response = test_planner.solve(request) diff --git a/tesseract_python/tests/tesseract_scene_graph/test_tesseract_scene_graph.py b/tesseract_python/tests/tesseract_scene_graph/test_tesseract_scene_graph.py index a593cb698..cef42d135 100644 --- a/tesseract_python/tests/tesseract_scene_graph/test_tesseract_scene_graph.py +++ b/tesseract_python/tests/tesseract_scene_graph/test_tesseract_scene_graph.py @@ -38,7 +38,7 @@ def test_tesseract_scene_graph(): joint_2.parent_link_name = "link_2" joint_2.child_link_name = "link_3" joint_2.type = sg.JointType_PLANAR - joint_2.limits = sg.JointLimits(-1,1,1,1,1) + joint_2.limits = sg.JointLimits(-1,1,1,1,1,1) assert g.addJoint(joint_2) joint_3 = sg.Joint("joint_3") @@ -53,7 +53,7 @@ def test_tesseract_scene_graph(): joint_4.parent_link_name = "link_2" joint_4.child_link_name = "link_5" joint_4.type = sg.JointType_REVOLUTE - joint_4.limits = sg.JointLimits(-1,1,1,1,1) + joint_4.limits = sg.JointLimits(-1,1,1,1,1,1) assert g.addJoint(joint_4) adjacent_links = g.getAdjacentLinkNames("link_3") @@ -165,7 +165,7 @@ def test_load_srdf_unit(): joint_2.parent_link_name = "link_1" joint_2.child_link_name = "link_2" joint_2.type = sg.JointType_REVOLUTE - joint_2.limits = sg.JointLimits(-1,1,1,1,1) + joint_2.limits = sg.JointLimits(-1,1,1,1,1,1) assert g.addJoint(joint_2) joint_3 = sg.Joint("joint_a3") @@ -173,7 +173,7 @@ def test_load_srdf_unit(): joint_3.parent_link_name = "link_2" joint_3.child_link_name = "link_3" joint_3.type = sg.JointType_REVOLUTE - joint_3.limits = sg.JointLimits(-1,1,1,1,1) + joint_3.limits = sg.JointLimits(-1,1,1,1,1,1) assert g.addJoint(joint_3) joint_4 = sg.Joint("joint_a4") @@ -181,7 +181,7 @@ def test_load_srdf_unit(): joint_4.parent_link_name = "link_3" joint_4.child_link_name = "link_4" joint_4.type = sg.JointType_REVOLUTE - joint_4.limits = sg.JointLimits(-1,1,1,1,1) + joint_4.limits = sg.JointLimits(-1,1,1,1,1,1) assert g.addJoint(joint_4) joint_5 = sg.Joint("joint_a5") @@ -189,7 +189,7 @@ def test_load_srdf_unit(): joint_5.parent_link_name = "link_4" joint_5.child_link_name = "link_5" joint_5.type = sg.JointType_REVOLUTE - joint_5.limits = sg.JointLimits(-1,1,1,1,1) + joint_5.limits = sg.JointLimits(-1,1,1,1,1,1) assert g.addJoint(joint_5) joint_6 = sg.Joint("joint_a6") @@ -197,7 +197,7 @@ def test_load_srdf_unit(): joint_6.parent_link_name = "link_5" joint_6.child_link_name = "link_6" joint_6.type = sg.JointType_REVOLUTE - joint_6.limits = sg.JointLimits(-1,1,1,1,1) + joint_6.limits = sg.JointLimits(-1,1,1,1,1,1) assert g.addJoint(joint_6) joint_7 = sg.Joint("joint_a7") @@ -205,7 +205,7 @@ def test_load_srdf_unit(): joint_7.parent_link_name = "link_6" joint_7.child_link_name = "link_7" joint_7.type = sg.JointType_REVOLUTE - joint_7.limits = sg.JointLimits(-1,1,1,1,1) + joint_7.limits = sg.JointLimits(-1,1,1,1,1,1) assert g.addJoint(joint_7) joint_tool0 = sg.Joint("base_joint") diff --git a/tesseract_python/tests/tesseract_task_composer/test_tesseract_task_composer.py b/tesseract_python/tests/tesseract_task_composer/test_tesseract_task_composer.py index 226825cd2..0b9d8d990 100644 --- a/tesseract_python/tests/tesseract_task_composer/test_tesseract_task_composer.py +++ b/tesseract_python/tests/tesseract_task_composer/test_tesseract_task_composer.py @@ -5,7 +5,7 @@ import numpy.testing as nptest from tesseract_robotics.tesseract_common import ResourceLocator, SimpleLocatedResource -from tesseract_robotics.tesseract_environment import Environment +from tesseract_robotics.tesseract_environment import Environment, AnyPoly_wrap_EnvironmentConst from tesseract_robotics.tesseract_common import FilesystemPath, Isometry3d, Translation3d, Quaterniond, \ ManipulatorInfo, AnyPoly, AnyPoly_wrap_double from tesseract_robotics.tesseract_command_language import CartesianWaypoint, WaypointPoly, \ @@ -15,7 +15,8 @@ AnyPoly_wrap_CompositeInstruction, DEFAULT_PROFILE_KEY, JointWaypoint, JointWaypointPoly, \ InstructionPoly_as_MoveInstructionPoly, WaypointPoly_as_StateWaypointPoly, \ MoveInstructionPoly_wrap_MoveInstruction, StateWaypointPoly_wrap_StateWaypoint, \ - CartesianWaypointPoly_wrap_CartesianWaypoint, JointWaypointPoly_wrap_JointWaypoint + CartesianWaypointPoly_wrap_CartesianWaypoint, JointWaypointPoly_wrap_JointWaypoint, \ + AnyPoly_wrap_ProfileDictionary # from tesseract_robotics.tesseract_motion_planners import PlannerRequest, PlannerResponse, generateInterpolatedProgram # from tesseract_robotics.tesseract_motion_planners_ompl import OMPLDefaultPlanProfile, RRTConnectConfigurator, \ @@ -26,7 +27,7 @@ # TrajOptProblemGeneratorFn, TrajOptMotionPlanner, ProfileDictionary_addProfile_TrajOptPlanProfile, \ # ProfileDictionary_addProfile_TrajOptCompositeProfile from tesseract_robotics.tesseract_task_composer import TaskComposerPluginFactory, \ - TaskComposerDataStorage, TaskComposerContext, PlanningTaskComposerProblem + TaskComposerDataStorage, TaskComposerContext, TaskComposerDataStorageUPtr from ..tesseract_support_resource_locator import TesseractSupportResourceLocator @@ -37,8 +38,8 @@ TESSERACT_TASK_COMPOSER_DIR = os.environ["TESSERACT_TASK_COMPOSER_DIR"] def get_environment(): - locator = TesseractSupportResourceLocator() env = Environment() + locator = TesseractSupportResourceLocator() tesseract_support = os.environ["TESSERACT_SUPPORT_DIR"] urdf_path = FilesystemPath(os.path.join(tesseract_support, "urdf/lbr_iiwa_14_r820.urdf")) srdf_path = FilesystemPath(os.path.join(tesseract_support, "urdf/lbr_iiwa_14_r820.srdf")) @@ -54,7 +55,7 @@ def freespace_example_progam_iiwa(manipulator_info, goal = None, composite_profi freespace_profile = DEFAULT_PROFILE_KEY): if goal is None: goal = Isometry3d.Identity() * Translation3d(0.2, 0.2, 1.0) - program = CompositeInstruction(DEFAULT_PROFILE_KEY, CompositeInstructionOrder_ORDERED, manipulator_info) + program = CompositeInstruction(DEFAULT_PROFILE_KEY, manipulator_info, CompositeInstructionOrder_ORDERED) joint_names = ["joint_a1", "joint_a2", "joint_a3", "joint_a4", "joint_a5", "joint_a6", "joint_a7"] joint_values = np.zeros((7,)) wp1 = StateWaypointPoly_wrap_StateWaypoint(StateWaypoint(joint_names, joint_values)) @@ -76,7 +77,6 @@ def freespace_example_progam_iiwa(manipulator_info, goal = None, composite_profi def test_task_composer_trajopt_example(): - planning_task_problem = None output_program = None future = None task_executor = None @@ -85,27 +85,33 @@ def test_task_composer_trajopt_example(): env, manip_info = get_environment() config_path = FilesystemPath(os.path.join(TESSERACT_TASK_COMPOSER_DIR, "config/task_composer_plugins_no_trajopt_ifopt.yaml")) - factory = TaskComposerPluginFactory(config_path) + p_locator = TesseractSupportResourceLocator() + factory = TaskComposerPluginFactory(config_path, p_locator) task = factory.createTaskComposerNode("TrajOptPipeline") print("trajopt task name: " + task.getName()) - output_key = task.getOutputKeys()[0] + output_key = task.getOutputKeys().get("program") + input_key = task.getInputKeys().get("planning_input") profiles = ProfileDictionary() program = freespace_example_progam_iiwa(manip_info) - problem_input = AnyPoly_wrap_CompositeInstruction(program) - - planning_task_problem = PlanningTaskComposerProblem(env, profiles) - planning_task_problem.input = problem_input + problem_anypoly = AnyPoly_wrap_CompositeInstruction(program) + environment_anypoly = AnyPoly_wrap_EnvironmentConst(env) + profiles_anypoly = AnyPoly_wrap_ProfileDictionary(profiles) + task_data = TaskComposerDataStorage() + task_data.setData(input_key, problem_anypoly) + task_data.setData("environment", environment_anypoly) + task_data.setData("profiles", profiles_anypoly) + task_executor = factory.createTaskComposerExecutor("TaskflowExecutor") output_program = None try: - future = task_executor.run(task.get(), planning_task_problem) + future = task_executor.run(task.get(), task_data) future.wait() output_program = AnyPoly_as_CompositeInstruction(future.context.data_storage.getData(output_key)) @@ -124,11 +130,15 @@ def test_task_composer_trajopt_example(): print("Done") finally: - # Cleanup memory to prevent segfault on exit - del planning_task_problem - del output_program + # del planning_task_problem + # del output_program + del task_data + del problem_anypoly + del environment_anypoly + del profiles_anypoly del future del task_executor del task + diff --git a/tesseract_viewer_python/examples/abb_irb2400_viewer.py b/tesseract_viewer_python/examples/abb_irb2400_viewer.py index eb31a60d6..8191763de 100644 --- a/tesseract_viewer_python/examples/abb_irb2400_viewer.py +++ b/tesseract_viewer_python/examples/abb_irb2400_viewer.py @@ -7,14 +7,14 @@ CompositeInstruction, MoveInstructionPoly, CartesianWaypointPoly, ProfileDictionary, \ CartesianWaypointPoly_wrap_CartesianWaypoint, MoveInstructionPoly_wrap_MoveInstruction -from tesseract_robotics.tesseract_motion_planners import PlannerRequest, PlannerResponse, generateInterpolatedProgram -from tesseract_robotics.tesseract_motion_planners_ompl import OMPLDefaultPlanProfile, RRTConnectConfigurator, \ - OMPLProblemGeneratorFn, OMPLMotionPlanner, ProfileDictionary_addProfile_OMPLPlanProfile +from tesseract_robotics.tesseract_motion_planners import PlannerRequest, PlannerResponse +from tesseract_robotics.tesseract_motion_planners_simple import generateInterpolatedProgram +from tesseract_robotics.tesseract_motion_planners_ompl import RRTConnectConfigurator, \ + OMPLMotionPlanner from tesseract_robotics.tesseract_time_parameterization import TimeOptimalTrajectoryGeneration, \ InstructionsTrajectory from tesseract_robotics.tesseract_motion_planners_trajopt import TrajOptDefaultPlanProfile, TrajOptDefaultCompositeProfile, \ - TrajOptProblemGeneratorFn, TrajOptMotionPlanner, ProfileDictionary_addProfile_TrajOptPlanProfile, \ - ProfileDictionary_addProfile_TrajOptCompositeProfile + TrajOptMotionPlanner import os import re @@ -77,12 +77,9 @@ profiles = ProfileDictionary() ProfileDictionary_addProfile_OMPLPlanProfile(profiles,OMPL_DEFAULT_NAMESPACE, "TEST_PROFILE", plan_profile) -cur_state = t_env.getState() - request = PlannerRequest() request.instructions = program request.env = t_env -request.env_state = cur_state request.profiles = profiles ompl_planner = OMPLMotionPlanner(OMPL_DEFAULT_NAMESPACE) @@ -91,7 +88,7 @@ assert response.successful results_instruction = response.results -interpolated_results_instruction = generateInterpolatedProgram(results_instruction, cur_state, t_env, 3.14, 1.0, 3.14, 10) +interpolated_results_instruction = generateInterpolatedProgram(results_instruction, t_env, 3.14, 1.0, 3.14, 10) trajopt_plan_profile = TrajOptDefaultPlanProfile() trajopt_composite_profile = TrajOptDefaultCompositeProfile() @@ -105,7 +102,6 @@ trajopt_request = PlannerRequest() request.instructions = interpolated_results_instruction request.env = t_env -request.env_state = cur_state request.profiles = trajopt_profiles trajopt_response = trajopt_planner.solve(request) @@ -115,9 +111,13 @@ time_parameterization = TimeOptimalTrajectoryGeneration() instructions_trajectory = InstructionsTrajectory(trajopt_results_instruction) -max_velocity = np.array([2.088, 2.082, 3.27, 3.6, 3.3, 3.078],dtype=np.float64) -max_acceleration = np.array([ 1, 1, 1, 1, 1, 1],dtype=np.float64) -assert time_parameterization.computeTimeStamps(instructions_trajectory, max_velocity, max_acceleration) +max_velocity = np.array([[2.088, 2.082, 3.27, 3.6, 3.3, 3.078]],dtype=np.float64) +max_velocity = np.hstack((-max_velocity.T, max_velocity.T)) +max_acceleration = np.array([[ 1, 1, 1, 1, 1, 1]],dtype=np.float64) +max_acceleration = np.hstack((-max_acceleration.T, max_acceleration.T)) +max_jerk = np.array([[ 1, 1, 1, 1, 1, 1]],dtype=np.float64) +max_jerk = np.hstack((-max_jerk.T, max_jerk.T)) +assert time_parameterization.compute(instructions_trajectory, max_velocity, max_acceleration, max_jerk) trajopt_results = trajopt_results_instruction.flatten() viewer.update_trajectory(trajopt_results) diff --git a/tesseract_viewer_python/examples/tesseract_material_mesh_viewer.py b/tesseract_viewer_python/examples/tesseract_material_mesh_viewer.py index fe48bccac..04e9d6058 100644 --- a/tesseract_viewer_python/examples/tesseract_material_mesh_viewer.py +++ b/tesseract_viewer_python/examples/tesseract_material_mesh_viewer.py @@ -1,5 +1,5 @@ from tesseract_robotics.tesseract_environment import Environment -from tesseract_robotics.tesseract_common import ResourceLocator, SimpleLocatedResource +from tesseract_robotics.tesseract_common import GeneralResourceLocator import os import re import traceback @@ -47,36 +47,10 @@ """ -TESSERACT_SUPPORT_DIR = os.environ["TESSERACT_SUPPORT_DIR"] - -class TesseractSupportResourceLocator(ResourceLocator): - def __init__(self): - super().__init__() - - def locateResource(self, url): - try: - try: - if os.path.exists(url): - return SimpleLocatedResource(url, url, self) - except: - pass - url_match = re.match(r"^package:\/\/tesseract_support\/(.*)$",url) - if (url_match is None): - print("url_match failed") - return None - if not "TESSERACT_SUPPORT_DIR" in os.environ: - return None - tesseract_support = os.environ["TESSERACT_SUPPORT_DIR"] - filename = os.path.join(tesseract_support, os.path.normpath(url_match.group(1))) - ret = SimpleLocatedResource(url, filename, self) - return ret - except: - traceback.print_exc() - t_env = Environment() # locator_fn must be kept alive by maintaining a reference -locator=TesseractSupportResourceLocator() +locator=GeneralResourceLocator() t_env.init(shapes_urdf, locator) viewer = TesseractViewer() diff --git a/tesseract_viewer_python/package.xml b/tesseract_viewer_python/package.xml index e6888570b..eb431ae5c 100644 --- a/tesseract_viewer_python/package.xml +++ b/tesseract_viewer_python/package.xml @@ -1,7 +1,7 @@ tesseract_viewer_python - 0.2.5 + 0.3.0 The tesseract_viewer_python package John Wason Apache 2.0 diff --git a/tesseract_viewer_python/tesseract_robotics_viewer/tesseract_env_to_gltf.py b/tesseract_viewer_python/tesseract_robotics_viewer/tesseract_env_to_gltf.py index 8b6cb572b..d46d53c82 100644 --- a/tesseract_viewer_python/tesseract_robotics_viewer/tesseract_env_to_gltf.py +++ b/tesseract_viewer_python/tesseract_robotics_viewer/tesseract_env_to_gltf.py @@ -164,7 +164,7 @@ def _append_link_recursive(gltf_dict, gltf_buf_io, link_map, joint_map, link_nam for visual in link.visual: visual_i += 1 _, visual_ind = _append_link_visual(gltf_dict, gltf_buf_io, link_name, visual, visual_i, shapes_mesh_inds) - child_inds.append(visual_ind) + child_inds.extend(visual_ind) child_joints = _find_child_joints(joint_map, link_name) for j in child_joints: @@ -182,121 +182,100 @@ def _append_link_recursive(gltf_dict, gltf_buf_io, link_map, joint_map, link_nam return link_node, link_ind -def _append_link_visual(gltf_dict, gltf_buf_io, link_name, visual, visual_i, shapes_mesh_inds): - - visual_u_name = visual.name + str(visual_i) - visual_name = "link_" + link_name + "_visual_" + visual_u_name - visual_node, visual_ind = _append_node(gltf_dict, visual.origin, visual_name) - +def _convert_mesh(gltf_dict, gltf_buf_io, visual_node, visual_name, mesh): tf_material = None - - visual_geom = visual.geometry - if (isinstance(visual_geom,tesseract_geometry.PolygonMesh)): + vertices = mesh.getVertices() + positions = np.zeros((len(vertices),3),dtype=np.float32) + for i in range(len(vertices)): + positions[i,:] = vertices[i].flatten() - mesh=visual_geom - vertices = mesh.getVertices() - positions = np.zeros((len(vertices),3),dtype=np.float32) - for i in range(len(vertices)): - positions[i,:] = vertices[i].flatten() - - indices = mesh.getFaces().flatten().astype(np.uint32).reshape((-1,4))[:,1:4].flatten() - - _, positions_ind = _append_accessor(gltf_dict, gltf_buf_io, positions) - _, indices_ind = _append_accessor(gltf_dict, gltf_buf_io, indices) - - normals_ind = None - normals = mesh.getNormals() - if normals is not None: - normals2 = np.zeros((len(normals),3),dtype=np.float32) - for i in range(len(normals)): - normals2[i,:] = normals[i].flatten() - _, normals_ind = _append_accessor(gltf_dict, gltf_buf_io, normals2) - - mesh_dict, mesh_ind = _append_dict_list(gltf_dict, "meshes", { - "primitives": [ - { - "attributes": { - "POSITION": positions_ind - }, - "indices": indices_ind - } - ], - "name": visual_name + "_mesh" - }) + indices = mesh.getFaces().flatten().astype(np.uint32).reshape((-1,4))[:,1:4].flatten() + + _, positions_ind = _append_accessor(gltf_dict, gltf_buf_io, positions) + _, indices_ind = _append_accessor(gltf_dict, gltf_buf_io, indices) + + normals_ind = None + normals = mesh.getNormals() + if normals is not None: + normals2 = np.zeros((len(normals),3),dtype=np.float32) + for i in range(len(normals)): + normals2[i,:] = normals[i].flatten() + _, normals_ind = _append_accessor(gltf_dict, gltf_buf_io, normals2) + + mesh_dict, mesh_ind = _append_dict_list(gltf_dict, "meshes", { + "primitives": [ + { + "attributes": { + "POSITION": positions_ind + }, + "indices": indices_ind + } + ], + "name": visual_name + "_mesh" + }) - # if normals_ind is not None: - # mesh_dict["primitives"][0]["attributes"]["NORMAL"] = normals_ind + # if normals_ind is not None: + # mesh_dict["primitives"][0]["attributes"]["NORMAL"] = normals_ind - visual_node["scale"] = list(mesh.getScale().flatten()) + visual_node["scale"] = list(mesh.getScale().flatten()) - if not mesh.getResource().getUrl().lower().endswith('.stl'): - mesh_material = mesh.getMaterial() - if mesh_material is not None: + if not mesh.getResource().getUrl().lower().endswith('.stl'): + mesh_material = mesh.getMaterial() + if mesh_material is not None: + + tf_material = { + "name": "material_" + visual_name, + "alphaMode": "MASK", + "alphaCutoff": 0.4 + } + + base_color_factor = mesh_material.getBaseColorFactor().flatten().tolist() - tf_material = { - "name": "material_" + visual_name, - "alphaMode": "MASK", - "alphaCutoff": 0.4 - } - - base_color_factor = mesh_material.getBaseColorFactor().flatten().tolist() - - tf_material["pbrMetallicRoughness"] = { - "baseColorFactor": base_color_factor, - "roughnessFactor": mesh_material.getRoughnessFactor(), - "metallicFactor": mesh_material.getMetallicFactor(), + tf_material["pbrMetallicRoughness"] = { + "baseColorFactor": base_color_factor, + "roughnessFactor": mesh_material.getRoughnessFactor(), + "metallicFactor": mesh_material.getMetallicFactor(), + } + + mesh_textures = mesh.getTextures() + if mesh_textures is not None and len(mesh_textures) > 0: + mesh_tex = mesh_textures[0] + mesh_tex_image = mesh_tex.getTextureImage() + tex_name = mesh_tex_image.getUrl() + tex_mimetype = "image/jpg" + if tex_name.endswith('png'): + tex_mimetype = "image/png" + + mesh_tex_image_bytes = bytearray(mesh_tex_image.getResourceContents()) + tex_img = cv2.imdecode(np.frombuffer(mesh_tex_image_bytes,dtype=np.uint8),flags=1) + img_h, img_w, _ = tex_img.shape + _, image_bufview_ind = _append_bufview(gltf_dict, gltf_buf_io, mesh_tex_image_bytes) + _, image_ind = _append_dict_list(gltf_dict, "images", { + "mimeType": tex_mimetype, + "bufferView": image_bufview_ind + }) + + _, tex_ind = _append_dict_list(gltf_dict, "textures", { + "source": image_ind + }) + + tf_material["pbrMetallicRoughness"]["baseColorTexture"] = { + "index": tex_ind, + "texCoord": 0 } - mesh_textures = mesh.getTextures() - if mesh_textures is not None and len(mesh_textures) > 0: - mesh_tex = mesh_textures[0] - mesh_tex_image = mesh_tex.getTextureImage() - tex_name = mesh_tex_image.getUrl() - tex_mimetype = "image/jpg" - if tex_name.endswith('png'): - tex_mimetype = "image/png" - - mesh_tex_image_bytes = bytearray(mesh_tex_image.getResourceContents()) - tex_img = cv2.imdecode(np.frombuffer(mesh_tex_image_bytes,dtype=np.uint8),flags=1) - img_h, img_w, _ = tex_img.shape - _, image_bufview_ind = _append_bufview(gltf_dict, gltf_buf_io, mesh_tex_image_bytes) - _, image_ind = _append_dict_list(gltf_dict, "images", { - "mimeType": tex_mimetype, - "bufferView": image_bufview_ind - }) - - _, tex_ind = _append_dict_list(gltf_dict, "textures", { - "source": image_ind - }) - - tf_material["pbrMetallicRoughness"]["baseColorTexture"] = { - "index": tex_ind, - "texCoord": 0 - } - - mesh_uvs = mesh_tex.getUVs() - tf_uvs = np.zeros((len(mesh_uvs),2),dtype=np.float32) - for i in range(len(mesh_uvs)): - tf_uvs1 = mesh_uvs[i].flatten() - tf_uvs[i,:] = [tf_uvs1[0], 1.0-tf_uvs1[1]] - _, tex_ind = _append_accessor(gltf_dict, gltf_buf_io, tf_uvs) - - mesh_dict["primitives"][0]["attributes"]["TEXCOORD_0"] = tex_ind + mesh_uvs = mesh_tex.getUVs() + tf_uvs = np.zeros((len(mesh_uvs),2),dtype=np.float32) + for i in range(len(mesh_uvs)): + tf_uvs1 = mesh_uvs[i].flatten() + tf_uvs[i,:] = [tf_uvs1[0], 1.0-tf_uvs1[1]] + _, tex_ind = _append_accessor(gltf_dict, gltf_buf_io, tf_uvs) - elif (isinstance(visual_geom,tesseract_geometry.Box)): - box=visual_geom - mesh_dict, mesh_ind = _append_shape_mesh(gltf_dict, gltf_buf_io, "cube_geometry", visual_name, shapes_mesh_inds) - visual_node["scale"] = [0.5*box.getX(), 0.5*box.getY(), 0.5*box.getZ()] + mesh_dict["primitives"][0]["attributes"]["TEXCOORD_0"] = tex_ind - elif (isinstance(visual_geom,tesseract_geometry.Sphere)): - sphere=visual_geom - mesh_dict, mesh_ind = _append_shape_mesh(gltf_dict, gltf_buf_io, "sphere_geometry", visual_name, shapes_mesh_inds) - visual_node["scale"] = [sphere.getRadius(), sphere.getRadius(), sphere.getRadius()] + return mesh_dict, mesh_ind, tf_material - elif (isinstance(visual_geom,tesseract_geometry.Cylinder)): - cylinder=visual_geom - mesh_dict, mesh_ind = _append_shape_mesh(gltf_dict, gltf_buf_io, "cylinder_geometry", visual_name, shapes_mesh_inds) - visual_node["scale"] = [cylinder.getRadius(), cylinder.getRadius(), 0.5*cylinder.getLength()] +def _apply_material(gltf_dict, gltf_buf_io, mesh_dict, visual_name, tf_material, visual_material): if tf_material is None: tf_material = { @@ -305,7 +284,7 @@ def _append_link_visual(gltf_dict, gltf_buf_io, link_name, visual, visual_i, sha "alphaCutoff": 0.4 } - material = visual.material + material = visual_material if material is None: tf_color = [0.5,0.5,0.5,1] @@ -322,10 +301,63 @@ def _append_link_visual(gltf_dict, gltf_buf_io, link_name, visual, visual_i, sha mesh_dict["primitives"][0]["material"] = material_ind +def _append_link_visual(gltf_dict, gltf_buf_io, link_name, visual, visual_i, shapes_mesh_inds): + + visual_geom = visual.geometry + + visual_u_name = visual.name + str(visual_i) + visual_name = "link_" + link_name + "_visual_" + visual_u_name + + if (isinstance(visual_geom,tesseract_geometry.CompoundMesh)): + meshes = visual_geom.getMeshes() + mesh_count = 0 + visual_nodes = [] + visual_inds = [] + for m in meshes: + visual_name1 = visual_name + "_mesh_" + str(mesh_count) + visual_node1, visual_ind1 = _append_node(gltf_dict, visual.origin, visual_name1) + mesh_dict1, mesh_ind1, tf_material1 = _convert_mesh(gltf_dict, gltf_buf_io, visual_node1, visual_name1, m) + _apply_material(gltf_dict, gltf_buf_io, mesh_dict1, visual_name1, tf_material1, visual.material) + visual_node1["mesh"] = mesh_ind1 + visual_nodes.append(visual_node1) + visual_inds.append(visual_ind1) + print(mesh_dict1) + mesh_count += 1 + + return visual_nodes, visual_inds + + visual_node, visual_ind = _append_node(gltf_dict, visual.origin, visual_name) + + tf_material = None + + if (isinstance(visual_geom,tesseract_geometry.PolygonMesh)): + + mesh=visual_geom + + mesh_dict, mesh_ind, tf_material = _convert_mesh(gltf_dict, gltf_buf_io, visual_node, visual_name, mesh) + + + elif (isinstance(visual_geom,tesseract_geometry.Box)): + box=visual_geom + mesh_dict, mesh_ind = _append_shape_mesh(gltf_dict, gltf_buf_io, "cube_geometry", visual_name, shapes_mesh_inds) + visual_node["scale"] = [0.5*box.getX(), 0.5*box.getY(), 0.5*box.getZ()] + + elif (isinstance(visual_geom,tesseract_geometry.Sphere)): + sphere=visual_geom + mesh_dict, mesh_ind = _append_shape_mesh(gltf_dict, gltf_buf_io, "sphere_geometry", visual_name, shapes_mesh_inds) + visual_node["scale"] = [sphere.getRadius(), sphere.getRadius(), sphere.getRadius()] + + elif (isinstance(visual_geom,tesseract_geometry.Cylinder)): + cylinder=visual_geom + mesh_dict, mesh_ind = _append_shape_mesh(gltf_dict, gltf_buf_io, "cylinder_geometry", visual_name, shapes_mesh_inds) + visual_node["scale"] = [cylinder.getRadius(), cylinder.getRadius(), 0.5*cylinder.getLength()] + + _apply_material(gltf_dict, gltf_buf_io, mesh_dict, visual_name, tf_material, visual.material) + visual_node["mesh"] = mesh_ind - return visual_node, visual_ind + return [visual_node], [visual_ind] _COMPONENT_TYPE_INT8 = 5120 _COMPONENT_TYPE_UINT8 = 5121