Compare commits

...

414 Commits

Author SHA1 Message Date
Captain Yoshi
1655762a63
Avoid duplicate scenes in Solution.msg from generator stages (#639)
Some checks failed
CI / ${{ matrix.env.IMAGE }}${{ matrix.env.NAME && ' • ' || ''}}${{ matrix.env.NAME }}${{ matrix.env.CATKIN_LINT && ' • catkin_lint' || ''}}${{ matrix.env.CLANG_TIDY && ' • clang-tidy' || '' }} (map[CLANG_TIDY:true IMAGE:noble-ci-testing TARGET_CMAKE_… (push) Has been cancelled
CI / ${{ matrix.env.IMAGE }}${{ matrix.env.NAME && ' • ' || ''}}${{ matrix.env.NAME }}${{ matrix.env.CATKIN_LINT && ' • catkin_lint' || ''}}${{ matrix.env.CLANG_TIDY && ' • clang-tidy' || '' }} (map[DOCKER_RUN_OPTS:-e PRELOAD=libasan.so.8 -e LSAN_OPTI… (push) Has been cancelled
CI / ${{ matrix.env.IMAGE }}${{ matrix.env.NAME && ' • ' || ''}}${{ matrix.env.NAME }}${{ matrix.env.CATKIN_LINT && ' • catkin_lint' || ''}}${{ matrix.env.CLANG_TIDY && ' • clang-tidy' || '' }} (map[IMAGE:jammy-ci]) (push) Has been cancelled
CI / ${{ matrix.env.IMAGE }}${{ matrix.env.NAME && ' • ' || ''}}${{ matrix.env.NAME }}${{ matrix.env.CATKIN_LINT && ' • catkin_lint' || ''}}${{ matrix.env.CLANG_TIDY && ' • clang-tidy' || '' }} (map[IMAGE:noble-ci NAME:ccov TARGET_CMAKE_ARGS:-DCMAKE_B… (push) Has been cancelled
Format / pre-commit (push) Has been cancelled
CI / doc (push) Has been cancelled
CI / deploy (push) Has been cancelled
If start and end scene of a stage are identical (e.g. from a generator), we can use an (empty) scene diff as well.
2025-09-10 17:33:31 +02:00
Gauthier Hentz
7384702448
Allow max Cartesian link speed in PlannerInterface (#277)
Some checks are pending
CI / ${{ matrix.env.IMAGE }}${{ matrix.env.NAME && ' • ' || ''}}${{ matrix.env.NAME }}${{ matrix.env.CATKIN_LINT && ' • catkin_lint' || ''}}${{ matrix.env.CLANG_TIDY && ' • clang-tidy' || '' }} (map[CLANG_TIDY:true IMAGE:noble-ci-testing TARGET_CMAKE_… (push) Waiting to run
CI / ${{ matrix.env.IMAGE }}${{ matrix.env.NAME && ' • ' || ''}}${{ matrix.env.NAME }}${{ matrix.env.CATKIN_LINT && ' • catkin_lint' || ''}}${{ matrix.env.CLANG_TIDY && ' • clang-tidy' || '' }} (map[DOCKER_RUN_OPTS:-e PRELOAD=libasan.so.8 -e LSAN_OPTI… (push) Waiting to run
CI / ${{ matrix.env.IMAGE }}${{ matrix.env.NAME && ' • ' || ''}}${{ matrix.env.NAME }}${{ matrix.env.CATKIN_LINT && ' • catkin_lint' || ''}}${{ matrix.env.CLANG_TIDY && ' • clang-tidy' || '' }} (map[IMAGE:jammy-ci]) (push) Waiting to run
CI / ${{ matrix.env.IMAGE }}${{ matrix.env.NAME && ' • ' || ''}}${{ matrix.env.NAME }}${{ matrix.env.CATKIN_LINT && ' • catkin_lint' || ''}}${{ matrix.env.CLANG_TIDY && ' • clang-tidy' || '' }} (map[IMAGE:noble-ci NAME:ccov TARGET_CMAKE_ARGS:-DCMAKE_B… (push) Waiting to run
CI / doc (push) Blocked by required conditions
CI / deploy (push) Blocked by required conditions
Format / pre-commit (push) Waiting to run
2025-09-10 14:30:26 +02:00
Robert Haschke
ac4b92fff9 Add missing dependency
Some checks failed
CI / ${{ matrix.env.IMAGE }}${{ matrix.env.NAME && ' • ' || ''}}${{ matrix.env.NAME }}${{ matrix.env.CATKIN_LINT && ' • catkin_lint' || ''}}${{ matrix.env.CLANG_TIDY && ' • clang-tidy' || '' }} (map[CLANG_TIDY:true IMAGE:noble-ci-testing TARGET_CMAKE_… (push) Has been cancelled
CI / ${{ matrix.env.IMAGE }}${{ matrix.env.NAME && ' • ' || ''}}${{ matrix.env.NAME }}${{ matrix.env.CATKIN_LINT && ' • catkin_lint' || ''}}${{ matrix.env.CLANG_TIDY && ' • clang-tidy' || '' }} (map[DOCKER_RUN_OPTS:-e PRELOAD=libasan.so.8 -e LSAN_OPTI… (push) Has been cancelled
CI / ${{ matrix.env.IMAGE }}${{ matrix.env.NAME && ' • ' || ''}}${{ matrix.env.NAME }}${{ matrix.env.CATKIN_LINT && ' • catkin_lint' || ''}}${{ matrix.env.CLANG_TIDY && ' • clang-tidy' || '' }} (map[IMAGE:jammy-ci]) (push) Has been cancelled
CI / ${{ matrix.env.IMAGE }}${{ matrix.env.NAME && ' • ' || ''}}${{ matrix.env.NAME }}${{ matrix.env.CATKIN_LINT && ' • catkin_lint' || ''}}${{ matrix.env.CLANG_TIDY && ' • clang-tidy' || '' }} (map[IMAGE:noble-ci NAME:ccov TARGET_CMAKE_ARGS:-DCMAKE_B… (push) Has been cancelled
Format / pre-commit (push) Has been cancelled
CI / doc (push) Has been cancelled
CI / deploy (push) Has been cancelled
2025-09-01 12:44:58 +02:00
Robert Haschke
e9eab62f5f
Enable collisions visualizations (#708)
Some checks failed
CI / ${{ matrix.env.IMAGE }}${{ matrix.env.NAME && ' • ' || ''}}${{ matrix.env.NAME }}${{ matrix.env.CATKIN_LINT && ' • catkin_lint' || ''}}${{ matrix.env.CLANG_TIDY && ' • clang-tidy' || '' }} (map[CLANG_TIDY:true IMAGE:noble-ci-testing TARGET_CMAKE_… (push) Has been cancelled
CI / ${{ matrix.env.IMAGE }}${{ matrix.env.NAME && ' • ' || ''}}${{ matrix.env.NAME }}${{ matrix.env.CATKIN_LINT && ' • catkin_lint' || ''}}${{ matrix.env.CLANG_TIDY && ' • clang-tidy' || '' }} (map[DOCKER_RUN_OPTS:-e PRELOAD=libasan.so.8 -e LSAN_OPTI… (push) Has been cancelled
CI / ${{ matrix.env.IMAGE }}${{ matrix.env.NAME && ' • ' || ''}}${{ matrix.env.NAME }}${{ matrix.env.CATKIN_LINT && ' • catkin_lint' || ''}}${{ matrix.env.CLANG_TIDY && ' • clang-tidy' || '' }} (map[IMAGE:jammy-ci]) (push) Has been cancelled
CI / ${{ matrix.env.IMAGE }}${{ matrix.env.NAME && ' • ' || ''}}${{ matrix.env.NAME }}${{ matrix.env.CATKIN_LINT && ' • catkin_lint' || ''}}${{ matrix.env.CLANG_TIDY && ' • clang-tidy' || '' }} (map[IMAGE:noble-ci NAME:ccov TARGET_CMAKE_ARGS:-DCMAKE_B… (push) Has been cancelled
Format / pre-commit (push) Has been cancelled
CI / doc (push) Has been cancelled
CI / deploy (push) Has been cancelled
2025-08-22 08:38:44 +02:00
Robert Haschke
336ad4456c
LimitSolutions wrapper stage (#710)
* Add python bindings for PassThrough
* Add LimitSolutions stage
2025-08-22 08:37:09 +02:00
Dhruv Patel
ed99e6b865
Add missing include fmt/ranges.h (#712)
Some checks failed
CI / ${{ matrix.env.IMAGE }}${{ matrix.env.NAME && ' • ' || ''}}${{ matrix.env.NAME }}${{ matrix.env.CATKIN_LINT && ' • catkin_lint' || ''}}${{ matrix.env.CLANG_TIDY && ' • clang-tidy' || '' }} (map[CLANG_TIDY:true IMAGE:noble-ci-testing TARGET_CMAKE_… (push) Has been cancelled
CI / ${{ matrix.env.IMAGE }}${{ matrix.env.NAME && ' • ' || ''}}${{ matrix.env.NAME }}${{ matrix.env.CATKIN_LINT && ' • catkin_lint' || ''}}${{ matrix.env.CLANG_TIDY && ' • clang-tidy' || '' }} (map[DOCKER_RUN_OPTS:-e PRELOAD=libasan.so.8 -e LSAN_OPTI… (push) Has been cancelled
CI / ${{ matrix.env.IMAGE }}${{ matrix.env.NAME && ' • ' || ''}}${{ matrix.env.NAME }}${{ matrix.env.CATKIN_LINT && ' • catkin_lint' || ''}}${{ matrix.env.CLANG_TIDY && ' • clang-tidy' || '' }} (map[IMAGE:jammy-ci]) (push) Has been cancelled
CI / ${{ matrix.env.IMAGE }}${{ matrix.env.NAME && ' • ' || ''}}${{ matrix.env.NAME }}${{ matrix.env.CATKIN_LINT && ' • catkin_lint' || ''}}${{ matrix.env.CLANG_TIDY && ' • clang-tidy' || '' }} (map[IMAGE:noble-ci NAME:ccov TARGET_CMAKE_ARGS:-DCMAKE_B… (push) Has been cancelled
Format / pre-commit (push) Has been cancelled
CI / doc (push) Has been cancelled
CI / deploy (push) Has been cancelled
Signed-off-by: Dhruv Patel <dhruvpatel2991998@gmail.com>
2025-08-17 07:18:57 +02:00
Robert Haschke
bf001bd093 Improve code documentation
Some checks failed
CI / ${{ matrix.env.IMAGE }}${{ matrix.env.NAME && ' • ' || ''}}${{ matrix.env.NAME }}${{ matrix.env.CATKIN_LINT && ' • catkin_lint' || ''}}${{ matrix.env.CLANG_TIDY && ' • clang-tidy' || '' }} (map[CLANG_TIDY:true IMAGE:noble-ci-testing TARGET_CMAKE_… (push) Has been cancelled
CI / ${{ matrix.env.IMAGE }}${{ matrix.env.NAME && ' • ' || ''}}${{ matrix.env.NAME }}${{ matrix.env.CATKIN_LINT && ' • catkin_lint' || ''}}${{ matrix.env.CLANG_TIDY && ' • clang-tidy' || '' }} (map[DOCKER_RUN_OPTS:-e PRELOAD=libasan.so.8 -e LSAN_OPTI… (push) Has been cancelled
CI / ${{ matrix.env.IMAGE }}${{ matrix.env.NAME && ' • ' || ''}}${{ matrix.env.NAME }}${{ matrix.env.CATKIN_LINT && ' • catkin_lint' || ''}}${{ matrix.env.CLANG_TIDY && ' • clang-tidy' || '' }} (map[IMAGE:jammy-ci]) (push) Has been cancelled
CI / ${{ matrix.env.IMAGE }}${{ matrix.env.NAME && ' • ' || ''}}${{ matrix.env.NAME }}${{ matrix.env.CATKIN_LINT && ' • catkin_lint' || ''}}${{ matrix.env.CLANG_TIDY && ' • clang-tidy' || '' }} (map[IMAGE:noble-ci NAME:ccov TARGET_CMAKE_ARGS:-DCMAKE_B… (push) Has been cancelled
Format / pre-commit (push) Has been cancelled
CI / doc (push) Has been cancelled
CI / deploy (push) Has been cancelled
2025-08-13 11:39:38 +02:00
Robert Haschke
87b3701223 CI: Fix Noble builds
Some checks failed
CI / ${{ matrix.env.IMAGE }}${{ matrix.env.NAME && ' • ' || ''}}${{ matrix.env.NAME }}${{ matrix.env.CATKIN_LINT && ' • catkin_lint' || ''}}${{ matrix.env.CLANG_TIDY && ' • clang-tidy' || '' }} (map[CLANG_TIDY:true IMAGE:noble-ci-testing TARGET_CMAKE_… (push) Has been cancelled
CI / ${{ matrix.env.IMAGE }}${{ matrix.env.NAME && ' • ' || ''}}${{ matrix.env.NAME }}${{ matrix.env.CATKIN_LINT && ' • catkin_lint' || ''}}${{ matrix.env.CLANG_TIDY && ' • clang-tidy' || '' }} (map[DOCKER_RUN_OPTS:-e PRELOAD=libasan.so.8 -e LSAN_OPTI… (push) Has been cancelled
CI / ${{ matrix.env.IMAGE }}${{ matrix.env.NAME && ' • ' || ''}}${{ matrix.env.NAME }}${{ matrix.env.CATKIN_LINT && ' • catkin_lint' || ''}}${{ matrix.env.CLANG_TIDY && ' • clang-tidy' || '' }} (map[IMAGE:jammy-ci]) (push) Has been cancelled
CI / ${{ matrix.env.IMAGE }}${{ matrix.env.NAME && ' • ' || ''}}${{ matrix.env.NAME }}${{ matrix.env.CATKIN_LINT && ' • catkin_lint' || ''}}${{ matrix.env.CLANG_TIDY && ' • clang-tidy' || '' }} (map[IMAGE:noble-ci NAME:ccov TARGET_CMAKE_ARGS:-DCMAKE_B… (push) Has been cancelled
Format / pre-commit (push) Has been cancelled
CI / doc (push) Has been cancelled
CI / deploy (push) Has been cancelled
- Fix clang error
  https://stackoverflow.com/questions/27954940/a-using-statement-compiles-with-g-fails-compilation-with-clang
- Don't run pick-place test with asan: it doesn't finish.
- Add PIP_BREAK_SYSTEM_PACKAGES
- Fix coverage creation + upload
2025-08-11 14:57:29 +02:00
Robert Haschke
86f0083566 CI: Drop Noetic build 2025-08-11 10:04:16 +02:00
Robert Haschke
f3659da82b Pick with custom max_velocity_scaling_factor during approach+lift 2025-08-10 11:53:09 +02:00
Robert Haschke
24f22484ca Rework pybind11 ABI compatibility checks
Some checks are pending
CI / ${{ matrix.env.IMAGE }}${{ matrix.env.NAME && ' • ' || ''}}${{ matrix.env.NAME }}${{ matrix.env.CATKIN_LINT && ' • catkin_lint' || ''}}${{ matrix.env.CLANG_TIDY && ' • clang-tidy' || '' }} (map[CLANG_TIDY:true IMAGE:noble-ci-testing TARGET_CMAKE_… (push) Waiting to run
CI / ${{ matrix.env.IMAGE }}${{ matrix.env.NAME && ' • ' || ''}}${{ matrix.env.NAME }}${{ matrix.env.CATKIN_LINT && ' • catkin_lint' || ''}}${{ matrix.env.CLANG_TIDY && ' • clang-tidy' || '' }} (map[DOCKER_RUN_OPTS:-e PRELOAD=libasan.so.5 -e LSAN_OPTI… (push) Waiting to run
CI / ${{ matrix.env.IMAGE }}${{ matrix.env.NAME && ' • ' || ''}}${{ matrix.env.NAME }}${{ matrix.env.CATKIN_LINT && ' • catkin_lint' || ''}}${{ matrix.env.CLANG_TIDY && ' • clang-tidy' || '' }} (map[IMAGE:jammy-ci]) (push) Waiting to run
CI / ${{ matrix.env.IMAGE }}${{ matrix.env.NAME && ' • ' || ''}}${{ matrix.env.NAME }}${{ matrix.env.CATKIN_LINT && ' • catkin_lint' || ''}}${{ matrix.env.CLANG_TIDY && ' • clang-tidy' || '' }} (map[IMAGE:noble-ci NAME:ccov TARGET_CMAKE_ARGS:-DCMAKE_B… (push) Waiting to run
CI / doc (push) Blocked by required conditions
CI / deploy (push) Blocked by required conditions
Format / pre-commit (push) Waiting to run
2025-08-10 11:53:09 +02:00
Robert Haschke
717771e25a Remove pybind11 submodule 2025-08-10 11:53:09 +02:00
Robert Haschke
d19b77782d Upgrade pybind11 to v3 2025-08-10 10:10:53 +02:00
Robert Haschke
c6521551a4 Update pre-commit versions 2025-08-07 12:42:39 +02:00
Robert Haschke
580dac9151 Modernize declaration of compile options 2025-08-07 12:42:39 +02:00
Robert Haschke
5ec63045e8 Factor out Python property handling
... to allow for reuse in custom Python wrappers
2025-08-07 12:42:39 +02:00
Robert Haschke
0cc398797f CI: Add noble-ci-testing
Some checks failed
CI / ${{ matrix.env.IMAGE }}${{ matrix.env.NAME && ' • ' || ''}}${{ matrix.env.NAME }}${{ matrix.env.CATKIN_LINT && ' • catkin_lint' || ''}}${{ matrix.env.CLANG_TIDY && ' • clang-tidy' || '' }} (map[CLANG_TIDY:true IMAGE:noble-ci-testing TARGET_CMAKE_… (push) Has been cancelled
CI / ${{ matrix.env.IMAGE }}${{ matrix.env.NAME && ' • ' || ''}}${{ matrix.env.NAME }}${{ matrix.env.CATKIN_LINT && ' • catkin_lint' || ''}}${{ matrix.env.CLANG_TIDY && ' • clang-tidy' || '' }} (map[DOCKER_RUN_OPTS:-e PRELOAD=libasan.so.5 -e LSAN_OPTI… (push) Has been cancelled
CI / ${{ matrix.env.IMAGE }}${{ matrix.env.NAME && ' • ' || ''}}${{ matrix.env.NAME }}${{ matrix.env.CATKIN_LINT && ' • catkin_lint' || ''}}${{ matrix.env.CLANG_TIDY && ' • clang-tidy' || '' }} (map[IMAGE:jammy-ci]) (push) Has been cancelled
CI / ${{ matrix.env.IMAGE }}${{ matrix.env.NAME && ' • ' || ''}}${{ matrix.env.NAME }}${{ matrix.env.CATKIN_LINT && ' • catkin_lint' || ''}}${{ matrix.env.CLANG_TIDY && ' • clang-tidy' || '' }} (map[IMAGE:noble-ci NAME:ccov TARGET_CMAKE_ARGS:-DCMAKE_B… (push) Has been cancelled
Format / pre-commit (push) Has been cancelled
CI / doc (push) Has been cancelled
CI / deploy (push) Has been cancelled
2025-05-02 10:48:26 +02:00
Robert Haschke
5a0059dc83 Fix pick+place: connect should plan both, arm and hand motion
Some checks failed
CI / ${{ matrix.env.IMAGE }}${{ matrix.env.NAME && ' • ' || ''}}${{ matrix.env.NAME }}${{ matrix.env.CATKIN_LINT && ' • catkin_lint' || ''}}${{ matrix.env.CLANG_TIDY && ' • clang-tidy' || '' }} (map[CLANG_TIDY:true IMAGE:noble-ci TARGET_CMAKE_ARGS:-DC… (push) Has been cancelled
CI / ${{ matrix.env.IMAGE }}${{ matrix.env.NAME && ' • ' || ''}}${{ matrix.env.NAME }}${{ matrix.env.CATKIN_LINT && ' • catkin_lint' || ''}}${{ matrix.env.CLANG_TIDY && ' • clang-tidy' || '' }} (map[DOCKER_RUN_OPTS:-e PRELOAD=libasan.so.5 -e LSAN_OPTI… (push) Has been cancelled
CI / ${{ matrix.env.IMAGE }}${{ matrix.env.NAME && ' • ' || ''}}${{ matrix.env.NAME }}${{ matrix.env.CATKIN_LINT && ' • catkin_lint' || ''}}${{ matrix.env.CLANG_TIDY && ' • clang-tidy' || '' }} (map[IMAGE:jammy-ci]) (push) Has been cancelled
CI / ${{ matrix.env.IMAGE }}${{ matrix.env.NAME && ' • ' || ''}}${{ matrix.env.NAME }}${{ matrix.env.CATKIN_LINT && ' • catkin_lint' || ''}}${{ matrix.env.CLANG_TIDY && ' • clang-tidy' || '' }} (map[IMAGE:noble-ci NAME:ccov TARGET_CMAKE_ARGS:-DCMAKE_B… (push) Has been cancelled
Format / pre-commit (push) Has been cancelled
CI / doc (push) Has been cancelled
CI / deploy (push) Has been cancelled
2025-04-29 14:25:35 +02:00
Captain Yoshi
a0da41a4aa
Fix clamping of joint constraints (#665)
Some checks failed
CI / ${{ matrix.env.IMAGE }}${{ matrix.env.NAME && ' • ' || ''}}${{ matrix.env.NAME }}${{ matrix.env.CATKIN_LINT && ' • catkin_lint' || ''}}${{ matrix.env.CLANG_TIDY && ' • clang-tidy' || '' }} (map[CLANG_TIDY:true IMAGE:noble-ci TARGET_CMAKE_ARGS:-DC… (push) Has been cancelled
CI / ${{ matrix.env.IMAGE }}${{ matrix.env.NAME && ' • ' || ''}}${{ matrix.env.NAME }}${{ matrix.env.CATKIN_LINT && ' • catkin_lint' || ''}}${{ matrix.env.CLANG_TIDY && ' • clang-tidy' || '' }} (map[DOCKER_RUN_OPTS:-e PRELOAD=libasan.so.5 -e LSAN_OPTI… (push) Has been cancelled
CI / ${{ matrix.env.IMAGE }}${{ matrix.env.NAME && ' • ' || ''}}${{ matrix.env.NAME }}${{ matrix.env.CATKIN_LINT && ' • catkin_lint' || ''}}${{ matrix.env.CLANG_TIDY && ' • clang-tidy' || '' }} (map[IMAGE:jammy-ci]) (push) Has been cancelled
CI / ${{ matrix.env.IMAGE }}${{ matrix.env.NAME && ' • ' || ''}}${{ matrix.env.NAME }}${{ matrix.env.CATKIN_LINT && ' • catkin_lint' || ''}}${{ matrix.env.CLANG_TIDY && ' • clang-tidy' || '' }} (map[IMAGE:noble-ci NAME:ccov TARGET_CMAKE_ARGS:-DCMAKE_B… (push) Has been cancelled
Format / pre-commit (push) Has been cancelled
CI / doc (push) Has been cancelled
CI / deploy (push) Has been cancelled
Do not enforce position bounds (by clamping to valid positions), but let the stage fail if joints are outside the limits.
2025-02-25 13:12:53 +01:00
Robert Haschke
bad8e13254 Fix error message
... due to copy-paster error in 9ea1692f96
2025-02-11 08:10:18 +01:00
Robert Haschke
9a98a252a0
Provide action feedback during task execution (#653) 2025-02-11 07:28:05 +01:00
Robert Haschke
9ea1692f96
Correctly report failures instead of issueing console warnings
Don't use command-line warnings, but spawn failure solutions.
2025-02-06 15:09:15 +01:00
Robert Haschke
8318546992 Increase minimum required CMake version to 3.16
supported by Ubuntu 20.04
2025-01-19 15:55:58 +01:00
Robert Haschke
d2c5150ec9 Python API: Allow passing a task's introspection object to SolutionBase::toMsg()
which is a prerequisite for correct filling of IDs.
2025-01-13 14:30:41 +01:00
Ali Haider
a924b566fb
Fix typo 2025-01-05 23:09:58 +01:00
Robert Haschke
2d0244b921 CI: Fix doc job 2024-12-26 13:22:38 +01:00
Robert Haschke
4dfcf09516 clang-format-14 2024-12-22 14:09:08 +01:00
Robert Haschke
cd0ab8838c CI: Add Noble build 2024-12-22 14:09:08 +01:00
Robert Haschke
8c0609a711 clang-tidy fixes: std::endl -> '\n' 2024-12-22 12:29:05 +01:00
Robert Haschke
45ea86c163 clang-tidy fixes: use uint8_t enums 2024-12-22 12:28:58 +01:00
Robert Haschke
5067dbb432 clang-tidy fixes 2024-12-22 12:28:26 +01:00
Robert Haschke
28b3e24a26 CI: Drop obsolete clang-tidy option AnalyzeTemporaryDtors 2024-12-21 17:41:29 +01:00
JafarAbdi
5d23cb89df Add support for GenerateRandomPose 2024-12-09 09:53:07 +01:00
Robert Haschke
f29784e750 Improve error message
Indicate source of error as CartesianPath.
2024-12-06 12:07:15 +01:00
Robert Haschke
d5e1376b08 Add Jammy build 2024-12-04 14:23:44 +01:00
JafarAbdi
f5a0a545e5 python: Add Task::setRobotModel 2024-12-04 13:26:33 +01:00
Robert Haschke
821b5951f1 Silent pybind11 warning 2024-12-02 11:21:22 +01:00
Robert Haschke
1acf72e0b4 examples: add orientation path constraint
constrained.py: constrain orientation of attached object
pickplace.py: keep object upright during transport
2024-10-15 17:07:17 +02:00
Robert Haschke
69b4606bca CI: Return to custom cache action 2024-10-13 23:53:06 +02:00
Robert Haschke
0a502ce7fe Add path_constraints property to Connect stage 2024-10-11 22:11:33 +02:00
Michael Görner
721ff356b7
provide a fmt wrapper (#615)
for special eigen formatter, which is not available in fmt9

https://github.com/fmtlib/fmt/issues/3465
https://stackoverflow.com/a/73755864/21260084
2024-09-24 20:41:12 +02:00
Michael Görner
d50c846a7b
Ignore Debian-specific catkin_lint error around urdfdom_headers (#614)
Without the patch:

$ catkin_lint rviz_marker_tools/
rviz_marker_tools: package.xml: error: missing build_depend on 'urdfdom_headers'

requires catkin_lint 1.6.24 for successful error suppression.
2024-09-24 20:32:42 +02:00
Robert Haschke
99ccc115e0
Update API: JumpThreshold -> CartesianPrecision (#611)
Python: Access properties via writable references
This allows to modify properties in place, e.g. cartesian_solver.precision.translational = 0.01
2024-09-17 10:59:14 +02:00
Michael Görner
5a44808146
clean up dependencies for rviz_marker_tools (#610)
liburdfdom-dev is not actually used. Only liburdfdom-headers-dev.
2024-09-02 22:52:47 +02:00
Robert Haschke
184d39897a Add missing test dependency 2024-09-02 11:20:45 +02:00
Robert Haschke
237a07f96f CI: Update actions 2024-09-02 09:25:12 +02:00
Captain Yoshi
fdc06c3b91
Reduce stop time due to preempt (#598)
The preempt_request_ flag was only checked at the top-level task container before each compute iteration.
As a single sweep might take a while, we should check the flag before computing each stage.
2024-07-19 15:46:12 +02:00
Robert Haschke
4f69a22ddb Silent error "Found empty JointState message" 2024-07-18 13:48:32 +02:00
Robert Haschke
7631486648
Add unittest for #581 2024-07-17 15:10:53 +02:00
Captain Yoshi
cd28bdcd1f
Fix early planning preemption (#597)
Calling preempt() before plan() is able to reset the preempt_requested_ flag causes the preemption request to get lost. To avoid this issue, we allow a) manual resetting of the request and b) reset the request before leaving plan().
2024-07-17 14:56:01 +02:00
Paul Gesel
60ccd74443
MoveRelative: fix segfault on empty trajectory (#595)
Check that at least one robot state exists in the robot trajectory before accessing it.

Signed-off-by: Paul Gesel <paul.gesel@picknik.ai>
2024-07-16 17:18:16 +02:00
Robert Haschke
2e9a223827
MoveRelative: handle equal min/max distance (#593)
When min_distance == max_distance > 0.0, the minimal distance might be missed due to numerical errors.
To avoid this, deactivate the minimal distance check and run the full distance as given by max_distance.
2024-07-16 08:24:55 +02:00
Robert Haschke
8d2baf2739 Cleanup unit tests
... and allow them to run via both, cmdline and pytest
2024-07-13 01:59:22 +02:00
Robert Haschke
6d376fb8b9 Connect: Relax validity check of reached end state 2024-07-12 06:31:33 +02:00
Robert Haschke
0fed09d431 Unify Python demo scripts 2024-07-12 06:31:33 +02:00
Robert Haschke
93ef98ad67 Switch shebang to python3 2024-07-11 15:44:28 +02:00
Robert Haschke
4debc37904 Silence gcc's overloaded-virtual warnings 2024-07-11 09:50:35 +02:00
Fabian Schuetze
702710dec5
Improve comments for pick-and-place task (#238) 2024-07-06 19:59:05 +02:00
Robert Haschke
fbc05e4496
Add property to enable/disable pruning at runtime (#590) 2024-07-06 16:51:14 +02:00
Robert Haschke
907014cd57 Disable pruning by default 2024-07-04 12:05:31 +02:00
Robert Haschke
93a3964138 test_pruning.cpp: Add new test 2024-07-04 11:57:31 +02:00
Robert Haschke
48bde8daab test_pruning.cpp: Extend test to ParallelContainer 2024-07-04 11:57:31 +02:00
Robert Haschke
42a08d6444 PassThrough: cleanup unused headers 2024-07-04 11:57:31 +02:00
Robert Haschke
a84479cc58 Avoid segfault if TimeParameterization is not set 2024-07-03 09:32:30 +02:00
Robert Haschke
b1336dc210 CartesianPath: allow ik_frame definition
... if start and end are given as joint-space poses
2024-06-29 18:02:05 +02:00
Robert Haschke
177e19de1f Generalize utils::getRobotTipForFrame()
... to return error_msg instead of calling markAsFailure() on a solution
2024-06-29 17:55:45 +02:00
Robert Haschke
8fa66a6d2c Update pybind11 to version 2.12.0
Currently used branch was not interoperable with a standard version of pybind11.
2024-05-29 15:43:46 +02:00
VideoSystemsTech
ad5c878f19
ComputeIK: Allow additional constraints for filtering solutions (#464)
Add "constraint" property.

Co-authored-by: Robert Haschke <rhaschke@techfak.uni-bielefeld.de>
2024-05-28 16:54:10 +02:00
VideoSystemsTech
227d475282 Expose MultiPlanner to Python (#474)
Co-authored-by: Robert Haschke <rhaschke@techfak.uni-bielefeld.de>
2024-05-27 19:17:45 +02:00
Captain Yoshi
3b4ea48c18
Add unittest cartesianCollisionMinMaxDistance (#538)
... to illustrate that MoveRelative's min-max constraint fails with PipelinePlanners (e.g. Pilz) returning a partially invalid trajectory: MTC does not truncate the trajectory to its valid part and thus fails, even if the valid part fits the given min-max range. This logic is only supported for the CartesianPath planner for now.

Co-authored-by: Robert Haschke <rhaschke@techfak.uni-bielefeld.de>
2024-05-27 16:16:57 +02:00
Robert Haschke
92efc14043 Rename ros-planning org 2024-05-25 21:30:30 +02:00
Robert Haschke
7666f73e04
Simplify formatting code with https://github.com/fmtlib (#499)
* Simplify formatting code with https://github.com/fmtlib
* Update to clang-format-12
2024-05-25 19:12:42 +02:00
Captain Yoshi
a0c00646c2
Add NoOp stage (#534)
This stage can be used to add arbitrary user-defined properties w/o modifying the PlanningScene state or adding a trajectory.

Co-authored-by: Robert Haschke <rhaschke@techfak.uni-bielefeld.de>
2024-05-24 14:34:27 +02:00
Robert Haschke
54e653ebdb Fix failing assertion
The cost of a newly created Priority does not need to be finite.
An example occurs in the Pick+Place demo: the CurrentState solution
is filtered by an applicibility test, which may set the cost to infinity
while lifting the solution to the wrapper stage.
In this case, new InterfaceStates are created from the infinite cost solution.
Consequently, the state should be marked as PRUNED.
2024-05-24 12:02:20 +02:00
Robert Haschke
9c05305eff ModifyPlanningScene: check state for collisions 2024-05-24 10:49:14 +02:00
Robert Haschke
a8896e4e5d Improve TypeError exceptions
- use pybind11 API instead of Python C API
- provide more informative error messages
2024-05-23 14:08:49 +02:00
Robert Haschke
74b1e5e5d3 CI: prerelease.yaml: make ROS_DISTRO an interactive input 2024-05-23 14:08:49 +02:00
Robert Haschke
8ede80d26d Example of constrained orientation planning 2024-05-23 14:08:49 +02:00
Robert Haschke
92b917d264 CI: update actions 2024-05-23 14:08:49 +02:00
Robert Haschke
e2812d9333 Drop Kinetic support 2024-05-23 14:08:49 +02:00
Robert Haschke
9fd2f1f858 Drop Melodic support
- py_binding_tools is released into Noetic only
- drop Python2 support
- drop TYPED_TEST_CASE
2024-05-23 14:08:49 +02:00
Robert Haschke
405755595a Switch to package py_binding_tools 2024-05-23 14:08:49 +02:00
Captain Yoshi
ad19ea5479
Add ability to move CollisionObjects (#567) 2024-05-12 11:03:47 +02:00
Sebastian Castro
819e560ed4
Improve description of max_distance property of Connect stage (#564) 2024-05-05 19:30:36 +02:00
Robert Haschke
6b0f2c872f rviz_marker_tools: add missing dependency on urdfdom 2024-04-02 10:38:39 +02:00
Daniel García López
560aa0a476
Add Generator::spawn(from, to, trajectory) variant (#546) 2024-03-20 15:32:15 +01:00
Robert Haschke
55c4b52b13 Cosmetic fixes 2024-03-09 11:11:01 +01:00
Robert Haschke
795bde7b62 Fix Solution::fillMessage() (#432)
The scene_diff field usually describes PlanningScene changes of the end scene relative to the start scene.
For backwards planning, this direction is reversed: the start scene is derived from the end scene.
Thus, we need to generate a full planning scene message for the end scene if planning progressed backwards.

Fixes #405.
2024-03-08 17:31:52 +01:00
Robert Haschke
2450127eb5 Clear JointStates in scene diff before execution
Joints are handled in trajectories.
Scene diffs should not modify joints during execution.
Fixes #353.

Alternative to #504. The previous solution, to always clear the joint states
during message generation, broke the visualization in rviz.
2024-03-08 17:16:19 +01:00
Robert Haschke
5a8925df11 Revert "Disable MPS tests, failing due to #432"
This reverts commit 5382338933.
2024-03-08 17:16:19 +01:00
Robert Haschke
4aac679257 Fix generation of Solution msg: consider backward operation
Need to generate full planning scene message if planning progressed backwards:
The scene_diff field describes PlanningScene changes of the end scene relative to the start scene,
while during planning it might be inversed.
2024-03-08 17:16:19 +01:00
Robert Haschke
5a42ca0ef8 CI: update action versions 2024-03-08 11:32:27 +01:00
Abishalini
e09fecd71f Propagate errors from planners to solution comment (#525) 2024-03-08 11:13:10 +01:00
Michael Wiznitzer
0827a0d260 JointInterpolationPlanner: Check joint bounds (#505) 2024-03-07 20:33:43 +01:00
Robert Haschke
d0a904c313 Add property trajectory_execution_info (#355, #502)
... to pass a list of controller names to PlanExecution

---------

Co-authored-by: Luca Lach <llach@techfak.uni-bielefeld.de>
2024-03-07 20:01:03 +01:00
Robert Haschke
8695def99d ExecuteTaskSolutionCapability: Rename goalCallback() -> execCallback() 2024-03-07 14:36:43 +01:00
Robert Haschke
a75d50e0ed Clear JointStates in scene diff (#504)
Joints are handled in trajectories.
Scene diffs should not modify joints during execution.
Fixes #353.
2024-03-07 12:23:07 +01:00
Sebastian Jahr
b0f388a88f Set a non-infinite default timeout in CurrentState stage (#491)
Co-authored-by: Mario Prats <mario.prats@picknik.ai>
2024-03-07 12:23:07 +01:00
Robert Haschke
48ca3d46f6 rviz_marker_tools: drop rviz dependency 2024-03-07 12:23:07 +01:00
Henning Kayser
111d9ba207
Add GenerateRandomPose stage (#166)
Implements a generic pose randomizer

Co-authored-by: JafarAbdi <cafer.abdi@gmail.com>
Co-authored-by: Sebastian Jahr <sebastian.jahr@picknik.ai>
Co-authored-by: Robert Haschke <rhaschke@techfak.uni-bielefeld.de>
2024-03-06 22:32:13 +01:00
Captain Yoshi
911bc67c4d
GenerateGraspPose: Expose rotation_axis as property (#535) 2024-03-06 11:02:44 +01:00
Captain Yoshi
5720b83dce
Connect: ensure end-state matches goal state (#532) 2024-02-25 16:58:56 +01:00
Michael Görner
7638e5ff8b
Merge pull request #495 from ubi-agni/debug-#485
[WIP] Debug/Fix #485
2024-02-16 12:27:36 +01:00
Robert Haschke
275c15422d Adaptions for https://github.com/ros-planning/moveit/pull/3534
make_box() is a public method now
2024-02-15 22:20:34 +01:00
Robert Haschke
e163f57f9c Cleanup debug output 2024-02-15 21:42:18 +01:00
Robert Haschke
9dfd6bd2eb Fix duplicate solutions
When adding pending state pairs for a new incoming state to Connect,
we have to re-enable opposite states from ARMED state.
This changes the order of states in the interface.
If we do this while iterating over the states, we might add pairs multiple times,
because iteration continues with same state at an earlier position.
2024-02-15 21:42:18 +01:00
Robert Haschke
bf2714b42d printPendingPairs(os) -> os<<pendingPairsPrinter() 2023-10-25 22:21:36 +02:00
Robert Haschke
13264ba21d Fix leaking of failures into enumerated solutions 2023-10-25 22:21:36 +02:00
Robert Haschke
7b965863e3 Add more debugging output
- Add Stage::introspection() accessor
- Introspection: debug-log solution registration
- RemoteSolutionModel: show internal solution id as tooltip in 1st column
2023-10-25 22:19:49 +02:00
Robert Haschke
08b102d5b0 Unit tests for #485 2023-10-20 09:23:32 +02:00
Robert Haschke
605a1e5419 DelayingWrapper stage to delay solution shipping in unit tests 2023-10-20 09:23:32 +02:00
Robert Haschke
0fd989cb27 Simplify tests 2023-10-13 00:25:39 +02:00
Michael Görner
a90557279c
Hide button to show rviz-based task construction (#492) 2023-10-11 14:52:28 +02:00
Michael Görner
dcde89d4d5
Skip Fallbacks::replaceImpl() when already correctly initialized (#494)
Do not reinitialize a Fallbacks' pimpl if it already serves the desired interface.
2023-10-11 14:45:24 +02:00
Michael Görner
ab4bb40dfd
Fix demos (#493)
* improve demo rviz config
  - orbit view centered at workspace instead of 2d orbit
  - REALTIME trajectory display
* FixedState: setter for ignore_collisions property
* Fix clearance_cost demo
  - verify default state
  - use model frame over virtual "world" which is not known in fresh PlanningScene
  - ignore initial collisions with the IK seed
* improve documentation in Fallbacks demo
2023-10-11 14:30:54 +02:00
Henning Kayser
64fd3df563
README: Add branch description (#488) 2023-10-03 17:28:27 +02:00
Robert Haschke
f847d765ce Fix Qt 5.15 deprecation warnings 2023-09-18 10:51:25 +02:00
Robert Haschke
647f3ec413 Limit time to wait for execute_task_solution action server 2023-09-18 10:35:23 +02:00
Robert Haschke
c605a0059a Replace namespace robot_[model|state] with moveit::core 2023-05-26 01:18:17 +02:00
Robert Haschke
6f7282423d MPS: fixup processCollisionObject
- Declare CollisionObject argument as constant:
  Internally the argument is temporarily modified, but for a caller it is effectively const.
- Correctly restore the old operation mode
- Fixup check in unit test
2023-05-26 01:15:50 +02:00
Robert Haschke
3ec69fb59d
Merge PR #460: improvements to ModifyPlanningScene stage 2023-05-25 22:29:31 +02:00
Robert Haschke
2728b3c94c Remove downstream package mtc_pour
... as fillMessage() was renamed to toMsg()
2023-05-25 22:07:35 +02:00
Robert Haschke
5382338933 Disable MPS tests, failing due to #432 2023-05-25 21:52:07 +02:00
Robert Haschke
78da3e46e6
Gracefully handle NULL robot_trajectory (#469) 2023-05-25 17:49:23 +02:00
Robert Haschke
76d293863b
introspection: remove any invalid ROS-name chars from hostname (#465) 2023-05-19 08:48:28 +02:00
Robert Haschke
397fc070ea Fix SolutionBase::fillMessage(): also write start_scene
This method was only doing half of the job, namely adding subsolutions
to the message fields. However, the start_scene was not yet written.
This was handled manually in some but not all callers.
To avoid this inconsistency, the new method toMsg() takes care of both
actions now, while the old fillMessage() method was renamed to appendTo().
2023-05-19 08:43:14 +02:00
Robert Haschke
bd400de184 Fix add/remove object in backward operation
- addObject() will actually remove the object from scene
- removeObject() is not supported (we would need to know which object to add)
2023-05-19 08:43:12 +02:00
Robert Haschke
1daef934ee Add python binding for ModifyPlanningScene::removeObject 2023-05-19 08:43:12 +02:00
Robert Haschke
6a01550e8d ComputeIK: update RobotState before calling setFromIK()
This became necessary due to https://github.com/ros-planning/moveit/issues/3388.
2023-05-17 21:01:57 +02:00
Michael Görner
4d2a5714fa
Use pluginlib consistently (#463) 2023-05-17 19:55:18 +02:00
VideoSystemsTech
dee73b2dde
Expose argument of PipelinePlanner's constructor to Python (#462) 2023-05-16 14:32:17 +02:00
Robert Haschke
d59acdb969 Fix allowCollisions(object, enable_collision)
Call ACM::setDefaultEntry to also cover objects that are not yet known to the ACM.
2023-05-16 09:43:00 +02:00
Robert Haschke
45ca1a67a9 TestModifyPlanningScene 2023-05-16 09:42:25 +02:00
Robert Haschke
b318c3cae9 Basic Move test: MoveRelative + MoveTo 2023-05-05 00:21:11 +02:00
Robert Haschke
84cb880a9d Add python binding for ModifyPlanningScene::allowCollisions(std::string, bool) 2023-05-05 00:21:11 +02:00
Robert Haschke
1002d84946 Add python binding for Task::insert 2023-05-04 23:22:49 +02:00
Robert Haschke
d1a6916206
Stage::explainFailure() (#445)
... to facilitate spotting the stage causing a task to fail
2023-05-04 17:07:11 +02:00
Robert Haschke
ede7cb71cc
Improve documentation (#431)
* Improve general description of stage and container types
* Clarify purpose of `CurrentState` stage
* Add troubleshooting section
2023-05-04 17:02:00 +02:00
Robert Haschke
7f10292ab3
JointInterpolationPlanner: pass optional max_effort property along to GripperCommand (#458)
MoveIt passes the effort field of the last trajectory point as the max_effort for a GripperCommand.
Thus we pass the max_effort property to the effort field of the trajectory's last waypoint.
2023-05-04 15:29:53 +02:00
Robert Haschke
b346e7eb78
Task: findChild() and operator[] should directly operate on stages() (#435)
Considering the (fixed) name of the top-level container is meaningless.
2023-05-04 15:23:23 +02:00
Robert Haschke
f07b81fcd0 cleanup 2023-05-04 14:26:34 +02:00
Robert Haschke
4781ed5636 ros1-0.1.3 2023-03-06 16:37:54 +01:00
Robert Haschke
c57a0bcc12 Fixup: Remove moveit/__init__.py during .deb build
Fix install path recognition (6671aedeb7). Debian install process targets:
/tmp/binarydeb/ros-noetic-moveit-task-constructor-core-<version>/debian/ros-noetic-moveit-task-constructor-core/opt/ros/noetic/lib/python3/dist-packages/moveit
2023-03-06 16:31:59 +01:00
Robert Haschke
75e4260e2a
MoveRelative: Allow backwards operation for joint-space delta (#436) 2023-03-06 16:27:09 +01:00
Robert Haschke
08dc34c5b3
Use const reference instead of reference for ros::NodeHandle (#437) 2023-03-06 16:26:41 +01:00
Robert Haschke
61bb2fdc58
ComputeIK: Limit collision checking to JMG (#428)
That's what MoveIt is doing as well.
2023-02-28 13:18:53 +01:00
Robert Haschke
a3cb8c6584 Fix: Fetch pybind11 submodule if not yet present
cmake's execute_process pipes COMMANDs together.
Thus, `git submodule update` received the output of `git submodule init` as input
and didn't do anything.
2023-02-28 11:15:05 +01:00
Robert Haschke
ca1c7c7e24 Fix typo 2023-02-28 11:13:24 +01:00
Robert Haschke
d07aece163 ros1-0.1.2 2023-02-24 00:02:47 +01:00
Robert Haschke
6671aedeb7 Remove moveit/__init__.py during .deb builds 2023-02-23 23:55:38 +01:00
Robert Haschke
4a320f309c
Merge PR #429: MultiPlanner
MultiPlanner
2023-02-23 20:44:28 +01:00
Robert Haschke
ea776e35f6
Fix marker creation: allow zero scale for geometric shapes (#430)
Boxes, spheres, and cylinders might explicitly have a zero size. Don't reset their scaling to (1,1,1)
2023-02-23 20:42:23 +01:00
Robert Haschke
db6d90ab69 CartesianPath: Deprecate redundant property setters 2023-02-17 22:37:49 +01:00
Robert Haschke
6dc70b1d49 PlannerInterface: provide "timeout" property
The MultiPlanner requires to set individual timeouts for its planners.
2023-02-17 18:28:02 +01:00
Robert Haschke
573858e51a PlannerInterface: provide setters for properties 2023-02-17 17:33:06 +01:00
Robert Haschke
052a56a333 Add MultiPlanner solvers
a planner that tries multiple planners in sequence
2023-02-17 17:33:06 +01:00
Robert Haschke
3d3236575d JointInterpolation: fix timeout handling
The timeout parameter was essentially ignored and the check was always true.
2023-02-17 11:41:32 +01:00
Robert Haschke
eae0bdc27f ros1-0.1.1 2023-02-15 23:20:32 +01:00
Robert Haschke
7926f69e97 Provide ComputeIK.ik_frame as full PoseStamped
Released MoveIt doesn't (yet) provide a conversion from string to PoseStamped.
2023-02-15 17:05:25 +01:00
Robert Haschke
885ac49ffb Use MoveIt's roscpp_initialize 2023-02-15 17:05:25 +01:00
Robert Haschke
d95a2fc787 Use MoveIt's libmoveit_python_tools.so
Drop our own version of that lib and thus resolve a deploy conflict.
2023-02-15 14:30:14 +01:00
Robert Haschke
ca5716a5c3 CI: Add prerelease workflow
Add missing test dependency
2023-02-13 15:49:39 +01:00
Robert Haschke
3b05949be9 pick_place_task: monitor last state before Connect
... to prune solutions as much as possible
2023-02-13 15:08:30 +01:00
Robert Haschke
4b5de15e1b Remove unused eigen_conversions includes 2023-02-11 13:47:35 +01:00
Robert Haschke
f2d97e524d Fix odr compiler warning on build farm
https://build.ros.org/job/Ndev_db__moveit_task_constructor__debian_buster_amd64/3
2023-02-11 13:31:50 +01:00
Michael Görner
f921553fa4
revert to upstream pybind repository (#424)
Robert's fork is not required anymore.
However, we can't forward to the current branch head
because it breaks python2 support.
2023-02-06 14:35:13 +01:00
Robert Haschke
2843459b34 Fix buildfarm errors
- Add missing package dependencies
- Add author tags to package.xml
2023-02-03 12:24:48 +01:00
Robert Haschke
49b3bd0d56 ros1-0.1.0 2023-02-02 10:02:45 +01:00
Robert Haschke
93b46ef214 GeneratePose: forward registered properties from received solution 2023-01-28 12:01:45 +01:00
Robert Haschke
d9cff5510f Fix more -Wold-style-cast warnings 2023-01-05 16:24:02 +01:00
Robert Haschke
bd3a7d10a2 Fix name of test artifact 2023-01-05 12:43:58 +01:00
Robert Haschke
6362d8ec11 CI: Reorder steps 2023-01-05 12:43:02 +01:00
Robert Haschke
54579ca227 fixup! Fix clang-tidy warnings 2023-01-05 12:32:12 +01:00
Robert Haschke
91d9518d3c Replace namespace robot_model -> moveit::core 2023-01-05 03:14:11 +01:00
JafarAbdi
f055fabbd1 Fix clang-tidy warnings
- Replace old-style casts
- Fix missing initialization
2023-01-04 23:52:38 +01:00
Jochen Sprickerhof
770a8a0b17 Fix call of PropagatingEitherWay::computeGeneric()
computeGeneric is templated and private so it will be undefined in the
shared object and when using the computeForward/Backward functions.
2023-01-04 21:39:33 +01:00
Robert Haschke
f872d58520
Expose ContainerBase::childByIndex via operator[](int) (#413) 2022-12-12 20:44:37 +01:00
Robert Haschke
26c690c3b6 Merge PRs #412 (fix-ci) and #409 (more cost-terms) 2022-12-11 22:38:32 +01:00
Robert Haschke
33e9c5d212 Fix clang-tidy issues 2022-12-11 22:37:10 +01:00
Robert Haschke
8e83f2be63 CI: Limit "deploy doc" job to master branch 2022-12-11 21:34:53 +01:00
Robert Haschke
ced37a029c Update/Stick pybind11 to version 2.9.1-smart_holder
... to maintain compatibility with Python 3.6 and 2.7
2022-12-11 21:34:53 +01:00
Robert Haschke
7c35f6c580 Update/Stick pybind11 to version 2.9.1-smart_holder
... to maintain compatibility with Python 3.6 and 2.7
2022-12-11 17:42:07 +01:00
Robert Haschke
341f1838b5 Build python bindings with size optimization 2022-12-11 17:39:55 +01:00
Robert Haschke
4886fc3f7b Expose CostTerms to python 2022-12-11 17:39:55 +01:00
Robert Haschke
6de570c7a6 new TrajectoryCostTerm: DistanceToReference 2022-12-09 11:19:41 +01:00
Robert Haschke
a9b4947845 PathLength: allow weighting of different joints 2022-12-09 10:16:19 +01:00
Robert Haschke
de22cba99a Fetch pybind11 submodule if not yet present 2022-12-09 09:05:17 +01:00
Robert Haschke
0b132e6f65 CI: Use ubuntu-20.04
22.04 doesn't provide clang-format-10.
clang-format-12 is the latest support by 20.04, but this introduces some undesired reformatting.
2022-12-08 16:24:49 +01:00
Robert Haschke
274c75f68f Fix SimpleGrasp/SimpleUnGrasp wrapper
- Wrap common base class SimpleGraspBase to reduce redundancy
- Use correct defaults for stage name
2022-12-08 09:37:33 +01:00
Robert Haschke
7200bbbfb1 Fix base class for container stages Pick, Place, SimpleGrasp, SimpleUnGrasp 2022-12-08 08:59:41 +01:00
Robert Haschke
2cf4f8f046 Drop breathe (not working) and directly link doxygen docs 2022-11-27 01:19:56 +01:00
Robert Haschke
c9ba6918c8 GHA: Build and deploy documentation 2022-11-27 01:19:56 +01:00
Robert Haschke
54e766f139 Link presentation talks in README 2022-11-25 02:14:03 +01:00
Robert Haschke
954089eae4 Merge PR #99: Python API
... based on pybind11
2022-11-22 21:24:17 +01:00
Robert Haschke
c1df1fbee8 [Container|Task]::add(...) for sequence (python) 2022-11-22 21:22:29 +01:00
Robert Haschke
42835380bf Check for moveit::core::MoveItErrorCode::toString 2022-11-19 10:40:09 +01:00
Robert Haschke
e4c4224aa0 Fix syntax error 2022-11-18 12:59:10 +01:00
Robert Haschke
164ce4eab8 clang-tidy: fix variable/method naming 2022-11-03 18:30:21 +01:00
Robert Haschke
4f74af8fb7 Show clang-tidy results 2022-11-03 17:31:17 +01:00
Robert Haschke
1f1a4eb142 Revert "Disable identifier naming checks"
This reverts commit 38cc5613c0.
2022-11-03 17:12:32 +01:00
Robert Haschke
38cc5613c0 Disable identifier naming checks 2022-11-02 18:44:38 +01:00
Robert Haschke
f0a5785074 Disable clang-tidy checks generating false positives 2022-11-02 18:44:38 +01:00
Robert Haschke
7d5f9fe6e3 Fix clang-tidy warnings 2022-11-02 18:44:38 +01:00
Robert Haschke
7ff2f70b66 CI: make clang-tidy checks pedantic 2022-11-02 08:13:28 +01:00
Robert Haschke
f88e25fe74 Merge PR #380: Fix Cartesian interpolation
Correctly consider an offset transform from link to reference frame,
such that rotations w.r.t. the reference frame don't move its origin.
2022-11-01 23:51:53 +01:00
Robert Haschke
0c9524930a CI: stricter warnings 2022-11-01 23:41:17 +01:00
Robert Haschke
d66b2262ed Rename variables in visualizePlan()
- link_pose -> start_pose
- pos_link -> pos_start
2022-10-31 19:53:07 +01:00
Robert Haschke
32d3454c1f Fix getRobotTipForFrame()
When passing the root frame, getRigidlyConnectedParentLinkModel() returns
a nullptr for robot_link, causing a segfault.
Actually, we don't need to use that method at all. We just need to find
the robot_link of an associated body.
2022-10-31 19:53:07 +01:00
Robert Haschke
fd123cc4a7 Fix handling of ik_frame in Cartesian path planning
The ik_frame should move in a straight-line Cartesian path.
However, so far the link frame was following a Cartesian path.
2022-10-31 19:53:07 +01:00
Robert Haschke
ec366b26ee MoveRelative: Correctly compute motion transform
The twist motion performs an angular rotation about the given axis _and_
the origin of ik_frame as well as a linear translation.
Both transforms are expressed w.r.t. the model frame and thus require
left-multiplication to ik_frame's current pose.
2022-10-31 19:53:07 +01:00
Robert Haschke
076957d4dc Simplify MoveRelative 2022-10-31 19:53:07 +01:00
Robert Haschke
402d6a4bfe Improve unittest for move_relative 2022-10-31 19:53:07 +01:00
Robert Haschke
64a8df1fde Merge CI fixes/improvements 2022-10-24 10:21:34 +02:00
Robert Haschke
b69366456d Fix TaskListModelTest.deletion: run required Qt event loop 2022-10-24 10:21:28 +02:00
Robert Haschke
4c9b16511b Hopefully fix spurious test failure 2022-10-24 10:21:28 +02:00
v4hn
c60cd6c86f Avoid unused-parameter warnings 2022-10-24 10:21:28 +02:00
Robert Haschke
5a30f8a8a7 Suppress unused-function warning 2022-10-24 10:08:26 +02:00
v4hn
2b2689e8d7 Use catkin_INCLUDE_DIRS as system includes
... to suppress warnings outside the code base
2022-10-24 10:07:38 +02:00
Robert Haschke
c0cd6dcd01 Update GHA versions 2022-10-24 10:07:38 +02:00
Robert Haschke
b5f5c1fef8 Limit strict compiler warnings to target workspace 2022-10-24 10:07:38 +02:00
Robert Haschke
987c1485a6 Implicitly use clang in clang-tidy builds 2022-10-23 22:16:58 +02:00
Robert Haschke
28fb974adb Remove MoveIt compatibility code 2022-10-23 22:16:58 +02:00
Robert Haschke
f6b925c2fe Drop support for MoveIt's melodic-devel branch 2022-10-23 22:16:58 +02:00
v4hn
6df359ed3b Do not dictate C++ standard
C++14 is default in clang/gcc anyway and latest log4cxx requires C++17.
Qt on Ubuntu 18.04 sets C++11. Hence we use MoveIt's cmake macro to ensure C++14 at least.
2022-10-23 22:15:23 +02:00
Robert Haschke
268fee9ac1 fixup! demo: add example cartesian.py 2022-08-28 11:17:23 +02:00
Niklas Fiedler
30512bcbb4
Report 1st collision pair for invalid IK solutions (#376) 2022-07-22 13:38:05 +02:00
Jafar
e923fbc0c6
Fix Task's move constructor (#371)
* Add unit test
* Fix TaskPrivate's move assignment operator
* Slightly simplify code

Co-authored-by: Robert Haschke <rhaschke@techfak.uni-bielefeld.de>
2022-06-14 22:13:19 +02:00
Wyatt Rees
60229db010
Add KinematicsQueryOptions property in CartesianPath solver (#370) 2022-06-03 19:39:51 +03:00
Robert Haschke
d2918f130d
Pruning: Relax too strong assertion: PRUNED => !ARMED (#340) 2022-05-08 11:56:17 +02:00
Robert Haschke
9026ac8746
Make TimeParamerization configurable (#339) 2022-05-08 11:54:05 +02:00
Stephanie Eng
8beb0f4243
Update black version (#348) 2022-03-29 18:59:20 +03:00
Christian Petersmeier
e57d198dd7 Use verbose delimiters for source code instead of line numbers 2022-03-10 13:46:01 +01:00
Robert Haschke
813d9a7de7 Simplify file names, move tutorials into subfolder 2022-03-10 13:33:10 +01:00
Christian Petersmeier
d45f9faf63 Add example script: properties 2022-03-10 13:33:10 +01:00
Christian Petersmeier
60bc5262f5 PickPlace tutorial, stage extension howto guide 2022-03-10 13:33:10 +01:00
Christian Petersmeier
13128390b6 add cartesian and first steps tutorials 2022-03-10 13:33:10 +01:00
Christian Petersmeier
f082fb772b Add property tutorial, restructure tutorial files 2022-03-10 13:33:10 +01:00
Christian Petersmeier
1f330bed89 Reformat Documentation
- create tutorial, how-to-guide, topic and reference chapters
  - move implementation examples from api documentation into
    how-to-guides
  - create a documentation overview on the first page
2022-03-10 13:33:10 +01:00
Robert Haschke
940a843f05 Rework stages doc 2022-03-10 13:33:06 +01:00
Robert Haschke
096c671887 Pruning: Relax too strong assertion: PRUNED => !ARMED
If two Connect stages are sequenced, both sides can become ARMED.
However, that means that the wave of PRUNED status updates, shouldn't
overwrite a present ARMED state.
Added unit test.
2022-03-04 15:01:19 +01:00
Robert Haschke
8d7225d3b6 Connect: better document suppressing recursive loop 2022-03-04 15:01:19 +01:00
v4hn
6d104e837e polish: FixedState supports collision checking 2022-03-04 14:41:00 +01:00
v4hn
0a3dd3a314 properly set comment markAsFailure without prior comment 2022-03-04 14:39:53 +01:00
Robert Haschke
5310f9063a operator<< for Interface::Direction 2022-03-04 13:13:01 +01:00
Robert Haschke
ee7cec2aab FixedState: ignore_collisions=false
Check collisions for FixedState's scene and report failure if needed.
Optionally, disable the check via the property ignore_collisions=true.
2022-03-03 17:01:34 +01:00
Robert Haschke
6ffc508225 Finetune solvers documentation 2022-03-03 10:50:30 +01:00
Robert Haschke
e77fa83a42 Rework core documentation 2022-03-03 10:50:30 +01:00
Christian Petersmeier
9103af2704 Rework python documentation 2022-03-03 10:50:30 +01:00
Robert Haschke
95f24747b0 Pick+Place: Correctly configure forwarding of "pregrasp" property
... from Grasp to UnGrasp stage
2022-03-03 10:50:30 +01:00
Robert Haschke
d90b566b08 GeneratePlacePose: Remove property "ik_frame" from stage
- Instead, set ik_frame property on solution from passed object (frame).
- Allow subframes to be used as "object" frames
2022-03-03 10:50:30 +01:00
Robert Haschke
0dffefd6ae Cleanup pickplace.py 2022-03-03 10:50:30 +01:00
Robert Haschke
d408d73018 Fix ComputeIK demo 2022-03-03 10:50:30 +01:00
Robert Haschke
ca38d11303 Enable InterfaceState's copy operator 2022-02-02 20:51:58 +01:00
Robert Haschke
4b9e85395d Fix pre-commit: python formatting 2022-01-07 16:20:42 +01:00
Robert Haschke
b28290c5b9 Rework doc for properties.cpp
- Use simple description strings
  Only for multi-line doc strings, use C++11 raw string syntax `R"(...)"`
- Auto-generate signature
- Name arguments via `py::arg()`
2022-01-07 16:20:42 +01:00
Robert Haschke
11686468ad Update rosdoc + sphinx config 2022-01-07 16:20:42 +01:00
Robert Haschke
cfcc7ba112 Improve top-level descriptions 2022-01-07 16:20:42 +01:00
Robert Haschke
603e337c77 fix typo 2022-01-07 16:20:42 +01:00
Robert Haschke
d732bfe3eb Fix demo/scripts/fixed_state.py 2022-01-07 16:20:42 +01:00
cpetersmeier
0fbdfbc818 mwe examples for core classes 2022-01-07 16:20:42 +01:00
cpetersmeier
1b1dadb94a core python docs
- add python docs to core classes
    - rework whats included in the docs and what not
2022-01-07 16:20:42 +01:00
cpetersmeier
31577c10c0 add further examples for basic stages
- create more granular mwe examples as reference material
      in the documentation
2022-01-07 16:20:42 +01:00
cpetersmeier
923022c13b stages docs and bindings
- pybind bindings for all the examples
    - correctly format docstrings
2022-01-07 16:20:42 +01:00
cpetersmeier
d6f1b69474 add demo pickplace example script 2022-01-07 16:20:42 +01:00
cpetersmeier
f7c2fadde6 core docs, mwe monitoring generator 2022-01-07 16:20:42 +01:00
cpetersmeier
65bc0a8703 expand core docs, add detailed examples 2022-01-07 16:20:42 +01:00
cpetersmeier
4f53663756 add docstrings and mwe's 2022-01-07 16:20:42 +01:00
cpetersmeier
5207a8b2b5 comply to google format + add docstrings 2022-01-07 16:20:42 +01:00
cpetersmeier
0e7f2d5981 add docstrings, custom signatures 2022-01-07 16:20:42 +01:00
cpetersmeier
d1c947c973 add python docstrings 2022-01-07 16:20:42 +01:00
cpetersmeier
136c5c425e sphinx configuration
- Remove warning, originating from intersphinx configuration.
- Add .rst files for the api documentation.
2022-01-07 16:20:42 +01:00
Robert Haschke
a08face4a0 Fixup: Provide wrapper for moveit::core::MoveItErrorCode 2022-01-07 16:20:04 +01:00
Robert Haschke
86093be94a Merge branch 'master' into wip-python-api 2022-01-07 16:20:00 +01:00
Robert Haschke
b2c990b675 core: export rviz_marker_tools dependency 2022-01-07 14:35:15 +01:00
Robert Haschke
c7b2067b86 Merge PR #309: Fix Pruning 2022-01-05 20:19:51 +01:00
Robert Haschke
5956e70956 Merge PR #311: fix Fallbacks 2022-01-05 19:49:38 +01:00
Robert Haschke
4cc1f567d6 FallbacksPrivateConnect
Implement Fallbacks behavior for children of type Connecting.
All other connect-like children are currently infeasible to handle,
because we cannot forward a single job, i.e. a pair (from, to)
to the next child, but only individual states.
However, passing states, will cause creation of undesired state pairs
as jobs in subsequent children.
2022-01-05 18:16:34 +01:00
Robert Haschke
7a04a9f603 ParallelContainerBasePrivate::propagateStateTo*All*Children
rename method to emphasize that state updates are propagated to all children
2022-01-05 16:45:57 +01:00
Robert Haschke
986d3c8766 FallbacksPrivateCommon: shared between Generator + Propagator 2022-01-05 16:45:57 +01:00
Robert Haschke
b82b70ed64 FallbacksPrivate::nextChild()
... factoring out functionality shared between FallbacksPrivateGenerator
and FallbacksPrivatePropagator to switch to next child in nextJob().
2022-01-05 16:37:46 +01:00
Robert Haschke
7af3d8ebd7 Improve readability 2022-01-05 16:37:46 +01:00
Robert Haschke
b2c116edab reset(new Interface()) -> std::make_shared<Interface>() 2022-01-05 16:37:46 +01:00
Robert Haschke
442d39ad3e Improve comments 2022-01-05 16:37:46 +01:00
Robert Haschke
b2056745a8 Generalize connectStageInsideFallbacks
Let's consider the following simple situation, where generators produce solutions in the given order.

GEN           1 3
Fallbacks     |X
GEN           2 4

When passing state 4 to the Fallbacks' connector, it forms pending pairs with both 1 and 3.
Thus, the container needs to check whether 1-4 or 3-4 was processed when receiving a success or failure,
to correctly forward the failed one to the next child.
2022-01-05 16:37:46 +01:00
Robert Haschke
184fab8e0a GeneratePlacePose: add property 'allow_z_flip' 2022-01-04 15:58:19 +01:00
Robert Haschke
9630f4d789 ComputeIK: Improve markers
- always provide eef markers (also in case of success)
- tint failures in red
- use different names for "ik frame" and "target frame" markers
- reduce code duplication
2022-01-04 15:57:34 +01:00
Robert Haschke
91fe791bdb Add comment 2022-01-04 15:12:06 +01:00
Robert Haschke
72510818c8 Export libmoveit_python_tools.so 2022-01-04 15:11:23 +01:00
Jafar Abdi
7dbe0b87e1
Return MoveItErrorCode from task::plan (#319)
... to know whether the plan failed due to timeout, preemption, or actual planning failure
2022-01-02 15:32:37 +01:00
Michael Görner
f9c0a894f9
Merge pull request #320 from v4hn/pr-master-fix-move-rel-ikframe
Fix using IKFrame with MoveRelative
2021-12-20 10:40:12 +01:00
v4hn
84f96ec74c MoveRelative: Interpret direction relative to IKFrame
bugfix
2021-12-20 10:19:52 +01:00
v4hn
191ff253fd add tests for MoveRelative 2021-12-20 10:19:52 +01:00
Robert Haschke
4be448641f Improve debug output
- printChildrenInterfaces(): fix/add usage
- printPendingPairs(): full colorization according to status
2021-11-28 18:28:20 +01:00
Robert Haschke
0587129916 CI: asan with debug symbols 2021-11-25 22:02:21 +01:00
Robert Haschke
b4a9e2033d
Stage::reset() should reset total_compute_time_ (#310) 2021-11-25 19:47:00 +03:00
Robert Haschke
aef991602f Propagate either STATUS or PRIORITY updates into a container 2021-11-22 00:12:50 +01:00
Robert Haschke
a48b932dce Distinguish STATUS and PRIORITY updates in notify() callbacks
to allow propagating status updates only if the STATUS actually changed.
2021-11-21 15:05:29 +01:00
Robert Haschke
fdd5ff880b templatize: pullInterface(dir) -> pullInterface<dir>()
Also remove unused pushInterface(dir)
2021-11-21 14:15:59 +01:00
Robert Haschke
a31e52dd53 Propagate status across Connecting gap
Not only propagate updates along solution paths, but also bridge
the gap of a `Connecting` stage.

- If a state becomes enabled, re-enable opposite `ARMED` states as well.
- If a state becomes pruned, also prune opposite states if they don't have alternatives.
- Make sure that we don't run into a recursive update loop by disabling notify() callbacks.
2021-11-21 14:15:59 +01:00
Robert Haschke
3648db43ed Never overwrite ARMED with PRUNED 2021-11-21 14:00:50 +01:00
Robert Haschke
9d37495c0b Recombine both variants of Interface::updatePriority()
As only the InterfaceState* variant is actually called,
we can drop the splitting introduced for performance reasons in
29d1e44c5d
2021-11-21 14:00:50 +01:00
Robert Haschke
ffb8598363 Recursively prune new CONNECT state if there is no enabled opposite
This also requires to drop the assertion in SerialContainer::onNewSolution()
that new solutions will have enabled start+end states (a CONNECT stage's solution might not).
2021-11-21 14:00:50 +01:00
Robert Haschke
ab9af6a0fa Recursively re-enable states when matching an ARMED state 2021-11-21 14:00:50 +01:00
Robert Haschke
3c4ef68dbe Rename Interface::Status FAILED -> ARMED
... to better indicate that such a state can be immediately re-enabled.
2021-11-21 14:00:50 +01:00
Robert Haschke
1dda3d14d8 Switch order of function declarations
... to avoid explicit template initialization
2021-11-20 06:24:19 +01:00
Robert Haschke
52dc494525 Fix test Pruning.NoPruningIfAlternativesExist 2021-11-20 06:24:19 +01:00
Robert Haschke
06ae5ddf9c Fix hasPendingOpposites()
- Switch directions: FORWARD <-> BACKWARD to make the function reusable for status propagation.
- We need to ignore the source state when looking for opposite states of the target state.
  Thus add both, source and target state arguments.
2021-11-20 01:24:29 +01:00
Robert Haschke
29d1e44c5d Rework updatePriority() functions
- Centrally distinguish between have owner() or not in InterfaceState::updatePriority()
- Have a separate updateStatus() method to just update the pruning status
- Split Interface::updatePriority() into a method taking the InterfaceState*
  and one taking an Interface::iterator (for efficiency)
- Early return in container.cpp's updateStatePrios()
2021-11-19 23:03:15 +01:00
Robert Haschke
4170a1c93a Drop unused and misleading Direction enums 2021-11-19 23:03:15 +01:00
Robert Haschke
1ddf7dd3f0 Never remove pending CONNECT pairs
Both, failed and pruned states might get re-enabled later!
This also required rework (simplification) of the sorting function for pending pairs.
2021-11-19 23:03:15 +01:00
Robert Haschke
97c2130404 Improve readability 2021-11-19 21:49:44 +01:00
Robert Haschke
718170ab1e Always skip pruning if there exist alternative enabled solutions 2021-11-19 21:49:44 +01:00
Robert Haschke
c617e3353d Disable failing tests 2021-11-19 21:46:09 +01:00
Robert Haschke
011e4be059 Add more pruning tests 2021-11-19 09:42:30 +01:00
Robert Haschke
66e141db7b Fix printChildrenInterfaces() 2021-11-19 09:42:30 +01:00
Robert Haschke
ed459702bd Fix -Wsign-compare 2021-11-19 02:05:46 +01:00
v4hn
b675876d3b add debug logs to pruning code 2021-11-16 22:52:06 +01:00
JafarAbdi
2d775ffe28 GeneratorMockup: Add solutions_per_compute argument 2021-11-16 22:51:29 +01:00
Robert Haschke
ced362f5f6 Merge branch 'master' into wip-python-api 2021-06-11 09:57:55 +02:00
Robert Haschke
244c999514 Skip some python tests on incompatible pybind11 versions
If MoveIt and MTC use incompatible versions of pybind11, the tests
will fail because MoveIt objects like RobotModel or PlanningScene
cannot be passed to MTC objects and vice versa.
2021-06-10 19:31:05 +02:00
Robert Haschke
e3ee75dc16 Fix utf8 encoding 2021-06-10 12:06:22 +02:00
Robert Haschke
81fae53009 PyMonitoringGenerator 2021-05-21 12:20:54 +02:00
Robert Haschke
9f7139f376 Fix names of trampoline classes
Using template names T is not a good idea, because this name is used
verbatim for some error reporting, resulting e.g. in:
Tried to call pure virtual function "T::canCompute"
2021-05-21 11:40:12 +02:00
Robert Haschke
0967aa808b Update pybind11 submodule 2021-05-21 01:55:39 +02:00
Robert Haschke
1e7fe85573 smart_holder: conservative mode
... to become compatible with classical pybind11 modules, e.g. the MoveIt packages
2021-05-21 01:55:39 +02:00
Robert Haschke
7d0b6da04f Generator::spawn() 2021-05-21 01:55:39 +02:00
Robert Haschke
a03ea5a4a9 Use pybind11's smart_holder branch 2021-05-20 03:56:47 +02:00
Robert Haschke
defe144b55 Use py:overload_cast<>() 2021-05-20 03:52:32 +02:00
Robert Haschke
fc0abe9dd9 Merge branch 'master' into wip-python-api 2021-05-20 03:49:52 +02:00
Robert Haschke
3c286b2226 Augment license/disclaimer 2021-05-19 23:04:44 +02:00
Robert Haschke
f006ff7a37 Allow casting of PoseStamped from string 2021-05-19 23:04:44 +02:00
Robert Haschke
a204d94a19 Configure namespace package 2021-03-31 00:19:51 +02:00
Robert Haschke
5b4ca1d21a Rename wrapper -> bindings 2021-03-31 00:19:51 +02:00
Robert Haschke
045b358c30 rosdoc_lite configuration 2021-03-31 00:19:51 +02:00
Robert Haschke
3cf92442fb Simplify generation of pybind11 modules
* Install module libs into CATKIN_GLOBAL_PYTHON_DESTINATION (assuming unique names).
  This avoids the need to link them into the source space, because they are found also from devel space.
* Use pybind11's def_submodule() to create the `core` and `stages` submodules,
  everything linked into the same lib
2021-03-31 00:19:51 +02:00
Robert Haschke
d9b7aa37a3 clang-format python wrapping code 2021-03-31 00:19:51 +02:00
cpetersmeier
14e06659d5 Towards inherited classes in Python 2021-03-31 00:19:51 +02:00
cpetersmeier
6ab50fcc40 PythonWrapper: Use collective includes 2021-03-19 18:39:47 +01:00
Robert Haschke
1f5c684909 Simplify wrapper code 2021-03-19 18:39:47 +01:00
Robert Haschke
d7d54d828f Access to container's children 2021-03-19 18:39:47 +01:00
Robert Haschke
fd25a0626a auto-format python code with black 2021-03-19 18:39:44 +01:00
Robert Haschke
10adb63f4a Travis: Testing on Noetic as well 2020-12-08 16:10:28 +01:00
Robert Haschke
b2adcf0247 Python3 compatibility 2020-11-30 13:34:33 +01:00
Robert Haschke
c97b462024 migration: boost::python -> pybind11 2020-11-30 13:34:33 +01:00
Robert Haschke
8433e460e4 Merge branch 'master' into wip-python-api 2020-11-26 12:05:03 +01:00
cpetersmeier
012a10be25 StagesWrapper: Python Function Bindings
Add python bindings for

    - SimpleGrasp::setIKFrame
    - Pick::setApproachMotion
    - Pick::setLiftMotion
2020-11-26 11:47:20 +01:00
cpetersmeier
ede5fe396f StagesWrapper: Add std::map setGoal overload.
Adding a further overload of the `setGoal()` function that is exposed to
the python api. This should provide an interface for passing in dicts as
joint name and -angle configurations.
2020-10-21 16:45:05 +02:00
Robert Haschke
49b2ff8ce5 Test Merger 2020-09-22 11:06:34 +02:00
Robert Haschke
745d6739bd fix typo 2020-09-22 11:06:01 +02:00
Robert Haschke
97e0b437f2 Silence -Wdeprecated-declarations due to std::auto_ptr 2020-09-07 23:18:43 +02:00
Robert Haschke
7306878b9e Merge branch master into wip-python-api 2020-09-07 23:18:20 +02:00
Robert Haschke
d10565cabf fix memory access issue
Fixes #132.
2020-02-18 09:01:00 +01:00
Robert Haschke
569836e198 demo: add example cartesian.py 2020-02-13 14:18:09 +01:00
Robert Haschke
5a85343490 Merge branch master into wip-python-api 2020-01-11 09:17:32 +01:00
Robert Haschke
f88bd8f54d RosMsgConverter: ensure that python and C++ types match 2020-01-11 07:56:47 +01:00
Robert Haschke
5293d2ed27 simplify method overloads 2020-01-11 07:56:47 +01:00
Robert Haschke
7d0daa6cd4 boost::python: provide generic converter for std::map 2020-01-11 07:55:31 +01:00
Robert Haschke
fd9462c27f Merge branch master into wip-python-api 2019-07-26 11:11:41 +02:00
Robert Haschke
874994bdb5 test packages are required 2019-07-12 09:35:48 +02:00
Robert Haschke
dd80c675fa cannot use cmake generator expressions in COMMENT 2019-02-21 01:03:03 +01:00
Robert Haschke
36b63f0e35 fixup wrapping of solvers
- wrap JointInterPolationPlanner
- remove properties: group, timeout
- add actual planner properties
2019-02-21 01:01:33 +01:00
Robert Haschke
95d05ea05c expose PropagatingEitherWay::restrictDirection()
fixup! expose PropagatingEitherWay::restrictDirectio()
2019-02-21 00:57:17 +01:00
janEbert
c59e41042f Add test for all stages' PropertyMaps
Iterate over all stages and their properties to see check for missing conversion functions.
2018-10-29 18:00:49 +01:00
Robert Haschke
bb5b29d5fd disable python default constructors for some classes
these classes do not yet handle nullptr as their argument
2018-10-29 18:00:49 +01:00
janEbert
00d5b00ec4 Provide default constructors for all stages 2018-10-29 18:00:49 +01:00
Robert Haschke
df820738c8 cleanup converter for ros::Duration 2018-10-29 18:00:49 +01:00
janEbert
e79f0f3054 add converter for std::set<std::string> 2018-10-29 18:00:49 +01:00
janEbert
fab40f64ed register enum Connect::MergeMode 2018-10-29 18:00:49 +01:00
Robert Haschke
2f8ded2a4e fixup! generalize Property conversion between C++ and Python 2018-10-29 18:00:49 +01:00
Robert Haschke
ed44c2cb33 adapt API: MoveRelative::setGoal -> setDirection 2018-10-25 03:27:10 +02:00
Robert Haschke
ff6661e8ba python wrappers for new functionality since last merge 2018-10-25 03:26:43 +02:00
Robert Haschke
377dd11950 Merge branch 'master' into boost-python 2018-10-25 03:24:24 +02:00
Robert Haschke
b511110ae6 remove dummy file 2018-10-23 01:30:33 +02:00
Robert Haschke
3ad03cf719 generalize Property conversion between C++ and Python
register appropriate converters for boost::any
2018-10-18 10:00:45 +02:00
Robert Haschke
14d10a2d93 replace MessageSignature with simple ros-msg-name string 2018-10-18 10:00:45 +02:00
Robert Haschke
c145b4c5f8 better robustness against already registered boost::python type converters 2018-10-18 10:00:45 +02:00
Robert Haschke
074a3b7f03 RosMsgConverter: do not allow custom message name 2018-10-18 10:00:45 +02:00
Robert Haschke
fdec258b2c ROSMsgConverter -> RosMsgConverter 2018-10-18 10:00:45 +02:00
Robert Haschke
1b267768fa cleanup type conversion 2018-10-18 10:00:45 +02:00
Robert Haschke
3786ce5a15 protect fromPython / toPython 2018-10-17 02:00:14 +02:00
Robert Haschke
5d78b10366 fix compiler warnings 2018-10-17 02:00:14 +02:00
Robert Haschke
5bae196b21 fixes for Bionic
boost::python 1.65 is more picky about exactly returning the placeholder type in __init__ functions.
2018-10-17 01:18:39 +02:00
Robert Haschke
c9e735ded3 fix API to match MoveTo / MoveRelative stages 2018-10-17 01:18:39 +02:00
Robert Haschke
fbbb39592b allow PoseStamped as property 2018-10-17 01:18:39 +02:00
Martin Meier
47232dd92b pass verbose InitStageException from C++ to python
reverts df43ba1d68bb5c628a8e8f13729e7cdda872f1f9
2018-10-17 01:18:39 +02:00
Robert Haschke
effa437a53 Solution.toMsg() 2018-10-17 01:18:39 +02:00
Robert Haschke
14bd867df8 expose MonitoringGenerator's setMonitoringStage() 2018-10-17 01:18:39 +02:00
Robert Haschke
ef1bbaa439 Task::init(): verbose exception output 2018-10-17 01:18:39 +02:00
Robert Haschke
ea4b1e0c21 Properties: exposeTo(), configureInitFrom() 2018-10-17 01:18:39 +02:00
Robert Haschke
c80755348c moved python includes to global include folder too
catkin package expects all includes to be in one global location
2018-10-17 01:18:39 +02:00
Robert Haschke
b36d91cc02 import .core by default 2018-10-17 01:18:39 +02:00
janEbert
3c77fd1325 add reference test for properties 2018-10-17 01:18:36 +02:00
Robert Haschke
4ac0b6e2c4 merge fixes 2018-10-16 23:22:10 +02:00
Robert Haschke
4cebd95468 Merge branch master into boost-python 2018-06-03 13:20:02 +02:00
Robert Haschke
d3fdee9095 expose solutions, publish + execute 2018-06-03 13:03:41 +02:00
Robert Haschke
c0a16971f4 remove redundant exposure of smart pointers 2018-06-03 13:03:41 +02:00
Robert Haschke
5e2bff59fc cleanup Property access 2018-06-03 13:03:41 +02:00
Robert Haschke
d5c7bfb07e cleanup 2018-06-03 13:03:41 +02:00
janEbert
e264bddc3e PropertyMap iterator, PropertyMap.update(dict) 2018-06-03 13:03:41 +02:00
janEbert
2345b6c9a6 unit tests for all stages 2018-06-03 13:03:41 +02:00
janEbert
f15cbd7141 overload constructors 2018-06-03 13:03:41 +02:00
janEbert
93c62e833d container wrappers 2018-06-03 13:03:41 +02:00
Robert Haschke
09b9698e7b add ROS unittest 2018-06-03 13:03:41 +02:00
Robert Haschke
56546f190f roscpp_init: provide init_options AnonymousName, NoRosout 2018-06-03 13:03:41 +02:00
janEbert
1f85b00e14 more wrappers, unittest, fixes 2018-06-03 13:03:41 +02:00
Robert Haschke
de9fa8405d register ROS msg types with boost::python's type converters 2018-06-03 13:03:41 +02:00
Robert Haschke
f4ffccdc1f handle std::unique_ptr<Stage>
Class holder needs to be a smart pointer, e.g. std::auto_ptr.
This can be released(), such that ownership can be passed.
Further, derived types need to be declared as implicitly_convertible to base type.
2018-06-03 13:03:41 +02:00
Robert Haschke
47a2a28698 cleanup, unittest for properties 2018-06-03 13:03:41 +02:00
Robert Haschke
ee13295c92 separate .core and .stages modules 2018-06-03 13:03:41 +02:00
Robert Haschke
e615b7c9da local names for python wrapper libs 2018-06-03 13:03:41 +02:00
Robert Haschke
f7ddd43d53 cleanup folder structure 2018-06-03 13:03:41 +02:00
Robert Haschke
135c9c2148 solvers + stages 2018-06-03 13:03:41 +02:00
Robert Haschke
3ef0bd426b Properties 2018-06-03 13:03:41 +02:00
janEbert
226b0b03b4 basic boost::python wrappers 2018-06-03 13:03:41 +02:00
205 changed files with 8688 additions and 1443 deletions

View File

@ -1,5 +1,7 @@
---
Checks: 'performance-*,
-performance-noexcept-move-constructor,
-clang-analyzer-core.CallAndMessage,
llvm-namespace-comment,
modernize-redundant-void-arg,
modernize-use-nullptr,
@ -15,7 +17,6 @@ Checks: 'performance-*,
readability-identifier-naming,
'
HeaderFilterRegex: '.*/moveit/task_constructor/.*\.h'
AnalyzeTemporaryDtors: false
CheckOptions:
- key: llvm-namespace-comment.ShortNamespaceLines
value: '10'

View File

@ -8,80 +8,158 @@ on:
pull_request:
push:
permissions:
contents: read
pages: write
id-token: write
jobs:
default:
ici:
strategy:
fail-fast: false
matrix:
env:
- IMAGE: melodic-source
- IMAGE: noble-ci
NAME: ccov
TARGET_CMAKE_ARGS: -DCMAKE_BUILD_TYPE=Debug -DCMAKE_CXX_FLAGS="--coverage"
- IMAGE: master-source
CXXFLAGS: >-
-Werror -Wall -Wextra -Wwrite-strings -Wunreachable-code -Wpointer-arith -Wredundant-decls
-Wno-unused-parameter -Wno-unused-function -Wno-deprecated-copy -Wno-unused-but-set-parameter
- IMAGE: noetic-source
CXX: clang++
- IMAGE: noble-ci-testing
CLANG_TIDY: true
CXXFLAGS: >-
-Werror -Wall -Wextra -Wwrite-strings -Wunreachable-code -Wpointer-arith -Wredundant-decls
-Wno-unused-parameter -Wno-unused-function -Wno-deprecated-copy
- IMAGE: noetic-source
TARGET_CMAKE_ARGS: >-
-DCMAKE_BUILD_TYPE=Release
-DCMAKE_CXX_FLAGS="-Werror -Wall -Wextra -Wwrite-strings -Wunreachable-code -Wpointer-arith -Wredundant-decls -Wold-style-cast"
- IMAGE: jammy-ci
- IMAGE: noble-ci-testing
NAME: asan
DOCKER_RUN_OPTS: >-
-e PRELOAD=libasan.so.5
-e PRELOAD=libasan.so.8
-e LSAN_OPTIONS="suppressions=$PWD/.github/workflows/lsan.suppressions"
TARGET_CMAKE_ARGS: -DCMAKE_CXX_FLAGS="-fsanitize=address -fno-omit-frame-pointer -O1"
TARGET_CMAKE_ARGS: -DCMAKE_BUILD_TYPE=Release -DCMAKE_CXX_FLAGS="-fsanitize=address -fno-omit-frame-pointer -O1 -g"
env:
PIP_BREAK_SYSTEM_PACKAGES: 1
CATKIN_LINT: true
CLANG_TIDY_ARGS: --fix --fix-errors --format-style=file
CLANG_TIDY_ARGS: -quiet -export-fixes ${{ github.workspace }}/.work/clang-tidy-fixes.yaml
DOCKER_IMAGE: moveit/moveit:${{ matrix.env.IMAGE }}
UNDERLAY: /root/ws_moveit/install
DOWNSTREAM_WORKSPACE: "github:ubi-agni/mtc_demos#master github:TAMS-Group/mtc_pour#master"
UNDERLAY: ${{ endsWith(matrix.env.IMAGE, '-source') && '/root/ws_moveit/install' || '' }}
DOWNSTREAM_WORKSPACE: "github:ubi-agni/mtc_demos#master"
CCACHE_DIR: ${{ github.workspace }}/.ccache
BASEDIR: ${{ github.workspace }}/.work
CACHE_PREFIX: "${{ matrix.env.IMAGE }}${{ contains(matrix.env.TARGET_CMAKE_ARGS, '--coverage') && '-ccov' || '' }}"
# perform full clang-tidy check only on manual trigger (workflow_dispatch), PRs do check changed files, otherwise nothing
CLANG_TIDY_BASE_REF: ${{ github.event_name != 'workflow_dispatch' && (github.base_ref || github.ref) || '' }}
CC: ${{ matrix.env.CLANG_TIDY && 'clang' }}
CXX: ${{ matrix.env.CLANG_TIDY && 'clang++' }}
CXX: ${{ matrix.env.CLANG_TIDY && 'clang++ -std=c++17' }}
name: "${{ matrix.env.IMAGE }}${{ matrix.env.NAME && ' • ' || ''}}${{ matrix.env.NAME }}${{ matrix.env.CATKIN_LINT && ' • catkin_lint' || ''}}${{ matrix.env.CLANG_TIDY && ' • clang-tidy' || '' }}"
runs-on: ubuntu-latest
steps:
- uses: actions/checkout@v2
- uses: actions/checkout@v4
with:
submodules: recursive
- name: Cache ccache
uses: pat-s/always-upload-cache@v2.1.5
uses: rhaschke/cache@main
with:
path: ${{ env.CCACHE_DIR }}
key: ccache-${{ env.CACHE_PREFIX }}-${{ github.sha }}-${{ github.run_id }}
restore-keys: |
ccache-${{ env.CACHE_PREFIX }}-${{ github.sha }}
ccache-${{ env.CACHE_PREFIX }}
env:
GHA_CACHE_SAVE: always
- id: ici
name: Run industrial_ci
uses: ros-industrial/industrial_ci@master
uses: rhaschke/industrial_ci@master
env: ${{ matrix.env }}
- name: Upload ici's target_ws/install folder
uses: rhaschke/upload-ici-workspace@main
if: success() && matrix.env.IMAGE == 'jammy-ci'
with:
subdir: target_ws/install
- name: Upload test artifacts (on failure)
uses: actions/upload-artifact@v2
uses: actions/upload-artifact@v4
if: failure() && (steps.ici.outputs.run_target_test || steps.ici.outputs.target_test_results)
with:
name: test-results-${{ matrix.env.IMAGE }}
name: test-results-${{ matrix.env.IMAGE }}${{ matrix.env.NAME && '-' || ''}}${{ matrix.env.NAME }}${{ matrix.env.CLANG_TIDY && '-clang-tidy' || '' }}
path: ${{ env.BASEDIR }}/target_ws/**/test_results/**/*.xml
- name: Upload clang-tidy fixes (on failure)
uses: actions/upload-artifact@v4
if: failure() && steps.ici.outputs.clang_tidy_checks
with:
name: clang-tidy-fixes.yaml
path: ${{ env.BASEDIR }}/clang-tidy-fixes.yaml
- name: Show clang-tidy warnings
if: always() && matrix.env.CLANG_TIDY
uses: asarium/clang-tidy-action@v1
with:
fixesFile: ${{ env.BASEDIR }}/clang-tidy-fixes.yaml
- name: Generate codecov report
uses: rhaschke/lcov-action@main
if: contains(matrix.env.TARGET_CMAKE_ARGS, '--coverage') && steps.ici.outputs.target_test_results == '0'
with:
docker: $DOCKER_IMAGE
workdir: ${{ env.BASEDIR }}/target_ws
lcov_capture_args: --ignore-errors=gcov,gcov,mismatch,mismatch,negative,negative,source
ignore: '"*/target_ws/build/*" "*/target_ws/install/*" "*/test/*"'
- name: Upload codecov report
uses: codecov/codecov-action@v2
uses: codecov/codecov-action@v5
if: contains(matrix.env.TARGET_CMAKE_ARGS, '--coverage') && steps.ici.outputs.target_test_results == '0'
with:
files: ${{ env.BASEDIR }}/target_ws/coverage.info
doc:
# https://devblogs.microsoft.com/cppblog/clear-functional-c-documentation-with-sphinx-breathe-doxygen-cmake
if: github.repository_owner == 'moveit'
runs-on: ubuntu-latest
needs: ici
container:
image: moveit/moveit:jammy-ci
steps:
- name: Checkout
uses: actions/checkout@v4
- name: Setup Pages
uses: actions/configure-pages@v5
- name: Install dependencies
run: |
apt-get update -q
apt-get install -y doxygen graphviz python3-pip ros-one-moveit-ros ros-one-py-binding-tools libfmt8
pip install -r core/doc/requirements.txt
- name: Download ICI workspace
uses: rhaschke/download-ici-workspace@main
- name: Build Documentation
shell: bash
run: |
source ici/setup.bash
(cd core/doc; doxygen)
sphinx-build core/doc _site
- name: Validate links
if: false
shell: bash
run: |
source ici/setup.bash
sphinx-build -W -b linkcheck core/doc _site
- name: Upload pages artifact
uses: actions/upload-pages-artifact@v3
# Deployment job
deploy:
if: github.repository_owner == 'moveit' && github.ref == 'refs/heads/master'
runs-on: ubuntu-latest
needs: doc
environment:
name: github-pages
url: ${{ steps.deployment.outputs.page_url }}
steps:
- name: Deploy to GitHub Pages
id: deployment
uses: actions/deploy-pages@v4

View File

@ -13,14 +13,13 @@ jobs:
name: pre-commit
runs-on: ubuntu-latest
steps:
- uses: actions/checkout@v2
- uses: actions/setup-python@v2
- name: Install clang-format-10
run: sudo apt-get install clang-format-10
- uses: rhaschke/install-catkin_lint-action@v1.0
- uses: actions/checkout@v4
with:
distro: noetic
- uses: pre-commit/action@v2.0.0
submodules: recursive
- name: Install clang-format-14
run: sudo apt-get install clang-format-14
- uses: rhaschke/install-catkin_lint-action@main
- uses: pre-commit/action@v3.0.1
id: precommit
- name: Upload pre-commit changes
if: failure() && steps.precommit.outcome == 'failure'

41
.github/workflows/prerelease.yaml vendored Normal file
View File

@ -0,0 +1,41 @@
# This config uses industrial_ci (https://github.com/ros-industrial/industrial_ci.git).
# For troubleshooting, see readme (https://github.com/ros-industrial/industrial_ci/blob/master/README.rst)
name: pre-release
on:
workflow_dispatch:
inputs:
ROS_DISTRO:
type: string
required: true
description: 'ROS distribution codename:'
default: noetic
permissions:
contents: read # to fetch code (actions/checkout)
jobs:
default:
env:
ROS_DISTRO: ${{ inputs.ROS_DISTRO }}
PRERELEASE: true
BASEDIR: ${{ github.workspace }}/.work
name: "${{ inputs.ROS_DISTRO }}"
runs-on: ubuntu-latest
steps:
- name: "Free up disk space"
run: |
sudo apt-get -qq purge build-essential "ghc*"
sudo apt-get clean
# cleanup docker images not used by us
docker system prune -af
# free up a lot of stuff from /usr/local
sudo rm -rf /usr/local
df -h
- uses: actions/checkout@v4
with:
submodules: recursive
- name: industrial_ci
uses: ros-industrial/industrial_ci@master

2
.gitignore vendored
View File

@ -1 +1,3 @@
*.swp
*.pyc
__pycache__/

3
.gitmodules vendored Normal file
View File

@ -0,0 +1,3 @@
[submodule "core/src/scope_guard"]
path = core/src/scope_guard
url = https://github.com/ricab/scope_guard

View File

@ -15,7 +15,7 @@
repos:
# Standard hooks
- repo: https://github.com/pre-commit/pre-commit-hooks
rev: v4.0.1
rev: v5.0.0
hooks:
- id: check-added-large-files
- id: check-case-conflict
@ -29,23 +29,33 @@ repos:
- id: trailing-whitespace
- repo: https://github.com/psf/black
rev: 21.11b1
rev: 25.1.0
hooks:
- id: black
args: ["--line-length", "100"]
- repo: local
hooks:
- id: clang-format
name: clang-format
description: Format files with ClangFormat.
entry: clang-format-10
entry: clang-format-14
language: system
files: \.(c|cc|cxx|cpp|frag|glsl|h|hpp|hxx|ih|ispc|ipp|java|js|m|proto|vert)$
args: ['-fallback-style=none', '-i']
args: ["-fallback-style=none", "-i"]
- id: catkin_lint
name: catkin_lint
description: Check package.xml and cmake files
entry: catkin_lint .
entry: catkin_lint . --ignore duplicate_cmd
language: system
always_run: true
pass_filenames: false
- repo: local
hooks:
- id: misspelled-moveit
name: misspelled-moveit
description: MoveIt should be spelled exactly as MoveIt
language: pygrep
entry: Moveit|MoveIt!
exclude: .pre-commit-config.yaml|README.md

View File

@ -5,13 +5,26 @@ It draws on the planning capabilities of [MoveIt](https://moveit.ros.org/) to so
A common interface, based on MoveIt's PlanningScene is used to pass solution hypotheses between stages.
The framework enables the hierarchical organization of basic stages using *containers*, allowing for sequential as well as parallel compositions.
## Video
## Branches
[![Video associated with ICRA 2019 paper](https://img.youtube.com/vi/fCORKVYsdDI/0.jpg )](https://www.youtube.com/watch?v=fCORKVYsdDI)
This repository provides the following branches:
- **master**: ROS 1 development
- **ros2**: ROS 2 development, compatible with MoveIt 2 `main`
- **humble**: ROS 2 stable branch for Humble support
## Videos
- Demo video associated with [ICRA 2019 paper](https://pub.uni-bielefeld.de/download/2918864/2933599/paper.pdf)
[![](https://img.youtube.com/vi/fCORKVYsdDI/0.jpg)](https://www.youtube.com/watch?v=fCORKVYsdDI)
- [Presentation @ ROSCon 2018 (Madrid)](https://vimeo.com/293432325)
- [Presentation @ MoveIt workshop 2019 (Macau)](https://www.youtube.com/watch?v=a8r7O2bs1Mc)
## Tutorial
We provide a tutorial for a pick-and-place pipeline without bells & whistles [as part of the MoveIt tutorials](https://ros-planning.github.io/moveit_tutorials/doc/moveit_task_constructor/moveit_task_constructor_tutorial.html).
We provide a tutorial for a pick-and-place pipeline without bells & whistles [as part of the MoveIt tutorials](https://moveit.github.io/moveit_tutorials/doc/moveit_task_constructor/moveit_task_constructor_tutorial.html).
## Roadmap
@ -32,10 +45,9 @@ Ideas and requests for other interesting/useful features are welcome.
If you use this framework in your project, please cite the associated paper:
Michael Görner*, Robert Haschke*, Helge Ritter, and Jianwei Zhang,
MoveIt! Task Constructor for Task-Level Motion Planning,
International Conference on Robotics and Automation, ICRA 2019, Montreal, Canada.
"MoveIt! Task Constructor for Task-Level Motion Planning",
_International Conference on Robotics and Automation (ICRA)_, 2019, Montreal, Canada.
[[DOI]](https://doi.org/10.1109/ICRA.2019.8793898) [[PDF]](https://pub.uni-bielefeld.de/download/2918864/2933599/paper.pdf).

View File

@ -0,0 +1,17 @@
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
Changelog for package moveit_task_constructor_capabilities
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
0.1.3 (2023-03-06)
------------------
0.1.2 (2023-02-24)
------------------
0.1.1 (2023-02-15)
------------------
0.1.0 (2023-02-02)
------------------
* Initial release
* Contributors: Michael Görner, Robert Haschke

View File

@ -1,8 +1,7 @@
cmake_minimum_required(VERSION 3.1.3)
cmake_minimum_required(VERSION 3.16)
project(moveit_task_constructor_capabilities)
add_compile_options(-std=c++14)
find_package(fmt REQUIRED)
find_package(catkin REQUIRED COMPONENTS
actionlib
moveit_core
@ -13,23 +12,28 @@ find_package(catkin REQUIRED COMPONENTS
std_msgs
)
moveit_build_options()
catkin_package(
LIBRARIES $PROJECT_NAME}
LIBRARIES ${PROJECT_NAME}
CATKIN_DEPENDS
actionlib
moveit_task_constructor_msgs
std_msgs
)
include_directories(
${catkin_INCLUDE_DIRS}
)
# Check for availability of moveit::core::MoveItErrorCode::toString
set(CMAKE_REQUIRED_INCLUDES ${catkin_INCLUDE_DIRS})
set(CMAKE_REQUIRED_LIBRARIES ${catkin_LIBRARIES})
set(CMAKE_REQUIRED_FLAGS -Wno-error)
include(CheckCXXSymbolExists)
add_library(${PROJECT_NAME}
src/execute_task_solution_capability.cpp
)
add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS})
target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES})
target_include_directories(${PROJECT_NAME} SYSTEM PUBLIC ${catkin_INCLUDE_DIRS})
target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} fmt::fmt)
install(TARGETS ${PROJECT_NAME}
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}

View File

@ -1,6 +1,6 @@
<package format="2">
<name>moveit_task_constructor_capabilities</name>
<version>0.0.0</version>
<version>0.1.3</version>
<description>
MoveGroupCapabilites to interact with MoveIt
</description>
@ -12,6 +12,7 @@
<buildtool_depend>catkin</buildtool_depend>
<depend>fmt</depend>
<depend>moveit_core</depend>
<depend>moveit_ros_move_group</depend>
<depend>actionlib</depend>

View File

@ -36,16 +36,15 @@
#include "execute_task_solution_capability.h"
#include <moveit/task_constructor/moveit_compat.h>
#include <moveit/plan_execution/plan_execution.h>
#include <moveit/trajectory_processing/trajectory_tools.h>
#include <moveit/kinematic_constraints/utils.h>
#include <moveit/move_group/capability_names.h>
#include <moveit/robot_state/conversions.h>
#if MOVEIT_HAS_MESSAGE_CHECKS
#include <moveit/utils/message_checks.h>
#endif
#include <moveit/utils/moveit_error_code.h>
#include <fmt/format.h>
#include <fmt/ranges.h>
namespace {
@ -88,12 +87,12 @@ void ExecuteTaskSolutionCapability::initialize() {
// configure the action server
as_.reset(new actionlib::SimpleActionServer<moveit_task_constructor_msgs::ExecuteTaskSolutionAction>(
root_node_handle_, "execute_task_solution",
std::bind(&ExecuteTaskSolutionCapability::goalCallback, this, std::placeholders::_1), false));
std::bind(&ExecuteTaskSolutionCapability::execCallback, this, std::placeholders::_1), false));
as_->registerPreemptCallback(std::bind(&ExecuteTaskSolutionCapability::preemptCallback, this));
as_->start();
}
void ExecuteTaskSolutionCapability::goalCallback(
void ExecuteTaskSolutionCapability::execCallback(
const moveit_task_constructor_msgs::ExecuteTaskSolutionGoalConstPtr& goal) {
moveit_task_constructor_msgs::ExecuteTaskSolutionResult result;
@ -112,8 +111,7 @@ void ExecuteTaskSolutionCapability::goalCallback(
result.error_code = context_->plan_execution_->executeAndMonitor(plan);
}
const std::string response = context_->plan_execution_->getErrorCodeString(result.error_code);
const std::string response = moveit::core::MoveItErrorCode::toString(result.error_code);
if (result.error_code.val == moveit_msgs::MoveItErrorCodes::SUCCESS)
as_->setSucceeded(result, response);
else if (result.error_code.val == moveit_msgs::MoveItErrorCodes::PREEMPTED)
@ -129,9 +127,9 @@ void ExecuteTaskSolutionCapability::preemptCallback() {
bool ExecuteTaskSolutionCapability::constructMotionPlan(const moveit_task_constructor_msgs::Solution& solution,
plan_execution::ExecutableMotionPlan& plan) {
robot_model::RobotModelConstPtr model = context_->planning_scene_monitor_->getRobotModel();
moveit::core::RobotModelConstPtr model = context_->planning_scene_monitor_->getRobotModel();
robot_state::RobotState state(model);
moveit::core::RobotState state(model);
{
planning_scene_monitor::LockedPlanningSceneRO scene(context_->planning_scene_monitor_);
state = scene->getCurrentState();
@ -156,8 +154,9 @@ bool ExecuteTaskSolutionCapability::constructMotionPlan(const moveit_task_constr
if (!joint_names.empty()) {
group = findJointModelGroup(*model, joint_names);
if (!group) {
ROS_ERROR_STREAM_NAMED("ExecuteTaskSolution", "Could not find JointModelGroup that actuates {"
<< boost::algorithm::join(joint_names, ", ") << "}");
ROS_ERROR_STREAM_NAMED(
"ExecuteTaskSolution",
fmt::format("Could not find JointModelGroup that actuates {{{}}}", fmt::join(joint_names, ", ")));
return false;
}
ROS_DEBUG_NAMED("ExecuteTaskSolution", "Using JointModelGroup '%s' for execution",
@ -166,26 +165,30 @@ bool ExecuteTaskSolutionCapability::constructMotionPlan(const moveit_task_constr
}
exec_traj.trajectory_ = std::make_shared<robot_trajectory::RobotTrajectory>(model, group);
exec_traj.trajectory_->setRobotTrajectoryMsg(state, sub_traj.trajectory);
exec_traj.controller_names_ = sub_traj.execution_info.controller_names;
/* TODO add action feedback and markers */
exec_traj.effect_on_success_ = [this, sub_traj,
description](const plan_execution::ExecutableMotionPlan* /*plan*/) {
#if MOVEIT_HAS_MESSAGE_CHECKS
if (!moveit::core::isEmpty(sub_traj.scene_diff)) {
#else
if (!planning_scene::PlanningScene::isEmpty(sub_traj.scene_diff)) {
#endif
ROS_DEBUG_STREAM_NAMED("ExecuteTaskSolution", "apply effect of " << description);
return context_->planning_scene_monitor_->newPlanningSceneMessage(sub_traj.scene_diff);
}
return true;
};
exec_traj.effect_on_success_ =
[this, &scene_diff = const_cast<::moveit_msgs::PlanningScene&>(sub_traj.scene_diff), description, i,
no = solution.sub_trajectory.size()](const plan_execution::ExecutableMotionPlan* /*plan*/) {
// publish feedback
moveit_task_constructor_msgs::ExecuteTaskSolutionFeedback feedback;
feedback.sub_id = i;
feedback.sub_no = no;
as_->publishFeedback(feedback);
// Never modify joint state directly (only via robot trajectories)
scene_diff.robot_state.joint_state = sensor_msgs::JointState();
scene_diff.robot_state.multi_dof_joint_state = sensor_msgs::MultiDOFJointState();
scene_diff.robot_state.is_diff = true; // silent empty JointState msg error
if (!moveit::core::isEmpty(scene_diff)) {
ROS_DEBUG_STREAM_NAMED("ExecuteTaskSolution", "apply effect of " << description);
return context_->planning_scene_monitor_->newPlanningSceneMessage(scene_diff);
}
return true;
};
#if MOVEIT_HAS_MESSAGE_CHECKS
if (!moveit::core::isEmpty(sub_traj.scene_diff.robot_state) &&
#else
if (!planning_scene::PlanningScene::isEmpty(sub_traj.scene_diff.robot_state) &&
#endif
!moveit::core::robotStateMsgToRobotState(sub_traj.scene_diff.robot_state, state, true)) {
ROS_ERROR_STREAM_NAMED("ExecuteTaskSolution",
"invalid intermediate robot state in scene diff of SubTrajectory " << description);
@ -198,5 +201,5 @@ bool ExecuteTaskSolutionCapability::constructMotionPlan(const moveit_task_constr
} // namespace move_group
#include <class_loader/class_loader.hpp>
CLASS_LOADER_REGISTER_CLASS(move_group::ExecuteTaskSolutionCapability, move_group::MoveGroupCapability)
#include <pluginlib/class_list_macros.hpp>
PLUGINLIB_EXPORT_CLASS(move_group::ExecuteTaskSolutionCapability, move_group::MoveGroupCapability)

View File

@ -61,7 +61,7 @@ private:
bool constructMotionPlan(const moveit_task_constructor_msgs::Solution& solution,
plan_execution::ExecutableMotionPlan& plan);
void goalCallback(const moveit_task_constructor_msgs::ExecuteTaskSolutionGoalConstPtr& goal);
void execCallback(const moveit_task_constructor_msgs::ExecuteTaskSolutionGoalConstPtr& goal);
void preemptCallback();
std::unique_ptr<actionlib::SimpleActionServer<moveit_task_constructor_msgs::ExecuteTaskSolutionAction>> as_;

37
core/CHANGELOG.rst Normal file
View File

@ -0,0 +1,37 @@
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
Changelog for package moveit_task_constructor_core
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
0.1.3 (2023-03-06)
------------------
* MoveRelative: Allow backwards operation for joint-space delta (`#436 <https://github.com/ros-planning/moveit_task_constructor/issues/436>`_)
* ComputeIK: Limit collision checking to JMG (`#428 <https://github.com/ros-planning/moveit_task_constructor/issues/428>`_)
* Fix: Fetch pybind11 submodule if not yet present
* Contributors: Robert Haschke
0.1.2 (2023-02-24)
------------------
* Remove moveit/__init__.py during .deb builds to avoid installation conflicts
* MultiPlanner solver (`#429 <https://github.com/ros-planning/moveit_task_constructor/issues/429>`_): a planner that tries multiple planners in sequence
* CartesianPath: Deprecate redundant property setters
* PlannerInterface: provide "timeout" property
* PlannerInterface: provide setters for properties
* JointInterpolation: fix timeout handling
* Contributors: Robert Haschke
0.1.1 (2023-02-15)
------------------
* Resort to MoveIt's python tools
* Provide ComputeIK.ik_frame as full PoseStamped
* Fixed build farm issues
* Removed unused eigen_conversions includes
* Fixed odr compiler warning on Buster
* Fixed missing dependency declarations
* Contributors: Robert Haschke
0.1.0 (2023-02-02)
------------------
* Initial release
* Contributors: Michael Görner, Robert Haschke, Captain Yoshi, Christian Petersmeier, Henning Kayser, Jafar Abdi, Tyler Weaver

View File

@ -1,8 +1,10 @@
cmake_minimum_required(VERSION 3.1.3)
cmake_minimum_required(VERSION 3.16)
project(moveit_task_constructor_core)
find_package(Boost REQUIRED)
find_package(fmt REQUIRED)
find_package(catkin REQUIRED COMPONENTS
roslint
tf2_eigen
geometry_msgs
moveit_core
@ -12,29 +14,36 @@ find_package(catkin REQUIRED COMPONENTS
roscpp
visualization_msgs
rviz_marker_tools
py_binding_tools
)
option(MOVEIT_CI_WARNINGS "Enable all warnings used by CI" OFF) # We use our own set of warnings
moveit_build_options()
catkin_python_setup()
catkin_package(
LIBRARIES
${PROJECT_NAME}
${PROJECT_NAME}_stages
${PROJECT_NAME}_stage_plugins
${PROJECT_NAME}_python_tools
INCLUDE_DIRS
include
CATKIN_DEPENDS
geometry_msgs
moveit_core
moveit_task_constructor_msgs
rviz_marker_tools
visualization_msgs
)
set(CMAKE_CXX_STANDARD 14)
add_compile_options(-fvisibility-inlines-hidden)
set(CMAKE_VISIBILITY_INLINES_HIDDEN TRUE)
set(PROJECT_INCLUDE ${CMAKE_CURRENT_SOURCE_DIR}/include/moveit/task_constructor)
add_subdirectory(src)
add_subdirectory(python)
add_subdirectory(test)
install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}

21
core/doc/Doxyfile Normal file
View File

@ -0,0 +1,21 @@
PROJECT_NAME = MTC
INPUT = ../include/ ../src/
RECURSIVE = YES
GENERATE_HTML = YES
GENERATE_LATEX = NO
GENERATE_XML = NO
HTML_OUTPUT = _cpp
XML_OUTPUT = _doxygenxml
XML_PROGRAMLISTING = YES
ALIASES = "rst=\verbatim embed:rst"
ALIASES += "endrst=\endverbatim"
QUIET = YES
WARNINGS = YES
WARN_IF_UNDOCUMENTED = NO
EXCLUDE_SYMBOLS = *Private
EXCLUDE_SYMBOLS += class_
EXCLUDE_SYMBOLS += declval*

View File

@ -0,0 +1,34 @@
{{ fullname | escape | underline}}
.. currentmodule:: {{ module }}
.. autoclass:: {{ objname }}
:members:
:show-inheritance:
:inherited-members:
:special-members: __len__, __getitem__, __iter__, __call__, __add__, __mul__
{% block methods %}
{% if methods %}
.. rubric:: {{ _('Methods') }}
.. autosummary::
:nosignatures:
{% for item in methods %}
{%- if not item.startswith('_') %}
~{{ name }}.{{ item }}
{%- endif -%}
{%- endfor %}
{% endif %}
{% endblock %}
{% block attributes %}
{% if attributes %}
.. rubric:: {{ _('Attributes') }}
.. autosummary::
{% for item in attributes %}
~{{ name }}.{{ item }}
{%- endfor %}
{% endif %}
{% endblock %}

View File

@ -0,0 +1,66 @@
{{ fullname | escape | underline}}
.. automodule:: {{ fullname }}
{% block attributes %}
{% if attributes %}
.. rubric:: Module attributes
.. autosummary::
:toctree:
{% for item in attributes %}
{{ item }}
{%- endfor %}
{% endif %}
{% endblock %}
{% block functions %}
{% if functions %}
.. rubric:: {{ _('Functions') }}
.. autosummary::
:toctree:
:nosignatures:
{% for item in functions %}
{{ item }}
{%- endfor %}
{% endif %}
{% endblock %}
{% block classes %}
{% if classes %}
.. rubric:: {{ _('Classes') }}
.. autosummary::
:toctree:
:template: custom-class-template.rst
:nosignatures:
{% for item in classes %}
{{ item }}
{%- endfor %}
{% endif %}
{% endblock %}
{% block exceptions %}
{% if exceptions %}
.. rubric:: {{ _('Exceptions') }}
.. autosummary::
:toctree:
{% for item in exceptions %}
{{ item }}
{%- endfor %}
{% endif %}
{% endblock %}
{% block modules %}
{% if modules %}
.. autosummary::
:toctree:
:template: custom-module-template.rst
:recursive:
{% for item in modules %}
{{ item }}
{%- endfor %}
{% endif %}
{% endblock %}

20
core/doc/api.rst Normal file
View File

@ -0,0 +1,20 @@
.. _sec-api:
API reference
-------------
`C++ <_static/index.html>`_
^^^^^^^^^^^^^^^^^^^^^^^^^^^
Python
^^^^^^
.. autosummary::
:toctree: _autosummary
:caption: API
:recursive:
:template: custom-module-template.rst
moveit.task_constructor
pymoveit_mtc.core
pymoveit_mtc.stages

47
core/doc/basics.rst Normal file
View File

@ -0,0 +1,47 @@
Basic Concepts
==============
The fundamental idea of MTC is that complex motion planning problems can be composed into a set of simpler subproblems. The top-level planning problem is specified as a Task while all subproblems are specified by Stages. Stages can be arranged in any arbitrary order and hierarchy only limited by the individual stages types. The order in which stages can be arranged is restricted by the direction in which results are passed. There are three possible stage types w.r.t. their data flow: generators, propagators, and connector stages:
.. glossary::
Generators
compute their results independently of their neighboring stages and pass them in both directions, backwards and forwards. Examples include pose generators, e.g. for grasping or placing, as well as ``ComputeIK``, which computes IK solutions for Cartesian targets. neighboring stages can continue processing from the generated states. The most important generator stage, however, is ``CurrentState``, which provides the current robot state as the starting point for a planning pipeline.
Propagators
receive the result of *one* neighboring stage as input, plan towards a goal state, and then propagate their result to the opposite interface site. Propagating stages can receive their input on either interface, begin or end. The flow of information (forwards or backwards) is determined by the input interface of the stage. An example is a stage that computes a Cartesian path based on either a start or a goal state.
Connectors
do not propagate any results, but rather attempt to bridge the gap between the resulting states of both its neighboring stages. It receives input states from both, the begin and end interface and attempts to connect them via a suitable motion plan. Obviously, any pair of input states needs to be *compatible*, i.e. their states (including collision and attached objects as well as joint poses) need to match except for those joints that are part of the given planning group.
Processing starts from generator stages, expands via propagators, and finally connects partial solution sequences via connector stages.
Obviously, there exist limitations on how stages can be connected to each other. For example, two generator stages cannot occur in sequence as they would both attempt to *write* into their interfaces, but non of them is actually *reading*. Same applies for two connectors in a row: they would both attempt to read, while no stage is actually writing.
The compatibility of stages is automatically checked once before planning by ``Task::init()``.
To compose a planning pipeline from multiple seemingly independent parts, for example grasping an object and placing it at a new location, one needs to break the typical linear pipeline structure: the place pose is another generator stage (additionally to the ``CurrentState`` stage we are starting from) serving as a seed for the placing sub solution. However, this stage is not completely independent from the grasping sub solution: it should continue where grasping left off, namely with the grasped object attached to the gripper. To convey this state information, the place pose generator should inherit from ``MonitoringGenerator``, which monitors the solutions of another stage in the pipeline in fast-forwards them for further processing in the dependent stage.
In order to hierarchically organize planning pipelines and to allow for reuse of sub pipelines, e.g. for grasping or placing, one can encapsulate stages within various containers.
Stages without children are called primitive stages. We provide three types of containers:
.. glossary::
Wrappers
encapsulate a single subordinate stage and modify or filter its results. For example, a filter stage that only accepts solutions of its child stage that satisfy a certain constraint can be realized as a wrapper. Another standard use of this type includes the IK wrapper stage, which generates inverse kinematics solutions based on planning scenes annotated with a pose target property.
Serial containers
hold a sequence of subordinate stages and only consider end-to-end solutions as results. An example is a picking motion that consists of a sequence of coherent steps.
Parallel containers
consider the solutions of all their children as feasible. Different sub types are available, namely:
``Alternative`` container
processes all children simultaneously (currently in a round-robin fashion). For example, one could plan a grasping sequence for a left and right arm in parallel if the actual choice of the arm doesn't matter for the task.
``Fallback`` container
processes their children sequentially: only if the current child has exhausted all its solution candidates (and didn't produce any feasible solution), the next child is considered.
Use this container, e.g. if you prefer grasping with the right arm, but allow falling back to the left if really needed.
``Merger`` container
processes their children simultaneously and combine their solutions into an joint solution. It is assumed that children operate on disjoint joint model groups, e.g. the arm and the hand, such that their solution trajectories can be executed in parallel after being merged.
Stages not only support solving motion planning problems. They can also be used for all kinds of state transitions, as for instance modifying the planning scene. Combined with the possibility of using class inheritance it is possible to construct very complex behavior while only relying on a well-structured set of primitive stages.

9
core/doc/concepts.rst Normal file
View File

@ -0,0 +1,9 @@
.. _sec-concepts:
Concepts
------------
.. toctree::
:maxdepth: 2
basics

243
core/doc/conf.py Normal file
View File

@ -0,0 +1,243 @@
# Configuration file for the Sphinx documentation builder.
#
# This file does only contain a selection of the most common options.
# For a full list, refer to: http://www.sphinx-doc.org/en/master/config
from lxml import etree
import os
import subprocess
import sys
from pathlib import Path
DIR = Path(__file__).parent.resolve()
# If extensions (or modules to document with autodoc) are in another directory,
# add these directories to sys.path here. If the directory is relative to the
# documentation root, use os.path.abspath to make it absolute, like shown here.
# sys.path.insert(0, os.path.abspath('.'))
# -- General configuration ------------------------------------------------
# If your documentation needs a minimal Sphinx version, state it here.
# needs_sphinx = '1.0'
# Add any Sphinx extension module names here, as strings. They can be
# extensions coming with Sphinx (named 'sphinx.ext.*') or your custom
# ones.
extensions = [
"sphinx_copybutton",
"sphinx.ext.autodoc",
"sphinx.ext.autosummary",
"sphinx.ext.intersphinx",
"sphinx.ext.napoleon",
"sphinx.ext.extlinks",
]
autosummary_generate = True
autoclass_content = "both" # Add __init__ doc (ie. params) to class summaries
html_show_sourcelink = False # Remove 'view source code' from top of page (for html, not python)
autodoc_inherit_docstrings = True # If no docstring, inherit from base class
set_type_checking_flag = True # Enable 'expensive' imports for sphinx_autodoc_typehints
add_module_names = False
# Add any paths that contain templates here, relative to this directory.
templates_path = ["_templates"]
# The suffix(es) of source filenames.
# You can specify multiple suffix as a list of string:
# source_suffix = ['.rst', '.md']
source_suffix = ".rst"
# The encoding of source files.
# source_encoding = 'utf-8-sig'
# The master toctree document.
master_doc = "index"
# General information about the project.
project = "MoveIt Task Constructor"
author = "Michael Görner, Robert Haschke"
# The version info for the project you're documenting, acts as replacement for
# |version| and |release|, also used in various other places throughout the
# built documents.
#
# Read version from package.xml
xml = etree.parse("../package.xml")
version = str(xml.xpath("/package/version/text()")[0])
# The language for content autogenerated by Sphinx. Refer to documentation
# for a list of supported languages.
#
# This is also used if you do content translation via gettext catalogs.
# Usually you set "language" from the command line for these cases.
language = "en"
# There are two options for replacing |today|: either, you set today to some
# non-false value, then it is used:
# today = ''
# Else, today_fmt is used as the format for a strftime call.
# today_fmt = '%B %d, %Y'
# List of patterns, relative to source directory, that match files and
# directories to ignore when looking for source files.
exclude_patterns = [".build", "python/pybind11"]
# The reST default role (used for this markup: `text`) to use for all
# documents.
# default_role = None
# If true, '()' will be appended to :func: etc. cross-reference text.
# add_function_parentheses = True
# If true, the current module name will be prepended to all description
# unit titles (such as .. function::).
# add_module_names = True
# If true, sectionauthor and moduleauthor directives will be shown in the
# output. They are ignored by default.
# show_authors = False
# The name of the Pygments (syntax highlighting) style to use.
# pygments_style = 'monokai'
# A list of ignored prefixes for module index sorting.
# modindex_common_prefix = []
# If true, keep warnings as "system message" paragraphs in the built documents.
# keep_warnings = False
# If true, `todo` and `todoList` produce output, else they produce nothing.
todo_include_todos = False
# -- Options for HTML output ----------------------------------------------
# The theme to use for HTML and HTML Help pages. See the documentation for
# a list of builtin themes.
html_theme = "furo"
# Theme options are theme-specific and customize the look and feel of a theme
# further. For a list of options available for each theme, see the
# documentation.
# html_theme_options = {}
# Add any paths that contain custom themes here, relative to this directory.
# html_theme_path = []
# The name for this set of Sphinx documents. If None, it defaults to
# "<project> v<release> documentation".
# html_title = None
# A shorter title for the navigation bar. Default is the same as html_title.
# html_short_title = None
# The name of an image file (relative to this directory) to place at the top
# of the sidebar.
# html_logo = None
# The name of an image file (within the static path) to use as favicon of the
# docs. This file should be a Windows icon file (.ico) being 16x16 or 32x32
# pixels large.
# html_favicon = None
# Add any paths that contain custom static files (such as style sheets) here,
# relative to this directory. They are copied after the builtin static files,
# so a file named "default.css" will overwrite the builtin "default.css".
html_static_path = ["_cpp"]
# Add any extra paths that contain custom files (such as robots.txt or
# .htaccess) here, relative to this directory. These files are copied
# directly to the root of the documentation.
# html_extra_path = []
# If not '', a 'Last updated on:' timestamp is inserted at every page bottom,
# using the given strftime format.
# html_last_updated_fmt = '%b %d, %Y'
# If true, SmartyPants will be used to convert quotes and dashes to
# typographically correct entities.
# html_use_smartypants = True
# Custom sidebar templates, maps document names to template names.
# html_sidebars = {}
# Additional templates that should be rendered to pages, maps page names to
# template names.
# html_additional_pages = {}
# If false, no module index is generated.
# html_domain_indices = True
# If false, no index is generated.
# html_use_index = True
# If true, the index is split into individual pages for each letter.
# html_split_index = False
# If true, links to the reST sources are added to the pages.
# html_show_sourcelink = True
# If true, "Created using Sphinx" is shown in the HTML footer. Default is True.
# html_show_sphinx = True
# If true, "(C) Copyright ..." is shown in the HTML footer. Default is True.
# html_show_copyright = True
# If true, an OpenSearch description file will be output, and all pages will
# contain a <link> tag referring to it. The value of this option must be the
# base URL from which the finished HTML is served.
# html_use_opensearch = ''
# This is the file name suffix for HTML files (e.g. ".xhtml").
# html_file_suffix = None
# Language to be used for generating the HTML full-text search index.
# Sphinx supports the following languages:
# html_search_language = 'en'
# A dictionary with options for the search language support, empty by default.
# Now only 'ja' uses this config value
# html_search_options = {'type': 'default'}
# The name of a javascript file (relative to the configuration directory) that
# implements a search results scorer. If empty, the default will be used.
# html_search_scorer = 'scorer.js'
# Output file base name for HTML help builder.
htmlhelp_basename = "mtcdoc"
# Example configuration for intersphinx: refer to the Python standard library.
intersphinx_mapping = {"python": ("https://docs.python.org/3", None)}
ros_distro = "noetic"
ros_docs = f"http://docs.ros.org/{ros_distro}/api"
extlinks = {
"rosdocs": (f"{ros_docs}/%s", "%s"),
"msgs": (f"{ros_docs}/moveit_task_constructor/html/msg/%s.html", "%s"),
"moveit_msgs": (f"{ros_docs}/moveit_msgs/html/msg/%s.html", "%s"),
"geometry_msgs": (f"{ros_docs}/geometry_msgs/html/msg/%s.html", "%s"),
"visualization_msgs": (f"{ros_docs}/visualization_msgs/html/msg/%s.html", "%s"),
}
def generate_doxygen_xml(app):
build_dir = os.path.join(app.confdir, ".build")
if not os.path.exists(build_dir):
os.mkdir(build_dir)
print("Running doxygen")
try:
subprocess.call(["doxygen", "--version"])
retcode = subprocess.call(["doxygen"], cwd=app.confdir)
if retcode < 0:
sys.stderr.write(f"doxygen error code: {-retcode}\n")
except OSError as e:
sys.stderr.write(f"doxygen execution failed: {e}\n")
def setup(app):
# Add hook for building doxygen xml when needed
# app.connect("builder-inited", generate_doxygen_xml)
pass

301
core/doc/howto.rst Normal file
View File

@ -0,0 +1,301 @@
.. _sec-howtoguides:
How-To Guides
=============
.. _subsec-howto-stage-usage:
Stage Usage
-----------
.. _subsubsec-howto-alternatives:
Alternatives
^^^^^^^^^^^^
Using the ``alternatives`` stage, you can plan for multiple
execution paths.
Download the full example code here: :download:`Source <./../../demo/scripts/alternatives.py>`
.. literalinclude:: ./../../demo/scripts/alternatives.py
:language: python
:start-after: [initAndConfigAlternatives]
:end-before: [initAndConfigAlternatives]
.. _subsubsec-howto-fallbacks:
Fallbacks
^^^^^^^^^
The ``fallbacks`` stage provides alternative motion planners
if planning fails with the primary one.
Download the full example code here: :download:`Source <./../../demo/scripts/fallbacks.py>`
.. literalinclude:: ./../../demo/scripts/fallbacks.py
:language: python
:start-after: [initAndConfigFallbacks]
:end-before: [initAndConfigFallbacks]
.. _subsubsec-howto-merger:
Merger
^^^^^^
Plan and execute sequences in parallel using the ``merger`` stage.
Download the full example code here: :download:`Source <./../../demo/scripts/merger.py>`
.. literalinclude:: ./../../demo/scripts/merger.py
:language: python
:start-after: [initAndConfigMerger]
:end-before: [initAndConfigMerger]
.. _subsubsec-howto-connect:
Connect
^^^^^^^
Connect two stages by finding a motion plan between them.
The code snippet is part of the :ref:`Pick and Place <subsec-tut-pick-place>` guide.
Download the full example code here: :download:`Source <./../../demo/scripts/pickplace.py>`
.. literalinclude:: ./../../demo/scripts/pickplace.py
:language: python
:start-after: [initAndConfigConnect]
:end-before: [initAndConfigConnect]
.. _subsubsec-howto-fix-collision-objects:
FixCollisionObjects
^^^^^^^^^^^^^^^^^^^
Check for collisions and resolve them if applicable.
Download the full example code here: :download:`Source <./../../demo/scripts/fix_collision_objects.py>`
.. literalinclude:: ./../../demo/scripts/fix_collision_objects.py
:language: python
:start-after: [initAndConfig]
:end-before: [initAndConfig]
.. _subsubsec-howto-generate-place-pose:
GeneratePlacePose
^^^^^^^^^^^^^^^^^^^
Sample feasible poses around an object pose.
Considers geometry of primitive object type.
Solutions can be used for inverse
kinematics calculations.
The code snippet is part of the :ref:`Pick and Place <subsec-tut-pick-place>` guide.
Download the full example code here: :download:`Source <./../../demo/scripts/pickplace.py>`
.. literalinclude:: ./../../demo/scripts/pickplace.py
:language: python
:start-after: [initCollisionObject]
:end-before: [initCollisionObject]
.. literalinclude:: ./../../demo/scripts/pickplace.py
:language: python
:start-after: [initAndConfigGeneratePlacePose]
:end-before: [initAndConfigGeneratePlacePose]
.. _subsubsec-howto-generate-grasp-pose:
GenerateGraspPose
^^^^^^^^^^^^^^^^^
Sample poses around an object pose by providing
sample density ``angle_delta``.
Solutions can be used for inverse kinematics
calculations.
The code snippet is part of the :ref:`Pick and Place <subsec-tut-pick-place>` guide.
Download the full example code here: :download:`Source <./../../demo/scripts/pickplace.py>`
.. literalinclude:: ./../../demo/scripts/pickplace.py
:language: python
:start-after: [initAndConfigGenerateGraspPose]
:end-before: [initAndConfigGenerateGraspPose]
.. _subsubsec-howto-generate-pose:
GeneratePose
^^^^^^^^^^^^
Spawn a pose on new solutions of the monitored stage.
Download the full example code here: :download:`Source <./../../demo/scripts/generate_pose.py>`
.. literalinclude:: ./../../demo/scripts/generate_pose.py
:language: python
:start-after: [initAndConfigGeneratePose]
:end-before: [initAndConfigGeneratePose]
.. _subsubsec-howto-pick:
Pick
^^^^
Wraps the task pipeline to execute a pick.
The code snippet is part of the :ref:`Pick and Place <subsec-tut-pick-place>` guide.
Download the full example code here: :download:`Source <./../../demo/scripts/pickplace.py>`
.. literalinclude:: ./../../demo/scripts/pickplace.py
:language: python
:start-after: [initAndConfigPick]
:end-before: [initAndConfigPick]
.. _subsubsec-howto-place:
Place
^^^^^
Wraps the task pipeline to execute a pick.
The code snippet is part of the :ref:`Pick and Place <subsec-tut-pick-place>` guide.
Download the full example code here: :download:`Source <./../../demo/scripts/pickplace.py>`
.. literalinclude:: ./../../demo/scripts/pickplace.py
:language: python
:start-after: [initAndConfigPlace]
:end-before: [initAndConfigPlace]
.. _subsubsec-howto-simplegrasp:
SimpleGrasp
^^^^^^^^^^^
Wraps the pose generation and inverse kinematics
computation for the pick pipeline.
The code snippet is part of the :ref:`Pick and Place <subsec-tut-pick-place>` guide.
Download the full example code here: :download:`Source <./../../demo/scripts/pickplace.py>`
.. literalinclude:: ./../../demo/scripts/pickplace.py
:language: python
:start-after: [initAndConfigSimpleGrasp]
:end-before: [initAndConfigSimpleGrasp]
.. _subsubsec-howto-simpleungrasp:
SimpleUnGrasp
^^^^^^^^^^^^^
Wraps the pose generation and inverse kinematics
computation for the place pipeline.
The code snippet is part of the :ref:`Pick and Place <subsec-tut-pick-place>` guide.
Download the full example code here: :download:`Source <./../../demo/scripts/pickplace.py>`
.. literalinclude:: ./../../demo/scripts/pickplace.py
:language: python
:start-after: [initAndConfigSimpleUnGrasp]
:end-before: [initAndConfigSimpleUnGrasp]
.. _subsubsec-howto-modify-planning-scene:
ModifyPlanningScene
^^^^^^^^^^^^^^^^^^^
Modify the planning scene.
Download the full example code here: :download:`Source <./../../demo/scripts/modify_planning_scene.py>`
.. literalinclude:: ./../../demo/scripts/modify_planning_scene.py
:language: python
:start-after: [initAndConfigModifyPlanningScene]
:end-before: [initAndConfigModifyPlanningScene]
.. _subsubsec-howto-fixed-state:
FixedState
^^^^^^^^^^
Spawn a pre-defined state.
Download the full example code here: :download:`Source <./../../demo/scripts/fixed_state.py>`
.. literalinclude:: ./../../demo/scripts/fixed_state.py
:language: python
:start-after: [initAndConfigFixedState]
:end-before: [initAndConfigFixedState]
.. _subsubsec-howto-compute-ik:
ComputeIK
^^^^^^^^^
Compute the inverse kinematics of the monitored stages'
solution. Be sure to correctly configure the ``target_pose``
property to be derived from the monitored stage as shown
in the example.
Download the full example code here: :download:`Source <./../../demo/scripts/compute_ik.py>`
.. literalinclude:: ./../../demo/scripts/compute_ik.py
:language: python
:start-after: [initAndConfigComputeIk]
:end-before: [initAndConfigComputeIk]
.. _subsubsec-howto-move-to:
MoveTo
^^^^^^
Use planners to compute a motion plan.
Download the full example code here: :download:`Source <../../demo/scripts/cartesian.py>`
.. literalinclude:: ../../demo/scripts/cartesian.py
:language: python
:start-after: [initAndConfigMoveTo]
:end-before: [initAndConfigMoveTo]
.. _subsubsec-howto-move-relative:
MoveRelative
^^^^^^^^^^^^
Move along a relative offset.
Download the full example code here: :download:`Source <../../demo/scripts/cartesian.py>`
.. literalinclude:: ../../demo/scripts/cartesian.py
:language: python
:start-after: [initAndConfigMoveRelative]
:end-before: [initAndConfigMoveRelative]
.. _subsec-howto-stage-extension:
Stage Extension
---------------
.. _subsubsec-howto-move-relative-extension:
MoveRelative
^^^^^^^^^^^^
You may derive from this stage to extend its functionality.
``MoveRelative`` itself derives from the propagator stage that
alters solutions (i.e. computes a motion plan) when they are
passed through the stage.
.. literalinclude:: ./../../core/python/test/rostest_trampoline.py
:language: python
:pyobject: PyMoveRelX
.. _subsubsec-howto-generator-extension:
Generator
^^^^^^^^^
Derive from the ``Generator`` stage to implement your own
logic in the compute function.
.. literalinclude:: ./../../core/python/test/rostest_trampoline.py
:language: python
:pyobject: PyGenerator
.. _subsubsec-howto-monitoring-generator-extension:
MonitoringGenerator
^^^^^^^^^^^^^^^^^^^
Derive from the ``MonitoringGenerator`` stage to
implement your own logic in the compute function.
Use the monitoring generator instead of the normal
generator if you need to access solutions of the
monitored stage (e.g. computation of inverse kinematics).
.. literalinclude:: ./../../core/python/test/rostest_trampoline.py
:language: python
:pyobject: PyMonitoringGenerator

31
core/doc/index.rst Normal file
View File

@ -0,0 +1,31 @@
MoveIt Task Constructor (MTC)
=============================
The Task Constructor framework provides a flexible and transparent way
to define and plan actions that consist of multiple interdependent subtasks.
It draws on the planning capabilities of MoveIt to solve individual subproblems
in black-box planning stages.
A common interface, based on MoveIts PlanningScene is used to pass solution
hypotheses between stages. The framework enables the hierarchical organization of
basic stages using containers, allowing for sequential as well as parallel compositions.
For more details, please refer to the associated `ICRA 2019 publication <https://pub.uni-bielefeld.de/download/2918864/2933599/paper.pdf>`_.
Organization of the documentation
---------------------------------
- :ref:`sec-tutorials` provide examples how to setup your task pipeline.
Start with :ref:`subsec-tut-firststeps` if you are new to MTC.
- :ref:`sec-concepts` discuss the architecture and terminology of MTC on a fairly high level.
- :ref:`sec-howtoguides` help solving specific problems and use cases.
- The :ref:`sec-api` provides quick access to available classes, functions, and their parameters.
.. toctree::
:maxdepth: 2
:hidden:
tutorials/index
concepts
howto
api
troubleshooting

View File

@ -0,0 +1,4 @@
furo
lxml
sphinx
sphinx-copybutton

View File

@ -0,0 +1,31 @@
.. _sec-troubleshooting:
Troubleshooting
===============
``Task::init()`` reports mismatching interfaces
-----------------------------------------------
Before planning, the planning pipeline is checked for consistency. Each stage type has a specific flow interface, e.g. generator-type stages write into both, their begin and end interface, propagator-type stages read from one and write to the opposite, and connector-type stages read from both sides. Obviously these interfaces need to match. If they don't, an ``InitStageException`` is thrown. Per default, this is not very verbose, but you can use the following code snippet to get some more info:
.. code-block:: c
try {
task.plan();
} catch (const InitStageException& e) {
std::cerr << e << std::endl;
throw;
}
For example, the following pipeline:
- ↕ current
- ⛓ connect
- ↑ moveTo
throws the error: ``task pipeline: end interface (←) of 'moveto' does not match mine (→)``.
The validation process runs sequentially through a ``SerialContainer``. Here, ``current``, as a generator stage is compatible to ``connect``, writing to the interface read by ``connect``.
``moveTo`` as a propagator can operate, in principle, forwards and backwards. By default, the operation direction is inferred automatically. Here, as ``connect`` requires a reading end-interface, moveTo should seemingly operate backwards. However, now the whole pipeline is incompatible to the enclosing container's interface: a task requires a generator-type interface as it generates solutions - there is no input interface to read from. Hence, the *reading* end interface (←) of ``moveto`` conflicts with a *writing* end interface of the task.
Obviously, in a ``ParallelContainer`` all (direct) children need to share a common interface.

View File

@ -0,0 +1,86 @@
.. _subsec-tut-cartesian:
Cartesian
---------
The following example demonstrates how to compute a simple point to point motion
plan using the moveit task constructor. You can take a look at the full
source code here:
:download:`Source <../../../demo/scripts/cartesian.py>`
First, lets make sure we specify the planning group and the
end effector that we want to use.
.. literalinclude:: ../../../demo/scripts/cartesian.py
:language: python
:start-after: [cartesianTut1]
:end-before: [cartesianTut1]
The moveit task constructor provides different planners.
We will use the ``CartesianPath`` and ``JointInterpolation``
planners for this example.
.. literalinclude:: ../../../demo/scripts/cartesian.py
:language: python
:start-after: [cartesianTut2]
:end-before: [cartesianTut2]
Lets start by initializing a task and adding the current
planning scene state and robot state to it.
This will be the starting state for our motion plan.
.. literalinclude:: ../../../demo/scripts/cartesian.py
:language: python
:start-after: [cartesianTut3]
:end-before: [cartesianTut3]
To compute a relative motion in cartesian space, we can use
the ``MoveRelative`` stage. Specify the planning group and
frame relative to which you want to carry out the motion.
the relative direction can be specified by a ``Vector3Stamped``
geometry message.
.. literalinclude:: ../../../demo/scripts/cartesian.py
:language: python
:start-after: [initAndConfigMoveRelative]
:end-before: [initAndConfigMoveRelative]
Similarly we can move along a different axis.
.. literalinclude:: ../../../demo/scripts/cartesian.py
:language: python
:start-after: [cartesianTut4]
:end-before: [cartesianTut4]
The ``MoveRelative`` stage also offers an interface to
``Twist`` messages, allowing to specify rotations.
.. literalinclude:: ../../../demo/scripts/cartesian.py
:language: python
:start-after: [cartesianTut5]
:end-before: [cartesianTut5]
Lastly, we can compute linear movements in cartesian space
by providing offsets in joint space.
.. literalinclude:: ../../../demo/scripts/cartesian.py
:language: python
:start-after: [cartesianTut6]
:end-before: [cartesianTut6]
If we want to specify goals instead of directions,
we can use the ``MoveTo`` stage. In the following example
we use simple joint interpolation to move the robot to
a named pose. the named pose is defined in the urdf of
the robot configuration.
.. literalinclude:: ../../../demo/scripts/cartesian.py
:language: python
:start-after: [initAndConfigMoveTo]
:end-before: [initAndConfigMoveTo]
Lastly, we invoke the planning mechanism that traverses
the task hierarchy for us and compute a valid motion plan.
At this point, when you run this script you are able to
inspect the solutions of the individual stages in the rviz
mtc panel.

View File

@ -0,0 +1,22 @@
.. _subsec-tut-firststeps:
First Steps
-----------
The MoveIt Task Constructor package contains several basic examples and
a pick-and-place demo. For all demos you should launch the basic environment:
.. code-block::
roslaunch moveit_task_constructor_demo demo.launch
Subsequently, you can run the individual demos:
.. code-block::
rosrun moveit_task_constructor_demo cartesian
rosrun moveit_task_constructor_demo modular
roslaunch moveit_task_constructor_demo pickplace.launch
To inspect the task hierarchy, be sure that you selected the correct solution topic
in the reviz moveit task constructor plugin.

View File

@ -0,0 +1,15 @@
.. _sec-tutorials:
Tutorials
=========
The following tutorials take you step by step through the implementation of fundamental examples
of the moveit task constructor.
.. toctree::
:caption: Tutorials
first-steps
cartesian
properties
pick-and-place

View File

@ -0,0 +1,138 @@
.. _subsec-tut-pick-place:
Pick and Place
--------------
The following tutorial demonstrates how you can use the moveit
task constructor to plan and carry out pick and place movements.
First, lets specify the planning group and the
end effector that you want to use.
.. literalinclude:: ../../../demo/scripts/pickplace.py
:language: python
:start-after: [pickAndPlaceTut1]
:end-before: [pickAndPlaceTut1]
Next, we add the object that we want to displace to the
planning scene. To this end, make sure that the planning scene
not already contains such object.
.. literalinclude:: ../../../demo/scripts/pickplace.py
:language: python
:start-after: [pickAndPlaceTut2]
:end-before: [pickAndPlaceTut2]
At this point, we are ready to create the task hierarchy.
.. literalinclude:: ../../../demo/scripts/pickplace.py
:language: python
:start-after: [pickAndPlaceTut3]
:end-before: [pickAndPlaceTut3]
The pipeline planner encapsulates the moveit interface
to sampling-based geometric motion planners.
.. tip::
Planning does not proceed linearly from top to bottom.
Rather, it proceeds from the inside out.
Connect stages therefore compute a motion plan between
two previously calculated subordinate solutions.
For a clear visualization, inspect the rviz mtc panel.
Lets connect the current robot state with the solutions of
the following stages.
.. literalinclude:: ../../../demo/scripts/pickplace.py
:language: python
:start-after: [pickAndPlaceTut4]
:end-before: [pickAndPlaceTut4]
To pick the object, we first need to know possible end effector
poses with which we can perform a successful grasp.
For this, we use a ``GenerateGraspPoseStage``.
which essentially spawns poses
with a given ``angle_delta`` in circular fashion around a center
point.
.. literalinclude:: ../../../demo/scripts/pickplace.py
:language: python
:start-after: [pickAndPlaceTut5]
:end-before: [pickAndPlaceTut5]
Next, we need to compute the inverse kinematics of the robot arm
for all previously sampled poses. This way we can
rule out solutions that are not feasible due to the robot geometry.
The ``simpleGrasp`` stage combines ik calculation with motion plan
generation for opening and closing the end effector, as well as attaching
the object to the robot an disabling collision.
.. literalinclude:: ../../../demo/scripts/pickplace.py
:language: python
:start-after: [pickAndPlaceTut6]
:end-before: [pickAndPlaceTut6]
Lastly, we can insert all the previous steps into the ``Pick``
container stage. At this point we might also specify approach and
lift twists for the robot relative to the object we want to grasp.
.. literalinclude:: ../../../demo/scripts/pickplace.py
:language: python
:start-after: [pickAndPlaceTut7]
:end-before: [pickAndPlaceTut7]
Since all the previous stages were chained together via their
constructor arguments, we only need to add the top level ``Pick``
stage to the task hierarchy.
.. literalinclude:: ../../../demo/scripts/pickplace.py
:language: python
:start-after: [pickAndPlaceTut8]
:end-before: [pickAndPlaceTut8]
Thats everything we need for picking an object!
Lets find a motion plan to place the object
.. literalinclude:: ../../../demo/scripts/pickplace.py
:language: python
:start-after: [pickAndPlaceTut9]
:end-before: [pickAndPlaceTut9]
Similar to the picking procedure, we define the place task.
First, start with sampling place poses.
.. literalinclude:: ../../../demo/scripts/pickplace.py
:language: python
:start-after: [pickAndPlaceTut10]
:end-before: [pickAndPlaceTut10]
Next, wrap the inverse kinematics computation and gripper
movements.
.. literalinclude:: ../../../demo/scripts/pickplace.py
:language: python
:start-after: [pickAndPlaceTut11]
:end-before: [pickAndPlaceTut11]
Lastly, add place and retract motions and add the ``Place``
stage to the task hierarchy.
.. literalinclude:: ../../../demo/scripts/pickplace.py
:language: python
:start-after: [pickAndPlaceTut12]
:end-before: [pickAndPlaceTut12]
Finally, compute solutions for the task hierarchy and delete
the planner instances.
.. literalinclude:: ../../../demo/scripts/pickplace.py
:language: python
:start-after: [pickAndPlaceTut13]
:end-before: [pickAndPlaceTut13]
At this point, you might inspect the task hierarchy in the mtc rviz
plugin.
.. tip::
Use the mtc rviz plugin to graphically inspect the solutions
of individual stages in the task hierarchy.

View File

@ -0,0 +1,122 @@
.. _subsec-tut-properties:
Properties
----------
Properties are named attributes of a stage.
They can be used to configure the stages behaviour
and control further substages. Lets take a closer
look at how to work with properties.
Basic Operations with Properties
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
Lets define a property and assign a description, as well as
a value to it.
.. literalinclude:: ../../../demo/scripts/properties.py
:language: python
:start-after: [propertyTut1]
:end-before: [propertyTut1]
Notice that a property always has two values: the current value
and the default value. Before we use the property, we might want to
check if the current value defined.
.. literalinclude:: ../../../demo/scripts/properties.py
:language: python
:start-after: [propertyTut2]
:end-before: [propertyTut2]
Now we are ready to safely retrieve the values of the proprty!
.. literalinclude:: ../../../demo/scripts/properties.py
:language: python
:start-after: [propertyTut3]
:end-before: [propertyTut3]
The Property Map
^^^^^^^^^^^^^^^^
Usually a stage comprises multiple properties. A stage
contains a single PropertyMap that acts as a container for
all properties associated to that stage.
Lets first create a PropertyMap in isolation and initialize
some properties using a dict. As you can see, properties can be
of arbitrary type.
.. literalinclude:: ../../../demo/scripts/properties.py
:language: python
:start-after: [propertyTut4]
:end-before: [propertyTut4]
Properties can also be initialized using a more pythonic way.
.. literalinclude:: ../../../demo/scripts/properties.py
:language: python
:start-after: [propertyTut5]
:end-before: [propertyTut5]
There are two ways to retrieve properties back from the property map.
We might only be interested in in the value of the property:
.. literalinclude:: ../../../demo/scripts/properties.py
:language: python
:start-after: [propertyTut6]
:end-before: [propertyTut6]
Or we can obtain a reference to the whole property object.
.. literalinclude:: ../../../demo/scripts/properties.py
:language: python
:start-after: [propertyTut7]
:end-before: [propertyTut7]
The PropertyMap class additionally provides an iterator that can be used in loops.
.. literalinclude:: ../../../demo/scripts/properties.py
:language: python
:start-after: [propertyTut8]
:end-before: [propertyTut8]
Remember that wer initialized our PropertyMap by using a dict. In fact, you
can also use an existing PropertMap to copy over some properties.
.. literalinclude:: ../../../demo/scripts/properties.py
:language: python
:start-after: [propertyTut9]
:end-before: [propertyTut9]
Accessing Properties of a Stage
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
You can obtain a reference to the the PropertyMap of a stage like so
.. literalinclude:: ../../../demo/scripts/properties.py
:language: python
:start-after: [propertyTut10]
:end-before: [propertyTut10]
As mentioned, each stage contains a PropertyMap.
Stages communicate to each other via their interfaces.
If you want to forward properties through these interfaces,
you can use the reference of a stages' property object.
.. literalinclude:: ../../../demo/scripts/compute_ik.py
:language: python
:start-after: [propertyTut12]
:end-before: [propertyTut12]
.. literalinclude:: ../../../demo/scripts/compute_ik.py
:language: python
:start-after: [propertyTut13]
:end-before: [propertyTut13]
.. literalinclude:: ../../../demo/scripts/compute_ik.py
:language: python
:start-after: [propertyTut14]
:end-before: [propertyTut14]
Take a look at the :ref:`How-To-Guides <subsubsec-howto-compute-ik>`
for a full example of this.

View File

@ -0,0 +1,77 @@
#pragma once
#include <py_binding_tools/ros_msg_typecasters.h>
#include <moveit/task_constructor/properties.h>
#include <boost/any.hpp>
#include <typeindex>
namespace moveit {
namespace python {
class PYBIND11_EXPORT PropertyConverterBase
{
public:
using to_python_converter_function = pybind11::object (*)(const boost::any&);
using from_python_converter_function = boost::any (*)(const pybind11::object&);
protected:
static bool insert(const std::type_index& type_index, const std::string& ros_msg_name,
to_python_converter_function to, from_python_converter_function from);
};
/// utility class to register C++ / Python converters for a property of type T
template <typename T>
class PropertyConverter : protected PropertyConverterBase
{
public:
PropertyConverter() { insert(typeid(T), rosMsgName<T>(), &toPython, &fromPython); }
private:
static pybind11::object toPython(const boost::any& value) { return pybind11::cast(boost::any_cast<T>(value)); }
static boost::any fromPython(const pybind11::object& po) { return pybind11::cast<T>(po); }
template <class Q = T>
typename std::enable_if<ros::message_traits::IsMessage<Q>::value, std::string>::type rosMsgName() {
return ros::message_traits::DataType<T>::value();
}
template <class Q = T>
typename std::enable_if<!ros::message_traits::IsMessage<Q>::value, std::string>::type rosMsgName() {
return std::string();
}
};
namespace properties {
/** Extension for pybind11::class_ to allow convienient definition of properties
*
* New method property<PropertyType>(const char* name) adds a property getter/setter.
*/
template <typename type_, typename... options>
class class_ : public pybind11::classh<type_, options...> // NOLINT(readability-identifier-naming)
{
using base_class_ = pybind11::classh<type_, options...>;
public:
// forward all constructors
using base_class_::base_class_;
template <typename PropertyType, typename... Extra>
class_& property(const char* name, const Extra&... extra) {
PropertyConverter<PropertyType>(); // register corresponding property converter
auto getter = [name](type_& self) -> PropertyType& {
moveit::task_constructor::PropertyMap& props = self.properties();
return const_cast<PropertyType&>(props.get<PropertyType>(name));
};
auto setter = [name](type_& self, const PropertyType& value) {
moveit::task_constructor::PropertyMap& props = self.properties();
props.set(name, boost::any(value));
};
base_class_::def_property(name, getter, setter, pybind11::return_value_policy::reference_internal, extra...);
return *this;
}
};
} // namespace properties
} // namespace python
} // namespace moveit

View File

@ -51,8 +51,13 @@ public:
PRIVATE_CLASS(ContainerBase)
using pointer = std::unique_ptr<ContainerBase>;
/// Explicitly enable/disable pruning
void setPruning(bool pruning) { setProperty("pruning", pruning); }
bool pruning() const { return properties().get<bool>("pruning"); }
size_t numChildren() const;
Stage* findChild(const std::string& name) const;
Stage* operator[](int index) const;
/** Callback function type used by traverse functions
* Receives currently visited Stage and current depth in hierarchy
@ -75,6 +80,7 @@ public:
virtual bool canCompute() const = 0;
virtual void compute() = 0;
bool explainFailure(std::ostream& os) const override;
/// called by a (direct) child when a new solution becomes available
virtual void onNewSolution(const SolutionBase& s) = 0;
@ -167,12 +173,15 @@ public:
void reset() override;
void init(const moveit::core::RobotModelConstPtr& robot_model) override;
bool canCompute() const override;
void compute() override;
protected:
Fallbacks(FallbacksPrivate* impl);
void onNewSolution(const SolutionBase& s) override;
private:
// not needed, we directly use corresponding virtual methods of FallbacksPrivate
bool canCompute() const final { return false; }
void compute() final {}
};
class MergerPrivate;

View File

@ -76,6 +76,7 @@ namespace task_constructor {
class ContainerBasePrivate : public StagePrivate
{
friend class ContainerBase;
friend class ConnectingPrivate; // needed propagate setStatus() in newState()
public:
using container_type = StagePrivate::container_type;
@ -148,18 +149,19 @@ protected:
child->setNextStarts(allowed ? pending_forward_ : InterfacePtr());
}
/// Set ENABLED / PRUNED status of the solution tree starting from s into given direction
/// Set ENABLED/PRUNED status of a solution branch starting from target into the given direction
template <Interface::Direction dir>
void setStatus(const InterfaceState* s, InterfaceState::Status status);
/// non-template version
void setStatus(Interface::Direction dir, const InterfaceState* s, InterfaceState::Status status);
void setStatus(const Stage* creator, const InterfaceState* source, const InterfaceState* target,
InterfaceState::Status status);
/// copy external_state to a child's interface and remember the link in internal_external map
/// Copy external_state to a child's interface and remember the link in internal_external map
template <Interface::Direction>
void copyState(Interface::iterator external, const InterfacePtr& target, bool updated);
/// non-template version
void copyState(Interface::Direction dir, Interface::iterator external, const InterfacePtr& target, bool updated);
/// lift solution from internal to external level
void copyState(Interface::iterator external, const InterfacePtr& target, Interface::UpdateFlags updated);
// non-template version
void copyState(Interface::Direction dir, Interface::iterator external, const InterfacePtr& target,
Interface::UpdateFlags updated);
/// Lift solution from internal to external level
void liftSolution(const SolutionBasePtr& solution, const InterfaceState* internal_from,
const InterfaceState* internal_to);
@ -231,60 +233,90 @@ public:
protected:
void validateInterfaces(const StagePrivate& child, InterfaceFlags& external, bool first = false) const;
/// callback for new externally received states
template <typename Interface::Direction>
void propagateStateToChildren(Interface::iterator external, bool updated);
private:
// override for custom behavior on received interface states
/// notify callback for new externally received interface states
template <typename Interface::Direction>
void propagateStateToAllChildren(Interface::iterator external, Interface::UpdateFlags updated);
// override to customize behavior on received interface states (default: propagateStateToAllChildren())
virtual void initializeExternalInterfaces();
};
PIMPL_FUNCTIONS(ParallelContainerBase)
/* The Fallbacks container needs to implement different behaviour based on its interface.
* Thus, we implement 3 different classes: for Generator, Propagator, and Connect-like interfaces.
* FallbacksPrivate is the common base class for all of them, defining the common API to be used
* by the Fallbacks container.
* FallbacksPrivate is the common base class for all of them, defining the common API
* to be used by the Fallbacks container.
* The actual interface-specific class is instantiated in initializeExternalInterfaces()
* resp. Fallbacks::replaceImpl() when the actual interface is known.
* The key difference between the 3 variants is how the advance to the next job. */
* The key difference between the 3 variants is how they advance to the next job. */
class FallbacksPrivate : public ParallelContainerBasePrivate
{
public:
FallbacksPrivate(Fallbacks* me, const std::string& name);
FallbacksPrivate(FallbacksPrivate&& other);
// method overrides common to 3 variants
void initializeExternalInterfaces() final;
void onNewFailure(const Stage& child, const InterfaceState* from, const InterfaceState* to) override;
// virtual method specific to each variant
/// Advance to the next job, assuming that the current child is exhausted on the current job.
virtual bool nextJob() { return false; }
/// Reset data structures
virtual void reset();
container_type::const_iterator current_; // currently active child generator
bool job_has_solutions_; // flag indicating whether the current job generated solutions
// virtual methods specific to each variant
virtual void onNewSolution(const SolutionBase& s);
virtual void reset() {}
};
PIMPL_FUNCTIONS(Fallbacks)
/* Class shared between FallbacksPrivateGenerator and FallbacksPrivatePropagator,
which both have the notion of a currently active child stage */
class FallbacksPrivateCommon : public FallbacksPrivate
{
public:
FallbacksPrivateCommon(FallbacksPrivate&& other) : FallbacksPrivate(std::move(other)) {}
/// Advance to next child
inline void nextChild();
/// Advance to the next job, assuming that the current child is exhausted on the current job.
virtual bool nextJob() = 0;
void reset() override;
bool canCompute() const override;
void compute() override;
container_type::const_iterator current_; // currently active child
};
/// Fallbacks implementation for GENERATOR interface
struct FallbacksPrivateGenerator : FallbacksPrivate
struct FallbacksPrivateGenerator : FallbacksPrivateCommon
{
FallbacksPrivateGenerator(FallbacksPrivate&& old);
bool nextJob() override;
};
/// Fallbacks implementation for FORWARD or BACKWARD interface
struct FallbacksPrivatePropagator : FallbacksPrivate
struct FallbacksPrivatePropagator : FallbacksPrivateCommon
{
FallbacksPrivatePropagator(FallbacksPrivate&& old);
bool nextJob() override;
void reset() override;
void onNewSolution(const SolutionBase& s) override;
bool nextJob() override;
Interface::Direction dir_; // propagation direction
Interface::iterator job_; // pointer to currently processed external state
bool job_has_solutions_; // flag indicating whether the current job generated solutions
};
/// Fallbacks implementation for CONNECT interface
struct FallbacksPrivateConnect : FallbacksPrivate
{
FallbacksPrivateConnect(FallbacksPrivate&& old);
void reset() override;
bool canCompute() const override;
void compute() override;
void onNewFailure(const Stage& child, const InterfaceState* from, const InterfaceState* to) override;
template <Interface::Direction dir>
void propagateStateUpdate(Interface::iterator external, Interface::UpdateFlags updated);
mutable container_type::const_iterator active_; // child picked for compute()
};
class WrapperBasePrivate : public ParallelContainerBasePrivate

View File

@ -40,6 +40,7 @@
#include <moveit/task_constructor/storage.h>
#include <moveit/task_constructor/utils.h>
#include <moveit_msgs/RobotState.h>
namespace moveit {
namespace task_constructor {
@ -69,6 +70,14 @@ public:
class TrajectoryCostTerm : public CostTerm
{
public:
enum class Mode : uint8_t
{
AUTO /* TRAJECTORY, or START_INTERFACE if no trajectory is given */,
START_INTERFACE,
END_INTERFACE,
TRAJECTORY
};
double operator()(const SolutionSequence& s, std::string& comment) const override;
double operator()(const WrappedSolution& s, std::string& comment) const override;
};
@ -86,6 +95,7 @@ public:
LambdaCostTerm(const SubTrajectorySignature& term);
LambdaCostTerm(const SubTrajectoryShortSignature& term);
using TrajectoryCostTerm::operator();
double operator()(const SubTrajectory& s, std::string& comment) const override;
protected:
@ -113,21 +123,45 @@ public:
double cost;
};
/// trajectory length (interpolated between waypoints)
/// trajectory length with optional weighting for different joints
class PathLength : public TrajectoryCostTerm
{
public:
/// By default, all joints are considered with same weight of 1.0
PathLength() = default;
PathLength(std::vector<std::string> j) : joints{ std::move(j) } {};
/// Limit measurements to given joint names
PathLength(std::vector<std::string> joints);
/// Limit measurements to given joints and use given weighting
PathLength(std::map<std::string, double> j) : joints(std::move(j)) {}
using TrajectoryCostTerm::operator();
double operator()(const SubTrajectory& s, std::string& comment) const override;
std::vector<std::string> joints;
std::map<std::string, double> joints; //< joint weights
};
/// (weighted) joint-space distance to reference pose
class DistanceToReference : public TrajectoryCostTerm
{
public:
DistanceToReference(const moveit_msgs::RobotState& ref, Mode m = Mode::AUTO,
std::map<std::string, double> w = std::map<std::string, double>());
DistanceToReference(const std::map<std::string, double>& ref, Mode m = Mode::AUTO,
std::map<std::string, double> w = std::map<std::string, double>());
using TrajectoryCostTerm::operator();
double operator()(const SubTrajectory& s, std::string& comment) const override;
moveit_msgs::RobotState reference;
std::map<std::string, double> weights;
Mode mode;
};
/// execution duration of the whole trajectory
class TrajectoryDuration : public TrajectoryCostTerm
{
public:
using TrajectoryCostTerm::operator();
double operator()(const SubTrajectory& s, std::string& comment) const override;
};
@ -139,6 +173,7 @@ public:
std::string link_name;
using TrajectoryCostTerm::operator();
double operator()(const SubTrajectory& s, std::string& comment) const override;
};
@ -153,14 +188,6 @@ public:
class Clearance : public TrajectoryCostTerm
{
public:
enum class Mode
{
AUTO /* TRAJECTORY, or START_INTERFACE if no trajectory is given */,
START_INTERFACE,
END_INTERFACE,
TRAJECTORY
};
Clearance(bool with_world = true, bool cumulative = false, std::string group_property = "group",
Mode mode = Mode::AUTO);
bool with_world;
@ -171,6 +198,7 @@ public:
std::function<double(double)> distance_to_cost;
using TrajectoryCostTerm::operator();
double operator()(const SubTrajectory& s, std::string& comment) const override;
};

View File

@ -0,0 +1,49 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2024, University of Hamburg
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
/* Authors: Michael Goerner
Desc: Thin wrapper around fmt includes to provide Eigen formatters for fmt9
*/
#pragma once
#include <Eigen/Core>
#include <fmt/core.h>
#include <fmt/ostream.h>
#if FMT_VERSION >= 90000
template <typename T>
struct fmt::formatter<T, std::enable_if_t<std::is_base_of_v<Eigen::DenseBase<T>, T>, char>> : ostream_formatter
{};
#endif

View File

@ -39,6 +39,7 @@
#include <moveit/task_constructor/storage.h>
#include <moveit/robot_model/robot_model.h>
#include <moveit/robot_trajectory/robot_trajectory.h>
#include <moveit/trajectory_processing/time_parameterization.h>
namespace moveit {
namespace task_constructor {
@ -57,6 +58,7 @@ moveit::core::JointModelGroup* merge(const std::vector<const moveit::core::Joint
*/
robot_trajectory::RobotTrajectoryPtr
merge(const std::vector<robot_trajectory::RobotTrajectoryConstPtr>& sub_trajectories,
const moveit::core::RobotState& base_state, moveit::core::JointModelGroup*& merged_group);
const moveit::core::RobotState& base_state, moveit::core::JointModelGroup*& merged_group,
const trajectory_processing::TimeParameterization& time_parameterization);
} // namespace task_constructor
} // namespace moveit

View File

@ -39,21 +39,8 @@
#pragma once
#include <moveit/version.h>
#include <moveit/macros/class_forward.h>
#define MOVEIT_VERSION_GE(major, minor, patch) \
(MOVEIT_VERSION_MAJOR * 1'000'000 + MOVEIT_VERSION_MINOR * 1'000 + MOVEIT_VERSION_PATCH >= \
major * 1'000'000 + minor * 1'000 + patch)
// use collision env instead of collision robot/world
#define MOVEIT_HAS_COLLISION_ENV MOVEIT_VERSION_GE(1, 1, 0)
// cartesian interpolator got separated from RobotState at some point
#define MOVEIT_HAS_CARTESIAN_INTERPOLATOR MOVEIT_VERSION_GE(1, 1, 0)
// isEmpty got split off into its own header
#define MOVEIT_HAS_MESSAGE_CHECKS MOVEIT_VERSION_GE(1, 1, 0)
// use object shape poses relative to a single object pose
#define MOVEIT_HAS_OBJECT_POSE MOVEIT_VERSION_GE(1, 1, 6)
#define MOVEIT_HAS_STATE_RIGID_PARENT_LINK MOVEIT_VERSION_GE(1, 1, 6)

View File

@ -39,6 +39,7 @@
#pragma once
#include <moveit/task_constructor/solvers/planner_interface.h>
#include <moveit/robot_state/cartesian_interpolator.h>
namespace moveit {
namespace task_constructor {
@ -52,23 +53,33 @@ class CartesianPath : public PlannerInterface
public:
CartesianPath();
void setIKFrame(const geometry_msgs::PoseStamped& pose) { setProperty("ik_frame", pose); }
void setIKFrame(const Eigen::Isometry3d& pose, const std::string& link);
void setIKFrame(const std::string& link) { setIKFrame(Eigen::Isometry3d::Identity(), link); }
void setStepSize(double step_size) { setProperty("step_size", step_size); }
void setJumpThreshold(double jump_threshold) { setProperty("jump_threshold", jump_threshold); }
void setPrecision(const moveit::core::CartesianPrecision& precision) { setProperty("precision", precision); }
template <typename T = float>
void setJumpThreshold(double /*unused*/) {
static_assert(std::is_integral<T>::value, "setJumpThreshold() is deprecated. Replace with setPrecision.");
}
void setMinFraction(double min_fraction) { setProperty("min_fraction", min_fraction); }
void setMaxVelocityScaling(double factor) { setProperty("max_velocity_scaling_factor", factor); }
void setMaxAccelerationScaling(double factor) { setProperty("max_acceleration_scaling_factor", factor); }
[[deprecated("Replace with setMaxVelocityScalingFactor")]] // clang-format off
void setMaxVelocityScaling(double factor) { setMaxVelocityScalingFactor(factor); } // clang-format on
[[deprecated("Replace with setMaxAccelerationScalingFactor")]] // clang-format off
void setMaxAccelerationScaling(double factor) { setMaxAccelerationScalingFactor(factor); } // clang-format on
void init(const moveit::core::RobotModelConstPtr& robot_model) override;
bool plan(const planning_scene::PlanningSceneConstPtr& from, const planning_scene::PlanningSceneConstPtr& to,
const moveit::core::JointModelGroup* jmg, double timeout, robot_trajectory::RobotTrajectoryPtr& result,
const moveit_msgs::Constraints& path_constraints = moveit_msgs::Constraints()) override;
Result plan(const planning_scene::PlanningSceneConstPtr& from, const planning_scene::PlanningSceneConstPtr& to,
const moveit::core::JointModelGroup* jmg, double timeout, robot_trajectory::RobotTrajectoryPtr& result,
const moveit_msgs::Constraints& path_constraints = moveit_msgs::Constraints()) override;
bool plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link,
const Eigen::Isometry3d& target, const moveit::core::JointModelGroup* jmg, double timeout,
robot_trajectory::RobotTrajectoryPtr& result,
const moveit_msgs::Constraints& path_constraints = moveit_msgs::Constraints()) override;
Result plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link,
const Eigen::Isometry3d& offset, const Eigen::Isometry3d& target,
const moveit::core::JointModelGroup* jmg, double timeout, robot_trajectory::RobotTrajectoryPtr& result,
const moveit_msgs::Constraints& path_constraints = moveit_msgs::Constraints()) override;
};
} // namespace solvers
} // namespace task_constructor

View File

@ -58,14 +58,14 @@ public:
void init(const moveit::core::RobotModelConstPtr& robot_model) override;
bool plan(const planning_scene::PlanningSceneConstPtr& from, const planning_scene::PlanningSceneConstPtr& to,
const core::JointModelGroup* jmg, double timeout, robot_trajectory::RobotTrajectoryPtr& result,
const moveit_msgs::Constraints& path_constraints = moveit_msgs::Constraints()) override;
Result plan(const planning_scene::PlanningSceneConstPtr& from, const planning_scene::PlanningSceneConstPtr& to,
const core::JointModelGroup* jmg, double timeout, robot_trajectory::RobotTrajectoryPtr& result,
const moveit_msgs::Constraints& path_constraints = moveit_msgs::Constraints()) override;
bool plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link,
const Eigen::Isometry3d& target, const core::JointModelGroup* jmg, double timeout,
robot_trajectory::RobotTrajectoryPtr& result,
const moveit_msgs::Constraints& path_constraints = moveit_msgs::Constraints()) override;
Result plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link,
const Eigen::Isometry3d& offset, const Eigen::Isometry3d& target,
const moveit::core::JointModelGroup* jmg, double timeout, robot_trajectory::RobotTrajectoryPtr& result,
const moveit_msgs::Constraints& path_constraints = moveit_msgs::Constraints()) override;
};
} // namespace solvers
} // namespace task_constructor

View File

@ -0,0 +1,77 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2023, Bielefeld University
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Bielefeld University nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
/* Authors: Robert Haschke
Desc: meta planner, running multiple planners in parallel or sequence
*/
#pragma once
#include <moveit/task_constructor/solvers/planner_interface.h>
#include <vector>
namespace moveit {
namespace task_constructor {
namespace solvers {
MOVEIT_CLASS_FORWARD(MultiPlanner);
/** A meta planner that runs multiple alternative planners in sequence and returns the first found solution.
*
* This is useful to sequence different planning strategies of increasing complexity,
* e.g. Cartesian or joint-space interpolation first, then OMPL, ...
* This is (slightly) different from the Fallbacks container, as the MultiPlanner directly applies its planners to each
* individual planning job. In contrast, the Fallbacks container first runs the active child to exhaustion before
* switching to the next child, which possibly applies a different planning strategy.
*/
class MultiPlanner : public PlannerInterface, public std::vector<solvers::PlannerInterfacePtr>
{
public:
using PlannerList = std::vector<solvers::PlannerInterfacePtr>;
using PlannerList::PlannerList; // inherit all std::vector constructors
void init(const moveit::core::RobotModelConstPtr& robot_model) override;
Result plan(const planning_scene::PlanningSceneConstPtr& from, const planning_scene::PlanningSceneConstPtr& to,
const moveit::core::JointModelGroup* jmg, double timeout, robot_trajectory::RobotTrajectoryPtr& result,
const moveit_msgs::Constraints& path_constraints = moveit_msgs::Constraints()) override;
Result plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link,
const Eigen::Isometry3d& offset, const Eigen::Isometry3d& target,
const moveit::core::JointModelGroup* jmg, double timeout, robot_trajectory::RobotTrajectoryPtr& result,
const moveit_msgs::Constraints& path_constraints = moveit_msgs::Constraints()) override;
};
} // namespace solvers
} // namespace task_constructor
} // namespace moveit

View File

@ -39,6 +39,7 @@
#pragma once
#include <moveit/task_constructor/solvers/planner_interface.h>
#include <moveit_msgs/MotionPlanRequest.h>
#include <moveit/macros/class_forward.h>
namespace planning_pipeline {
@ -79,16 +80,19 @@ public:
void init(const moveit::core::RobotModelConstPtr& robot_model) override;
bool plan(const planning_scene::PlanningSceneConstPtr& from, const planning_scene::PlanningSceneConstPtr& to,
const core::JointModelGroup* jmg, double timeout, robot_trajectory::RobotTrajectoryPtr& result,
const moveit_msgs::Constraints& path_constraints = moveit_msgs::Constraints()) override;
Result plan(const planning_scene::PlanningSceneConstPtr& from, const planning_scene::PlanningSceneConstPtr& to,
const core::JointModelGroup* jmg, double timeout, robot_trajectory::RobotTrajectoryPtr& result,
const moveit_msgs::Constraints& path_constraints = moveit_msgs::Constraints()) override;
bool plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link,
const Eigen::Isometry3d& target, const core::JointModelGroup* jmg, double timeout,
robot_trajectory::RobotTrajectoryPtr& result,
const moveit_msgs::Constraints& path_constraints = moveit_msgs::Constraints()) override;
Result plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link,
const Eigen::Isometry3d& offset, const Eigen::Isometry3d& target,
const moveit::core::JointModelGroup* jmg, double timeout, robot_trajectory::RobotTrajectoryPtr& result,
const moveit_msgs::Constraints& path_constraints = moveit_msgs::Constraints()) override;
protected:
Result plan(const planning_scene::PlanningSceneConstPtr& from, const moveit_msgs::MotionPlanRequest& req,
robot_trajectory::RobotTrajectoryPtr& result);
std::string pipeline_name_;
planning_pipeline::PlanningPipelinePtr planner_;
};

View File

@ -49,6 +49,9 @@ MOVEIT_CLASS_FORWARD(PlanningScene);
namespace robot_trajectory {
MOVEIT_CLASS_FORWARD(RobotTrajectory);
}
namespace trajectory_processing {
MOVEIT_CLASS_FORWARD(TimeParameterization);
}
namespace moveit {
namespace core {
MOVEIT_CLASS_FORWARD(LinkModel);
@ -68,6 +71,14 @@ class PlannerInterface
PropertyMap properties_;
public:
struct Result
{
bool success;
std::string message;
operator bool() const { return success; }
};
PlannerInterface();
virtual ~PlannerInterface() {}
@ -75,20 +86,31 @@ public:
const PropertyMap& properties() const { return properties_; }
void setProperty(const std::string& name, const boost::any& value) { properties_.set(name, value); }
void setTimeout(double timeout) { properties_.set("timeout", timeout); }
void setMaxVelocityScalingFactor(double factor) { properties_.set("max_velocity_scaling_factor", factor); }
void setMaxAccelerationScalingFactor(double factor) { properties_.set("max_acceleration_scaling_factor", factor); }
void setMaxCartesianSpeed(double max) { setProperty("max_cartesian_speed", max); }
void setCartesianSpeedLimitedLink(const std::string& link) { setProperty("cartesian_speed_limited_link", link); }
void setTimeParameterization(const trajectory_processing::TimeParameterizationPtr& tp) {
properties_.set("time_parameterization", tp);
}
virtual void init(const moveit::core::RobotModelConstPtr& robot_model) = 0;
/// plan trajectory between to robot states
virtual bool plan(const planning_scene::PlanningSceneConstPtr& from, const planning_scene::PlanningSceneConstPtr& to,
const moveit::core::JointModelGroup* jmg, double timeout,
robot_trajectory::RobotTrajectoryPtr& result,
const moveit_msgs::Constraints& path_constraints = moveit_msgs::Constraints()) = 0;
virtual Result plan(const planning_scene::PlanningSceneConstPtr& from,
const planning_scene::PlanningSceneConstPtr& to, const moveit::core::JointModelGroup* jmg,
double timeout, robot_trajectory::RobotTrajectoryPtr& result,
const moveit_msgs::Constraints& path_constraints = moveit_msgs::Constraints()) = 0;
/// plan trajectory from current robot state to Cartesian target
virtual bool plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link,
const Eigen::Isometry3d& target, const moveit::core::JointModelGroup* jmg, double timeout,
robot_trajectory::RobotTrajectoryPtr& result,
const moveit_msgs::Constraints& path_constraints = moveit_msgs::Constraints()) = 0;
/// plan trajectory from current robot state to Cartesian target, such that pose(link)*offset == target
virtual Result plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link,
const Eigen::Isometry3d& offset, const Eigen::Isometry3d& target,
const moveit::core::JointModelGroup* jmg, double timeout,
robot_trajectory::RobotTrajectoryPtr& result,
const moveit_msgs::Constraints& path_constraints = moveit_msgs::Constraints()) = 0;
};
} // namespace solvers
} // namespace task_constructor

View File

@ -39,6 +39,7 @@
#pragma once
#include "trajectory_execution_info.h"
#include "utils.h"
#include <moveit/macros/class_forward.h>
#include <moveit/task_constructor/storage.h>
@ -71,7 +72,7 @@ MOVEIT_CLASS_FORWARD(RobotTrajectory);
namespace moveit {
namespace task_constructor {
enum InterfaceFlag
enum InterfaceFlag : uint8_t
{
READS_START = 0x01,
READS_END = 0x02,
@ -154,7 +155,7 @@ public:
*
* INTERFACE takes precedence over PARENT.
*/
enum PropertyInitializerSource
enum PropertyInitializerSource : uint8_t
{ // TODO: move to property.cpp
DEFAULT = 0,
MANUAL = 1,
@ -184,6 +185,7 @@ public:
void setName(const std::string& name);
uint32_t introspectionId() const;
Introspection* introspection() const;
/** set computation timeout (in seconds)
*
@ -201,6 +203,14 @@ public:
/// marker namespace of solution markers
const std::string& markerNS() { return properties().get<std::string>("marker_ns"); }
/// Set and get info to use when executing the stage's trajectory
void setTrajectoryExecutionInfo(TrajectoryExecutionInfo trajectory_execution_info) {
setProperty("trajectory_execution_info", trajectory_execution_info);
}
TrajectoryExecutionInfo trajectoryExecutionInfo() const {
return properties().get<TrajectoryExecutionInfo>("trajectory_execution_info");
}
/// forwarding of properties between interface states
void forwardProperties(const InterfaceState& source, InterfaceState& dest);
std::set<std::string> forwardedProperties() const {
@ -232,6 +242,8 @@ public:
/// Should we generate failure solutions? Note: Always report a failure!
bool storeFailures() const;
virtual bool explainFailure(std::ostream& /*os*/) const { return false; };
/// Get the stage's property map
PropertyMap& properties();
const PropertyMap& properties() const { return const_cast<Stage*>(this)->properties(); }
@ -271,7 +283,7 @@ class PropagatingEitherWayPrivate;
/** Base class for stages that can propagate InterfaceStates
*
* They read an InterfaceState on one side (start or end) and
* push a new InterfaceState to the opposite site.
* push a new InterfaceState to the opposite side.
* By default, the class auto-derives its actual propagation direction from the context.
* In order to enforce forward, backward, or bothway propagation, one can use restrictDirection().
*/
@ -281,7 +293,7 @@ public:
PRIVATE_CLASS(PropagatingEitherWay)
PropagatingEitherWay(const std::string& name = "propagating either way");
enum Direction
enum Direction : uint8_t
{
AUTO = 0x00, // auto-derive direction from context
FORWARD = 0x01, // propagate forward only
@ -291,8 +303,8 @@ public:
// Default implementations, using generic compute().
// Override if you want to use different code for FORWARD and BACKWARD directions.
virtual void computeForward(const InterfaceState& from) { computeGeneric<Interface::FORWARD>(from); }
virtual void computeBackward(const InterfaceState& to) { computeGeneric<Interface::BACKWARD>(to); }
virtual void computeForward(const InterfaceState& from);
virtual void computeBackward(const InterfaceState& to);
protected:
// constructor for use in derived classes
@ -353,6 +365,7 @@ public:
virtual bool canCompute() const = 0;
virtual void compute() = 0;
void spawn(InterfaceState&& from, InterfaceState&& to, SubTrajectory&& trajectory);
void spawn(InterfaceState&& state, SubTrajectory&& trajectory);
void spawn(InterfaceState&& state, double cost) {
SubTrajectory trajectory;

View File

@ -44,18 +44,30 @@
#include <moveit/task_constructor/cost_queue.h>
#include <ros/console.h>
#include <fmt/core.h>
#include <ostream>
#include <chrono>
// define pimpl() functions accessing correctly casted pimpl_ pointer
#define PIMPL_FUNCTIONS(Class) \
const Class##Private* Class::pimpl() const { return static_cast<const Class##Private*>(pimpl_); } \
Class##Private* Class::pimpl() { return static_cast<Class##Private*>(pimpl_); }
#define PIMPL_FUNCTIONS(Class) \
const Class##Private* Class::pimpl() const { \
return static_cast<const Class##Private*>(pimpl_); \
} \
Class##Private* Class::pimpl() { \
return static_cast<Class##Private*>(pimpl_); \
}
namespace moveit {
namespace task_constructor {
/// exception thrown by StagePrivate::runCompute()
class PreemptStageException : public std::exception
{
public:
explicit PreemptStageException() {}
};
class ContainerBase;
class StagePrivate
{
@ -99,17 +111,11 @@ public:
inline InterfaceConstPtr prevEnds() const { return prev_ends_.lock(); }
inline InterfaceConstPtr nextStarts() const { return next_starts_.lock(); }
/// direction-based access to pull/push interfaces
inline InterfacePtr& pullInterface(Interface::Direction dir) { return dir == Interface::FORWARD ? starts_ : ends_; }
inline InterfacePtr pushInterface(Interface::Direction dir) {
return dir == Interface::FORWARD ? next_starts_.lock() : prev_ends_.lock();
}
inline InterfaceConstPtr pullInterface(Interface::Direction dir) const {
return dir == Interface::FORWARD ? starts_ : ends_;
}
inline InterfaceConstPtr pushInterface(Interface::Direction dir) const {
return dir == Interface::FORWARD ? next_starts_.lock() : prev_ends_.lock();
}
/// direction-based access to pull interface
template <Interface::Direction dir>
inline InterfacePtr pullInterface();
// non-template version
inline InterfacePtr pullInterface(Interface::Direction dir) { return dir == Interface::FORWARD ? starts_ : ends_; }
/// set parent of stage
/// enforce only one parent exists
@ -142,6 +148,7 @@ public:
void sendBackward(InterfaceState&& from, const InterfaceState& to, const SolutionBasePtr& solution);
template <Interface::Direction>
inline void send(const InterfaceState& start, InterfaceState&& end, const SolutionBasePtr& solution);
void spawn(InterfaceState&& from, InterfaceState&& to, const SolutionBasePtr& solution);
void spawn(InterfaceState&& state, const SolutionBasePtr& solution);
void connect(const InterfaceState& from, const InterfaceState& to, const SolutionBasePtr& solution);
@ -149,7 +156,11 @@ public:
void newSolution(const SolutionBasePtr& solution);
bool storeFailures() const { return introspection_ != nullptr; }
void runCompute() {
ROS_DEBUG_STREAM_NAMED("Stage", "Computing stage '" << name() << "'");
ROS_DEBUG_STREAM_NAMED("Stage", fmt::format("Computing stage '{}'", name()));
if (preempted())
throw PreemptStageException();
auto compute_start_time = std::chrono::steady_clock::now();
try {
compute();
@ -163,6 +174,11 @@ public:
/** compute cost for solution through configured CostTerm */
void computeCost(const InterfaceState& from, const InterfaceState& to, SolutionBase& solution);
void setPreemptRequestedMember(const std::atomic<bool>* preempt_requested) {
preempt_requested_ = preempt_requested;
}
bool preempted() const { return preempt_requested_ != nullptr && *preempt_requested_; }
protected:
StagePrivate& operator=(StagePrivate&& other);
@ -201,10 +217,21 @@ private:
InterfaceWeakPtr next_starts_; // interface to be used for sendForward()
Introspection* introspection_; // task's introspection instance
const std::atomic<bool>* preempt_requested_;
};
PIMPL_FUNCTIONS(Stage)
std::ostream& operator<<(std::ostream& os, const StagePrivate& stage);
template <>
inline InterfacePtr StagePrivate::pullInterface<Interface::FORWARD>() {
return starts_;
}
template <>
inline InterfacePtr StagePrivate::pullInterface<Interface::BACKWARD>() {
return ends_;
}
template <>
inline void StagePrivate::send<Interface::FORWARD>(const InterfaceState& start, InterfaceState&& end,
const SolutionBasePtr& solution) {
@ -298,9 +325,20 @@ private:
};
PIMPL_FUNCTIONS(MonitoringGenerator)
// Print pending pairs of a ConnectingPrivate instance
class ConnectingPrivate;
struct PendingPairsPrinter
{
const ConnectingPrivate* const instance_;
PendingPairsPrinter(const ConnectingPrivate* c) : instance_(c) {}
};
std::ostream& operator<<(std::ostream& os, const PendingPairsPrinter& printer);
class ConnectingPrivate : public ComputeBasePrivate
{
friend class Connecting;
friend struct FallbacksPrivateConnect;
friend std::ostream& operator<<(std::ostream& os, const PendingPairsPrinter& printer);
public:
struct StatePair : std::pair<Interface::const_iterator, Interface::const_iterator>
@ -311,18 +349,15 @@ public:
}
static inline bool less(const InterfaceState::Priority& lhsA, const InterfaceState::Priority& lhsB,
const InterfaceState::Priority& rhsA, const InterfaceState::Priority& rhsB) {
unsigned char lhs = (lhsA.enabled() << 1) | lhsB.enabled(); // combine bits into two-digit binary number
unsigned char rhs = (rhsA.enabled() << 1) | rhsB.enabled();
bool lhs = lhsA.enabled() && lhsB.enabled();
bool rhs = rhsA.enabled() && rhsB.enabled();
if (lhs == rhs) // if enabled status is identical
return lhsA + lhsB < rhsA + rhsB; // compare the sums of both contributions
// one of the states in each pair should be enabled
assert(lhs != 0b00 && rhs != 0b00);
// both states valid (b11)
if (lhs == 0b11)
return true;
if (rhs == 0b11)
return false;
return lhs < rhs; // disabled states in 1st component go before disabled states in 2nd component
// sort both-enabled pairs first
static_assert(true > false, "Comparing enabled states requires true > false");
return lhs > rhs;
}
};
@ -334,22 +369,23 @@ public:
// Check whether there are pending feasible states that could connect to source
template <Interface::Direction dir>
bool hasPendingOpposites(const InterfaceState* source) const;
bool hasPendingOpposites(const InterfaceState* source, const InterfaceState* target) const;
std::ostream& printPendingPairs(std::ostream& os = std::cerr) const;
PendingPairsPrinter pendingPairsPrinter() const { return PendingPairsPrinter(this); }
private:
// Create a pair of Interface states for pending list, such that the order (start, end) is maintained
template <Interface::Direction other>
inline StatePair make_pair(Interface::const_iterator first, Interface::const_iterator second);
// get informed when new start or end state becomes available
// notify callback to get informed about newly inserted (or updated) start or end states
template <Interface::Direction other>
void newState(Interface::iterator it, bool updated);
void newState(Interface::iterator it, Interface::UpdateFlags updated);
// ordered list of pending state pairs
ordered<StatePair> pending;
};
PIMPL_FUNCTIONS(Connecting)
} // namespace task_constructor
} // namespace moveit

View File

@ -47,7 +47,10 @@
#include "stages/generate_grasp_pose.h"
#include "stages/generate_place_pose.h"
#include "stages/generate_pose.h"
#include "stages/generate_random_pose.h"
#include "stages/limit_solutions.h"
#include "stages/modify_planning_scene.h"
#include "stages/move_relative.h"
#include "stages/move_to.h"
#include "stages/passthrough.h"
#include "stages/predicate_filter.h"

View File

@ -67,7 +67,7 @@ protected:
bool compatible(const InterfaceState& from_state, const InterfaceState& to_state) const override;
public:
enum MergeMode
enum MergeMode : uint8_t
{
SEQUENTIAL = 0,
WAYPOINTS = 1
@ -76,6 +76,7 @@ public:
using GroupPlannerVector = std::vector<std::pair<std::string, solvers::PlannerInterfacePtr> >;
Connect(const std::string& name = "connect", const GroupPlannerVector& planners = {});
void setMaxDistance(double max_distance) { setProperty("max_distance", max_distance); }
void setPathConstraints(moveit_msgs::Constraints path_constraints) {
setProperty("path_constraints", std::move(path_constraints));
}

View File

@ -48,9 +48,11 @@ namespace stages {
class FixedState : public Generator
{
public:
FixedState(const std::string& name = "initial state", planning_scene::PlanningScenePtr = nullptr);
FixedState(const std::string& name = "initial state", planning_scene::PlanningScenePtr scene = nullptr);
void setState(const planning_scene::PlanningScenePtr& scene);
void setIgnoreCollisions(bool ignore) { setProperty("ignore_collisions", ignore); }
void reset() override;
bool canCompute() const override;
void compute() override;

View File

@ -55,6 +55,7 @@ public:
void setEndEffector(const std::string& eef) { setProperty("eef", eef); }
void setObject(const std::string& object) { setProperty("object", object); }
void setAngleDelta(double delta) { setProperty("angle_delta", delta); }
void setRotationAxis(const Eigen::Vector3d& axis) { setProperty("rotation_axis", axis); }
void setPreGraspPose(const std::string& pregrasp) { properties().set("pregrasp", pregrasp); }
void setPreGraspPose(const moveit_msgs::RobotState& pregrasp) { properties().set("pregrasp", pregrasp); }

View File

@ -0,0 +1,112 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2020, PickNik Inc
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of PickNik Inc nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
/* Authors: Henning Kayser
Desc: Generator Stage for randomized poses based on GeneratePose
*/
#pragma once
#include <moveit/task_constructor/stages/generate_pose.h>
#include <random>
namespace moveit {
namespace task_constructor {
namespace stages {
class GenerateRandomPose : public GeneratePose
{
public:
GenerateRandomPose(const std::string& name = "generate random pose");
bool canCompute() const override;
void compute() override;
/** Function signature for pose dimension samplers.
* The input parameter is the seed, the returned value is the sampling result. */
using PoseDimensionSampler = std::function<double(double)>;
enum PoseDimension : uint8_t
{
X,
Y,
Z,
ROLL,
PITCH,
YAW
};
/** Configure a RandomNumberDistribution sampler for a PoseDimension (X/Y/Z/ROLL/PITCH/YAW)
*
* RandomNumberDistribution is a template class that generates random numbers according to a specific distribution
* (see https://en.cppreference.com/w/cpp/numeric/random).
* Supported distributions are: std::normal_distribution and std::uniform_real_distribution.
* The width parameter specifies the standard deviation resp. the range of the uniform distribution.
*
* The order in which the PoseDimension samplers are specified matters as the samplers are applied in sequence.
* That way it's possible to implement different Euler angles (i.e. XYZ, ZXZ, YXY) or even construct more complex
* sampling regions by applying translations after rotations.
*/
template <template <class Realtype = double> class RandomNumberDistribution>
void sampleDimension(const PoseDimension pose_dimension, double width) {
sampleDimension(pose_dimension, getPoseDimensionSampler<RandomNumberDistribution>(width));
}
/** Specify a sampling function of type PoseDimensionSampler for randomizing a pose dimension. */
void sampleDimension(const PoseDimension pose_dimension, const PoseDimensionSampler& pose_dimension_sampler) {
pose_dimension_samplers_.emplace_back(std::make_pair(pose_dimension, pose_dimension_sampler));
}
/** Limit the number of generated solutions */
void setMaxSolutions(size_t max_solutions) { setProperty("max_solutions", max_solutions); }
private:
/** Allocate the sampler function for the specified random distribution */
template <template <class Realtype = double> class RandomNumberDistribution>
PoseDimensionSampler getPoseDimensionSampler(double /* width */) {
static_assert(sizeof(RandomNumberDistribution<double>) == -1, "This distribution type is not supported!");
throw 0; // suppress -Wreturn-type
}
std::vector<std::pair<PoseDimension, PoseDimensionSampler>> pose_dimension_samplers_;
};
template <>
GenerateRandomPose::PoseDimensionSampler
GenerateRandomPose::getPoseDimensionSampler<std::normal_distribution>(double stddev);
template <>
GenerateRandomPose::PoseDimensionSampler
GenerateRandomPose::getPoseDimensionSampler<std::uniform_real_distribution>(double range);
} // namespace stages
} // namespace task_constructor
} // namespace moveit

View File

@ -0,0 +1,65 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the copyright holders nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
/* Authors: Joseph Moore */
#pragma once
#include <moveit/task_constructor/container.h>
namespace moveit {
namespace task_constructor {
namespace stages {
/** A wrapper which lazily passes a limited number of solutions
*
* The best solution at each call to compute is passed on
*
*/
class LimitSolutions : public WrapperBase
{
public:
LimitSolutions(const std::string& name = "LimitSolutions", Stage::pointer&& child = Stage::pointer());
void reset() override;
bool canCompute() const override;
void compute() override;
void onNewSolution(const SolutionBase& s) override;
void setMaxSolutions(uint32_t max_solutions) { setProperty("max_solutions", max_solutions); }
private:
ordered<const SolutionBase*> upstream_solutions_;
uint32_t forwarded_solutions;
};
} // namespace stages
} // namespace task_constructor
} // namespace moveit

View File

@ -81,6 +81,8 @@ public:
void addObject(const moveit_msgs::CollisionObject& collision_object);
/// Remove an object from the planning scene
void removeObject(const std::string& object_name);
/// Move an object from the planning scene
void moveObject(const moveit_msgs::CollisionObject& collision_object);
/// conviency methods accepting a single object name
inline void attachObject(const std::string& object, const std::string& link);
@ -149,8 +151,9 @@ protected:
protected:
// apply stored modifications to scene
InterfaceState apply(const InterfaceState& from, bool invert);
void processCollisionObject(planning_scene::PlanningScene& scene, const moveit_msgs::CollisionObject& object);
std::pair<InterfaceState, SubTrajectory> apply(const InterfaceState& from, bool invert);
void processCollisionObject(planning_scene::PlanningScene& scene, const moveit_msgs::CollisionObject& object,
bool invert);
void attachObjects(planning_scene::PlanningScene& scene, const std::pair<std::string, std::pair<Names, bool>>& pair,
bool invert);
void allowCollisions(planning_scene::PlanningScene& scene, const CollisionMatrixPairs& pairs, bool invert);

View File

@ -66,10 +66,8 @@ public:
void setIKFrame(const geometry_msgs::PoseStamped& pose) { setProperty("ik_frame", pose); }
void setIKFrame(const Eigen::Isometry3d& pose, const std::string& link);
template <typename T>
void setIKFrame(const T& p, const std::string& link) {
Eigen::Isometry3d pose;
pose = p;
setIKFrame(pose, link);
void setIKFrame(const T& pose, const std::string& link) {
setIKFrame(Eigen::Isometry3d(pose), link);
}
void setIKFrame(const std::string& link) { setIKFrame(Eigen::Isometry3d::Identity(), link); }

View File

@ -0,0 +1,64 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2024, Sherbrooke University
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Bielefeld University nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
/* Authors: Captain Yoshi */
#pragma once
#include <moveit/task_constructor/stage.h>
#include <moveit/planning_scene/planning_scene.h>
namespace moveit {
namespace task_constructor {
namespace stages {
/** no-op stage, which doesn't modify the interface state nor adds a trajectory.
* However, it can be used to store custom stage properties,
* which in turn can be queried post-planning to steer the execution.
*/
class NoOp : public PropagatingEitherWay
{
public:
NoOp(const std::string& name = "no-op") : PropagatingEitherWay(name){};
private:
bool compute(const InterfaceState& state, planning_scene::PlanningScenePtr& scene, SubTrajectory& /*trajectory*/,
Interface::Direction /*dir*/) override {
scene = state.scene()->diff();
return true;
};
};
} // namespace stages
} // namespace task_constructor
} // namespace moveit

View File

@ -36,9 +36,6 @@
#pragma once
#include <moveit/task_constructor/container.h>
#include <moveit/task_constructor/cost_queue.h>
#include <geometry_msgs/PoseStamped.h>
#include <Eigen/Geometry>
namespace moveit {
namespace task_constructor {

View File

@ -80,13 +80,12 @@ public:
/// set properties of IK solver
void setIKFrame(const geometry_msgs::PoseStamped& transform) { setProperty("ik_frame", transform); }
void setIKFrame(const Eigen::Isometry3d& pose, const std::string& link);
template <typename T>
void setIKFrame(const T& t, const std::string& link) {
Eigen::Isometry3d transform;
transform = t;
setIKFrame(transform, link);
}
void setIKFrame(const std::string& link) { setIKFrame(Eigen::Isometry3d::Identity(), link); }
/// allow setting IK frame from any type T that converts to Eigen::Isometry3d
template <typename T>
void setIKFrame(const T& transform, const std::string& link) {
setIKFrame(Eigen::Isometry3d(transform), link);
}
void setMaxIKSolutions(uint32_t max_ik_solutions) { setProperty("max_ik_solutions", max_ik_solutions); }
};

View File

@ -42,6 +42,7 @@
#include <moveit/macros/class_forward.h>
#include <moveit/task_constructor/properties.h>
#include <moveit/task_constructor/cost_queue.h>
#include <moveit/task_constructor/utils.h>
#include <moveit_task_constructor_msgs/Solution.h>
#include <visualization_msgs/MarkerArray.h>
@ -79,29 +80,28 @@ class InterfaceState
friend class ContainerBasePrivate; // allow setting priority_ for pruning
public:
enum Status
enum Status : uint8_t
{
ENABLED, // state is actively considered during planning
PRUNED, // state is disabled because a required connected state failed
FAILED, // state that failed, causing the whole partial solution to be disabled
ARMED, // disabled state in a Connecting interface that will become re-enabled with a new opposite state
PRUNED, // disabled state on a pruned solution branch
};
static const char* colorForStatus(unsigned int s) { return STATUS_COLOR_[s]; }
/** InterfaceStates are ordered according to two values:
* Depth of interlinked trajectory parts and accumulated trajectory costs along that path.
* Preference ordering considers high-depth first and within same depth, minimal cost paths.
*/
struct Priority : std::tuple<Status, unsigned int, double>
{
Priority(unsigned int depth, double cost, Status status = ENABLED)
: std::tuple<Status, unsigned int, double>(status, depth, cost) {
assert(std::isfinite(cost));
}
Priority(unsigned int depth, double cost, Status status)
: std::tuple<Status, unsigned int, double>(status, depth, cost) {}
Priority(unsigned int depth, double cost) : Priority(depth, cost, std::isfinite(cost) ? ENABLED : PRUNED) {}
// Constructor copying depth and cost, but modifying its status
Priority(const Priority& other, Status status) : Priority(other.depth(), other.cost(), status) {}
inline Status status() const { return std::get<0>(*this); }
inline bool enabled() const { return std::get<0>(*this) == ENABLED; }
inline bool failed() const { return std::get<0>(*this) == FAILED; }
inline bool pruned() const { return std::get<0>(*this) == PRUNED; }
inline unsigned int depth() const { return std::get<1>(*this); }
inline double cost() const { return std::get<2>(*this); }
@ -128,6 +128,7 @@ public:
/// copy an existing InterfaceState, but not including incoming/outgoing trajectories
InterfaceState(const InterfaceState& other);
InterfaceState(InterfaceState&& other) = default;
InterfaceState& operator=(const InterfaceState& other) = default;
inline const planning_scene::PlanningSceneConstPtr& scene() const { return scene_; }
inline const Solutions& incomingTrajectories() const { return incoming_trajectories_; }
@ -138,15 +139,24 @@ public:
/// states are ordered by priority
inline bool operator<(const InterfaceState& other) const { return this->priority_ < other.priority_; }
inline const Priority& priority() const { return priority_; }
/// Update priority and call owner's notify() if possible
void updatePriority(const InterfaceState::Priority& priority);
/// Update status, but keep current priority
void updateStatus(Status status);
Interface* owner() const { return owner_; }
private:
// these methods should be only called by SolutionBase::set[Start|End]State()
inline void addIncoming(SolutionBase* t) { incoming_trajectories_.push_back(t); }
inline void addOutgoing(SolutionBase* t) { outgoing_trajectories_.push_back(t); }
// Set new priority without updating the owning interface (USE WITH CARE)
inline void setPriority(const Priority& prio) { priority_ = prio; }
private:
static const char* STATUS_COLOR_[];
planning_scene::PlanningSceneConstPtr scene_;
PropertyMap properties_;
/// trajectories which are *timewise before* this state
@ -187,14 +197,31 @@ public:
const InterfaceState* operator->() const noexcept { return base_type::const_iterator::operator*(); }
};
enum Direction
enum Direction : uint8_t
{
FORWARD,
BACKWARD,
START = FORWARD,
END = BACKWARD
};
using NotifyFunction = std::function<void(iterator, bool)>;
enum Update : uint8_t
{
STATUS = 1 << 0,
PRIORITY = 1 << 1,
ALL = STATUS | PRIORITY,
};
using UpdateFlags = utils::Flags<Update>;
using NotifyFunction = std::function<void(iterator, UpdateFlags)>;
class DisableNotify
{
Interface& if_;
Interface::NotifyFunction old_;
public:
DisableNotify(Interface& i) : if_(i) { old_.swap(if_.notify_); }
~DisableNotify() { old_.swap(if_.notify_); }
};
friend class DisableNotify;
Interface(const NotifyFunction& notify = NotifyFunction());
/// add a new InterfaceState
@ -205,9 +232,10 @@ public:
/// update state's priority (and call notify_ if it really has changed)
void updatePriority(InterfaceState* state, const InterfaceState::Priority& priority);
inline bool notifyEnabled() const { return static_cast<bool>(notify_); }
private:
const NotifyFunction notify_;
NotifyFunction notify_;
// restrict access to some functions to ensure consistency
// (we need to set/unset InterfaceState::owner_)
@ -220,6 +248,17 @@ private:
std::ostream& operator<<(std::ostream& os, const InterfaceState::Priority& prio);
std::ostream& operator<<(std::ostream& os, const Interface& interface);
std::ostream& operator<<(std::ostream& os, Interface::Direction dir);
/// Find index of the iterator in the container. Counting starts at 1. Zero corresponds to not found.
template <typename T>
size_t getIndex(const T& container, typename T::const_iterator search) {
size_t index = 1;
for (typename T::const_iterator it = container.begin(), end = container.end(); it != end; ++it, ++index)
if (it == search)
return index;
return 0;
}
class CostTerm;
class StagePrivate;
@ -271,9 +310,11 @@ public:
auto& markers() { return markers_; }
const auto& markers() const { return markers_; }
/// convert solution to message
void toMsg(moveit_task_constructor_msgs::Solution& solution, Introspection* introspection = nullptr) const;
/// append this solution to Solution msg
virtual void fillMessage(moveit_task_constructor_msgs::Solution& solution,
Introspection* introspection = nullptr) const = 0;
virtual void appendTo(moveit_task_constructor_msgs::Solution& solution,
Introspection* introspection = nullptr) const = 0;
void fillInfo(moveit_task_constructor_msgs::SolutionInfo& info, Introspection* introspection = nullptr) const;
/// required to dispatch to type-specific CostTerm methods via vtable
@ -294,7 +335,7 @@ private:
// comment for this solution, e.g. explanation of failure
std::string comment_;
// markers for this solution, e.g. target frame or collision indicators
std::deque<visualization_msgs::Marker> markers_;
std::vector<visualization_msgs::Marker> markers_;
// begin and end InterfaceState of this solution/trajectory
const InterfaceState* start_ = nullptr;
@ -314,7 +355,7 @@ public:
robot_trajectory::RobotTrajectoryConstPtr trajectory() const { return trajectory_; }
void setTrajectory(const robot_trajectory::RobotTrajectoryPtr& t) { trajectory_ = t; }
void fillMessage(moveit_task_constructor_msgs::Solution& msg, Introspection* introspection = nullptr) const override;
void appendTo(moveit_task_constructor_msgs::Solution& msg, Introspection* introspection = nullptr) const override;
double computeCost(const CostTerm& cost, std::string& comment) const override;
@ -347,7 +388,7 @@ public:
void push_back(const SolutionBase& solution);
/// append all subsolutions to solution
void fillMessage(moveit_task_constructor_msgs::Solution& msg, Introspection* introspection) const override;
void appendTo(moveit_task_constructor_msgs::Solution& msg, Introspection* introspection) const override;
double computeCost(const CostTerm& cost, std::string& comment) const override;
@ -378,8 +419,8 @@ public:
: SolutionBase(creator, cost), wrapped_(wrapped) {}
explicit WrappedSolution(Stage* creator, const SolutionBase* wrapped)
: WrappedSolution(creator, wrapped, wrapped->cost()) {}
void fillMessage(moveit_task_constructor_msgs::Solution& solution,
Introspection* introspection = nullptr) const override;
void appendTo(moveit_task_constructor_msgs::Solution& solution,
Introspection* introspection = nullptr) const override;
double computeCost(const CostTerm& cost, std::string& comment) const override;

View File

@ -47,6 +47,7 @@
#include <moveit/macros/class_forward.h>
#include <moveit_msgs/MoveItErrorCodes.h>
#include <moveit/utils/moveit_error_code.h>
namespace moveit {
namespace core {
@ -72,6 +73,8 @@ class Task : protected WrapperBase
{
public:
PRIVATE_CLASS(Task)
using WrapperBase::setCostTerm;
using WrapperBase::operator[];
Task(const std::string& ns = "", bool introspection = true,
ContainerBase::pointer&& container = std::make_unique<SerialContainer>("task pipeline"));
@ -82,6 +85,9 @@ public:
const std::string& name() const { return stages()->name(); }
void setName(const std::string& name) { stages()->setName(name); }
Stage* findChild(const std::string& name) const { return stages()->findChild(name); }
Stage* operator[](int index) const { return stages()->operator[](index); }
const moveit::core::RobotModelConstPtr& getRobotModel() const;
/// setting the robot model also resets the task
void setRobotModel(const moveit::core::RobotModelConstPtr& robot_model);
@ -111,21 +117,28 @@ public:
using WrapperBase::setTimeout;
using WrapperBase::timeout;
using WrapperBase::pruning;
using WrapperBase::setPruning;
/// reset all stages
void reset() final;
/// initialize all stages with given scene
void init();
/// reset, init scene (if not yet done), and init all stages, then start planning
bool plan(size_t max_solutions = 0);
/// interrupt current planning (or execution)
moveit::core::MoveItErrorCode plan(size_t max_solutions = 0);
/// interrupt current planning
void preempt();
void resetPreemptRequest();
/// execute solution, return the result
moveit_msgs::MoveItErrorCodes execute(const SolutionBase& s);
moveit::core::MoveItErrorCode execute(const SolutionBase& s);
/// print current task state (number of found solutions and propagated states) to std::cout
void printState(std::ostream& os = std::cout) const;
/// print an explanation for a planning failure to os
bool explainFailure(std::ostream& os = std::cout) const override;
size_t numSolutions() const { return solutions().size(); }
const ordered<SolutionBaseConstPtr>& solutions() const { return stages()->solutions(); }
const std::list<SolutionBaseConstPtr>& failures() const { return stages()->failures(); }

View File

@ -63,7 +63,7 @@ private:
std::string ns_;
robot_model_loader::RobotModelLoaderPtr robot_model_loader_;
moveit::core::RobotModelConstPtr robot_model_;
bool preempt_requested_;
std::atomic<bool> preempt_requested_;
// introspection and monitoring
std::unique_ptr<Introspection> introspection_;

View File

@ -0,0 +1,45 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2022, PickNik Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of PickNik Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
/* Authors: Joe Schornak, Sebastian Jahr */
#pragma once
#include <moveit_task_constructor_msgs/TrajectoryExecutionInfo.h>
namespace moveit {
namespace task_constructor {
using TrajectoryExecutionInfo = moveit_task_constructor_msgs::TrajectoryExecutionInfo;
} // namespace task_constructor
} // namespace moveit

View File

@ -45,6 +45,9 @@
#include <Eigen/Geometry>
#include <moveit/macros/class_forward.h>
#include <moveit/robot_trajectory/robot_trajectory.h>
#include <moveit/collision_detection/collision_common.h>
#include <moveit/task_constructor/solvers/planner_interface.h>
namespace planning_scene {
MOVEIT_CLASS_FORWARD(PlanningScene);
@ -60,7 +63,6 @@ MOVEIT_CLASS_FORWARD(RobotState);
namespace task_constructor {
MOVEIT_CLASS_FORWARD(Property);
MOVEIT_CLASS_FORWARD(SolutionBase);
namespace utils {
@ -140,14 +142,23 @@ private:
Int i;
};
#define DECLARE_FLAGS(Flags, Enum) using Flags = QFlags<Enum>;
const moveit::core::LinkModel* getRigidlyConnectedParentLinkModel(const moveit::core::RobotState& state,
std::string frame);
bool getRobotTipForFrame(const Property& property, const planning_scene::PlanningScene& scene,
const moveit::core::JointModelGroup* jmg, SolutionBase& solution,
/** For a PoseStamped property, lookup the associated LinkModel* and yield the pose in global frame */
bool getRobotTipForFrame(const Property& tip_pose, const planning_scene::PlanningScene& scene,
const moveit::core::JointModelGroup* jmg, std::string& error_msg,
const moveit::core::LinkModel*& robot_link, Eigen::Isometry3d& tip_in_global_frame);
/** Add sphere markers to visualize given collisions */
void addCollisionMarkers(std::vector<visualization_msgs::Marker>& markers, const std::string& frame_id,
const collision_detection::CollisionResult::ContactMap& contacts, double radius = 0.035);
/** Add sphere markers to visualize collisions within the trajectory */
void addCollisionMarkers(std::vector<visualization_msgs::Marker>& markers,
const robot_trajectory::RobotTrajectory& trajectory,
const planning_scene::PlanningSceneConstPtr& planning_scene);
/** Returns true if the result provides hints that planning failed due to collisions */
bool hints_at_collisions(const solvers::PlannerInterface::Result& result);
} // namespace utils
} // namespace task_constructor
} // namespace moveit

View File

@ -1,31 +1,42 @@
<package format="2">
<package format="3">
<name>moveit_task_constructor_core</name>
<version>0.0.0</version>
<version>0.1.3</version>
<description>MoveIt Task Pipeline</description>
<url type="website">https://github.com/moveit/moveit_task_constructor</url>
<url type="repository">https://github.com/moveit/moveit_task_constructor</url>
<url type="bugtracker">https://github.com/moveit/moveit_task_constructor/issues</url>
<license>BSD</license>
<maintainer email="me@v4hn.de">Michael Goerner</maintainer>
<maintainer email="rhaschke@techfak.uni-bielefeld.de">Robert Haschke</maintainer>
<buildtool_depend>catkin</buildtool_depend>
<buildtool_depend condition="$ROS_PYTHON_VERSION == 2">python-setuptools</buildtool_depend>
<buildtool_depend condition="$ROS_PYTHON_VERSION == 3">python3-setuptools</buildtool_depend>
<build_depend>roscpp</build_depend>
<build_depend>roslint</build_depend>
<exec_depend>roscpp</exec_depend>
<depend>fmt</depend>
<depend>tf2_eigen</depend>
<depend>geometry_msgs</depend>
<depend>moveit_core</depend>
<depend>moveit_ros_planning</depend>
<depend>moveit_ros_planning_interface</depend>
<depend>moveit_task_constructor_msgs</depend>
<depend>py_binding_tools</depend>
<depend>visualization_msgs</depend>
<depend>rviz_marker_tools</depend>
<test_depend>rosunit</test_depend>
<test_depend>rostest</test_depend>
<test_depend>moveit_resources_fanuc_moveit_config</test_depend>
<test_depend>moveit_planners</test_depend>
<test_depend>moveit_commander</test_depend>
<export>
<moveit_task_constructor_core plugin="${prefix}/motion_planning_stages_plugin_description.xml"/>
<moveit_task_constructor_core plugin="${prefix}/motion_planning_stages_plugin_description.xml" />
</export>
</package>

View File

@ -0,0 +1,22 @@
# pybind11 must use the ROS python version
set(PYBIND11_PYTHON_VERSION ${PYTHON_VERSION})
find_package(pybind11 3.0 REQUIRED)
# C++ wrapper code
add_subdirectory(bindings)
if(CATKIN_ENABLE_TESTING)
find_package(rostest REQUIRED)
# Add folders to be run by python nosetests
if(DEFINED ENV{PRELOAD})
message(WARNING "Skipping python tests due to asan")
else()
catkin_add_nosetests(test)
# Add rostests
add_rostest(test/rostest_mtc.test)
endif()
endif()
roslint_python()

View File

@ -0,0 +1,28 @@
set(INCLUDES ${PROJECT_SOURCE_DIR}/include/moveit/python/task_constructor)
add_library(${PROJECT_NAME}_python_tools SHARED
${INCLUDES}/properties.h
src/properties.cpp
)
target_link_libraries(${PROJECT_NAME}_python_tools PUBLIC ${PROJECT_NAME} pybind11::pybind11)
# Use minimum-size optimization for pybind11 bindings
target_link_libraries(${PROJECT_NAME}_python_tools PUBLIC pybind11::opt_size)
# catkin_lint cannot detect target declarations in functions, here in pybind11_add_module
#catkin_lint: ignore undefined_target
# moveit.task_constructor
pybind11_add_module(pymoveit_mtc
src/solvers.cpp
src/core.cpp
src/stages.cpp
src/module.cpp
)
target_link_libraries(pymoveit_mtc PUBLIC ${PROJECT_NAME} ${PROJECT_NAME}_stages ${PROJECT_NAME}_python_tools)
set_target_properties(pymoveit_mtc PROPERTIES LIBRARY_OUTPUT_DIRECTORY ${CATKIN_DEVEL_PREFIX}/${CATKIN_GLOBAL_PYTHON_DESTINATION})
# install libs
install(TARGETS ${PROJECT_NAME}_python_tools
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION})
install(TARGETS pymoveit_mtc
LIBRARY DESTINATION ${CATKIN_GLOBAL_PYTHON_DESTINATION})

View File

@ -0,0 +1,505 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2020, Bielefeld University
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Bielefeld University nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
#include "core.h"
#include "utils.h"
#include <pybind11/stl.h>
#include <pybind11/functional.h>
#include <moveit/python/task_constructor/properties.h>
#include <moveit/task_constructor/container_p.h>
#include <moveit/task_constructor/task.h>
#include <moveit/planning_scene/planning_scene.h>
#include <moveit/planning_scene_interface/planning_scene_interface.h>
#include <moveit/move_group_interface/move_group_interface.h>
namespace py = pybind11;
using namespace py::literals;
using namespace moveit::task_constructor;
namespace moveit {
namespace python {
namespace {
py::list getForwardedProperties(const Stage& self) {
py::list l;
for (const std::string& value : self.forwardedProperties())
l.append(value);
return l;
}
void setForwardedProperties(Stage& self, const py::object& names) {
std::set<std::string> s;
try {
// handle string argument as single name
if (PyBytes_Check(names.ptr()))
s.emplace(names.cast<std::string>());
else // expect iterable otherwise
for (auto item : names)
s.emplace(item.cast<std::string>());
} catch (const py::cast_error& e) {
// translate cast_error to type_error with an informative message
throw py::type_error("Expecting a string or a list of strings");
}
self.setForwardedProperties(s);
}
} // anonymous namespace
void export_core(pybind11::module& m) {
/// translate InitStageException into InitStageError
PYBIND11_CONSTINIT static py::gil_safe_call_once_and_store<py::object> exc_storage;
exc_storage.call_once_and_store_result([&]() { return py::exception<InitStageException>(m, "InitStageError"); });
/// provide extended error description for InitStageException
py::register_exception_translator([](std::exception_ptr p) { // NOLINT(performance-unnecessary-value-param)
try {
if (p)
std::rethrow_exception(p);
} catch (const InitStageException& e) {
std::stringstream message;
message << e;
py::set_error(exc_storage.get_stored(), message.str().c_str());
}
});
py::classh<Introspection>(m, "Introspection", "Introspection class");
py::classh<SolutionBase>(m, "Solution", "Abstract base class for solutions of a stage")
.def_property("cost", &SolutionBase::cost, &SolutionBase::setCost, "float: Cost associated with the solution")
.def_property("comment", &SolutionBase::comment, &SolutionBase::setComment,
"str: Comment associated with the solution")
.def("markAsFailure", &SolutionBase::markAsFailure, "Mark the SubTrajectory as a failure", "comment"_a)
.def_property_readonly("isFailure", &SolutionBase::isFailure,
"bool: True if the trajectory is marked as a failure (read-only)")
.def_property_readonly("start", &SolutionBase::start, "InterfaceState: Start of the trajectory (read-only)")
.def_property_readonly("end", &SolutionBase::end, "InterfaceState: End of the trajectory (read-only)")
.def_property_readonly(
"markers", py::overload_cast<>(&SolutionBase::markers),
":visualization_msgs:`Marker`: Markers to visualize important aspects of the trajectory (read-only)")
.def(
"toMsg",
[](const SolutionBase& self, moveit::task_constructor::Introspection* introspection) {
moveit_task_constructor_msgs::Solution msg;
self.toMsg(msg, introspection);
return msg;
},
"Convert to the ROS message ``Solution``", py::arg("introspection") = nullptr);
py::classh<SubTrajectory, SolutionBase>(m, "SubTrajectory",
"Solution trajectory connecting two InterfaceStates of a stage")
.def(py::init<>())
.def_property("trajectory", &SubTrajectory::trajectory, &SubTrajectory::setTrajectory,
":moveit_msgs:`RobotTrajectory`: Actual robot trajectory");
using Solutions = ordered<SolutionBaseConstPtr>;
py::classh<Solutions>(m, "Solutions", "Cost-ordered list of solutions")
.def("__len__", &Solutions::size)
.def("__getitem__", &get_item<Solutions>)
.def(
"__iter__", [](Solutions& self) { return py::make_iterator(self.begin(), self.end()); },
py::keep_alive<0, 1>());
py::classh<InterfaceState>(m, "InterfaceState",
"Describes a potential start or goal state of a Stage. "
"It comprises a PlanningScene as well as a PropertyMap.")
.def(py::init<const planning_scene::PlanningScenePtr&>(), "scene"_a)
.def_property_readonly("properties", py::overload_cast<>(&InterfaceState::properties),
"PropertyMap: PropertyMap of the state (read-only).")
.def_property_readonly("scene", &InterfaceState::scene,
"PlanningScene: PlanningScene of the state (read-only).");
py::classh<moveit::core::MoveItErrorCode>(m, "MoveItErrorCode", "Encapsulates moveit error code message")
.def_readonly("val", &moveit::core::MoveItErrorCode::val, ":moveit_msgs:`MoveItErrorCodes`: error code")
.def(PYBIND11_BOOL_ATTR,
[](const moveit::core::MoveItErrorCode& err) { return pybind11::cast(static_cast<bool>(err)); });
py::classh<CostTerm>(m, "CostTerm", "Base class for cost calculation in stages");
auto tct = py::classh<TrajectoryCostTerm, CostTerm>(m, "TrajectoryCostTerm",
"Base class for cost calculation of trajectories");
py::enum_<TrajectoryCostTerm::Mode>(tct, "Mode", "Specify which states are considered for collision checking")
.value("AUTO", TrajectoryCostTerm::Mode::AUTO, "TRAJECTORY (if available) or START_INTERFACE")
.value("START_INTERFACE", TrajectoryCostTerm::Mode::START_INTERFACE, "Only consider start state")
.value("END_INTERFACE", TrajectoryCostTerm::Mode::END_INTERFACE, "Only consider end state")
.value("TRAJECTORY", TrajectoryCostTerm::Mode::TRAJECTORY, "Consider whole trajectory");
py::classh<cost::PathLength, TrajectoryCostTerm>(m, "PathLength",
"Computes joint-based path length along trajectory")
.def(py::init<>())
.def(py::init<std::vector<std::string>>())
.def(py::init<std::map<std::string, double>>());
py::classh<cost::DistanceToReference, TrajectoryCostTerm>(m, "DistanceToReference",
"Computes joint-based distance to reference pose")
.def(py::init<const moveit_msgs::RobotState&, TrajectoryCostTerm::Mode, std::map<std::string, double>>(),
"reference"_a, "mode"_a = TrajectoryCostTerm::Mode::AUTO, "weights"_a = std::map<std::string, double>())
.def(py::init<const std::map<std::string, double>&, TrajectoryCostTerm::Mode, std::map<std::string, double>>(),
"reference"_a, "mode"_a = TrajectoryCostTerm::Mode::AUTO, "weights"_a = std::map<std::string, double>());
py::classh<cost::TrajectoryDuration, TrajectoryCostTerm>(m, "TrajectoryDuration", "Computes duration of trajectory")
.def(py::init<>());
py::classh<cost::LinkMotion, TrajectoryCostTerm>(m, "LinkMotion",
"Computes Cartesian path length of given link along trajectory")
.def(py::init<std::string>(), "link_name"_a);
py::classh<cost::Clearance, TrajectoryCostTerm>(m, "Clearance", "Computes inverse distance to collision objects")
.def(py::init<bool, bool, std::string, TrajectoryCostTerm::Mode>(), "with_world"_a = true,
"cumulative"_a = false, "group_property"_a = "group", "mode"_a = TrajectoryCostTerm::Mode::AUTO);
auto stage =
properties::class_<Stage, PyStage<>>(m, "Stage", "Abstract base class of all stages.")
.property<double>("timeout", "float: Maximally allowed time [s] per computation step")
.property<std::string>("marker_ns", "str: Namespace for any markers that are associated to the stage")
.def_property("forwarded_properties", getForwardedProperties, setForwardedProperties,
"list: set of properties forwarded from input to output InterfaceState")
.def_property("name", &Stage::name, &Stage::setName, "str: name of the stage displayed e.g. in rviz")
.def_property_readonly("properties", py::overload_cast<>(&Stage::properties),
"PropertyMap: PropertyMap of the stage (read-only)")
.def_property_readonly("solutions", &Stage::solutions, "Successful Solutions of the stage (read-only)")
.def_property_readonly("failures", &Stage::failures, "Solutions: Failed Solutions of the stage (read-only)")
.def<void (Stage::*)(const CostTermConstPtr&)>("setCostTerm", &Stage::setCostTerm,
"Specify a CostTerm for calculation of stage costs")
.def(
"setCostTerm", [](Stage& self, const LambdaCostTerm::SubTrajectorySignature& f) { self.setCostTerm(f); },
"Specify a function to calculate trajectory costs")
.def(
"setCostTerm",
[](Stage& self, const LambdaCostTerm::SubTrajectoryShortSignature& f) { self.setCostTerm(f); },
"Specify a function to calculate trajectory costs")
.def("reset", &Stage::reset, "Reset the Stage. Clears all solutions, interfaces and inherited properties")
.def("init", &Stage::init,
"Initialize the stage once before planning. "
"Will setup properties configured for initialization from parent.",
"robot_model"_a);
py::enum_<Stage::PropertyInitializerSource>(
stage, "PropertyInitializerSource",
"OR-combinable flags defining a source to initialize a specific property from. "
"Used in :doc:`pymoveit_mtc.core.PropertyMap` ``configureInitFrom()``. ")
.value("PARENT", Stage::PARENT, "Inherit properties from parent stage")
.value("INTERFACE", Stage::INTERFACE, "Inherit properties from the input InterfaceState");
auto either_way = py::classh<PropagatingEitherWay, Stage, PyPropagatingEitherWay<>>(
m, "PropagatingEitherWay", "Base class for propagator-like stages")
.def(py::init<const std::string&>(), "name"_a = std::string("PropagatingEitherWay"))
.def("restrictDirection", &PropagatingEitherWay::restrictDirection,
"Explicitly specify computation direction")
.def("computeForward", &PropagatingEitherWay::computeForward, "Compute forward")
.def("computeBackward", &PropagatingEitherWay::computeBackward, "Compute backward")
//.def("sendForward", &PropagatingEitherWay::sendForward)
//.def("sendBackward", &PropagatingEitherWay::sendBackward)
;
py::enum_<PropagatingEitherWay::Direction>(either_way, "Direction", "Propagation direction")
.value("AUTO", PropagatingEitherWay::AUTO)
.value("FORWARD", PropagatingEitherWay::FORWARD, "Propagating forwards from start to end")
.value("BACKWARD", PropagatingEitherWay::BACKWARD, "Propagating backwards from end to start");
py::classh<PropagatingForward, Stage, PyPropagatingEitherWay<PropagatingForward>>(
m, "PropagatingForward", "Base class for forward-propagating stages")
.def(py::init<const std::string&>(), "name"_a = std::string("PropagatingForward"));
py::classh<PropagatingBackward, Stage, PyPropagatingEitherWay<PropagatingBackward>>(
m, "PropagatingBackward", "Base class for backward-propagating stages")
.def(py::init<const std::string&>(), "name"_a = std::string("PropagatingBackward"));
properties::class_<Generator, Stage, PyGenerator<>>(m, "Generator", R"(
Base class for generator-like stages
Derive from this stage to implement a custom generator stage that can produce new seed states w/o prior knowledge.
Implement the virtual methods as follows::
class MyGenerator(core.Generator):
"""Implements a custom 'Generator' stage that produces maximally 3 solutions."""
def __init__(self, name="Generator"):
core.Generator.__init__(self, name)
self.reset()
def init(self, robot_model):
self.ps = PlanningScene(robot_model)
def reset(self):
core.Generator.reset(self)
def canCompute(self):
return len(self.solutions) < 3 # maximally produce 3 solutions
def compute(self):
self.spawn(core.InterfaceState(self.ps), cost=len(self.solutions))
)")
.def(py::init<const std::string&>(), "name"_a = std::string("Generator"))
.def("canCompute", &Generator::canCompute, "Return ``True`` if the stage can still produce solutions.")
.def("compute", &Generator::compute, "Compute an actual solution and ``spawn`` an ``InterfaceState``")
.def(
"spawn", [](Generator& self, InterfaceState& state, double cost) { self.spawn(std::move(state), cost); },
"Spawn an ``InterfaceState`` to both, start and end interface", "state"_a, "cost"_a);
properties::class_<MonitoringGenerator, Generator, PyMonitoringGenerator<>>(m, "MonitoringGenerator", R"(
Base class for monitoring generator stages
To implement a generator stage that draws on some previously computed solution, you need to derive
from ``MonitoringGenerator`` - monitoring the solutions produced by another stage.
Each time, the monitored stage produces a new solution, the method ``onNewSolution()`` of the
MonitoringGenerator is called. Usually, you schedule this solution for later processing in ``compute()``::
class PyMonitoringGenerator(core.MonitoringGenerator):
""" Implements a custom 'MonitoringGenerator' stage."""
solution_multiplier = 2
def __init__(self, name="MonitoringGenerator"):
core.MonitoringGenerator.__init__(self, name)
self.reset()
def reset(self):
core.MonitoringGenerator.reset(self)
self.pending = []
def onNewSolution(self, sol):
self.pending.append(sol)
def canCompute(self):
return bool(self.pending)
def compute(self):
# fetch first pending upstream solution ...
scene = self.pending.pop(0).end.scene
# ... and generate new solutions derived from it
for i in range(self.solution_multiplier):
self.spawn(core.InterfaceState(scene), i)
Upon creation of the stage, assign the monitored stage as follows::
jointspace = core.JointInterpolationPlanner()
task = core.Task()
current = stages.CurrentState("current")
task.add(current)
connect = stages.Connect(planners=[('panda_arm', jointspace)])
task.add(connect)
mg = PyMonitoringGenerator("generator")
mg.setMonitoredStage(task["current"])
task.add(mg)
)")
.def(py::init<const std::string&>(), "name"_a = std::string("generator"))
.def("setMonitoredStage", &MonitoringGenerator::setMonitoredStage, "Set the monitored ``Stage``", "stage"_a)
.def("_onNewSolution", &PubMonitoringGenerator::onNewSolution);
py::classh<ContainerBase, Stage>(m, "ContainerBase", R"(
Abstract base class for container stages
Containers allow encapsulation and reuse of planning functionality in a hierachical fashion.
You can iterate of the children of a container and access them by name.)")
.def(
"add",
[](ContainerBase& c, const py::args& args) {
for (auto it = args.begin(), end = args.end(); it != end; ++it)
c.add(it->cast<Stage::pointer>());
},
"Insert a stage at the end of the current children list")
.def("insert", &ContainerBase::insert, "stage"_a, "before"_a = -1,
"Insert a stage before the given index into the children list")
.def("remove", py::overload_cast<int>(&ContainerBase::remove), "Remove child stage by index", "pos"_a)
.def("remove", py::overload_cast<Stage*>(&ContainerBase::remove), "Remove child stage by instance", "child"_a)
.def("clear", &ContainerBase::clear, "Remove all stages from the container")
.def("__len__", &ContainerBase::numChildren)
.def(
"__getitem__",
[](const ContainerBase& c, const std::string& name) -> Stage* {
Stage* child = c.findChild(name);
if (!child)
throw py::index_error();
return child;
},
py::return_value_policy::reference_internal)
.def(
"__getitem__",
[](const ContainerBase& c, int idx) -> Stage* {
Stage* child = c[idx];
if (!child)
throw py::index_error();
return child;
},
py::return_value_policy::reference_internal)
.def(
"__iter__",
[](const ContainerBase& c) {
const auto& children = c.pimpl()->children();
return py::make_iterator(children.begin(), children.end());
},
py::keep_alive<0, 1>()) // keep container alive as long as iterator lives
;
py::classh<SerialContainer, ContainerBase>(m, "SerialContainer", "Container implementing a linear planning sequence")
.def(py::init<const std::string&>(), "name"_a = std::string("SerialContainer"));
py::classh<ParallelContainerBase, ContainerBase>(m, "ParallelContainerBase",
"Abstract base class for parallel containers");
py::classh<Alternatives, ParallelContainerBase>(m, "Alternatives", R"(
Plan for different alternatives in parallel.
Solutions of all children are considered simultaneously.
See :ref:`How-To-Guides <subsubsec-howto-alternatives>` for an example.
)")
.def(py::init<const std::string&>(), "name"_a = std::string("Alternatives"));
py::classh<Fallbacks, ParallelContainerBase>(m, "Fallbacks", R"(
Plan for different alternatives in sequence
Try to find feasible solutions using the children in sequence. The behaviour slightly differs for the indivual stage types:
- Generator: Proceed to next child if currently active one exhausted its solution, i.e. returns ``canCompute() == False``.
- Propagator: Forward an incoming ``InterfaceState`` to the next child if the current one ultimately failed on it.
- Connect: Only ``Connect`` stages are supported. Pairs of ``InterfaceStates`` are forward to the next child on failure of the current child.
See :ref:`How-To-Guides <subsubsec-howto-fallbacks>` for an example.
)")
.def(py::init<const std::string&>(), "name"_a = std::string("Fallbacks"));
py::classh<Merger, ParallelContainerBase>(m, "Merger", R"(
Plan for different sub tasks in parallel and eventually merge all sub solutions into a single trajectory
This requires all children to operate on disjoint ``JointModelGroups``.
See :ref:`How-To-Guides <subsubsec-howto-merger>` for an example.
)")
.def(py::init<const std::string&>(), "name"_a = std::string("merger"));
py::classh<WrapperBase, ParallelContainerBase>(m, "WrapperBase", R"(
Base class for wrapping containers, which can be used to filter or modify solutions generated by the single child.
Implementations of this interface need to implement ``onNewSolution()`` to process a solution generated by the child.
The wrapper may reject the solution or create one or multiple derived solutions, potentially adapting the cost,
the trajectory and output ``InterfaceStates``.
)");
py::classh<Task>(m, "Task", R"(Root stage of a planning pipeline.
A task stage usually wraps a single container (by default ``SerialContainer``) stage.
The class provides methods to ``plan()`` for the configured pipeline and retrieve full solutions.)")
.def(py::init<const std::string&, bool>(), "ns"_a = std::string(), "introspection"_a = true)
.def(py::init<const std::string&, bool, ContainerBase::pointer&&>(), "ns"_a = std::string(),
"introspection"_a = true, "container"_a)
.def_property_readonly("properties", py::overload_cast<>(&Task::properties),
"PropertyMap: PropertyMap of the stage (read-only)")
.def_property_readonly("solutions", &Task::solutions, "Successful Solutions of the stage (read-only)")
.def_property_readonly("failures", &Task::failures, "Solutions: Failed Solutions of the stage (read-only)")
.def_property("name", &Task::name, &Task::setName, "str: name of the task displayed e.g. in rviz")
.def("loadRobotModel", &Task::loadRobotModel, "robot_description"_a = "robot_description",
"Load robot model from given ROS parameter")
.def("setRobotModel", &Task::setRobotModel, "robot_model"_a, "Set the robot model for the task")
.def("getRobotModel", &Task::getRobotModel)
.def("enableIntrospection", &Task::enableIntrospection, "enabled"_a = true,
"Enable publishing intermediate results for inspection in ``rviz``")
.def("clear", &Task::clear, "Reset the stage task (and all its stages)")
.def(
"add",
[](Task& t, const py::args& args) {
for (auto it = args.begin(), end = args.end(); it != end; ++it)
t.add(it->cast<Stage::pointer>());
},
"Append stage(s) to the task's top-level container")
.def("insert", &Task::insert, "stage"_a, "before"_a = -1, "Insert stage before given index")
.def("__len__", [](const Task& t) { t.stages()->numChildren(); })
.def(
"__getitem__",
[](const Task& t, const std::string& name) -> Stage* {
Stage* child = t.stages()->findChild(name);
if (!child)
throw py::index_error();
return child;
},
py::return_value_policy::reference_internal)
.def(
"__getitem__",
[](const Task& t, int idx) -> Stage* {
Stage* child = t.stages()->operator[](idx);
if (!child)
throw py::index_error();
return child;
},
py::return_value_policy::reference_internal)
.def(
"__iter__",
[](const Task& t) {
const auto& children = t.stages()->pimpl()->children();
return py::make_iterator(children.begin(), children.end());
},
py::keep_alive<0, 1>()) // keep container alive as long as iterator lives
.def(
"setCostTerm", [](Task& self, const CostTermConstPtr& c) { self.setCostTerm(c); },
"Specify a CostTerm for calculation of stage costs")
.def(
"setCostTerm", [](Task& self, const LambdaCostTerm::SubTrajectorySignature& f) { self.setCostTerm(f); },
"Specify a function to calculate trajectory costs")
.def(
"setCostTerm", [](Task& self, const LambdaCostTerm::SubTrajectoryShortSignature& f) { self.setCostTerm(f); },
"Specify a function to calculate trajectory costs")
.def("introspection", &Task::introspection, py::return_value_policy::reference_internal,
"Access introspection object")
.def("reset", &Task::reset, "Reset task (and all its stages)")
.def("init", py::overload_cast<>(&Task::init), "Initialize the task (and all its stages)")
.def("plan", &Task::plan, "max_solutions"_a = 0, R"(
Reset, init, and plan. Planning is limited to ``max_allowed_solutions``.
Returns if planning was successful.)")
.def("preempt", &Task::preempt, "Interrupt current planning (or execution)")
.def(
"publish",
[](Task& self, const SolutionBasePtr& solution) { self.introspection().publishSolution(*solution); },
"solution"_a, "Publish the given solution to the ROS topic ``solution``")
.def_static(
"execute",
[](const SolutionBasePtr& solution) {
using namespace moveit::planning_interface;
PlanningSceneInterface psi;
MoveGroupInterface mgi(solution->start()->scene()->getRobotModel()->getJointModelGroupNames()[0]);
MoveGroupInterface::Plan plan;
moveit_task_constructor_msgs::Solution serialized;
solution->toMsg(serialized);
for (const moveit_task_constructor_msgs::SubTrajectory& traj : serialized.sub_trajectory) {
if (!traj.trajectory.joint_trajectory.points.empty()) {
plan.trajectory_ = traj.trajectory;
if (!mgi.execute(plan)) {
ROS_ERROR("Execution failed! Aborting!");
return;
}
}
psi.applyPlanningScene(traj.scene_diff);
}
ROS_INFO("Executed successfully");
},
"solution"_a, "Send given solution to ``move_group`` node for execution");
}
} // namespace python
} // namespace moveit

View File

@ -0,0 +1,110 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2020, Bielefeld University
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Bielefeld University nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
#pragma once
#include <moveit/task_constructor/stage.h>
#include <moveit/task_constructor/container.h>
#include <moveit/task_constructor/cost_queue.h>
#include <moveit/task_constructor/cost_terms.h>
#include <moveit/utils/moveit_error_code.h>
#include <pybind11/pybind11.h>
/** Trampoline classes to allow inheritance in Python (overriding virtual functions) */
namespace moveit {
namespace task_constructor {
class Task;
namespace solvers {
class PlannerInterface;
}
template <class Stage = moveit::task_constructor::Stage>
class PyStage : public Stage, public pybind11::trampoline_self_life_support
{
public:
using Stage::Stage;
void init(const moveit::core::RobotModelConstPtr& robot_model) override {
PYBIND11_OVERRIDE(void, Stage, init, robot_model);
}
void reset() override { PYBIND11_OVERRIDE(void, Stage, reset, ); }
};
template <class Generator = moveit::task_constructor::Generator>
class PyGenerator : public PyStage<Generator>
{
public:
using PyStage<Generator>::PyStage;
bool canCompute() const override { PYBIND11_OVERRIDE_PURE(bool, Generator, canCompute, ); }
void compute() override { PYBIND11_OVERRIDE_PURE(void, Generator, compute, ); }
};
template <class MonitoringGenerator = moveit::task_constructor::MonitoringGenerator>
class PyMonitoringGenerator : public PyGenerator<MonitoringGenerator>
{
public:
using PyGenerator<MonitoringGenerator>::PyGenerator;
void onNewSolution(const SolutionBase& s) override {
// pass solution as pointer to trigger passing by reference
PYBIND11_OVERRIDE_PURE(void, MonitoringGenerator, onNewSolution, &s);
}
};
// Helper class to expose protected member function onNewSolution
// https://pybind11.readthedocs.io/en/stable/advanced/classes.html#binding-protected-member-functions
class PubMonitoringGenerator : public MonitoringGenerator
{
public:
using MonitoringGenerator::onNewSolution;
};
template <class PropagatingEitherWay = moveit::task_constructor::PropagatingEitherWay>
class PyPropagatingEitherWay : public PyStage<PropagatingEitherWay>
{
public:
using PyStage<PropagatingEitherWay>::PyStage;
void computeForward(const InterfaceState& from_state) override {
// pass InterfaceState as pointer to trigger passing by reference
PYBIND11_OVERRIDE_PURE(void, PropagatingEitherWay, computeForward, &from_state);
}
void computeBackward(const InterfaceState& to_state) override {
// pass InterfaceState as pointer to trigger passing by reference
PYBIND11_OVERRIDE_PURE(void, PropagatingEitherWay, computeBackward, &to_state);
}
};
} // namespace task_constructor
} // namespace moveit

View File

@ -0,0 +1,59 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2020, Bielefeld University
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Bielefeld University nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
#include <pybind11/pybind11.h>
namespace moveit {
namespace python {
void export_properties(pybind11::module& m);
void export_solvers(pybind11::module& m);
void export_core(pybind11::module& m);
void export_stages(pybind11::module& m);
} // namespace python
} // namespace moveit
PYBIND11_MODULE(pymoveit_mtc, m) {
auto msub = m.def_submodule("core", "Provides wrappers for core C++ classes. "
"**Import as** :doc:`moveit.task_constructor.core`.");
moveit::python::export_properties(msub);
moveit::python::export_solvers(msub);
moveit::python::export_core(msub);
msub = m.def_submodule("stages", "Provides wrappers of standard MTC stages. "
"**Import as** :doc:`moveit.task_constructor.stages`.");
moveit::python::export_stages(msub);
}

View File

@ -0,0 +1,230 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2020, Bielefeld University
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Bielefeld University nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
#include <moveit/python/task_constructor/properties.h>
#include <boost/core/demangle.hpp>
namespace py = pybind11;
using namespace py::literals;
using namespace moveit::task_constructor;
namespace moveit {
namespace python {
namespace {
/** In order to assign new property values in Python, we need to convert the Python object
* to a boost::any instance of the correct type. As the C++ type cannot be inferred from
* the Python type, we can support this assignment only for a few basic types (see fromPython())
* as well as ROS message types. For other types, a generic assignment via
* stage.properties["property"] = value
* is not possible. Instead, use the .property<Type> declaration on the stage to allow for
* direct assignment like this: stage.property = value
**/
class PropertyConverterRegistry
{
struct Entry
{
PropertyConverterBase::to_python_converter_function to_;
PropertyConverterBase::from_python_converter_function from_;
};
// map from type_index to corresponding converter functions
using RegistryMap = std::map<std::type_index, Entry>;
RegistryMap types_;
// map from ros-msg-names to entry in types_
using RosMsgTypeNameMap = std::map<std::string, RegistryMap::iterator>;
RosMsgTypeNameMap msg_names_;
public:
PropertyConverterRegistry();
inline bool insert(const std::type_index& type_index, const std::string& ros_msg_name,
PropertyConverterBase::to_python_converter_function to,
PropertyConverterBase::from_python_converter_function from);
static py::object toPython(const boost::any& value);
static boost::any fromPython(const py::object& bpo);
};
static PropertyConverterRegistry REGISTRY_SINGLETON;
PropertyConverterRegistry::PropertyConverterRegistry() {
// register property converters
PropertyConverter<bool>();
PropertyConverter<int>();
PropertyConverter<unsigned int>();
PropertyConverter<long>();
PropertyConverter<float>();
PropertyConverter<double>();
PropertyConverter<std::string>();
PropertyConverter<std::set<std::string>>();
PropertyConverter<std::map<std::string, double>>();
}
bool PropertyConverterRegistry::insert(const std::type_index& type_index, const std::string& ros_msg_name,
PropertyConverterBase::to_python_converter_function to,
PropertyConverterBase::from_python_converter_function from) {
auto it_inserted = types_.insert(std::make_pair(type_index, Entry{ to, from }));
if (!it_inserted.second)
return false;
if (!ros_msg_name.empty()) // is this a ROS msg type?
msg_names_.insert(std::make_pair(ros_msg_name, it_inserted.first));
return true;
}
py::object PropertyConverterRegistry::toPython(const boost::any& value) {
if (value.empty())
return py::object();
auto it = REGISTRY_SINGLETON.types_.find(value.type());
if (it == REGISTRY_SINGLETON.types_.end()) {
std::string name = boost::core::demangle(value.type().name());
throw py::type_error("No Python -> C++ conversion for: " + name);
}
return it->second.to_(value);
}
std::string rosMsgName(PyObject* object) {
py::object o = py::reinterpret_borrow<py::object>(object);
try {
return o.attr("_type").cast<std::string>();
} catch (const py::error_already_set& e) {
// object is not a ROS message type, return it's class name instead
return o.attr("__class__").attr("__name__").cast<std::string>();
}
}
boost::any PropertyConverterRegistry::fromPython(const py::object& po) {
PyObject* o = po.ptr();
if (PyBool_Check(o))
return (o == Py_True);
if (PyLong_Check(o))
return PyLong_AS_LONG(o);
if (PyFloat_Check(o))
return PyFloat_AS_DOUBLE(o);
if (PyUnicode_Check(o))
return py::cast<std::string>(o);
const std::string& ros_msg_name = rosMsgName(o);
auto it = REGISTRY_SINGLETON.msg_names_.find(ros_msg_name);
if (it == REGISTRY_SINGLETON.msg_names_.end())
throw py::type_error("No C++ conversion available for (property) type: " + ros_msg_name);
return it->second->second.from_(po);
}
} // end anonymous namespace
bool PropertyConverterBase::insert(const std::type_index& type_index, const std::string& ros_msg_name,
moveit::python::PropertyConverterBase::to_python_converter_function to,
moveit::python::PropertyConverterBase::from_python_converter_function from) {
return REGISTRY_SINGLETON.insert(type_index, ros_msg_name, to, from);
}
__attribute__((visibility("default"))) // export this symbol as visible in the shared library
void export_properties(py::module& m) {
// clang-format off
py::classh<Property>(m, "Property", "Holds an arbitrarily typed value and a default value")
.def(py::init<>())
.def("setValue", [](Property& self, const py::object& value)
{ self.setValue(PropertyConverterRegistry::fromPython(value)); },
"Set current and default value.", "value"_a)
.def("setCurrentValue", [](Property& self, const py::object& value)
{ self.setCurrentValue(PropertyConverterRegistry::fromPython(value)); },
"Set the current value only, w/o touching the default.", "value"_a)
.def("value", [](const Property& self)
{ return PropertyConverterRegistry::toPython(self.value()); }, "Retrieve the stored value.")
.def("defaultValue", [](const Property& self)
{ return PropertyConverterRegistry::toPython(self.defaultValue()); },
"Retrieve the default value.")
.def("reset", &Property::reset, "Reset the value to the stored default.")
.def("defined", &Property::defined, "Was a (non-default) value stored?")
.def("description", &Property::description, "Retrive the property description string")
.def("setDescription", &Property::setDescription,
"Set the property's description", "desc"_a);
py::classh<PropertyMap>(m, "PropertyMap", "Dictionary of named :doc:`properties <pymoveit_mtc.core.Property>`")
.def(py::init<>())
.def("__bool__", [](const PropertyMap& self) { return self.begin() == self.end();})
.def("__iter__", [](PropertyMap& self) { return py::make_key_iterator(self.begin(), self.end()); },
py::keep_alive<0, 1>()) // Essential: keep list alive while iterator exists
.def("items", [](const PropertyMap& self) { return py::make_iterator(self.begin(), self.end()); },
py::keep_alive<0, 1>(), "Retrieve an iterator over the items of the dictionary.")
.def("__contains__", [](const PropertyMap& self, const std::string &key) { return self.hasProperty(key); })
.def("property", [](PropertyMap& self, const std::string& key)
{ return self.property(key); }, py::return_value_policy::reference_internal, R"(
Retrieve the property instance for the given key.
This is in contrast to ``map[key]``, which returns ``map.property(key).value()``.)",
"key"_a)
.def("__getitem__", [](const PropertyMap& self, const std::string& key)
{ return PropertyConverterRegistry::toPython(self.get(key)); })
.def("__setitem__", [](PropertyMap& self, const std::string& key, const py::object& value)
{ self.set(key, PropertyConverterRegistry::fromPython(value)); })
.def("reset", &PropertyMap::reset, "Reset all properties to their default values")
.def("update", [](PropertyMap& self, const py::dict& values) {
for (auto it = values.begin(), end = values.end(); it != end; ++it) {
self.set(it->first.cast<std::string>(),
PropertyConverterRegistry::fromPython(py::reinterpret_borrow<py::object>(it->second)));
}
}, "Update property values from another dictionary", "values"_a)
.def("configureInitFrom", [](PropertyMap& self, Property::SourceFlags sources, const py::list& names) {
std::set<std::string> s;
for (auto& item : names)
s.insert(item.cast<std::string>());
self.configureInitFrom(sources, s);
}, "Configure initialization of listed (or all) properties from given source(s).",
"sources"_a, "names"_a = py::list())
.def("exposeTo", [](PropertyMap& self, PropertyMap& other, const std::string& name) {
self.exposeTo(other, name, name);
}, "Declare ``named`` property in ``other`` PropertyMap - using same name.",
"other"_a, "name"_a)
.def("exposeTo", [](PropertyMap& self, PropertyMap& other, const std::string& name, const std::string& other_name) {
self.exposeTo(other, name, other_name);
}, "Declare ``named`` property in ``other`` PropertyMap - using ``other_name``.",
"other"_a, "name"_a, "other_name"_a)
.def("exposeTo", [](PropertyMap& self, PropertyMap& other, const py::list& names) {
std::set<std::string> s;
for (auto& item : names)
s.insert(item.cast<std::string>());
self.exposeTo(other, s);
}, "Declare `all` ``named`` properties in ``other`` PropertyMap - using the same names.",
"other"_a, "names"_a)
;
// clang-format on
}
} // namespace python
} // namespace moveit

View File

@ -0,0 +1,155 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2020, Bielefeld University
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Bielefeld University nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
#include <moveit/python/task_constructor/properties.h>
#include <moveit/task_constructor/solvers/cartesian_path.h>
#include <moveit/task_constructor/solvers/pipeline_planner.h>
#include <moveit/task_constructor/solvers/joint_interpolation.h>
#include <moveit/task_constructor/solvers/multi_planner.h>
#include <moveit/task_constructor/fmt_p.h>
#include <moveit_msgs/WorkspaceParameters.h>
#include "utils.h"
namespace py = pybind11;
using namespace py::literals;
using namespace moveit::task_constructor;
using namespace moveit::task_constructor::solvers;
namespace moveit {
namespace python {
void export_solvers(py::module& m) {
properties::class_<PlannerInterface>(m, "PlannerInterface", "Abstract base class for planning algorithms")
.property<double>("max_velocity_scaling_factor", "float: Reduce the maximum velocity by scaling between (0,1]")
.property<double>("max_acceleration_scaling_factor",
"float: Reduce the maximum acceleration by scaling between (0,1]")
.property<double>("max_cartesian_speed", "float: maximum cartesian speed")
.property<std::string>("cartesian_speed_limited_link", "str: link with limited cartesian speed")
.def_property_readonly("properties", py::overload_cast<>(&PlannerInterface::properties),
py::return_value_policy::reference_internal, "Properties of the planner");
properties::class_<PipelinePlanner, PlannerInterface>(m, "PipelinePlanner",
R"(Plan using MoveIt's ``PlanningPipeline``
::
from moveit.task_constructor import core
# Create and configure a planner instance
pipelinePlanner = core.PipelinePlanner()
pipelinePlanner.planner = 'PRMkConfigDefault'
pipelinePlanner.num_planning_attempts = 10
)")
.property<std::string>("planner", "str: Planner ID")
.property<uint>("num_planning_attempts", "int: Number of planning attempts")
.property<moveit_msgs::WorkspaceParameters>(
"workspace_parameters",
":moveit_msgs:`WorkspaceParameters`: Specifies workspace box to be used for Cartesian sampling")
.property<double>("goal_joint_tolerance", "float: Tolerance for reaching joint goals")
.property<double>("goal_position_tolerance", "float: Tolerance for reaching position goals")
.property<double>("goal_orientation_tolerance", "float: Tolerance for reaching orientation goals")
.property<bool>("display_motion_plans", "bool: Publish generated solutions via a topic")
.property<bool>("publish_planning_requests", "bool: Publish motion planning requests via a topic")
.def(py::init<const std::string&>(), "pipeline"_a = std::string("ompl"));
properties::class_<JointInterpolationPlanner, PlannerInterface>(
m, "JointInterpolationPlanner",
R"(Perform linear interpolation between joint space poses.
Fails on collision along the interpolation path. There is no obstacle avoidance. ::
from moveit.task_constructor import core
# Instantiate joint-space interpolation planner
jointPlanner = core.JointInterpolationPlanner()
jointPlanner.max_step = 0.1
)")
.property<double>("max_step", "float: Limit any (single) joint change between two waypoints to this amount")
.def(py::init<>());
const moveit::core::CartesianPrecision default_precision;
py::class_<moveit::core::CartesianPrecision>(m, "CartesianPrecision", "precision for Cartesian interpolation")
.def(py::init([](double translational, double rotational, double max_resolution) {
return new moveit::core::CartesianPrecision{ translational, rotational, max_resolution };
}),
py::arg("translational") = default_precision.translational,
py::arg("rotational") = default_precision.rotational,
py::arg("max_resolution") = default_precision.max_resolution)
.def_readwrite("translational", &moveit::core::CartesianPrecision::translational)
.def_readwrite("rotational", &moveit::core::CartesianPrecision::rotational)
.def_readwrite("max_resolution", &moveit::core::CartesianPrecision::max_resolution)
.def("__str__", [](const moveit::core::CartesianPrecision& self) {
return fmt::format("CartesianPrecision(translational={}, rotational={}, max_resolution={}",
self.translational, self.rotational, self.max_resolution);
});
properties::class_<CartesianPath, PlannerInterface>(m, "CartesianPath", R"(
Perform linear interpolation between Cartesian poses.
Fails on collision along the interpolation path. There is no obstacle avoidance. ::
from moveit.task_constructor import core
# Instantiate Cartesian-space interpolation planner
cartesianPlanner = core.CartesianPath()
cartesianPlanner.step_size = 0.01
cartesianPlanner.precision.translational = 0.001
)")
.property<double>("step_size", "float: Limit the Cartesian displacement between consecutive waypoints "
"In contrast to joint-space interpolation, the Cartesian planner can also "
"succeed when only a fraction of the linear path was feasible.")
.property<moveit::core::CartesianPrecision>("precision", "Cartesian interpolation precision")
.property<double>("min_fraction", "float: Fraction of overall distance required to succeed.")
.def(py::init<>());
properties::class_<MultiPlanner, PlannerInterface>(m, "MultiPlanner", R"(
A meta planner that runs multiple alternative planners in sequence and returns the first found solution. ::
from moveit.task_constructor import core
# Instantiate MultiPlanner
multiPlanner = core.MultiPlanner()
)")
.def("__len__", &MultiPlanner::size)
.def("__getitem__", &get_item<MultiPlanner>)
.def(
"add",
[](MultiPlanner& self, const py::args& args) {
for (auto it = args.begin(), end = args.end(); it != end; ++it)
self.push_back(it->cast<PlannerInterfacePtr>());
},
"Insert one or more planners")
.def(
"clear", [](MultiPlanner& self) { self.clear(); }, "Remove all planners")
.def(py::init<>());
}
} // namespace python
} // namespace moveit

View File

@ -0,0 +1,566 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2020, Bielefeld University
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Bielefeld University nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
#include "stages.h"
#include <moveit/python/task_constructor/properties.h>
#include <moveit/task_constructor/stages.h>
#include <moveit/task_constructor/stages/pick.h>
#include <moveit/task_constructor/stages/simple_grasp.h>
#include <moveit/task_constructor/solvers/cartesian_path.h>
#include <moveit/planning_scene/planning_scene.h>
#include <moveit_msgs/PlanningScene.h>
#include <pybind11/stl.h>
#include <py_binding_tools/ros_msg_typecasters.h>
namespace py = pybind11;
using namespace py::literals;
using namespace moveit::task_constructor;
using namespace moveit::task_constructor::stages;
namespace moveit {
namespace python {
namespace {
/// extract from python argument a vector<T>, where arg maybe a single T or a list of Ts
template <typename T>
std::vector<T> elementOrList(const py::object& arg) {
try {
return std::vector<T>{ arg.cast<T>() };
} catch (const py::cast_error&) {
return arg.cast<std::vector<T>>();
}
}
} // anonymous namespace
void export_stages(pybind11::module& m) {
// clang-format off
properties::class_<ModifyPlanningScene, Stage>(m, "ModifyPlanningScene", R"(
Apply modifications to the PlanningScene w/o moving the robot
This stage takes the incoming planning scene and applies previously scheduled changes to it, for example:
* Modify allowed collision matrix, enabling or disabling collision pairs
* Attach or detach objects to robot links
* Add or remove objects
For an example, see :ref:`How-To-Guides <subsubsec-howto-modify-planning-scene>`.
)")
.def(py::init<const std::string&>(), "name"_a = std::string("modify planning scene"))
.def("attachObject", &ModifyPlanningScene::attachObject, "Attach an object to a robot link", "name"_a, "link"_a)
.def("detachObject", &ModifyPlanningScene::detachObject, "Detach an object from a robot link", "name"_a, "link"_a)
.def("attachObjects", [](ModifyPlanningScene& self, const py::object& names,
const std::string& attach_link, bool attach) {
self.attachObjects(elementOrList<std::string>(names), attach_link, attach);
}, "Attach multiple objects to a robot link", "names"_a, "attach_link"_a, "attach"_a = true)
.def("detachObjects", [](ModifyPlanningScene& self, const py::object& names,
const std::string& attach_link) {
self.attachObjects(elementOrList<std::string>(names), attach_link, false);
}, "Detach multiple objects from a robot link", "names"_a, "attach_link"_a)
.def("allowCollisions", [](ModifyPlanningScene& self, const std::string& object, bool allow) {self.allowCollisions(object, allow);},
"Allow or disable all collisions involving the given object", "object"_a, "enable_collision"_a = true)
.def("allowCollisions", [](ModifyPlanningScene& self,
const py::object& first, const py::object& second, bool enable_collision) {
self.allowCollisions(elementOrList<std::string>(first), elementOrList<std::string>(second), enable_collision);
}, "Allow or disable collisions between links and objects", "first"_a, "second"_a, "enable_collision"_a = true)
.def("addObject", &ModifyPlanningScene::addObject, R"(
Add a CollisionObject_ to the planning scene
.. _CollisionObject: https://docs.ros.org/en/noetic/api/moveit_msgs/html/msg/CollisionObject.html
)", "collision_object"_a)
.def("removeObject", &ModifyPlanningScene::removeObject,
"Remove a CollisionObject_ from the planning scene", "name"_a)
;
properties::class_<CurrentState, Stage>(m, "CurrentState", R"(
Fetch the current PlanningScene / real robot state via the ``get_planning_scene`` service.
Usually, this stage should be used *once* at the beginning of your pipeline to start from the current state.
.. literalinclude:: ./../../../demo/scripts/current_state.py
:language: python
)")
.def(py::init<const std::string&>(), "name"_a = std::string("current state"));
properties::class_<FixedState, Stage>(m, "FixedState", R"(
Spawn a pre-defined PlanningScene state.
Take a look at the :ref:`How-To-Guides <subsubsec-howto-fixed-state>`
for an implementation of a task hierarchy that makes use of the
``FixedState`` stage.
)")
.def("setState", &FixedState::setState, R"(
Use a planning scene pointer to specify which state the Fixed State
stage should have.
)", "scene"_a)
.def(py::init<const std::string&>(), "name"_a = std::string("fixed state"));
#if 0
.def("setState", [](FixedState& stage, const moveit_msg::PlanningScene& scene_msg) {
// TODO: How to initialize the PlanningScene?
planning_scene::PlanningScenePtr scene;
scene->setPlanningSceneMsg(scene_msg);
stage.setState(scene);
})
#endif
;
properties::class_<ComputeIK, Stage>(m, "ComputeIK", R"(
Wrapper for any pose generator stage to compute the inverse
kinematics for a pose in Cartesian space.
The wrapper reads a ``target_pose`` from the interface state of
solutions provided by the wrapped stage. This cartesian pose
(``PoseStamped`` msg) is used as a goal pose for inverse
kinematics.
Usually, the end effector's parent link or the group's tip link
is used as the inverse kinematics frame, which should be
moved to the goal frame. However, any other inverse kinematics
frame can be defined (which is linked to the tip of the group).
Properties of the internally received ``InterfaceState`` can be
forwarded to the newly generated, externally exposed ``InterfaceState``.
Take a look at the :ref:`How-To-Guides <subsubsec-howto-compute-ik>`
for an implementation of a task hierarchy that makes use of the
``ComputeIK`` stage.
)")
.property<std::string>("eef", R"(
str: Specify which end effector of the active planning group
should be used.
)")
.property<std::string>("group", R"(
str: Specify which planning group
should be used.
)")
.property<std::string>("default_pose", R"(
str: Default joint pose of the active group
(defines cost of the inverse kinematics).
)")
.property<uint32_t>("max_ik_solutions", "uint: max number of solutions to return")
.property<bool>("ignore_collisions", R"(
bool: Specify if collisions with other members of
the planning scene are allowed.
)")
.property<double>("min_solution_distance", "reject solution that are closer than this to previously found solutions")
.property<moveit_msgs::Constraints>("constraints", "additional constraints to obey")
.property<geometry_msgs::PoseStamped>("ik_frame", R"(
PoseStamped_: Specify the frame with respect
to which the inverse kinematics
should be calculated.
.. _PoseStamped: https://docs.ros.org/en/api/geometry_msgs/html/msg/PoseStamped.html
)")
.property<geometry_msgs::PoseStamped>("target_pose", R"(
PoseStamped_: Specify the pose on which
the inverse kinematics should be
calculated on. Since this property should
almost always be set
in the Interface State which is sent by the child,
if possible, avoid setting it manually.
.. _PoseStamped: https://docs.ros.org/en/api/geometry_msgs/html/msg/PoseStamped.html
)")
// methods of base class py::class_ need to be called last!
.def(py::init<const std::string&, Stage::pointer&&>(), "name"_a, "stage"_a);
properties::class_<MoveTo, PropagatingEitherWay, PyMoveTo<>>(m, "MoveTo", R"(
Compute a trajectory between the robot state from the
interface state of the preceeding stage and a specified
goal.
Take a look at the :ref:`How-To-Guides <subsubsec-howto-move-to>`
for an implementation of a task hierarchy that makes use of the
``MoveTo`` stage.
)")
.property<std::string>("group", R"(
str: Planning group which should be utilized for planning and execution.
)")
.property<geometry_msgs::PoseStamped>("ik_frame", R"(
PoseStamped_: IK reference frame for the goal pose
.. _PoseStamped: https://docs.ros.org/en/api/geometry_msgs/html/msg/PoseStamped.html
)")
.property<moveit_msgs::Constraints>("path_constraints", R"(
Constraints_: Set path constraints via the corresponding moveit message type
.. _Constraints: https://docs.ros.org/en/api/moveit_msgs/html/msg/Constraints.html
)")
.def(py::init<const std::string&, const solvers::PlannerInterfacePtr&>(), "name"_a, "planner"_a)
.def("setGoal", py::overload_cast<const geometry_msgs::PoseStamped&>(&MoveTo::setGoal), R"(
Move link to a given PoseStamped_
.. _PoseStamped: https://docs.ros.org/en/api/geometry_msgs/html/msg/PoseStamped.html
)", "goal"_a)
.def("setGoal", py::overload_cast<const geometry_msgs::PointStamped&>(&MoveTo::setGoal), R"(
Move link to given PointStamped_, keeping current orientation
.. _PointStamped: https://docs.ros.org/en/api/geometry_msgs/html/msg/PointStamped.html
)", "goal"_a)
.def("setGoal", py::overload_cast<const moveit_msgs::RobotState&>(&MoveTo::setGoal), R"(
Move joints specified in RobotState_ to their target values
.. _RobotState: https://docs.ros.org/en/noetic/api/moveit_msgs/html/msg/RobotState.html
)", "goal"_a)
.def("setGoal", py::overload_cast<const std::map<std::string, double>&>(&MoveTo::setGoal), R"(
Move joints by name to their mapped target value provided by dict goal argument
)", "goal"_a)
.def("setGoal", py::overload_cast<const std::string&>(&MoveTo::setGoal), R"(
Move joint model group to given named pose provided as a str argument
)", "goal"_a);
properties::class_<MoveRelative, PropagatingEitherWay, PyMoveRelative<>>(m, "MoveRelative", R"(
Perform a Cartesian motion relative to some link.
Take a look at the :ref:`How-To-Guides <subsubsec-howto-move-relative>`
for an implementation of a task hierarchy that makes use of the
``MoveRelative`` stage.
To implement your own propagtor logic on top of the ``MoveRelative`` class' functionality,
you may derive from the stage. Take a look at the corresponding
:ref:`How-To-Guide <subsubsec-howto-move-relative>`.
)")
.property<std::string>("group", R"(
str: Planning group which should be utilized for planning and execution.
)")
.property<geometry_msgs::PoseStamped>("ik_frame", R"(
PoseStamped_: IK reference frame for the goal pose.
.. _PoseStamped: https://docs.ros.org/en/api/geometry_msgs/html/msg/PoseStamped.html
)")
.property<double>("min_distance", "float: Set the minimum distance to move")
.property<double>("max_distance", "float: Set the maximum distance to move")
.property<moveit_msgs::Constraints>("path_constraints", R"(
Constraints_: These are the path constraints.
.. _Constraints: https://docs.ros.org/en/api/moveit_msgs/html/msg/Constraints.html
)")
.def(py::init<const std::string&, const solvers::PlannerInterfacePtr&>(), "name"_a, "planner"_a)
.def("setDirection", py::overload_cast<const geometry_msgs::TwistStamped&>(&MoveRelative::setDirection), R"(
Perform twist motion on specified link.
.. _Twist: https://docs.ros.org/en/api/geometry_msgs/html/msg/Twist.html
)", "twist"_a)
.def("setDirection", py::overload_cast<const geometry_msgs::Vector3Stamped&>(&MoveRelative::setDirection), R"(
Translate link along given direction.
.. _Vector3Stamped: https://docs.ros.org/en/noetic/api/geometry_msgs/html/msg/Vector3Stamped.html
)", "direction"_a)
.def("setDirection", py::overload_cast<const std::map<std::string, double>&>(&MoveRelative::setDirection), R"(
Move specified joint variables by given amount.
)", "joint_deltas"_a);
py::enum_<stages::Connect::MergeMode>(m, "MergeMode", R"(
Define the merge strategy to use when performing planning operations
with e.g. the connect stage.
)")
.value("SEQUENTIAL", stages::Connect::MergeMode::SEQUENTIAL, "Store sequential trajectories")
.value("WAYPOINTS", stages::Connect::MergeMode::WAYPOINTS, "Join trajectories by their waypoints");
PropertyConverter<stages::Connect::MergeMode>();
properties::class_<Connect, Stage>(m, "Connect", R"(
Connect arbitrary InterfaceStates by motion planning.
You can specify the planning groups and the planners you
want to utilize.
The states may differ in various planning groups.
To connect both states, the planners provided for
individual sub groups are applied in the specified order.
Each planner only plan for joints within the corresponding
planning group. Finally, an attempt is made to merge the
sub trajectories of individual planning results.
If this fails, the sequential planning result is returned.
For an example, see :ref:`How-To-Guides <subsubsec-howto-connect>`.
)")
.property<stages::Connect::MergeMode>("merge_mode", "Defines the merge strategy to use")
.property<double>("max_distance", "maximally accepted distance between end and goal sate")
.property<moveit_msgs::Constraints>("path_constraints", R"(
Constraints_: These are the path constraints.
.. _Constraints: https://docs.ros.org/en/api/moveit_msgs/html/msg/Constraints.html
)")
.def(py::init<const std::string&, const Connect::GroupPlannerVector&>(),
"name"_a = std::string("connect"), "planners"_a);
properties::class_<FixCollisionObjects, Stage>(m, "FixCollisionObjects", R"(
Test for collisions and find a correction for applicable objects.
Move the objects out of the way along the correction direction.
Take a look at the :ref:`How-To-Guides <subsubsec-howto-fix-collision-objects>`
for an implementation of a task hierarchy that makes use of the
``FixCollisionObjects`` stage.
)")
.property<double>("max_penetration", R"(
float: Cutoff length up to which collision objects get fixed.
)")
.def(py::init<const std::string&>(), "name"_a = std::string("fix collisions"));
properties::class_<GeneratePlacePose, MonitoringGenerator>(m, "GeneratePlacePose", R"(
GeneratePlacePose stage derives from monitoring generator and generates poses
for the place pipeline. Notice that whilst GenerateGraspPose spawns poses with an
``angle_delta`` intervall, GeneratePlacePose samples a fixed amount, which is dependent
on the objects shape.
Take a look at the :ref:`How-To-Guides <subsubsec-howto-generate-place-pose>`
for a snippet that demonstrates usage of the `GeneratePlacePose` stage.
)")
.property<std::string>("object", R"(
str: Name of the object in the planning scene, attached to the robot which should be placed
)")
.property<std::string>("eef", "str: Name of the end effector that should be used for grasping")
.property<geometry_msgs::PoseStamped>("pose", R"(
PoseStamped_: The pose where the object should be placed, i.e. states should be sampled
.. _PoseStamped: https://docs.ros.org/en/api/geometry_msgs/html/msg/PoseStamped.html
)")
.def(py::init<const std::string&>(), "name"_a = std::string("Generate Place Pose"));
properties::class_<GenerateGraspPose, MonitoringGenerator>(m, "GenerateGraspPose", R"(
GenerateGraspPose stage derives from monitoring generator and can
be used to generate poses for grasping. Set the desired attributes
of the grasp using the stages properties.
Take a look at the :ref:`How-To-Guides <subsubsec-howto-generate-grasp-pose>`
for a snippet that demonstrates usage of the `GenerateGraspPose` stage.
)")
.property<std::string>("object", R"(
str: Name of the Object in the planning scene, which should be grasped
)")
.property<std::string>("eef", R"(
str: Name of the end effector that should be used for grasping
)")
.property<std::string>("pregrasp", "str: Name of the pre-grasp pose")
.property<std::string>("grasp", "str: Name of the grasp pose")
.property<double>("angle_delta", R"(
float: Angular step distance in rad with which positions around the object are sampled.
)")
.def(py::init<const std::string&>(), "name"_a = std::string("Generate Grasp Pose"));
properties::class_<GeneratePose, MonitoringGenerator>(m, "GeneratePose", R"(
Monitoring generator stage which can be used to generate a pose, based on solutions provided
by the monitored stage.
Take a look at the :ref:`How-To-Guides <subsubsec-howto-fix-collision-objects>`
for an implementation of a task hierarchy that makes use of the
``GeneratePose`` stage.
)")
.property<geometry_msgs::PoseStamped>("pose", R"(
PoseStamped_: Set the pose, which should be spawned on each new solution of the monitored stage.
.. _PoseStamped: https://docs.ros.org/en/api/geometry_msgs/html/msg/PoseStamped.html
)")
.def(py::init<const std::string&>(), "name"_a);
py::enum_<GenerateRandomPose::PoseDimension>(m, "PoseDimension",
R"(Define the dimensions of a pose that can be randomized.)")
.value("X", GenerateRandomPose::PoseDimension::X, "X dimension")
.value("Y", GenerateRandomPose::PoseDimension::Y, "Y dimension")
.value("Z", GenerateRandomPose::PoseDimension::Z, "Z dimension")
.value("ROLL", GenerateRandomPose::PoseDimension::ROLL, "Roll dimension")
.value("PITCH", GenerateRandomPose::PoseDimension::PITCH, "Pitch dimension")
.value("YAW", GenerateRandomPose::PoseDimension::YAW, "Yaw dimension");
properties::class_<GenerateRandomPose, GeneratePose>(m, "GenerateRandomPose", R"(
Monitoring generator stage which can be used to generate random poses, based on solutions provided
by the monitored stage and the specified pose dimension samplers.
)")
.def(py::init<const std::string&>(), "name"_a)
.def("set_max_solutions", &GenerateRandomPose::setMaxSolutions, "max_solutions"_a)
.def("sample_dimension", [](GenerateRandomPose& self, const GenerateRandomPose::PoseDimension pose_dimension,
const double width) {
self.sampleDimension<std::uniform_real_distribution>(pose_dimension, width);
}, "pose_dimension"_a, "width"_a);
properties::class_<Pick, SerialContainer>(m, "Pick", R"(
The Pick stage is a specialization of the PickPlaceBase class, which
wraps the pipeline to pick or place an object with a given end effector.
Picking consist of the following sub stages:
- Linearly approaching the object along an approach direction/twist "grasp" end effector posture
- Attach the object
- Lift along a given direction/twist
The end effector postures corresponding to pre-grasp and grasp as well
as the end effector's cartesian pose needs to be provided by an external
grasp stage.
Take a look at the :ref:`Pick and Place Tutorial <subsec-tut-pick-place>` for an in-depth look,
as well as the :ref:`How-To Guide <subsubsec-howto-pick>` for a minimal implementation
of a task hierarchy that makes use of the
``Pick`` stage.
)")
.property<std::string>("object", "str: Name of object to pick")
.property<std::string>("eef", "str: The End effector name")
.property<std::string>("eef_frame", "str: Name of the end effector frame")
.property<std::string>("eef_group", "str: Joint model group of the end effector")
.property<std::string>("eef_parent_group", "str: Joint model group of the eef's parent")
.def(py::init<Stage::pointer&&, const std::string&>(), "grasp_generator"_a,
"name"_a = std::string("pick"))
.def_property_readonly("cartesian_solver", &Pick::cartesianSolver)
.def("setApproachMotion", &Pick::setApproachMotion, R"(
The approaching motion towards the grasping state is represented
by a twist message.
Additionally specify the minimum and maximum allowed distances to travel.
.. _Twist: https://docs.ros.org/en/api/geometry_msgs/html/msg/Twist.html
)", "motion"_a, "min_distance"_a, "max_distance"_a)
.def("setLiftMotion", py::overload_cast<const geometry_msgs::TwistStamped&, double, double>(&Pick::setLiftMotion), R"(
The lifting motion away from the grasping state is represented by a twist message.
Additionally specify the minimum and maximum allowed distances to travel.
.. _Twist: https://docs.ros.org/en/api/geometry_msgs/html/msg/Twist.html
)", "motion"_a, "min_distance"_a, "max_distance"_a)
.def("setLiftMotion", py::overload_cast<const std::map<std::string, double>&>(&Pick::setLiftMotion), R"(
The lifting motion away from the grasping state is represented by its destination as joint-value pairs
)", "place"_a);
properties::class_<Place, SerialContainer>(m, "Place", R"(
The Place stage is a specialization of the PickPlaceBase class, which
wraps the pipeline to pick or place an object with a given end effector.
Placing consist of the inverse order of stages:
- Place down along a given direction
- Detach the object
- Linearly retract end effector
The end effector postures corresponding to pre-grasp and grasp as well
as the end effector's Cartesian pose needs to be provided by an external
grasp stage.
Take a look at the :ref:`Pick and Place Tutorial <subsec-tut-pick-place>` for an in-depth look,
as well as the :ref:`How-To Guide <subsubsec-howto-place>` for a minimal implementation
of a task hierarchy that makes use of the
``Place`` stage.
)")
.property<std::string>("object", "str: Name of object to pick")
.property<std::string>("eef", "str: The End effector name")
.property<std::string>("eef_frame", "str: Name of the end effector frame")
.property<std::string>("eef_group", "str: Joint model group of the end effector")
.property<std::string>("eef_parent_group", "str: Joint model group of the eef's parent")
.def("setRetractMotion", &Place::setRetractMotion, R"(
The retract motion towards the final state is represented
by a Twist_ message. Additionally specify the minimum and
maximum allowed distances to travel.
.. _Twist: https://docs.ros.org/en/api/geometry_msgs/html/msg/Twist.html
)", "motion"_a, "min_distance"_a, "max_distance"_a)
.def("setPlaceMotion", py::overload_cast<const geometry_msgs::TwistStamped&, double, double>(&Place::setPlaceMotion), R"(
The object-placing motion towards the final state is represented by a twist message.
Additionally specify the minimum and maximum allowed distances to travel.
.. _Twist: https://docs.ros.org/en/api/geometry_msgs/html/msg/Twist.html
)", "motion"_a, "min_distance"_a, "max_distance"_a )
.def("setPlaceMotion", py::overload_cast<const std::map<std::string, double>&>(&Place::setPlaceMotion), R"(
The placing motion to the final state is represented by its destination as joint-value pairs
)", "joints"_a )
.def(py::init<Stage::pointer&&, const std::string&>(), "place_generator"_a,
"name"_a = std::string("place"));
properties::class_<SimpleGraspBase, SerialContainer>(m, "SimpleGraspBase", "Abstract base class for grasping and releasing")
.property<std::string>("eef", "str: The end effector of the robot")
.property<std::string>("object", "str: The object to grasp (Must be present in the planning scene)")
.property<geometry_msgs::PoseStamped>("ik_frame", R"(
PoseStamped_: Set the frame for which the inverse kinematics is calculated
with respect to each pose generated by the pose_generator.
.. _PoseStamped: https://docs.ros.org/en/api/geometry_msgs/html/msg/PoseStamped.html
)")
.def<void (SimpleGraspBase::*)(const geometry_msgs::PoseStamped&)>("setIKFrame", &SimpleGraspBase::setIKFrame, R"(
Set the frame as a PoseStamped_ for which the inverse kinematics are calculated with respect to
each pose generated by the pose_generator.
.. _PoseStamped: https://docs.ros.org/en/api/geometry_msgs/html/msg/PoseStamped.html
)", "transform"_a)
.def<void (SimpleGraspBase::*)(const Eigen::Isometry3d&, const std::string&)>("setIKFrame", &SimpleGraspBase::setIKFrame, R"(
Set the frame as a PoseStamped_ for which the inverse kinematics are calculated
with respect to each pose generated by the pose_generator.
.. _PoseStamped: https://docs.ros.org/en/api/geometry_msgs/html/msg/PoseStamped.html
)", "pose"_a, "link"_a)
.def<void (SimpleGraspBase::*)(const std::string&)>("setIKFrame", &SimpleGraspBase::setIKFrame, R"(
Set the frame for which the inverse kinematics are calculated
with respect to each pose generated by the pose_generator.
)", "link"_a)
.def("setMaxIKSolutions", &SimpleGraspBase::setMaxIKSolutions, R"(
Set the maximum number of inverse kinematics solutions that should be computed.
)", "max_ik_solutions"_a)
;
properties::class_<SimpleGrasp, SimpleGraspBase>(m, "SimpleGrasp", R"(
Specialization of SimpleGraspBase to realize grasping.
Take a look at the :ref:`Pick and Place Tutorial <subsec-tut-pick-place>` for an in-depth look,
as well as the :ref:`How-To Guide <subsubsec-howto-simplegrasp>` for a minimal implementation
of a task hierarchy that makes use of the
``SimpleGrasp`` stage.
)")
.def(py::init<Stage::pointer&&, const std::string&>(), "pose_generator"_a,
"name"_a = std::string("grasp"));
properties::class_<SimpleUnGrasp, SimpleGraspBase>(m, "SimpleUnGrasp", R"(
Specialization of SimpleGraspBase to realize ungrasping
Take a look at the :ref:`Pick and Place Tutorial <subsec-tut-pick-place>` for an in-depth look,
as well as the :ref:`How-To Guide <subsubsec-howto-simplegrasp>` for a minimal implementation
of a task hierarchy that makes use of the
``SimpleUnGrasp`` stage.
)")
.property<std::string>("pregrasp", "str: Name of the pre-grasp pose")
.property<std::string>("grasp", "str: Name of the grasp pose")
.def(py::init<Stage::pointer&&, const std::string&>(), "pose_generator"_a,
"name"_a = std::string("ungrasp"));
properties::class_<PassThrough, Stage>(m, "PassThrough", R"(
Wrapper which simply forwards all solutions of a child stage,
allowing the wrapper to redefine costs, w/o loosing original costs.
)")
.def(py::init<const std::string&, Stage::pointer&&>(), "name"_a, "stage"_a);
properties::class_<LimitSolutions, Stage>(m, "LimitSolutions", R"(
Wrapper for any stage to limit the total number of solutions returned.
The wrapper stores solutions of its child stage, and on each compute will
pass on the lowest cost solution available, until the maximum number of solutions
is reached.
)")
.property<uint32_t>("max_solutions", "uint: maximum number of solutions that should be passed on")
.def(py::init<const std::string&, Stage::pointer&&>(), "name"_a, "stage"_a);
}
} // namespace python
} // namespace moveit

View File

@ -0,0 +1,84 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2020, Bielefeld University
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Bielefeld University nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
#pragma once
#include "core.h"
#include <moveit/task_constructor/stages/move_relative.h>
#include <moveit/task_constructor/stages/move_to.h>
/** Trampoline classes to allow inheritance in Python (overriding virtual functions) */
namespace moveit {
namespace task_constructor {
namespace stages {
template <class T = MoveTo>
class PyMoveTo : public PyPropagatingEitherWay<T>
{
public:
using PyPropagatingEitherWay<T>::PyPropagatingEitherWay;
void computeForward(const InterfaceState& from_state) override {
// pass InterfaceState as pointer to trigger passing by reference
PYBIND11_OVERRIDE_IMPL(void, T, "computeForward", &from_state);
return T::computeForward(from_state);
}
void computeBackward(const InterfaceState& to_state) override {
// pass InterfaceState as pointer to trigger passing by reference
PYBIND11_OVERRIDE_IMPL(void, T, "computeBackward", &to_state);
return T::computeBackward(to_state);
}
};
template <class T = MoveRelative>
class PyMoveRelative : public PyPropagatingEitherWay<T>
{
public:
using PyPropagatingEitherWay<T>::PyPropagatingEitherWay;
void computeForward(const InterfaceState& from_state) override {
// pass InterfaceState as pointer to trigger passing by reference
PYBIND11_OVERRIDE_IMPL(void, T, "computeForward", &from_state);
return T::computeForward(from_state);
}
void computeBackward(const InterfaceState& to_state) override {
// pass InterfaceState as pointer to trigger passing by reference
PYBIND11_OVERRIDE_IMPL(void, T, "computeBackward", &to_state);
return T::computeBackward(to_state);
}
};
} // namespace stages
} // namespace task_constructor
} // namespace moveit

View File

@ -0,0 +1,26 @@
#include <pybind11/pybind11.h>
namespace moveit {
namespace python {
namespace {
// utility function to normalize index: negative indeces reference from the end
size_t normalize_index(size_t size, long index) {
if (index < 0)
index += size;
if (index >= long(size) || index < 0)
throw pybind11::index_error("Index out of range");
return index;
}
// implement operator[](index)
template <typename T>
typename T::value_type get_item(const T& container, long index) {
auto it = container.begin();
std::advance(it, normalize_index(container.size(), index));
return *it;
}
} // namespace
} // namespace python
} // namespace moveit

View File

@ -0,0 +1,2 @@
# https://packaging.python.org/guides/packaging-namespace-packages/#pkgutil-style-namespace-packages
__path__ = __import__("pkgutil").extend_path(__path__, __name__)

View File

@ -0,0 +1,3 @@
from . import core, stages
__doc__ = "top-level module of MoveIt Task constructor"

View File

@ -0,0 +1,3 @@
from pymoveit_mtc.core import *
__doc__ = "Provides wrappers for :doc:`core C++ classes <pymoveit_mtc.core>`."

View File

@ -0,0 +1,3 @@
from pymoveit_mtc.stages import *
__doc__ = "Provides wrappers for :doc:`stage C++ classes <pymoveit_mtc.stages>`."

119
core/python/test/rostest_mps.py Executable file
View File

@ -0,0 +1,119 @@
#! /usr/bin/env python3
import unittest
import rostest
from py_binding_tools import roscpp_init
from moveit_commander import PlanningSceneInterface
from moveit.task_constructor import core, stages
from geometry_msgs.msg import PoseStamped
def setUpModule():
roscpp_init("test_mtc")
def make_pose(x, y, z):
pose = PoseStamped()
pose.header.frame_id = "base_link"
pose.pose.position.x = x
pose.pose.position.y = y
pose.pose.position.z = z
pose.pose.orientation.w = 1.0
return pose
class TestModifyPlanningScene(unittest.TestCase):
PLANNING_GROUP = "manipulator"
def setUp(self):
super(TestModifyPlanningScene, self).setUp()
self.psi = PlanningSceneInterface(synchronous=True)
self.make_box = self.psi.make_box
# insert a box to collide with
self.psi.add_box("box", make_pose(0.8, 0.25, 1.25), [0.2, 0.2, 0.2])
self.task = task = core.Task()
task.add(stages.CurrentState("current"))
def insertMove(self, position=-1):
# colliding motion
move = stages.MoveRelative("move", core.JointInterpolationPlanner())
move.group = self.PLANNING_GROUP
move.setDirection({"joint_1": 0.3})
self.task.insert(move, position)
def test_collision(self):
self.insertMove()
self.assertFalse(self.task.plan())
def test_allow_collision_list(self):
mps = stages.ModifyPlanningScene("allow specific collisions for box")
mps.allowCollisions("box", ["link_4", "link_5", "link_6"], True)
self.task.add(mps)
self.insertMove()
self.assertTrue(self.task.plan())
def test_allow_collision_all(self):
# insert an extra collision object that is unknown to ACM
self.psi.add_box("block", make_pose(0.8, 0.55, 1.25), [0.2, 0.2, 0.2])
# attach box to end effector
self.psi.attach_box("link_6", "box")
self.insertMove()
self.assertFalse(self.task.plan())
# allow all collisions for attached "box" object
mps = stages.ModifyPlanningScene("allow all collisions for box")
mps.allowCollisions("box", True)
self.task.insert(mps, 1)
self.assertTrue(self.task.plan())
# restore original state
self.psi.remove_attached_object("link_6", "box")
self.psi.remove_world_object("block")
def test_fw_add_object(self):
mps = stages.ModifyPlanningScene("addObject(block)")
mps.addObject(self.make_box("block", make_pose(0.8, 0.55, 1.25), [0.2, 0.2, 0.2]))
self.task.add(mps)
self.insertMove()
self.assertFalse(self.task.plan())
def test_fw_remove_object(self):
mps = stages.ModifyPlanningScene("removeObject(box)")
mps.removeObject("box")
self.task.insert(mps, 1)
self.assertTrue(self.task.plan())
s = self.task.solutions[0].toMsg()
self.assertEqual(s.sub_trajectory[1].scene_diff.world.collision_objects[0].id, "box")
def test_bw_add_object(self):
# add object to move_group's planning scene
self.psi.add_box("block", make_pose(0.8, 0.55, 1.25), [0.2, 0.2, 0.2])
# backward operation will actually remove the object
mps = stages.ModifyPlanningScene("addObject(block) backwards")
mps.addObject(self.make_box("block", make_pose(0.8, 0.55, 1.25), [0.2, 0.2, 0.2]))
self.task.insert(mps, 0)
self.insertMove(0)
self.assertTrue(self.task.plan())
self.psi.remove_world_object("block") # restore original state
s = self.task.solutions[0].toMsg()
# block shouldn't be in start scene
objects = [o.id for o in s.start_scene.world.collision_objects]
self.assertTrue(objects == ["box"])
# only addObject(block) should add it
objects = [o.id for o in s.sub_trajectory[1].scene_diff.world.collision_objects]
self.assertTrue(objects == ["block", "box"])
def test_bw_remove_object(self):
mps = stages.ModifyPlanningScene("removeObject(box) backwards")
mps.removeObject("box")
self.task.insert(mps, 0)
self.insertMove(0)
self.assertFalse(self.task.plan())
if __name__ == "__main__":
rostest.rosrun("mtc", "mps", TestModifyPlanningScene)

60
core/python/test/rostest_mtc.py Executable file
View File

@ -0,0 +1,60 @@
#! /usr/bin/env python3
import unittest
import rostest
from py_binding_tools import roscpp_init
from moveit.task_constructor import core, stages
from geometry_msgs.msg import PoseStamped
from geometry_msgs.msg import Vector3Stamped, Vector3
from std_msgs.msg import Header
def setUpModule():
roscpp_init("test_mtc")
class Test(unittest.TestCase):
PLANNING_GROUP = "manipulator"
def test_MoveAndExecute(self):
moveRel = stages.MoveRelative("moveRel", core.JointInterpolationPlanner())
moveRel.group = self.PLANNING_GROUP
moveRel.setDirection({"joint_1": 0.2, "joint_2": 0.4})
moveTo = stages.MoveTo("moveTo", core.JointInterpolationPlanner())
moveTo.group = self.PLANNING_GROUP
moveTo.setGoal("all-zeros")
task = core.Task()
task.add(stages.CurrentState("current"), moveRel, moveTo)
self.assertTrue(task.plan())
self.assertEqual(len(task.solutions), 1)
task.execute(task.solutions[0])
def test_Merger(self):
cartesian = core.CartesianPath()
def createDisplacement(group, displacement):
move = stages.MoveRelative("displace", cartesian)
move.group = group
move.ik_frame = PoseStamped(header=Header(frame_id="tool0"))
dir = Vector3Stamped(header=Header(frame_id="base_link"), vector=Vector3(*displacement))
move.setDirection(dir)
move.restrictDirection(stages.MoveRelative.Direction.FORWARD)
return move
task = core.Task()
task.add(stages.CurrentState("current"))
merger = core.Merger("merger")
merger.insert(createDisplacement(self.PLANNING_GROUP, [-0.2, 0, 0]))
merger.insert(createDisplacement(self.PLANNING_GROUP, [0.2, 0, 0]))
task.add(merger)
task.enableIntrospection()
task.init()
self.assertFalse(task.plan())
if __name__ == "__main__":
rostest.rosrun("mtc", "base", Test)

View File

@ -0,0 +1,6 @@
<launch>
<include file="$(find moveit_resources_fanuc_moveit_config)/launch/test_environment.launch" />
<test pkg="moveit_task_constructor_core" type="rostest_mtc.py" test-name="rostest_mtc" time-limit="60" args="" />
<test pkg="moveit_task_constructor_core" type="rostest_mps.py" test-name="rostest_mps" time-limit="60" args="" />
<test pkg="moveit_task_constructor_core" type="rostest_trampoline.py" test-name="rostest_trampoline" time-limit="60" args="" />
</launch>

View File

@ -0,0 +1,132 @@
#! /usr/bin/env python3
# -*- coding: utf-8 -*-
import unittest
import rostest
from py_binding_tools import roscpp_init
from moveit.task_constructor import core, stages
from moveit.core.planning_scene import PlanningScene
from geometry_msgs.msg import Vector3Stamped, Vector3, PoseStamped
from std_msgs.msg import Header
PLANNING_GROUP = "manipulator"
def setUpModule():
roscpp_init("test_mtc")
class PyGenerator(core.Generator):
"""Implements a custom 'Generator' stage."""
max_calls = 3
def __init__(self, name="Generator"):
core.Generator.__init__(self, name)
self.reset()
def init(self, robot_model):
self.ps = PlanningScene(robot_model)
def reset(self):
core.Generator.reset(self)
self.num = self.max_calls
def canCompute(self):
return self.num > 0
def compute(self):
self.num = self.num - 1
self.spawn(core.InterfaceState(self.ps), self.num)
class PyMonitoringGenerator(core.MonitoringGenerator):
"""Implements a custom 'MonitoringGenerator' stage."""
solution_multiplier = 2
def __init__(self, name="MonitoringGenerator"):
core.MonitoringGenerator.__init__(self, name)
self.reset()
def reset(self):
core.MonitoringGenerator.reset(self)
self.upstream_solutions = list()
def onNewSolution(self, sol):
self.upstream_solutions.append(sol)
def canCompute(self):
return bool(self.upstream_solutions)
def compute(self):
scene = self.upstream_solutions.pop(0).end.scene
for i in range(self.solution_multiplier):
self.spawn(core.InterfaceState(scene), i)
class PyMoveRelX(stages.MoveRelative):
"""Implements a custom propagator stage."""
def __init__(self, x, planner, name="Move ±x"):
stages.MoveRelative.__init__(self, name, planner)
self.group = PLANNING_GROUP
self.ik_frame = PoseStamped(header=Header(frame_id="tool0"))
self.setDirection(
Vector3Stamped(header=Header(frame_id="base_link"), vector=Vector3(x, 0, 0))
)
def computeForward(self, from_state):
return stages.MoveRelative.computeForward(self, from_state)
def computeBackward(self, to_state):
return stages.MoveRelative.computeBackward(self, to_state)
class TestTrampolines(unittest.TestCase):
def setUp(self):
self.cartesian = core.CartesianPath()
self.jointspace = core.JointInterpolationPlanner()
def create(self, *stages):
task = core.Task()
task.enableIntrospection()
for stage in stages:
task.add(stage)
return task
def plan(self, task, expected_solutions=None, wait=False):
try:
task.plan()
except TypeError as e:
self.fail(f"{e}\nMoveIt and MTC use ABI-incompatible pybind11 versions")
if expected_solutions is not None:
self.assertEqual(len(task.solutions), expected_solutions)
if wait:
input("Waiting for any key (allows inspection in rviz)")
def test_generator(self):
task = self.create(PyGenerator())
self.plan(task, expected_solutions=PyGenerator.max_calls)
def test_monitoring_generator(self):
task = self.create(
stages.CurrentState("current"),
stages.Connect(planners=[(PLANNING_GROUP, self.jointspace)]),
PyMonitoringGenerator("generator"),
)
task["generator"].setMonitoredStage(task["current"])
self.plan(task, expected_solutions=PyMonitoringGenerator.solution_multiplier)
def test_propagator(self):
task = self.create(
PyMoveRelX(-0.2, self.cartesian),
stages.CurrentState(),
PyMoveRelX(+0.2, self.cartesian),
)
self.plan(task, expected_solutions=1)
if __name__ == "__main__":
rostest.rosrun("mtc", "trampoline", TestTrampolines)

View File

@ -0,0 +1,345 @@
#! /usr/bin/env python3
# -*- coding: utf-8 -*-
import sys
import unittest
from geometry_msgs.msg import Pose, PoseStamped, PointStamped, TwistStamped, Vector3Stamped
from moveit_msgs.msg import RobotState, Constraints, MotionPlanRequest
from moveit.task_constructor import core, stages
class TestPropertyMap(unittest.TestCase):
def setUp(self):
self.props = core.PropertyMap()
def _check(self, name, value):
self.props[name] = value
self.assertEqual(self.props[name], value)
def test_assign(self):
self._check("double", 3.14)
self._check("long", 42)
self._check("long", 13)
self._check("bool", True)
self._check("bool", False)
self._check("string", "anything")
self._check("pose", PoseStamped())
# MotionPlanRequest is not registered as property type and should raise
self.assertRaises(TypeError, self._check, "request", MotionPlanRequest())
def test_assign_in_reference(self):
planner = core.PipelinePlanner()
props = planner.properties
props["goal_joint_tolerance"] = 3.14
self.assertEqual(props["goal_joint_tolerance"], 3.14)
self.assertEqual(planner.goal_joint_tolerance, 3.14)
planner.goal_joint_tolerance = 2.71
self.assertEqual(props["goal_joint_tolerance"], 2.71)
props["planner"] = "planner"
self.assertEqual(props["planner"], "planner")
self.assertEqual(planner.planner, "planner")
props["double"] = 3.14
a = props
props["double"] = 2.71
self.assertEqual(a["double"], 2.71)
planner.planner = "other"
self.assertEqual(props["planner"], "other")
self.assertEqual(planner.planner, "other")
del planner
# We can still access props, because actual destruction of planner is delayed
self.assertEqual(props["goal_joint_tolerance"], 2.71)
self.assertEqual(props["planner"], "other")
def test_iter(self):
# assign values so we can iterate over them
self.props["double"] = 3.14
self.props["bool"] = True
keys = [v for v in self.props]
self.assertEqual(len(keys), 2)
items = [(k, v) for (k, v) in self.props.items()]
self.assertEqual(keys, [k for (k, v) in items])
def test_update(self):
self.props["double"] = 3.14
self.props.update({"double": 2.72, "bool": True})
self.props.update({})
self.assertEqual(self.props["double"], 2.72)
self.assertEqual(self.props["bool"], True)
def test_expose(self):
self.props["double"] = 3.14
other = core.PropertyMap()
self.props.exposeTo(other, "double")
self.assertEqual(other["double"], self.props["double"])
self.props.exposeTo(other, "double", "float")
self.assertEqual(other["float"], self.props["double"])
class TestModifyPlanningScene(unittest.TestCase):
def setUp(self):
self.mps = stages.ModifyPlanningScene("mps")
def test_attach_objects_invalid_args(self):
for value in [None, 1, 1.5, {}]:
self.assertRaises(RuntimeError, self.mps.attachObjects, value, "link")
self.assertRaises(RuntimeError, self.mps.attachObjects, value, "link", True)
self.assertRaises(RuntimeError, self.mps.attachObjects, value, "link", False)
def test_attach_objects_valid_args(self):
self.mps.attachObject("object", "link")
self.mps.detachObject("object", "link")
self.mps.attachObjects("object", "link")
self.mps.detachObjects("object", "link")
self.mps.attachObjects("object", "link", True)
self.mps.attachObjects("object", "link", False)
self.mps.attachObjects([], "link")
self.mps.attachObjects(["object"], "link")
self.mps.attachObjects(["object1", "object2", "object3"], "link")
def test_allow_collisions(self):
self.mps.allowCollisions("first", "second")
self.mps.allowCollisions("first", "second", True)
self.mps.allowCollisions("first", "second", False)
self.mps.allowCollisions(["first"], ["second"])
class TestStages(unittest.TestCase):
def setUp(self):
self.planner = core.PipelinePlanner()
def _check(self, stage, name, value):
self._check_assign(stage, name, value)
self._check_invalid_args(stage, name, type(value))
def _check_assign(self, stage, name, value):
setattr(stage, name, value)
self.assertEqual(getattr(stage, name), value)
def _check_invalid_args(self, stage, name, target_type):
"""Check some basic types to raise an ArgumentError when assigned"""
for value in [None, 1, 1.0, "string", [], {}, set()]:
try:
target_type(value)
continue # ignore values that are implicitly convertible to target_type
except:
pass
try:
setattr(stage, name, value)
except TypeError:
pass
except:
msg = "Assigning {} did raise wrong exception: {}"
self.fail(msg.format(value, sys.exc_info()[0]))
else:
if value == "string" and target_type is PoseStamped:
continue # string is convertible to PoseStamped
msg = "Assigning {} did not raise an exception, result: {}"
self.fail(msg.format(value, getattr(stage, name)))
def test_CurrentState(self):
stage = stages.CurrentState("current")
def test_FixedState(self):
stage = stages.FixedState("fixed")
def test_ComputeIK(self):
generator_stage = stages.GeneratePose("generator")
stage = stages.ComputeIK("IK", generator_stage)
self._check(stage, "timeout", 0.5)
self._check(stage, "eef", "eef")
self._check(stage, "group", "group")
self._check(stage, "default_pose", "default_pose")
self._check(stage, "max_ik_solutions", 1)
self.assertRaises(TypeError, self._check_assign, stage, "max_ik_solutions", -1)
self._check(stage, "ignore_collisions", False)
self._check(stage, "ignore_collisions", True)
self._check(stage, "ik_frame", PoseStamped())
self._check(stage, "target_pose", PoseStamped())
self._check(stage, "forwarded_properties", ["name1", "name2", "name3"])
stage.forwarded_properties = "name"
self.assertRaises(TypeError, self._check_assign, stage, "forwarded_properties", [1, 2])
def test_MoveTo(self):
stage = stages.MoveTo("move", self.planner)
self._check(stage, "group", "group")
self._check(stage, "ik_frame", PoseStamped())
self._check(stage, "path_constraints", Constraints())
stage.setGoal(PoseStamped())
stage.setGoal(PointStamped())
stage.setGoal(RobotState())
stage.setGoal("named pose")
stage.setGoal(dict(joint1=1.0, joint2=2.0))
self._check(stage, "path_constraints", Constraints())
def test_MoveRelative(self):
stage = stages.MoveRelative("move", self.planner)
self._check(stage, "group", "group")
self._check(stage, "ik_frame", PoseStamped())
self._check(stage, "min_distance", 0.5)
self._check(stage, "max_distance", 0.25)
self._check(stage, "path_constraints", Constraints())
stage.setDirection(TwistStamped())
stage.setDirection(Vector3Stamped())
stage.setDirection({"joint": 0.1})
def test_Connect(self):
planner = core.PipelinePlanner()
stage = stages.Connect("connect", [("group1", planner), ("group2", planner)])
def test_FixCollisionObjects(self):
stage = stages.FixCollisionObjects("collision")
self._check(stage, "max_penetration", 0.5)
def test_GenerateGraspPose(self):
stage = stages.GenerateGraspPose("generate_grasp_pose")
self._check(stage, "eef", "eef")
self._check(stage, "pregrasp", "pregrasp")
self._check(stage, "object", "object")
self._check(stage, "angle_delta", 0.5)
def test_GeneratePose(self):
stage = stages.GeneratePose("generate_pose")
self._check(stage, "pose", PoseStamped())
def test_Pick(self):
generator_stage = stages.GeneratePose("generator")
stage = stages.Pick(generator_stage, "pick")
self._check(stage, "object", "object")
self._check(stage, "eef", "eef")
self._check(stage, "eef_frame", "eef_frame")
self._check(stage, "eef_group", "eef_group")
self._check(stage, "eef_parent_group", "eef_parent_group")
self._check(stage.cartesian_solver, "max_velocity_scaling_factor", 0.1)
def test_Place(self):
generator_stage = stages.GeneratePose("generator")
stage = stages.Place(generator_stage, "place")
self._check(stage, "object", "object")
self._check(stage, "eef", "eef")
self._check(stage, "eef_frame", "eef_frame")
self._check(stage, "eef_group", "eef_group")
self._check(stage, "eef_parent_group", "eef_parent_group")
def test_SimpleGrasp(self):
stage = stages.SimpleGrasp(stages.GenerateGraspPose("grasp"))
self._check(stage, "eef", "eef")
self._check(stage, "object", "object")
def test_SimpleUnGrasp(self):
stage = stages.SimpleUnGrasp(stages.GenerateGraspPose("ungrasp"))
self._check(stage, "eef", "eef")
self._check(stage, "object", "object")
def test_PropertyMaps(self):
for name in dir(stages):
if name.startswith("__") or name.endswith("__"):
continue
stage = getattr(stages, name)
try:
props = stage().properties
except:
continue
try:
for p in props:
pass
except Exception as ex:
print("error in class {}: {}".format(stage, ex))
raise
def test_CostTerm(self):
stage = stages.CurrentState()
weights = {"joint_{}".format(i + 1): 1.0 for i in range(6)}
costs = core.PathLength(weights)
stage.setCostTerm(costs)
class BaseTestCases:
class ContainerTest(unittest.TestCase):
def __init__(self, ContainerType, *args, **kwargs):
super(BaseTestCases.ContainerTest, self).__init__(*args, **kwargs)
self.ContainerType = ContainerType
self.container = container = ContainerType()
container.add(stages.CurrentState("1"))
container.add(stages.CurrentState("2"))
container.add(stages.CurrentState("3"))
def test_move(self):
container = self.ContainerType()
stage = stages.CurrentState()
container.add(stage)
with self.assertRaises(ValueError):
stage.name
def test_access_by_name(self):
with self.assertRaises(IndexError):
self.container["unknown"]
child = self.container["2"]
self.assertEqual(child.name, "2")
def test_access_by_iterator(self):
self.assertEqual([child.name for child in self.container], ["1", "2", "3"])
def test_access_by_index(self):
self.assertEqual(self.container[0].name, "1")
self.assertEqual(self.container[1].name, "2")
self.assertEqual(self.container[-1].name, "3")
self.assertEqual(self.container[-2].name, "2")
with self.assertRaises(IndexError):
self.container[3]
with self.assertRaises(IndexError):
self.container[-4]
class TestSerial(BaseTestCases.ContainerTest):
def __init__(self, *args, **kwargs):
super(TestSerial, self).__init__(core.SerialContainer, *args, **kwargs)
class TestTask(BaseTestCases.ContainerTest):
def __init__(self, *args, **kwargs):
super(TestTask, self).__init__(core.Task, *args, **kwargs)
def test(self):
task = core.Task()
current = stages.CurrentState("current")
self.assertEqual(current.name, "current")
current.timeout = 1.23
self.assertEqual(current.timeout, 1.23)
task.add(current)
# ownership of current was passed to task
with self.assertRaises(ValueError):
current.name
task.add(stages.Connect("connect", []))
task.add(stages.FixedState())
if __name__ == "__main__":
unittest.main()

10
core/rosdoc.yaml Normal file
View File

@ -0,0 +1,10 @@
- builder: sphinx
name: Python API
output_dir: python
sphinx_root_dir: doc
- builder: doxygen
name: C++ API
output_dir: cpp
file_patterns: "*.c *.cpp *.h *.cc *.hh *.dox"
exclude_patterns: "*/core/python/pybind11/* */core/python/bindings/* */test/* "
exclude_symbols: "*Private class_ declval*"

26
core/setup.py Normal file
View File

@ -0,0 +1,26 @@
#!/usr/bin/env python
from setuptools import find_packages, setup
from catkin_pkg.python_setup import generate_distutils_setup
d = generate_distutils_setup(
# list of packages to setup
packages=find_packages("python/src"),
# specify location of root ("") package dir
package_dir={"": "python/src"},
)
dist = setup(**d)
# Remove moveit/__init__.py when building .deb packages
# Otherwise, the installation procedure will complain about conflicting files (with moveit_core)
try:
# installation dir (.../lib/python3/dist-packages)
libdir = dist.command_obj["install_lib"].install_dir
if "/debian/ros-" in libdir and "moveit-task-constructor-core/opt/ros/" in libdir:
import os
import shutil
os.remove(os.path.join(libdir, "moveit", "__init__.py"))
shutil.rmtree(os.path.join(libdir, "moveit", "__pycache__"))
except AttributeError as e:
pass

View File

@ -18,6 +18,7 @@ add_library(${PROJECT_NAME}
${PROJECT_INCLUDE}/solvers/cartesian_path.h
${PROJECT_INCLUDE}/solvers/joint_interpolation.h
${PROJECT_INCLUDE}/solvers/pipeline_planner.h
${PROJECT_INCLUDE}/solvers/multi_planner.h
container.cpp
cost_terms.cpp
@ -32,15 +33,16 @@ add_library(${PROJECT_NAME}
solvers/planner_interface.cpp
solvers/cartesian_path.cpp
solvers/pipeline_planner.cpp
solvers/joint_interpolation.cpp
solvers/pipeline_planner.cpp
solvers/multi_planner.cpp
)
target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES})
target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} fmt::fmt)
target_include_directories(${PROJECT_NAME}
PUBLIC $<BUILD_INTERFACE:${PROJECT_SOURCE_DIR}/include>
PUBLIC $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}>
PUBLIC ${catkin_INCLUDE_DIRS}
)
target_include_directories(${PROJECT_NAME} SYSTEM PUBLIC ${catkin_INCLUDE_DIRS})
add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS})
add_subdirectory(stages)

View File

@ -37,7 +37,9 @@
#include <moveit/task_constructor/container_p.h>
#include <moveit/task_constructor/introspection.h>
#include <moveit/task_constructor/merge.h>
#include <moveit/task_constructor/fmt_p.h>
#include <moveit/planning_scene/planning_scene.h>
#include <moveit/trajectory_processing/time_optimal_trajectory_generation.h>
#include <ros/console.h>
@ -45,14 +47,42 @@
#include <iostream>
#include <algorithm>
#include <boost/range/adaptor/reversed.hpp>
#include <boost/format.hpp>
#include <functional>
using namespace std::placeholders;
using namespace trajectory_processing;
namespace moveit {
namespace task_constructor {
// for debugging of how children interfaces evolve over time
__attribute__((unused)) // silent unused-function warning
static void
printChildrenInterfaces(const ContainerBasePrivate& container, bool success, const Stage& creator,
std::ostream& os = std::cerr) {
static unsigned int id = 0;
const unsigned int width = 10; // indentation of name
os << '\n' << (success ? '+' : '-') << ' ' << creator.name() << ' ';
if (success)
os << ++id << ' ';
if (const auto conn = dynamic_cast<const ConnectingPrivate*>(creator.pimpl()))
os << conn->pendingPairsPrinter();
os << '\n';
for (const auto& child : container.children()) {
auto cimpl = child->pimpl();
os << std::setw(width) << std::left << child->name();
if (!cimpl->starts() && !cimpl->ends())
os << "\n";
if (cimpl->starts())
os << "" << *child->pimpl()->starts() << '\n';
if (cimpl->starts() && cimpl->ends())
os << std::setw(width) << " ";
if (cimpl->ends())
os << "" << *child->pimpl()->ends() << '\n';
}
}
ContainerBasePrivate::ContainerBasePrivate(ContainerBase* me, const std::string& name)
: StagePrivate(me, name)
, required_interface_(UNKNOWN)
@ -133,64 +163,38 @@ void ContainerBasePrivate::compute() {
static_cast<ContainerBase*>(me_)->compute();
}
void ContainerBasePrivate::onNewFailure(const Stage& child, const InterfaceState* from, const InterfaceState* to) {
switch (child.pimpl()->interfaceFlags()) {
case GENERATE:
// just ignore: the pair of (new) states isn't known to us anyway
// TODO: If child is a container, from and to might have associated solutions already!
break;
case PROPAGATE_FORWARDS: // mark from as failed (backwards)
setStatus<Interface::BACKWARD>(from, InterfaceState::Status::FAILED);
break;
case PROPAGATE_BACKWARDS: // mark to as failed (forwards)
setStatus<Interface::FORWARD>(to, InterfaceState::Status::FAILED);
break;
case CONNECT:
if (const Connecting* conn = dynamic_cast<const Connecting*>(&child)) {
auto cimpl = conn->pimpl();
ROS_DEBUG_STREAM_NAMED("Connecting", "'" << child.name() << "' generated a failure");
if (!cimpl->hasPendingOpposites<Interface::FORWARD>(from)) {
ROS_DEBUG_STREAM_NAMED("Connecting", "prune backward branch");
setStatus<Interface::BACKWARD>(from, InterfaceState::Status::FAILED);
}
if (!cimpl->hasPendingOpposites<Interface::BACKWARD>(to)) {
ROS_DEBUG_STREAM_NAMED("Connecting", "prune forward branch");
setStatus<Interface::FORWARD>(to, InterfaceState::Status::FAILED);
}
}
break;
}
// printChildrenInterfaces(*this, false, child);
}
template <Interface::Direction dir>
void ContainerBasePrivate::setStatus(const InterfaceState* s, InterfaceState::Status status) {
if (s->priority().status() == status)
void ContainerBasePrivate::setStatus(const Stage* creator, const InterfaceState* source, const InterfaceState* target,
InterfaceState::Status status) {
if (status != InterfaceState::Status::ENABLED && creator) {
if (const auto* conn = dynamic_cast<const Connecting*>(creator)) {
auto cimpl = conn->pimpl();
// if creator is a Connecting stage and target has enabled opposite states (other than source)
if (cimpl->hasPendingOpposites<dir>(source, target))
return; // don't prune
}
}
if (target->priority().status() == status)
return; // nothing changing
// if we should disable the state, only do so when there is no enabled alternative path
if (status == InterfaceState::PRUNED) {
// Skip disabling the state, if there are alternative enabled solutions
if (status != InterfaceState::ENABLED) {
auto solution_is_enabled = [](auto&& solution) {
return state<opposite<dir>()>(*solution)->priority().enabled();
};
const auto& alternatives = trajectories<opposite<dir>()>(*s);
const auto& alternatives = trajectories<opposite<dir>()>(*target);
auto alternative_path = std::find_if(alternatives.cbegin(), alternatives.cend(), solution_is_enabled);
if (alternative_path != alternatives.cend())
return;
}
// actually enable/disable the state
if (s->owner()) {
s->owner()->updatePriority(const_cast<InterfaceState*>(s), InterfaceState::Priority(s->priority(), status));
} else {
const_cast<InterfaceState*>(s)->priority_ = InterfaceState::Priority(s->priority(), status);
}
const_cast<InterfaceState*>(target)->updateStatus(status);
// if possible (i.e. if state s has an external counterpart), escalate setStatus to external interface
if (parent() && trajectories<dir>(*s).empty()) {
auto external{ internalToExternalMap().find(s) };
// if possible (i.e. if target has an external counterpart), escalate setStatus to external interface
if (parent() && trajectories<dir>(*target).empty()) {
// TODO: This was coded with SerialContainer in mind. Not sure, it works for ParallelContainers
auto external{ internalToExternalMap().find(target) };
if (external != internalToExternalMap().end()) { // do we have an external state?
// only escalate if there is no other *enabled* internal state connected to the same external one
// all internal states linked to external
@ -198,44 +202,78 @@ void ContainerBasePrivate::setStatus(const InterfaceState* s, InterfaceState::St
auto is_enabled = [](const auto& ext_int_pair) { return ext_int_pair.second->priority().enabled(); };
auto other_path{ std::find_if(internals.first, internals.second, is_enabled) };
if (other_path == internals.second)
parent()->pimpl()->setStatus<dir>(external->get<EXTERNAL>(), status);
parent()->pimpl()->setStatus<dir>(nullptr, nullptr, external->get<EXTERNAL>(), status);
return;
}
}
// To break symmetry between both ends of a partial solution sequence that gets disabled,
// we mark the first state with FAILED and all other states down the tree only with PRUNED.
// This allows us to re-enable the FAILED side, while not (yet) consider the PRUNED states again,
// we mark the first state with ARMED and all other states down the tree with PRUNED.
// This allows us to re-enable the ARMED state, but not the PRUNED states,
// when new states arrive in a Connecting stage.
// All PRUNED states are only re-enabled if the FAILED state actually gets connected.
// For details, see: https://github.com/ros-planning/moveit_task_constructor/pull/221
if (status == InterfaceState::Status::FAILED)
status = InterfaceState::Status::PRUNED; // only the first state is marked as FAILED
// For details, https://github.com/moveit/moveit_task_constructor/pull/309#issuecomment-974636202
if (status == InterfaceState::Status::ARMED)
status = InterfaceState::Status::PRUNED; // only the first state is marked as ARMED
// traverse solution tree
for (const SolutionBase* successor : trajectories<dir>(*s))
setStatus<dir>(state<dir>(*successor), status);
for (const SolutionBase* successor : trajectories<dir>(*target))
setStatus<dir>(successor->creator(), target, state<dir>(*successor), status);
}
void ContainerBasePrivate::setStatus(Interface::Direction dir, const InterfaceState* s, InterfaceState::Status status) {
if (dir == Interface::FORWARD)
setStatus<Interface::FORWARD>(s, InterfaceState::ENABLED);
else
setStatus<Interface::BACKWARD>(s, InterfaceState::ENABLED);
// recursively update state priorities along solution path
template <Interface::Direction dir>
inline void updateStatePrios(const InterfaceState& s, const InterfaceState::Priority& prio) {
InterfaceState::Priority priority(prio, s.priority().status());
if (s.priority() == priority)
return;
const_cast<InterfaceState&>(s).updatePriority(priority);
for (const SolutionBase* successor : trajectories<dir>(s))
updateStatePrios<dir>(*state<dir>(*successor), prio);
}
void ContainerBasePrivate::onNewFailure(const Stage& child, const InterfaceState* from, const InterfaceState* to) {
if (!static_cast<ContainerBase*>(me_)->pruning())
return;
ROS_DEBUG_STREAM_NAMED("Pruning", fmt::format("'{}' generated a failure", child.name()));
switch (child.pimpl()->interfaceFlags()) {
case GENERATE:
// just ignore: the pair of (new) states isn't known to us anyway
// TODO: If child is a container, from and to might have associated solutions already!
break;
case PROPAGATE_FORWARDS: // mark from as failed (backwards)
setStatus<Interface::BACKWARD>(nullptr, nullptr, from, InterfaceState::Status::PRUNED);
break;
case PROPAGATE_BACKWARDS: // mark to as failed (forwards)
setStatus<Interface::FORWARD>(nullptr, nullptr, to, InterfaceState::Status::PRUNED);
break;
case CONNECT:
setStatus<Interface::BACKWARD>(&child, to, from, InterfaceState::Status::ARMED);
setStatus<Interface::FORWARD>(&child, from, to, InterfaceState::Status::ARMED);
break;
}
// printChildrenInterfaces(*this, false, child);
}
template <Interface::Direction dir>
void ContainerBasePrivate::copyState(Interface::iterator external, const InterfacePtr& target, bool updated) {
void ContainerBasePrivate::copyState(Interface::iterator external, const InterfacePtr& target,
Interface::UpdateFlags updated) {
if (updated) {
// update prio of all internal states linked to external
auto internals{ externalToInternalMap().equal_range(&*external) };
for (auto& i = internals.first; i != internals.second; ++i) {
// TODO: Not only update status, but full priority!
setStatus<dir>(i->second, external->priority().status());
}
auto prio = external->priority();
auto internals = externalToInternalMap().equal_range(&*external);
if (updated.testFlag(Interface::Update::STATUS)) { // propagate external status updates to internal copies
for (auto& i = internals.first; i != internals.second; ++i)
setStatus<dir>(nullptr, nullptr, i->second, prio.status());
} else if (updated.testFlag(Interface::Update::PRIORITY)) {
for (auto& i = internals.first; i != internals.second; ++i)
updateStatePrios<opposite<dir>()>(*i->second, prio);
} else
assert(false); // Expecting either STATUS or PRIORITY updates, not both!
return;
}
// create a clone of external state within target interface (child's starts() or ends())
auto internal = states_.insert(states_.end(), InterfaceState(*external));
target->add(*internal);
@ -244,7 +282,7 @@ void ContainerBasePrivate::copyState(Interface::iterator external, const Interfa
}
void ContainerBasePrivate::copyState(Interface::Direction dir, Interface::iterator external, const InterfacePtr& target,
bool updated) {
Interface::UpdateFlags updated) {
if (dir == Interface::FORWARD)
copyState<Interface::FORWARD>(external, target, updated);
else
@ -287,7 +325,10 @@ void ContainerBasePrivate::liftSolution(const SolutionBasePtr& solution, const I
newSolution(solution);
}
ContainerBase::ContainerBase(ContainerBasePrivate* impl) : Stage(impl) {}
ContainerBase::ContainerBase(ContainerBasePrivate* impl) : Stage(impl) {
auto& p = properties();
p.declare<bool>("pruning", std::string("enable pruning?")).configureInitFrom(Stage::PARENT, "pruning");
}
size_t ContainerBase::numChildren() const {
return pimpl()->children().size();
@ -306,6 +347,12 @@ Stage* ContainerBase::findChild(const std::string& name) const {
return nullptr;
}
Stage* ContainerBase::operator[](int index) const {
auto impl = pimpl();
auto it = impl->childByIndex(index, false);
return it != impl->children().end() ? it->get() : nullptr;
}
bool ContainerBase::traverseChildren(const ContainerBase::StageCallback& processor) const {
return pimpl()->traverseStages(processor, 0, 1);
}
@ -405,40 +452,32 @@ void ContainerBase::init(const moveit::core::RobotModelConstPtr& robot_model) {
throw errors;
}
bool ContainerBase::explainFailure(std::ostream& os) const {
for (const auto& stage : pimpl()->children()) {
if (!stage->solutions().empty())
continue; // skip deeper traversal, this stage produced solutions
if (stage->numFailures()) {
os << stage->name() << " (0/" << stage->numFailures() << ")";
if (!stage->failures().empty())
os << ": " << stage->failures().front()->comment();
os << '\n';
return true;
}
if (stage->explainFailure(os)) // recursively process children
return true;
}
return false;
}
std::ostream& operator<<(std::ostream& os, const ContainerBase& container) {
ContainerBase::StageCallback processor = [&os](const Stage& stage, unsigned int depth) -> bool {
os << std::string(2 * depth, ' ') << *stage.pimpl() << std::endl;
os << std::string(2 * depth, ' ') << *stage.pimpl() << '\n';
return true;
};
container.traverseRecursively(processor);
return os;
}
// for debugging of how children interfaces evolve over time
static void printChildrenInterfaces(const ContainerBase& container, bool success, const Stage& creator,
std::ostream& os = std::cerr) {
static unsigned int id = 0;
const unsigned int width = 10; // indentation of name
os << std::endl << (success ? '+' : '-') << ' ' << creator.name() << ' ';
if (success)
os << ++id << ' ';
if (const Connecting* conn = dynamic_cast<const Connecting*>(&creator))
conn->pimpl()->printPendingPairs(os);
os << std::endl;
for (const auto& child : container.pimpl()->children()) {
auto cimpl = child->pimpl();
os << std::setw(width) << std::left << child->name();
if (!cimpl->starts() && !cimpl->ends())
os << "" << std::endl;
if (cimpl->starts())
os << "" << *child->pimpl()->starts() << std::endl;
if (cimpl->starts() && cimpl->ends())
os << std::setw(width) << " ";
if (cimpl->ends())
os << "" << *child->pimpl()->ends() << std::endl;
}
}
/** Collect all partial solution sequences originating from start into given direction */
template <Interface::Direction dir>
struct SolutionCollector
@ -457,7 +496,8 @@ struct SolutionCollector
solutions.emplace_back(std::make_pair(trace, prio));
} else {
for (SolutionBase* successor : next) {
assert(!successor->isFailure()); // We shouldn't have invalid solutions
if (successor->isFailure())
continue; // skip failures
trace.push_back(successor);
traverse(*successor, prio + InterfaceState::Priority(1, successor->cost()));
trace.pop_back();
@ -471,28 +511,13 @@ struct SolutionCollector
SolutionSequence::container_type trace;
};
inline void updateStatePrio(const InterfaceState* state, const InterfaceState::Priority& prio) {
if (state->owner()) // owner becomes NULL if state is removed from (pending) Interface list
state->owner()->updatePriority(const_cast<InterfaceState*>(state),
// update depth + cost, but keep current status
InterfaceState::Priority(prio, state->priority().status()));
}
template <Interface::Direction dir>
inline void updateStatePrios(const SolutionSequence::container_type& partial_solution_path,
const InterfaceState::Priority& prio) {
for (const SolutionBase* solution : partial_solution_path)
updateStatePrio(state<dir>(*solution), prio);
}
void SerialContainer::onNewSolution(const SolutionBase& current) {
ROS_DEBUG_STREAM_NAMED("SerialContainer", fmt::format("'{}' received solution of child stage '{}'", this->name(),
current.creator()->name()));
// failures should never trigger this callback
assert(!current.isFailure());
// states of solution must be active, otherwise this would not have been computed
assert(current.start()->priority().enabled());
assert(current.end()->priority().enabled());
auto impl = pimpl();
const Stage* creator = current.creator();
auto& children = impl->children();
@ -530,14 +555,12 @@ void SerialContainer::onNewSolution(const SolutionBase& current) {
}
if (prio.depth() > 1) {
// update state priorities along the whole partial solution path
updateStatePrio(current.start(), prio);
updateStatePrio(current.end(), prio);
updateStatePrios<Interface::BACKWARD>(in.first, prio);
updateStatePrios<Interface::FORWARD>(out.first, prio);
updateStatePrios<Interface::BACKWARD>(*current.start(), prio);
updateStatePrios<Interface::FORWARD>(*current.end(), prio);
}
}
}
// printChildrenInterfaces(*this, true, *current.creator());
// printChildrenInterfaces(*this->pimpl(), true, *current.creator());
// finally, store + announce new solutions to external interface
for (const auto& solution : sorted)
@ -559,10 +582,10 @@ void SerialContainerPrivate::connect(StagePrivate& stage1, StagePrivate& stage2)
else if ((flags1 & READS_END) && (flags2 & WRITES_PREV_END))
stage2.setPrevEnds(stage1.ends());
else {
boost::format desc("cannot connect end interface of '%1%' (%2%) to start interface of '%3%' (%4%)");
desc % stage1.name() % flowSymbol<END_IF_MASK>(flags1);
desc % stage2.name() % flowSymbol<START_IF_MASK>(flags2);
throw InitStageException(*me(), desc.str());
throw InitStageException(
*me(), fmt::format("cannot connect end interface of '{}' ({}) to start interface of '{}' ({})", //
stage1.name(), flowSymbol<END_IF_MASK>(flags1), //
stage2.name(), flowSymbol<START_IF_MASK>(flags2)));
}
}
@ -572,12 +595,10 @@ void SerialContainerPrivate::validateInterface(const StagePrivate& child, Interf
if (required == UNKNOWN)
return; // cannot yet validate
InterfaceFlags child_interface = child.interfaceFlags() & mask;
if (required != child_interface) {
boost::format desc("%1% interface (%3%) of '%2%' does not match mine (%4%)");
desc % (mask == START_IF_MASK ? "start" : "end") % child.name();
desc % flowSymbol<mask>(child_interface) % flowSymbol<mask>(required);
throw InitStageException(*me_, desc.str());
}
if (required != child_interface)
throw InitStageException(*me_, fmt::format("{0} interface ({2}) of '{1}' does not match mine ({3})",
(mask == START_IF_MASK ? "start" : "end"), child.name(),
flowSymbol<mask>(child_interface), flowSymbol<mask>(required)));
}
// called by parent asking for pruning of this' interface
@ -602,9 +623,9 @@ void SerialContainerPrivate::resolveInterface(InterfaceFlags expected) {
validateInterface<START_IF_MASK>(*first.pimpl(), expected);
// connect first child's (start) pull interface
if (const InterfacePtr& target = first.pimpl()->starts())
starts_.reset(new Interface([this, target](Interface::iterator it, bool updated) {
starts_ = std::make_shared<Interface>([this, target](Interface::iterator it, Interface::UpdateFlags updated) {
this->copyState<Interface::FORWARD>(it, target, updated);
}));
});
} catch (InitStageException& e) {
exceptions.append(e);
}
@ -628,15 +649,15 @@ void SerialContainerPrivate::resolveInterface(InterfaceFlags expected) {
validateInterface<END_IF_MASK>(*last.pimpl(), expected);
// connect last child's (end) pull interface
if (const InterfacePtr& target = last.pimpl()->ends())
ends_.reset(new Interface([this, target](Interface::iterator it, bool updated) {
ends_ = std::make_shared<Interface>([this, target](Interface::iterator it, Interface::UpdateFlags updated) {
this->copyState<Interface::BACKWARD>(it, target, updated);
}));
});
} catch (InitStageException& e) {
exceptions.append(e);
}
required_interface_ = (first.pimpl()->interfaceFlags() & START_IF_MASK) | // clang-format off
(last.pimpl()->interfaceFlags() & END_IF_MASK); // clang-format off
required_interface_ = (first.pimpl()->interfaceFlags() & START_IF_MASK) | //
(last.pimpl()->interfaceFlags() & END_IF_MASK);
if (exceptions)
throw exceptions;
@ -646,7 +667,7 @@ void SerialContainerPrivate::validateConnectivity() const {
ContainerBasePrivate::validateConnectivity();
InterfaceFlags mine = interfaceFlags();
// check that input / output interface of first / last child matches this' resp. interface
// check that input/output interface of first/last child matches this' resp. interface
validateInterface<START_IF_MASK>(*children().front()->pimpl(), mine);
validateInterface<END_IF_MASK>(*children().back()->pimpl(), mine);
@ -658,7 +679,7 @@ void SerialContainerPrivate::validateConnectivity() const {
const StagePrivate* const cur_impl = **cur;
InterfaceFlags required = cur_impl->interfaceFlags();
// get iterators to prev / next stage in sequence
// get iterators to prev/next stage in sequence
auto prev = cur;
--prev;
auto next = cur;
@ -686,9 +707,8 @@ bool SerialContainer::canCompute() const {
void SerialContainer::compute() {
for (const auto& stage : pimpl()->children()) {
if (!stage->pimpl()->canCompute())
continue;
stage->pimpl()->runCompute();
if (stage->pimpl()->canCompute())
stage->pimpl()->runCompute();
}
}
@ -729,13 +749,13 @@ void ParallelContainerBasePrivate::resolveInterface(InterfaceFlags expected) {
void ParallelContainerBasePrivate::initializeExternalInterfaces() {
// States received by the container need to be copied to all children's pull interfaces.
if (requiredInterface() & READS_START)
starts().reset(new Interface([this](Interface::iterator external, bool updated) {
this->propagateStateToChildren<Interface::FORWARD>(external, updated);
}));
starts() = std::make_shared<Interface>([this](Interface::iterator external, Interface::UpdateFlags updated) {
this->propagateStateToAllChildren<Interface::FORWARD>(external, updated);
});
if (requiredInterface() & READS_END)
ends().reset(new Interface([this](Interface::iterator external, bool updated) {
this->propagateStateToChildren<Interface::BACKWARD>(external, updated);
}));
ends() = std::make_shared<Interface>([this](Interface::iterator external, Interface::UpdateFlags updated) {
this->propagateStateToAllChildren<Interface::BACKWARD>(external, updated);
});
}
void ParallelContainerBasePrivate::validateInterfaces(const StagePrivate& child, InterfaceFlags& external,
@ -749,20 +769,18 @@ void ParallelContainerBasePrivate::validateInterfaces(const StagePrivate& child,
valid = valid & ((external & mask) == (child_interface & mask));
}
if (!valid) {
boost::format desc("interface of '%1%' (%3% %4%) does not match %2% (%5% %6%).");
desc % child.name();
desc % (first ? "external one" : "other children's");
desc % flowSymbol<START_IF_MASK>(child_interface) % flowSymbol<END_IF_MASK>(child_interface);
desc % flowSymbol<START_IF_MASK>(external) % flowSymbol<END_IF_MASK>(external);
throw InitStageException(*me_, desc.str());
}
if (!valid)
throw InitStageException(
*me_, fmt::format("interface of '{0}' ({2} {3}) does not match {1} ({4} {5}).", child.name(),
(first ? "external one" : "other children's"), //
flowSymbol<START_IF_MASK>(child_interface), flowSymbol<END_IF_MASK>(child_interface),
flowSymbol<START_IF_MASK>(external), flowSymbol<END_IF_MASK>(external)));
}
void ParallelContainerBasePrivate::validateConnectivity() const {
InterfaceFlags my_interface = interfaceFlags();
// check that input / output interfaces of all children are handled by my interface
// check that input/output interfaces of all children are handled by my interface
for (const auto& child : children())
validateInterfaces(*child->pimpl(), my_interface);
@ -770,9 +788,10 @@ void ParallelContainerBasePrivate::validateConnectivity() const {
}
template <Interface::Direction dir>
void ParallelContainerBasePrivate::propagateStateToChildren(Interface::iterator external, bool updated) {
void ParallelContainerBasePrivate::propagateStateToAllChildren(Interface::iterator external,
Interface::UpdateFlags updated) {
for (const Stage::pointer& stage : children())
copyState<dir>(external, stage->pimpl()->pullInterface(dir), updated);
copyState<dir>(external, stage->pimpl()->pullInterface<dir>(), updated);
}
ParallelContainerBase::ParallelContainerBase(ParallelContainerBasePrivate* impl) : ContainerBase(impl) {}
@ -780,8 +799,8 @@ ParallelContainerBase::ParallelContainerBase(const std::string& name)
: ParallelContainerBase(new ParallelContainerBasePrivate(this, name)) {}
void ParallelContainerBase::liftSolution(const SolutionBase& solution, double cost, std::string comment) {
pimpl()->liftSolution(std::make_shared<WrappedSolution>(this, &solution, cost, std::move(comment)),
solution.start(), solution.end());
pimpl()->liftSolution(std::make_shared<WrappedSolution>(this, &solution, cost, std::move(comment)), solution.start(),
solution.end());
}
void ParallelContainerBase::spawn(InterfaceState&& state, SubTrajectory&& t) {
@ -857,28 +876,14 @@ void Fallbacks::init(const moveit::core::RobotModelConstPtr& robot_model) {
pimpl()->reset();
}
bool Fallbacks::canCompute() const {
auto impl = const_cast<FallbacksPrivate*>(pimpl());
while(impl->current_ != impl->children().end() && // not completely exhausted
!(*impl->current_)->pimpl()->canCompute()) // but current child cannot compute
return impl->nextJob(); // advance to next job
// return value: current child is well defined and thus can compute?
return impl->current_ != impl->children().end();
}
void Fallbacks::compute() {
(*pimpl()->current_)->pimpl()->runCompute();
}
void Fallbacks::onNewSolution(const SolutionBase& s) {
pimpl()->job_has_solutions_ = true;
liftSolution(s);
pimpl()->onNewSolution(s);
}
inline void Fallbacks::replaceImpl() {
FallbacksPrivate *impl = pimpl();
FallbacksPrivate* impl = pimpl();
if (pimpl()->interfaceFlags() == pimpl()->requiredInterface())
return;
switch (pimpl()->requiredInterface()) {
case GENERATE:
impl = new FallbacksPrivateGenerator(std::move(*impl));
@ -888,96 +893,125 @@ inline void Fallbacks::replaceImpl() {
impl = new FallbacksPrivatePropagator(std::move(*impl));
break;
case CONNECT:
throw std::runtime_error("Not yet implemented");
// For now, we only support Connecting children
for (const auto& child : impl->children())
if (!dynamic_cast<Connecting*>(child.get()))
throw std::runtime_error("CONNECT-like interface is only supported for Connecting children");
impl = new FallbacksPrivateConnect(std::move(*impl));
break;
}
delete pimpl_;
pimpl_ = impl;
}
FallbacksPrivate::FallbacksPrivate(Fallbacks* me, const std::string& name)
: ParallelContainerBasePrivate(me, name) {}
FallbacksPrivate::FallbacksPrivate(Fallbacks* me, const std::string& name) : ParallelContainerBasePrivate(me, name) {}
FallbacksPrivate::FallbacksPrivate(FallbacksPrivate&& other)
: ParallelContainerBasePrivate(static_cast<Fallbacks*>(other.me()), "") {
: ParallelContainerBasePrivate(static_cast<Fallbacks*>(other.me()), "") {
// move contents of other
this->ParallelContainerBasePrivate::operator=(std::move(other));
}
void FallbacksPrivate::reset() {
current_ = children().begin();
job_has_solutions_ = false;
}
void FallbacksPrivate::initializeExternalInterfaces() {
// Here we know the final interface of the container (and all its children)
// Thus replace, this pimpl() with a new interface-specific one:
static_cast<Fallbacks*>(me())->replaceImpl();
}
void FallbacksPrivate::onNewFailure(const Stage& /*child*/, const InterfaceState* /*from*/, const InterfaceState* /*to*/) {
void FallbacksPrivate::onNewSolution(const SolutionBase& s) {
// printChildrenInterfaces(*this, true, *s.creator());
static_cast<Fallbacks*>(me())->liftSolution(s);
}
void FallbacksPrivate::onNewFailure(const Stage& child, const InterfaceState* /*from*/, const InterfaceState* /*to*/) {
// This override is deliberately empty.
// The method prunes solution paths when a child failed to find a valid solution for it,
// but in Fallbacks the next child might still yield a successful solution
// Thus pruning must only occur once the last child is exhausted (inside computePropagate)
// printChildrenInterfaces(*this, false, child);
(void)child;
}
void FallbacksPrivateCommon::reset() {
current_ = children().begin();
}
FallbacksPrivateGenerator::FallbacksPrivateGenerator(FallbacksPrivate&& old)
: FallbacksPrivate(std::move(old)) { reset(); }
bool FallbacksPrivateCommon::canCompute() const {
while (current_ != children().end() && // not completely exhausted
!(*current_)->pimpl()->canCompute()) // but current child cannot compute
return const_cast<FallbacksPrivateCommon*>(this)->nextJob(); // advance to next job
// return value: current child is well defined and thus can compute?
return current_ != children().end();
}
void FallbacksPrivateCommon::compute() {
(*current_)->pimpl()->runCompute();
}
inline void FallbacksPrivateCommon::nextChild() {
if (std::next(current_) != children().end())
ROS_DEBUG_STREAM_NAMED("Fallbacks", fmt::format("Child '{}' failed, trying next one.", (*current_)->name()));
++current_; // advance to next child
}
FallbacksPrivateGenerator::FallbacksPrivateGenerator(FallbacksPrivate&& old) : FallbacksPrivateCommon(std::move(old)) {
FallbacksPrivateCommon::reset();
}
bool FallbacksPrivateGenerator::nextJob() {
assert(current_ != children().end() && !(*current_)->pimpl()->canCompute());
// don't advance to next child when we already produced solutions
if (job_has_solutions_) {
if (!solutions_.empty()) {
current_ = children().end(); // indicate that we are exhausted
return false;
}
do {
if (std::next(current_) != children().end())
ROS_DEBUG_STREAM_NAMED("Fallbacks", "Generator '" << (*current_)->name() << "' failed, trying next one.");
++current_; // advance to next child
nextChild();
} while (current_ != children().end() && !(*current_)->pimpl()->canCompute());
// return value shall indicate current_->canCompute()
return current_ != children().end();
}
FallbacksPrivatePropagator::FallbacksPrivatePropagator(FallbacksPrivate&& old)
: FallbacksPrivate(std::move(old)) {
: FallbacksPrivateCommon(std::move(old)) {
switch (requiredInterface()) {
case PROPAGATE_FORWARDS:
dir_ = Interface::FORWARD;
starts() = std::make_shared<Interface>();
break;
case PROPAGATE_BACKWARDS:
dir_ = Interface::BACKWARD;
ends() = std::make_shared<Interface>();
break;
default:
assert(false);
case PROPAGATE_FORWARDS:
dir_ = Interface::FORWARD;
starts() = std::make_shared<Interface>();
break;
case PROPAGATE_BACKWARDS:
dir_ = Interface::BACKWARD;
ends() = std::make_shared<Interface>();
break;
default:
assert(false);
}
FallbacksPrivatePropagator::reset();
}
void FallbacksPrivatePropagator::reset() {
FallbacksPrivate::reset();
FallbacksPrivateCommon::reset();
job_ = pullInterface(dir_)->end(); // indicate fresh start
job_has_solutions_ = false;
}
void FallbacksPrivatePropagator::onNewSolution(const SolutionBase& s) {
job_has_solutions_ = true;
FallbacksPrivateCommon::onNewSolution(s);
}
bool FallbacksPrivatePropagator::nextJob() {
assert(current_ != children().end() && !(*current_)->pimpl()->canCompute());
const auto jobs = pullInterface(dir_);
if (job_ != jobs->end()) { // current job exists, but is exhausted on current child
if (!job_has_solutions_) { // job didn't produce solutions -> feed to next child
if (std::next(current_) != children().end())
ROS_DEBUG_STREAM_NAMED("Fallbacks", "Propagator '" << (*current_)->name() << "' failed, trying next one.");
++current_; // advance to next child
} else
if (job_ != jobs->end()) { // current job exists, but is exhausted on current child
if (!job_has_solutions_) // job didn't produce solutions -> feed to next child
nextChild();
else
current_ = children().end(); // indicate that this job is exhausted on all children
}
job_has_solutions_ = false;
@ -993,16 +1027,75 @@ bool FallbacksPrivatePropagator::nextJob() {
// pick next job if needed and possible
if (job_ == jobs->end()) { // need to pick next job
if (!jobs->empty() && jobs->front()->priority().enabled())
job_ = jobs->begin();
job_ = jobs->begin();
else
return false; // no more jobs available
return false; // no more jobs available
}
// When arriving here, we have a valid job_ and a current_ child to feed it. Let's do that.
copyState(dir_, job_, (*current_)->pimpl()->pullInterface(dir_), false);
copyState(dir_, job_, (*current_)->pimpl()->pullInterface(dir_), Interface::UpdateFlags());
return true;
}
FallbacksPrivateConnect::FallbacksPrivateConnect(FallbacksPrivate&& old) : FallbacksPrivate(std::move(old)) {
starts_ = std::make_shared<Interface>(std::bind(&FallbacksPrivateConnect::propagateStateUpdate<Interface::FORWARD>,
this, std::placeholders::_1, std::placeholders::_2));
ends_ = std::make_shared<Interface>(std::bind(&FallbacksPrivateConnect::propagateStateUpdate<Interface::BACKWARD>,
this, std::placeholders::_1, std::placeholders::_2));
FallbacksPrivateConnect::reset();
}
void FallbacksPrivateConnect::reset() {
active_ = children().end();
}
template <Interface::Direction dir>
void FallbacksPrivateConnect::propagateStateUpdate(Interface::iterator external, Interface::UpdateFlags updated) {
copyState<dir>(external, children().front()->pimpl()->pullInterface(dir), updated);
// As we use the Interface* from the first child for all children (we just populate their pending lists)
// there is no need to explicitly propagate state updates to other children.
}
bool FallbacksPrivateConnect::canCompute() const {
for (auto it = children().begin(), end = children().end(); it != end; ++it)
if ((*it)->pimpl()->canCompute()) {
active_ = it;
return true;
}
active_ = children().end();
return false;
}
void FallbacksPrivateConnect::compute() {
// Alternatively, we could also compute() all children that canCompute()
assert(active_ != children().end());
(*active_)->pimpl()->runCompute();
}
void FallbacksPrivateConnect::onNewFailure(const Stage& child, const InterfaceState* from, const InterfaceState* to) {
// expect failure to be reported from active child
assert(active_ != children().end() && active_->get() == &child);
(void)child;
// ... thus we can use std::next(active_) to find the next child
auto next = std::next(active_);
// NOLINTNEXTLINE(readability-identifier-naming)
auto findIteratorFor = [](const InterfaceState* state, const Interface& interface) {
auto it = std::find(interface.begin(), interface.end(), state);
assert(it != interface.end());
return it;
};
if (next != children().end()) { // pass job to next child
auto next_con = static_cast<ConnectingPrivate*>(const_cast<StagePrivate*>((*next)->pimpl()));
auto first_con = static_cast<const ConnectingPrivate*>(children().front()->pimpl());
auto from_it = findIteratorFor(from, *first_con->starts());
auto to_it = findIteratorFor(to, *first_con->ends());
next_con->pending.insert(std::make_pair(from_it, to_it));
} else // or report failure to parent
parent()->pimpl()->onNewFailure(*me(), from, to);
}
MergerPrivate::MergerPrivate(Merger* me, const std::string& name) : ParallelContainerBasePrivate(me, name) {}
@ -1023,7 +1116,10 @@ void MergerPrivate::resolveInterface(InterfaceFlags expected) {
}
}
Merger::Merger(const std::string& name) : Merger(new MergerPrivate(this, name)) {}
Merger::Merger(const std::string& name) : Merger(new MergerPrivate(this, name)) {
properties().declare<TimeParameterizationPtr>("time_parameterization",
std::make_shared<TimeOptimalTrajectoryGeneration>());
}
void Merger::reset() {
ParallelContainerBase::reset();
@ -1176,7 +1272,8 @@ void MergerPrivate::merge(const ChildSolutionList& sub_solutions,
moveit::core::JointModelGroup* jmg = jmg_merged_.get();
robot_trajectory::RobotTrajectoryPtr merged;
try {
merged = task_constructor::merge(sub_trajectories, start_scene->getCurrentState(), jmg);
auto timing = me_->properties().get<TimeParameterizationPtr>("time_parameterization");
merged = task_constructor::merge(sub_trajectories, start_scene->getCurrentState(), jmg, *timing);
} catch (const std::runtime_error& e) {
SubTrajectory t;
t.markAsFailure();
@ -1198,8 +1295,9 @@ void MergerPrivate::merge(const ChildSolutionList& sub_solutions,
oss << "Invalid waypoint(s): ";
if (invalid_index.size() == merged->getWayPointCount())
oss << "all";
else for (size_t i : invalid_index)
oss << i << ", ";
else
for (size_t i : invalid_index)
oss << i << ", ";
t.setComment(oss.str());
} else {
// accumulate costs and markers

View File

@ -36,15 +36,15 @@
#include <moveit/task_constructor/cost_terms.h>
#include <moveit/task_constructor/stage.h>
#include <moveit/task_constructor/moveit_compat.h>
#include <moveit/task_constructor/fmt_p.h>
#include <moveit/collision_detection/collision_common.h>
#include <moveit/robot_trajectory/robot_trajectory.h>
#include <moveit/planning_scene/planning_scene.h>
#include <moveit/robot_state/conversions.h>
#include <Eigen/Geometry>
#include <boost/format.hpp>
#include <utility>
namespace moveit {
@ -107,17 +107,23 @@ double Constant::operator()(const WrappedSolution& /*s*/, std::string& /*comment
return cost;
}
PathLength::PathLength(std::vector<std::string> joints) {
for (auto& j : joints)
this->joints.emplace(std::move(j), 1.0);
}
double PathLength::operator()(const SubTrajectory& s, std::string& /*comment*/) const {
const auto& traj = s.trajectory();
if (traj == nullptr || traj->getWayPointCount() == 0)
return 0.0;
std::vector<const robot_model::JointModel*> joint_models;
joint_models.reserve(joints.size());
std::map<const moveit::core::JointModel*, double> weights;
const auto& first_waypoint = traj->getWayPoint(0);
for (auto& joint : joints) {
joint_models.push_back(first_waypoint.getJointModel(joint));
for (auto& joint_weight : joints) {
const moveit::core::JointModel* jm = first_waypoint.getJointModel(joint_weight.first);
if (jm)
weights.emplace(jm, joint_weight.second);
}
double path_length{ 0.0 };
@ -127,14 +133,66 @@ double PathLength::operator()(const SubTrajectory& s, std::string& /*comment*/)
if (joints.empty()) {
path_length += last.distance(curr);
} else {
for (const auto& model : joint_models) {
path_length += last.distance(curr, model);
for (const auto& item : weights) {
path_length += item.second * last.distance(curr, item.first);
}
}
}
return path_length;
}
DistanceToReference::DistanceToReference(const moveit_msgs::RobotState& ref, Mode m, std::map<std::string, double> w)
: reference(ref), weights(std::move(w)), mode(m) {}
DistanceToReference::DistanceToReference(const std::map<std::string, double>& ref, Mode m,
std::map<std::string, double> w)
: weights(std::move(w)), mode(m) {
reference.joint_state.name.reserve(ref.size());
reference.joint_state.position.reserve(ref.size());
for (auto& item : ref) {
reference.joint_state.name.push_back(item.first);
reference.joint_state.position.push_back(item.second);
}
reference.is_diff = true;
}
double DistanceToReference::operator()(const SubTrajectory& s, std::string& /*comment*/) const {
const auto& state = (mode == Mode::END_INTERFACE) ? s.end() : s.start();
const auto& traj = s.trajectory();
moveit::core::RobotState ref_state = state->scene()->getCurrentState();
moveit::core::robotStateMsgToRobotState(reference, ref_state, false);
std::map<const moveit::core::JointModel*, double> w;
for (auto& item : weights) {
const moveit::core::JointModel* jm = ref_state.getJointModel(item.first);
if (jm)
w.emplace(jm, item.second);
}
auto distance = [this, &ref_state, &w](const moveit::core::RobotState& state) {
if (weights.empty()) {
return ref_state.distance(state);
} else {
double accumulated = 0.0;
for (const auto& item : w)
accumulated += item.second * ref_state.distance(state, item.first);
return accumulated;
}
};
if (mode == Mode::START_INTERFACE || mode == Mode::END_INTERFACE || (mode == Mode::AUTO && (traj == nullptr))) {
return distance(state->scene()->getCurrentState());
} else {
double accumulated = 0.0;
for (size_t i = 0; i < traj->getWayPointCount(); ++i)
accumulated += distance(traj->getWayPoint(i));
accumulated /= traj->getWayPointCount();
return accumulated;
}
}
double TrajectoryDuration::operator()(const SubTrajectory& s, std::string& /*comment*/) const {
return s.trajectory() ? s.trajectory()->getDuration() : 0.0;
}
@ -148,9 +206,7 @@ double LinkMotion::operator()(const SubTrajectory& s, std::string& comment) cons
return 0.0;
if (!traj->getWayPoint(0).knowsFrameTransform(link_name)) {
boost::format desc("LinkMotionCost: frame '%1%' unknown in trajectory");
desc % link_name;
comment = desc.str();
comment = fmt::format("LinkMotionCost: frame '{}' unknown in trajectory", link_name);
return std::numeric_limits<double>::infinity();
}
@ -196,18 +252,9 @@ double Clearance::operator()(const SubTrajectory& s, std::string& comment) const
auto check_distance{ [=](const InterfaceState* state, const moveit::core::RobotState& robot) {
collision_detection::DistanceResult result;
if (with_world)
#if MOVEIT_HAS_COLLISION_ENV
state->scene()->getCollisionEnv()->distanceRobot(request, result, robot);
#else
state->scene()->getCollisionWorld()->distanceRobot(request, result, *state->scene()->getCollisionRobot(),
robot);
#endif
else
#if MOVEIT_HAS_COLLISION_ENV
state->scene()->getCollisionEnv()->distanceSelf(request, result, robot);
#else
state->scene()->getCollisionRobot()->distanceSelf(request, result, robot);
#endif
if (result.minimum_distance.distance <= 0) {
return result.minimum_distance;
@ -225,11 +272,10 @@ double Clearance::operator()(const SubTrajectory& s, std::string& comment) const
return result.minimum_distance;
} };
auto collision_comment{ [=](const auto& distance) {
boost::format desc{ PREFIX + "allegedly valid solution collides between '%1%' and '%2%'" };
desc % distance.link_names[0] % distance.link_names[1];
return desc.str();
} };
auto collision_comment = [=](const auto& distance) {
return fmt::format(PREFIX + "allegedly valid solution collides between '{}' and '{}'", distance.link_names[0],
distance.link_names[1]);
};
double distance{ 0.0 };
@ -241,13 +287,11 @@ double Clearance::operator()(const SubTrajectory& s, std::string& comment) const
return std::numeric_limits<double>::infinity();
}
distance = distance_data.distance;
if (!cumulative) {
boost::format desc{ PREFIX + "distance %1% between '%2%' and '%3%'" };
desc % distance % distance_data.link_names[0] % distance_data.link_names[1];
comment = desc.str();
} else {
comment = PREFIX + "cumulative distance " + std::to_string(distance);
}
if (!cumulative)
comment = fmt::format(PREFIX + "distance {} between '{}' and '{}'", distance, distance_data.link_names[0],
distance_data.link_names[1]);
else
comment = fmt::format(PREFIX + "cumulative distance {}", distance);
} else { // check trajectory
for (size_t i = 0; i < s.trajectory()->getWayPointCount(); ++i) {
auto distance_data = check_distance(state, s.trajectory()->getWayPoint(i));
@ -258,10 +302,7 @@ double Clearance::operator()(const SubTrajectory& s, std::string& comment) const
distance += distance_data.distance;
}
distance /= s.trajectory()->getWayPointCount();
boost::format desc(PREFIX + "average%1% distance: %2%");
desc % (cumulative ? " cumulative" : "") % distance;
comment = desc.str();
comment = fmt::format(PREFIX + "average{} distance: {}", (cumulative ? " cumulative" : ""), distance);
}
return distance_to_cost(distance);

View File

@ -49,6 +49,14 @@
#include <sstream>
#include <boost/bimap.hpp>
namespace ros {
namespace names {
bool isValidCharInName(char c); // unfortunately this is not declared in ros/names.h
} // namespace names
} // namespace ros
static const char* LOGGER = "introspection";
namespace moveit {
namespace task_constructor {
@ -57,8 +65,10 @@ std::string getTaskId(const TaskPrivate* task) {
std::ostringstream oss;
char our_hostname[256] = { 0 };
gethostname(our_hostname, sizeof(our_hostname) - 1);
// Hostname could have `-` as a character but this is an invalid character in ROS so we replace it with `_`
std::replace(std::begin(our_hostname), std::end(our_hostname), '-', '_');
// Replace all invalid ROS-name chars with an underscore
std::replace_if(
our_hostname, our_hostname + strlen(our_hostname),
[](const char ch) { return !ros::names::isValidCharInName(ch); }, '_');
oss << our_hostname << "_" << getpid() << "_" << reinterpret_cast<std::size_t>(task);
return oss.str();
}
@ -146,9 +156,7 @@ void Introspection::registerSolution(const SolutionBase& s) {
}
void Introspection::fillSolution(moveit_task_constructor_msgs::Solution& msg, const SolutionBase& s) {
s.fillMessage(msg, this);
s.start()->scene()->getPlanningSceneMsg(msg.start_scene);
s.toMsg(msg, this);
msg.task_id = impl->task_id_;
}
@ -163,7 +171,7 @@ void Introspection::publishAllSolutions(bool wait) {
publishSolution(*solution);
if (wait) {
std::cout << "Press <Enter> to continue ..." << std::endl;
std::cout << "Press <Enter> to continue ...\n";
int ch = getchar();
if (ch == 'q' || ch == 'Q')
break;
@ -200,6 +208,9 @@ uint32_t Introspection::stageId(const Stage* const s) const {
uint32_t Introspection::solutionId(const SolutionBase& s) {
auto result = impl->id_solution_bimap_.left.insert(std::make_pair(1 + impl->id_solution_bimap_.size(), &s));
if (result.second) // new entry
ROS_DEBUG_STREAM_NAMED(LOGGER, "new solution #" << result.first->first << " (" << s.creator()->name()
<< "): " << s.cost() << " " << s.comment());
return result.first->first;
}

View File

@ -1,7 +1,6 @@
#include <moveit/task_constructor/marker_tools.h>
#include <moveit/planning_scene/planning_scene.h>
#include <moveit/robot_state/robot_state.h>
#include <eigen_conversions/eigen_msg.h>
namespace vm = visualization_msgs;
@ -123,6 +122,8 @@ void generateMarkers(const moveit::core::RobotState& robot_state, const MarkerCa
auto element_handler = [&](const T& element) {
if (element && element->geometry) {
createGeometryMarker(m, *element->geometry, element->origin, materialColor(*model, materialName(*element)));
if (m.scale.x == 0 && m.scale.y == 0 && m.scale.z == 0)
return; // skip zero-size marker
m.pose = rviz_marker_tools::composePoses(robot_state.getGlobalLinkTransform(name), m.pose);
callback(m, name);
valid_found = true;

View File

@ -36,7 +36,6 @@
/* Authors: Luca Lach, Robert Haschke */
#include <moveit/task_constructor/merge.h>
#include <moveit/trajectory_processing/iterative_time_parameterization.h>
#include <boost/range/adaptor/transformed.hpp>
#include <boost/algorithm/string/join.hpp>
@ -106,7 +105,8 @@ moveit::core::JointModelGroup* merge(const std::vector<const moveit::core::Joint
robot_trajectory::RobotTrajectoryPtr
merge(const std::vector<robot_trajectory::RobotTrajectoryConstPtr>& sub_trajectories,
const robot_state::RobotState& base_state, moveit::core::JointModelGroup*& merged_group) {
const moveit::core::RobotState& base_state, moveit::core::JointModelGroup*& merged_group,
const trajectory_processing::TimeParameterization& time_parameterization) {
if (sub_trajectories.size() <= 1)
throw std::runtime_error("Expected multiple sub solutions");
@ -141,7 +141,7 @@ merge(const std::vector<robot_trajectory::RobotTrajectoryConstPtr>& sub_trajecto
std::vector<double> values;
values.reserve(max_num_vars);
auto merged_state = std::make_shared<robot_state::RobotState>(base_state);
auto merged_state = std::make_shared<moveit::core::RobotState>(base_state);
while (true) {
bool finished = true;
size_t index = merged_traj->getWayPointCount();
@ -151,7 +151,7 @@ merge(const std::vector<robot_trajectory::RobotTrajectoryConstPtr>& sub_trajecto
continue; // no more waypoints in this sub solution
finished = false; // there was a waypoint, continue while loop
const robot_state::RobotState& sub_state = sub->getWayPoint(index);
const moveit::core::RobotState& sub_state = sub->getWayPoint(index);
sub_state.copyJointGroupPositions(sub->getGroup(), values);
merged_state->setJointGroupPositions(sub->getGroup(), values);
}
@ -162,12 +162,11 @@ merge(const std::vector<robot_trajectory::RobotTrajectoryConstPtr>& sub_trajecto
// add waypoint without timing
merged_traj->addSuffixWayPoint(merged_state, 0.0);
// create new RobotState for next waypoint
merged_state = std::make_shared<robot_state::RobotState>(*merged_state);
merged_state = std::make_shared<moveit::core::RobotState>(*merged_state);
}
// add timing
trajectory_processing::IterativeParabolicTimeParameterization timing;
timing.computeTimeStamps(*merged_traj, 1.0, 1.0);
time_parameterization.computeTimeStamps(*merged_traj, 1.0, 1.0);
return merged_traj;
}
} // namespace task_constructor

View File

@ -37,7 +37,7 @@
*/
#include <moveit/task_constructor/properties.h>
#include <boost/format.hpp>
#include <moveit/task_constructor/fmt_p.h>
#include <functional>
#include <ros/console.h>
@ -73,7 +73,8 @@ public:
const Entry& entry(const std::type_index& type_index) const {
auto it = types_.find(type_index);
if (it == types_.end()) {
ROS_ERROR_STREAM_NAMED(LOGNAME, "Unregistered type: " << type_index.name());
ROS_WARN_STREAM_THROTTLE_NAMED(10.0, LOGNAME,
"Unregistered property type: " << boost::core::demangle(type_index.name()));
return dummy_;
}
return it->second;
@ -289,8 +290,8 @@ void PropertyMap::performInitFrom(Property::SourceFlags source, const PropertyMa
} catch (const Property::undefined&) {
}
ROS_DEBUG_STREAM_NAMED(LOGNAME, pair.first << ": " << p.initialized_from_ << " -> " << source << ": "
<< Property::serialize(value));
ROS_DEBUG_STREAM_NAMED(LOGNAME, fmt::format("{}: {} -> {}: {}", pair.first, p.initialized_from_, source,
Property::serialize(value)));
p.setCurrentValue(value);
p.initialized_from_ = source;
}
@ -316,9 +317,8 @@ Property::undefined::undefined(const std::string& name, const std::string& msg)
setName(name);
}
static boost::format TYPE_ERROR_FMT("type (%1%) doesn't match property's declared type (%2%)");
Property::type_error::type_error(const std::string& current_type, const std::string& declared_type)
: Property::error(boost::str(TYPE_ERROR_FMT % current_type % declared_type)) {}
: Property::error(fmt::format("type {} doesn't match property's declared type {}", current_type, declared_type)) {}
} // namespace task_constructor
} // namespace moveit

1
core/src/scope_guard Submodule

@ -0,0 +1 @@
Subproject commit 71a04528184db1749dd08ebbbf4daf3b5dca21fd

View File

@ -37,13 +37,15 @@
*/
#include <moveit/task_constructor/solvers/cartesian_path.h>
#include <moveit/task_constructor/moveit_compat.h>
#include <moveit/task_constructor/utils.h>
#include <moveit/planning_scene/planning_scene.h>
#include <moveit/trajectory_processing/iterative_time_parameterization.h>
#if MOVEIT_HAS_CARTESIAN_INTERPOLATOR
#include <moveit/trajectory_processing/time_parameterization.h>
#include <moveit/trajectory_processing/limit_cartesian_speed.h>
#include <moveit/kinematics_base/kinematics_base.h>
#include <moveit/robot_state/cartesian_interpolator.h>
#endif
#include <tf2_eigen/tf2_eigen.h>
using namespace trajectory_processing;
namespace moveit {
namespace task_constructor {
@ -51,31 +53,49 @@ namespace solvers {
CartesianPath::CartesianPath() {
auto& p = properties();
p.declare<geometry_msgs::PoseStamped>("ik_frame", "frame to move linearly (use for joint-space target)");
p.declare<double>("step_size", 0.01, "step size between consecutive waypoints");
p.declare<double>("jump_threshold", 1.5, "acceptable fraction of mean joint motion per step");
p.declare<moveit::core::CartesianPrecision>("precision", moveit::core::CartesianPrecision(),
"precision of linear path");
p.declare<double>("min_fraction", 1.0, "fraction of motion required for success");
p.declare<kinematics::KinematicsQueryOptions>("kinematics_options", kinematics::KinematicsQueryOptions(),
"KinematicsQueryOptions to pass to CartesianInterpolator");
}
void CartesianPath::init(const core::RobotModelConstPtr& /*robot_model*/) {}
bool CartesianPath::plan(const planning_scene::PlanningSceneConstPtr& from,
const planning_scene::PlanningSceneConstPtr& to, const moveit::core::JointModelGroup* jmg,
double timeout, robot_trajectory::RobotTrajectoryPtr& result,
const moveit_msgs::Constraints& path_constraints) {
const moveit::core::LinkModel* link = jmg->getOnlyOneEndEffectorTip();
if (!link) {
ROS_WARN_STREAM("no unique tip for joint model group: " << jmg->getName());
return false;
}
// reach pose of forward kinematics
return plan(from, *link, to->getCurrentState().getGlobalLinkTransform(link), jmg, timeout, result, path_constraints);
void CartesianPath::setIKFrame(const Eigen::Isometry3d& pose, const std::string& link) {
geometry_msgs::PoseStamped pose_msg;
pose_msg.header.frame_id = link;
pose_msg.pose = tf2::toMsg(pose);
setIKFrame(pose_msg);
}
bool CartesianPath::plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link,
const Eigen::Isometry3d& target, const moveit::core::JointModelGroup* jmg, double timeout,
robot_trajectory::RobotTrajectoryPtr& result,
const moveit_msgs::Constraints& path_constraints) {
PlannerInterface::Result CartesianPath::plan(const planning_scene::PlanningSceneConstPtr& from,
const planning_scene::PlanningSceneConstPtr& to,
const moveit::core::JointModelGroup* jmg, double timeout,
robot_trajectory::RobotTrajectoryPtr& result,
const moveit_msgs::Constraints& path_constraints) {
const auto& props = properties();
const moveit::core::LinkModel* link;
std::string error_msg;
Eigen::Isometry3d ik_pose_world;
if (!utils::getRobotTipForFrame(props.property("ik_frame"), *from, jmg, error_msg, link, ik_pose_world))
return { false, "CartesianPath: " + error_msg };
Eigen::Isometry3d offset = from->getCurrentState().getGlobalLinkTransform(link).inverse() * ik_pose_world;
// reach pose of forward kinematics
return plan(from, *link, offset, to->getCurrentState().getGlobalLinkTransform(link), jmg,
std::min(timeout, props.get<double>("timeout")), result, path_constraints);
}
PlannerInterface::Result CartesianPath::plan(const planning_scene::PlanningSceneConstPtr& from,
const moveit::core::LinkModel& link, const Eigen::Isometry3d& offset,
const Eigen::Isometry3d& target, const moveit::core::JointModelGroup* jmg,
double /*timeout*/, robot_trajectory::RobotTrajectoryPtr& result,
const moveit_msgs::Constraints& path_constraints) {
const auto& props = properties();
planning_scene::PlanningScenePtr sandbox_scene = from->diff();
@ -86,32 +106,42 @@ bool CartesianPath::plan(const planning_scene::PlanningSceneConstPtr& from, cons
const double* joint_positions) {
state->setJointGroupPositions(jmg, joint_positions);
state->update();
return !sandbox_scene->isStateColliding(const_cast<const robot_state::RobotState&>(*state), jmg->getName()) &&
return !sandbox_scene->isStateColliding(const_cast<const moveit::core::RobotState&>(*state), jmg->getName()) &&
kcs.decide(*state).satisfied;
};
std::vector<moveit::core::RobotStatePtr> trajectory;
#if MOVEIT_HAS_CARTESIAN_INTERPOLATOR
double achieved_fraction = moveit::core::CartesianInterpolator::computeCartesianPath(
&(sandbox_scene->getCurrentStateNonConst()), jmg, trajectory, &link, target, true,
moveit::core::MaxEEFStep(props.get<double>("step_size")),
moveit::core::JumpThreshold(props.get<double>("jump_threshold")), is_valid);
#else
double achieved_fraction = sandbox_scene->getCurrentStateNonConst().computeCartesianPath(
jmg, trajectory, &link, target, true, props.get<double>("step_size"), props.get<double>("jump_threshold"),
is_valid);
#endif
props.get<moveit::core::CartesianPrecision>("precision"), is_valid,
props.get<kinematics::KinematicsQueryOptions>("kinematics_options"), offset);
assert(!trajectory.empty()); // there should be at least the start state
result = std::make_shared<robot_trajectory::RobotTrajectory>(sandbox_scene->getRobotModel(), jmg);
for (const auto& waypoint : trajectory)
result->addSuffixWayPoint(waypoint, 0.0);
trajectory_processing::IterativeParabolicTimeParameterization timing;
timing.computeTimeStamps(*result, props.get<double>("max_velocity_scaling_factor"),
props.get<double>("max_acceleration_scaling_factor"));
double max_cartesian_speed = props.get<double>("max_cartesian_speed");
auto timing = props.get<TimeParameterizationPtr>("time_parameterization");
// compute timing to move the eef with constant speed
if (max_cartesian_speed > 0.0) {
if (trajectory_processing::limitMaxCartesianLinkSpeed(*result, max_cartesian_speed,
props.get<std::string>("cartesian_speed_limited_link"))) {
ROS_INFO_STREAM("successfully set max " << max_cartesian_speed << " [m/s] for link "
<< props.get<std::string>("cartesian_speed_limited_link"));
} else {
ROS_ERROR_STREAM("failed to set max speed for link_ "
<< props.get<std::string>("cartesian_speed_limited_link"));
}
} else if (timing)
timing->computeTimeStamps(*result, props.get<double>("max_velocity_scaling_factor"),
props.get<double>("max_acceleration_scaling_factor"));
return achieved_fraction >= props.get<double>("min_fraction");
if (achieved_fraction < props.get<double>("min_fraction")) {
return { false, "CartesianPath: min_fraction not met. Achieved: " + std::to_string(achieved_fraction) };
}
return { true, "achieved fraction: " + std::to_string(achieved_fraction) };
}
} // namespace solvers
} // namespace task_constructor

View File

@ -38,7 +38,7 @@
#include <moveit/task_constructor/solvers/joint_interpolation.h>
#include <moveit/planning_scene/planning_scene.h>
#include <moveit/trajectory_processing/iterative_time_parameterization.h>
#include <moveit/trajectory_processing/time_parameterization.h>
#include <chrono>
@ -46,18 +46,22 @@ namespace moveit {
namespace task_constructor {
namespace solvers {
using namespace trajectory_processing;
JointInterpolationPlanner::JointInterpolationPlanner() {
auto& p = properties();
p.declare<double>("max_step", 0.1, "max joint step");
// allow passing max_effort to GripperCommand actions via
p.declare<double>("max_effort", "max_effort for GripperCommand actions");
}
void JointInterpolationPlanner::init(const core::RobotModelConstPtr& /*robot_model*/) {}
bool JointInterpolationPlanner::plan(const planning_scene::PlanningSceneConstPtr& from,
const planning_scene::PlanningSceneConstPtr& to,
const moveit::core::JointModelGroup* jmg, double /*timeout*/,
robot_trajectory::RobotTrajectoryPtr& result,
const moveit_msgs::Constraints& /*path_constraints*/) {
PlannerInterface::Result JointInterpolationPlanner::plan(const planning_scene::PlanningSceneConstPtr& from,
const planning_scene::PlanningSceneConstPtr& to,
const moveit::core::JointModelGroup* jmg, double /*timeout*/,
robot_trajectory::RobotTrajectoryPtr& result,
const moveit_msgs::Constraints& /*path_constraints*/) {
const auto& props = properties();
// Get maximum joint distance
@ -72,7 +76,10 @@ bool JointInterpolationPlanner::plan(const planning_scene::PlanningSceneConstPtr
// add first point
result->addSuffixWayPoint(from->getCurrentState(), 0.0);
if (from->isStateColliding(from_state, jmg->getName()))
return false;
return { false, "Start state is in collision!" };
if (!from_state.satisfiesBounds(jmg))
return { false, "Start state is out of bounds!" };
moveit::core::RobotState waypoint(from_state);
double delta = d < 1e-6 ? 1.0 : props.get<double>("max_step") / d;
@ -81,28 +88,48 @@ bool JointInterpolationPlanner::plan(const planning_scene::PlanningSceneConstPtr
result->addSuffixWayPoint(waypoint, t);
if (from->isStateColliding(waypoint, jmg->getName()))
return false;
return { false, "Waypoint is in collision!" };
if (!waypoint.satisfiesBounds(jmg))
return { false, "Waypoint is out of bounds!" };
}
// add goal point
result->addSuffixWayPoint(to_state, 1.0);
if (from->isStateColliding(to_state, jmg->getName()))
return false;
return { false, "Goal state is in collision!" };
// add timing, TODO: use a generic method to add timing via plugins
trajectory_processing::IterativeParabolicTimeParameterization timing;
timing.computeTimeStamps(*result, props.get<double>("max_velocity_scaling_factor"),
props.get<double>("max_acceleration_scaling_factor"));
if (!to_state.satisfiesBounds(jmg))
return { false, "Goal state is out of bounds!" };
return true;
auto timing = props.get<TimeParameterizationPtr>("time_parameterization");
if (timing)
timing->computeTimeStamps(*result, props.get<double>("max_velocity_scaling_factor"),
props.get<double>("max_acceleration_scaling_factor"));
// set max_effort on first and last waypoint (first, because we might reverse the trajectory)
const auto& max_effort = properties().get("max_effort");
if (!max_effort.empty()) {
double effort = boost::any_cast<double>(max_effort);
for (const auto* jm : jmg->getActiveJointModels()) {
if (jm->getVariableCount() != 1)
continue;
result->getFirstWayPointPtr()->dropAccelerations();
result->getFirstWayPointPtr()->setJointEfforts(jm, &effort);
result->getLastWayPointPtr()->dropAccelerations();
result->getLastWayPointPtr()->setJointEfforts(jm, &effort);
}
}
return { true, "" };
}
bool JointInterpolationPlanner::plan(const planning_scene::PlanningSceneConstPtr& from,
const moveit::core::LinkModel& link, const Eigen::Isometry3d& target_eigen,
const moveit::core::JointModelGroup* jmg, double timeout,
robot_trajectory::RobotTrajectoryPtr& result,
const moveit_msgs::Constraints& path_constraints) {
const auto start_time = std::chrono::steady_clock::now();
PlannerInterface::Result JointInterpolationPlanner::plan(
const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link,
const Eigen::Isometry3d& offset, const Eigen::Isometry3d& target, const moveit::core::JointModelGroup* jmg,
double timeout, robot_trajectory::RobotTrajectoryPtr& result, const moveit_msgs::Constraints& path_constraints) {
timeout = std::min(timeout, properties().get<double>("timeout"));
const auto deadline = std::chrono::steady_clock::now() + std::chrono::duration<double, std::ratio<1>>(timeout);
auto to{ from->diff() };
@ -116,17 +143,12 @@ bool JointInterpolationPlanner::plan(const planning_scene::PlanningSceneConstPtr
return to->isStateValid(*robot_state, constraints, jmg->getName());
} };
if (!to->getCurrentStateNonConst().setFromIK(jmg, target_eigen, link.getName(), timeout, is_valid)) {
// TODO(v4hn): planners need a way to add feedback to failing plans
// in case of an invalid solution feedback should include unwanted collisions or violated constraints
ROS_WARN_NAMED("JointInterpolationPlanner", "IK failed for pose target");
return false;
}
if (!to->getCurrentStateNonConst().setFromIK(jmg, target * offset.inverse(), link.getName(), timeout, is_valid))
return { false, "IK failed for pose target." };
to->getCurrentStateNonConst().update();
timeout = std::chrono::duration<double>(std::chrono::steady_clock::now() - start_time).count();
if (timeout <= 0.0)
return false;
if (std::chrono::steady_clock::now() >= deadline)
return { false, "timeout" };
return plan(from, to, jmg, timeout, result, path_constraints);
}

View File

@ -0,0 +1,107 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2023, Bielefeld University
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Bielefeld University nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
/* Authors: Michael Goerner, Robert Haschke
Desc: generate and validate a straight-line Cartesian path
*/
#include <moveit/task_constructor/solvers/multi_planner.h>
#include <moveit/robot_trajectory/robot_trajectory.h>
#include <chrono>
namespace moveit {
namespace task_constructor {
namespace solvers {
void MultiPlanner::init(const core::RobotModelConstPtr& robot_model) {
for (const auto& p : *this)
p->init(robot_model);
}
PlannerInterface::Result MultiPlanner::plan(const planning_scene::PlanningSceneConstPtr& from,
const planning_scene::PlanningSceneConstPtr& to,
const moveit::core::JointModelGroup* jmg, double timeout,
robot_trajectory::RobotTrajectoryPtr& result,
const moveit_msgs::Constraints& path_constraints) {
double remaining_time = std::min(timeout, properties().get<double>("timeout"));
auto start_time = std::chrono::steady_clock::now();
std::string comment = "No planner specified";
for (const auto& p : *this) {
if (remaining_time < 0)
return { false, "timeout" };
if (result)
result->clear();
auto r = p->plan(from, to, jmg, remaining_time, result, path_constraints);
if (r)
return r;
else
comment = r.message;
auto now = std::chrono::steady_clock::now();
remaining_time -= std::chrono::duration<double>(now - start_time).count();
start_time = now;
}
return { false, comment };
}
PlannerInterface::Result MultiPlanner::plan(const planning_scene::PlanningSceneConstPtr& from,
const moveit::core::LinkModel& link, const Eigen::Isometry3d& offset,
const Eigen::Isometry3d& target, const moveit::core::JointModelGroup* jmg,
double timeout, robot_trajectory::RobotTrajectoryPtr& result,
const moveit_msgs::Constraints& path_constraints) {
double remaining_time = std::min(timeout, properties().get<double>("timeout"));
auto start_time = std::chrono::steady_clock::now();
std::string comment = "No planner specified";
for (const auto& p : *this) {
if (remaining_time < 0)
return { false, "timeout" };
if (result)
result->clear();
auto r = p->plan(from, link, offset, target, jmg, remaining_time, result, path_constraints);
if (r)
return r;
else
comment = r.message;
auto now = std::chrono::steady_clock::now();
remaining_time -= std::chrono::duration<double>(now - start_time).count();
start_time = now;
}
return { false, comment };
}
} // namespace solvers
} // namespace task_constructor
} // namespace moveit

View File

@ -52,10 +52,10 @@ struct PlannerCache
{
using PlannerID = std::tuple<std::string, std::string>;
using PlannerMap = std::map<PlannerID, std::weak_ptr<planning_pipeline::PlanningPipeline> >;
using ModelList = std::list<std::pair<std::weak_ptr<const robot_model::RobotModel>, PlannerMap> >;
using ModelList = std::list<std::pair<std::weak_ptr<const moveit::core::RobotModel>, PlannerMap> >;
ModelList cache_;
PlannerMap::mapped_type& retrieve(const robot_model::RobotModelConstPtr& model, const PlannerID& id) {
PlannerMap::mapped_type& retrieve(const moveit::core::RobotModelConstPtr& model, const PlannerID& id) {
// find model in cache_ and remove expired entries while doing so
ModelList::iterator model_it = cache_.begin();
while (model_it != cache_.end()) {
@ -143,19 +143,22 @@ void initMotionPlanRequest(moveit_msgs::MotionPlanRequest& req, const PropertyMa
const moveit::core::JointModelGroup* jmg, double timeout) {
req.group_name = jmg->getName();
req.planner_id = p.get<std::string>("planner");
req.allowed_planning_time = timeout;
req.allowed_planning_time = std::min(timeout, p.get<double>("timeout"));
req.start_state.is_diff = true; // we don't specify an extra start state
req.num_planning_attempts = p.get<uint>("num_planning_attempts");
req.max_velocity_scaling_factor = p.get<double>("max_velocity_scaling_factor");
req.max_acceleration_scaling_factor = p.get<double>("max_acceleration_scaling_factor");
req.cartesian_speed_limited_link = p.get<std::string>("cartesian_speed_limited_link");
req.max_cartesian_speed = p.get<double>("max_cartesian_speed");
req.workspace_parameters = p.get<moveit_msgs::WorkspaceParameters>("workspace_parameters");
}
bool PipelinePlanner::plan(const planning_scene::PlanningSceneConstPtr& from,
const planning_scene::PlanningSceneConstPtr& to, const moveit::core::JointModelGroup* jmg,
double timeout, robot_trajectory::RobotTrajectoryPtr& result,
const moveit_msgs::Constraints& path_constraints) {
PlannerInterface::Result PipelinePlanner::plan(const planning_scene::PlanningSceneConstPtr& from,
const planning_scene::PlanningSceneConstPtr& to,
const moveit::core::JointModelGroup* jmg, double timeout,
robot_trajectory::RobotTrajectoryPtr& result,
const moveit_msgs::Constraints& path_constraints) {
const auto& props = properties();
moveit_msgs::MotionPlanRequest req;
initMotionPlanRequest(req, props, jmg, timeout);
@ -165,23 +168,22 @@ bool PipelinePlanner::plan(const planning_scene::PlanningSceneConstPtr& from,
props.get<double>("goal_joint_tolerance"));
req.path_constraints = path_constraints;
::planning_interface::MotionPlanResponse res;
bool success = planner_->generatePlan(from, req, res);
result = res.trajectory_;
return success;
return plan(from, req, result);
}
bool PipelinePlanner::plan(const planning_scene::PlanningSceneConstPtr& from, const moveit::core::LinkModel& link,
const Eigen::Isometry3d& target_eigen, const moveit::core::JointModelGroup* jmg,
double timeout, robot_trajectory::RobotTrajectoryPtr& result,
const moveit_msgs::Constraints& path_constraints) {
PlannerInterface::Result PipelinePlanner::plan(const planning_scene::PlanningSceneConstPtr& from,
const moveit::core::LinkModel& link, const Eigen::Isometry3d& offset,
const Eigen::Isometry3d& target_eigen,
const moveit::core::JointModelGroup* jmg, double timeout,
robot_trajectory::RobotTrajectoryPtr& result,
const moveit_msgs::Constraints& path_constraints) {
const auto& props = properties();
moveit_msgs::MotionPlanRequest req;
initMotionPlanRequest(req, props, jmg, timeout);
geometry_msgs::PoseStamped target;
target.header.frame_id = from->getPlanningFrame();
target.pose = tf2::toMsg(target_eigen);
target.pose = tf2::toMsg(target_eigen * offset.inverse());
req.goal_constraints.resize(1);
req.goal_constraints[0] = kinematic_constraints::constructGoalConstraints(
@ -189,11 +191,18 @@ bool PipelinePlanner::plan(const planning_scene::PlanningSceneConstPtr& from, co
props.get<double>("goal_orientation_tolerance"));
req.path_constraints = path_constraints;
return plan(from, req, result);
}
PlannerInterface::Result PipelinePlanner::plan(const planning_scene::PlanningSceneConstPtr& from,
const moveit_msgs::MotionPlanRequest& req,
robot_trajectory::RobotTrajectoryPtr& result) {
::planning_interface::MotionPlanResponse res;
bool success = planner_->generatePlan(from, req, res);
result = res.trajectory_;
return success;
return { success, success ? std::string() : static_cast<std::string>(res.error_code_) };
}
} // namespace solvers
} // namespace task_constructor
} // namespace moveit

View File

@ -37,6 +37,9 @@
*/
#include <moveit/task_constructor/solvers/planner_interface.h>
#include <moveit/trajectory_processing/time_optimal_trajectory_generation.h>
using namespace trajectory_processing;
namespace moveit {
namespace task_constructor {
@ -44,8 +47,12 @@ namespace solvers {
PlannerInterface::PlannerInterface() {
auto& p = properties();
p.declare<double>("timeout", std::numeric_limits<double>::infinity(), "timeout for planner (s)");
p.declare<double>("max_velocity_scaling_factor", 1.0, "scale down max velocity by this factor");
p.declare<double>("max_acceleration_scaling_factor", 1.0, "scale down max acceleration by this factor");
p.declare<double>("max_cartesian_speed", 0.0, "maximum cartesian speed");
p.declare<std::string>("cartesian_speed_limited_link", "", "link with limited cartesian speed");
p.declare<TimeParameterizationPtr>("time_parameterization", std::make_shared<TimeOptimalTrajectoryGeneration>());
}
} // namespace solvers
} // namespace task_constructor

View File

@ -38,14 +38,12 @@
#include <moveit/task_constructor/stage_p.h>
#include <moveit/task_constructor/container_p.h>
#include <moveit/task_constructor/introspection.h>
#include <moveit/task_constructor/moveit_compat.h>
#include <moveit/task_constructor/fmt_p.h>
#include <moveit/planning_scene/planning_scene.h>
#include <ros/console.h>
#include <boost/format.hpp>
#include <iostream>
#include <iomanip>
#include <algorithm>
@ -92,9 +90,9 @@ const char* InitStageException::what() const noexcept {
}
std::ostream& operator<<(std::ostream& os, const InitStageException& e) {
os << "Error initializing stage" << (e.errors_.size() > 1 ? "s" : "") << ":" << std::endl;
os << "Error initializing stage" << (e.errors_.size() > 1 ? "s" : "") << ":\n ";
for (const auto& pair : e.errors_)
os << pair.first->name() << ": " << pair.second << std::endl;
os << pair.first->name() << ": " << pair.second << '\n';
return os;
}
@ -104,7 +102,8 @@ StagePrivate::StagePrivate(Stage* me, const std::string& name)
, cost_term_{ std::make_unique<CostTerm>() }
, total_compute_time_{}
, parent_{ nullptr }
, introspection_{ nullptr } {}
, introspection_{ nullptr }
, preempt_requested_{ nullptr } {}
StagePrivate& StagePrivate::operator=(StagePrivate&& other) {
assert(typeid(*this) == typeid(other));
@ -126,8 +125,8 @@ StagePrivate& StagePrivate::operator=(StagePrivate&& other) {
prev_ends_ = std::move(other.prev_ends_);
next_starts_ = std::move(other.next_starts_);
parent_ = std::move(other.parent_);
it_ = std::move(other.it_);
parent_ = other.parent_;
it_ = other.it_;
other.unparent();
return *this;
@ -150,12 +149,11 @@ void StagePrivate::validateConnectivity() const {
// check that the required interface is provided
InterfaceFlags required = requiredInterface();
InterfaceFlags actual = interfaceFlags();
if ((required & actual) != required) {
boost::format desc("actual interface %1% %2% does not match required interface %3% %4%");
desc % flowSymbol<START_IF_MASK>(actual) % flowSymbol<END_IF_MASK>(actual);
desc % flowSymbol<START_IF_MASK>(required) % flowSymbol<END_IF_MASK>(required);
throw InitStageException(*me(), desc.str());
}
if ((required & actual) != required)
throw InitStageException(*me(),
fmt::format("actual interface {} {} does not match required interface {} {}",
flowSymbol<START_IF_MASK>(actual), flowSymbol<END_IF_MASK>(actual),
flowSymbol<START_IF_MASK>(required), flowSymbol<END_IF_MASK>(required)));
}
bool StagePrivate::storeSolution(const SolutionBasePtr& solution, const InterfaceState* from,
@ -220,28 +218,32 @@ void StagePrivate::sendBackward(InterfaceState&& from, const InterfaceState& to,
newSolution(solution);
}
void StagePrivate::spawn(InterfaceState&& state, const SolutionBasePtr& solution) {
void StagePrivate::spawn(InterfaceState&& from, InterfaceState&& to, const SolutionBasePtr& solution) {
assert(prevEnds() && nextStarts());
computeCost(state, state, *solution);
computeCost(from, to, *solution);
if (!storeSolution(solution, nullptr, nullptr))
return; // solution dropped
auto from = states_.insert(states_.end(), InterfaceState(state)); // copy
auto to = states_.insert(states_.end(), std::move(state));
auto from_it = states_.insert(states_.end(), std::move(from));
auto to_it = states_.insert(states_.end(), std::move(to));
solution->setStartState(*from);
solution->setEndState(*to);
solution->setStartState(*from_it);
solution->setEndState(*to_it);
if (!solution->isFailure()) {
prevEnds()->add(*from);
nextStarts()->add(*to);
prevEnds()->add(*from_it);
nextStarts()->add(*to_it);
}
newSolution(solution);
}
void StagePrivate::spawn(InterfaceState&& state, const SolutionBasePtr& solution) {
spawn(InterfaceState(state), std::move(state), solution);
}
void StagePrivate::connect(const InterfaceState& from, const InterfaceState& to, const SolutionBasePtr& solution) {
computeCost(from, to, *solution);
@ -309,6 +311,8 @@ Stage::Stage(StagePrivate* impl) : pimpl_(impl) {
auto& p = properties();
p.declare<double>("timeout", "timeout per run (s)");
p.declare<std::string>("marker_ns", name(), "marker namespace");
p.declare<TrajectoryExecutionInfo>("trajectory_execution_info", TrajectoryExecutionInfo(),
"settings used when executing the trajectory");
p.declare<std::set<std::string>>("forwarded_properties", std::set<std::string>(),
"set of interface properties to forward");
@ -343,6 +347,7 @@ void Stage::reset() {
impl->next_starts_.reset();
// reset inherited properties
impl->properties_.reset();
impl->total_compute_time_ = std::chrono::duration<double>::zero();
}
void Stage::init(const moveit::core::RobotModelConstPtr& /* robot_model */) {
@ -352,7 +357,7 @@ void Stage::init(const moveit::core::RobotModelConstPtr& /* robot_model */) {
impl->properties_.reset();
if (impl->parent()) {
try {
ROS_DEBUG_STREAM_NAMED("Properties", "init '" << name() << "'");
ROS_DEBUG_STREAM_NAMED("Properties", fmt::format("init '{}'", name()));
impl->properties_.performInitFrom(PARENT, impl->parent()->properties());
} catch (const Property::error& e) {
std::ostringstream oss;
@ -381,6 +386,9 @@ uint32_t Stage::introspectionId() const {
throw std::runtime_error("Task is not initialized yet or Introspection was disabled.");
return const_cast<const moveit::task_constructor::Introspection*>(pimpl_->introspection_)->stageId(this);
}
Introspection* Stage::introspection() const {
return pimpl_->introspection_;
}
void Stage::forwardProperties(const InterfaceState& source, InterfaceState& dest) {
const PropertyMap& src = source.properties();
@ -510,14 +518,14 @@ void PropagatingEitherWayPrivate::initInterface(PropagatingEitherWay::Direction
case PropagatingEitherWay::FORWARD:
required_interface_ = PROPAGATE_FORWARDS;
if (!starts_) // keep existing interface if possible
starts_.reset(new Interface());
starts_ = std::make_shared<Interface>();
ends_.reset();
return;
case PropagatingEitherWay::BACKWARD:
required_interface_ = PROPAGATE_BACKWARDS;
starts_.reset();
if (!ends_) // keep existing interface if possible
ends_.reset(new Interface());
ends_ = std::make_shared<Interface>();
return;
case PropagatingEitherWay::AUTO:
required_interface_ = UNKNOWN;
@ -534,17 +542,16 @@ void PropagatingEitherWayPrivate::resolveInterface(InterfaceFlags expected) {
dir = PropagatingEitherWay::FORWARD;
else if ((expected & START_IF_MASK) == WRITES_PREV_END || (expected & END_IF_MASK) == READS_END)
dir = PropagatingEitherWay::BACKWARD;
else {
boost::format desc("propagator cannot satisfy expected interface %1% %2%");
desc % flowSymbol<START_IF_MASK>(expected) % flowSymbol<END_IF_MASK>(expected);
throw InitStageException(*me(), desc.str());
}
if (configured_dir_ != PropagatingEitherWay::AUTO && dir != configured_dir_) {
boost::format desc("configured interface (%1% %2%) does not match expected one (%3% %4%)");
desc % flowSymbol<START_IF_MASK>(required_interface_) % flowSymbol<END_IF_MASK>(required_interface_);
desc % flowSymbol<START_IF_MASK>(expected) % flowSymbol<END_IF_MASK>(expected);
throw InitStageException(*me(), desc.str());
}
else
throw InitStageException(*me(),
fmt::format("propagator cannot satisfy expected interface {} {}",
flowSymbol<START_IF_MASK>(expected), flowSymbol<END_IF_MASK>(expected)));
if (configured_dir_ != PropagatingEitherWay::AUTO && dir != configured_dir_)
throw InitStageException(*me(),
fmt::format("configured interface ({} {}) does not match expected one ({} {})",
flowSymbol<START_IF_MASK>(required_interface_),
flowSymbol<END_IF_MASK>(required_interface_),
flowSymbol<START_IF_MASK>(expected), flowSymbol<END_IF_MASK>(expected)));
initInterface(dir);
}
@ -616,6 +623,14 @@ template void PropagatingEitherWay::send<Interface::FORWARD>(const InterfaceStat
template void PropagatingEitherWay::send<Interface::BACKWARD>(const InterfaceState& start, InterfaceState&& end,
SubTrajectory&& trajectory);
void PropagatingEitherWay::computeForward(const InterfaceState& from) {
computeGeneric<Interface::FORWARD>(from);
}
void PropagatingEitherWay::computeBackward(const InterfaceState& to) {
computeGeneric<Interface::BACKWARD>(to);
}
template <Interface::Direction dir>
void PropagatingEitherWay::computeGeneric(const InterfaceState& start) {
planning_scene::PlanningScenePtr end;
@ -670,6 +685,10 @@ void GeneratorPrivate::compute() {
Generator::Generator(GeneratorPrivate* impl) : ComputeBase(impl) {}
Generator::Generator(const std::string& name) : Generator(new GeneratorPrivate(this, name)) {}
void Generator::spawn(InterfaceState&& from, InterfaceState&& to, SubTrajectory&& t) {
pimpl()->spawn(std::move(from), std::move(to), std::make_shared<SubTrajectory>(std::move(t)));
}
void Generator::spawn(InterfaceState&& state, SubTrajectory&& t) {
pimpl()->spawn(std::move(state), std::make_shared<SubTrajectory>(std::move(t)));
}
@ -715,10 +734,10 @@ void MonitoringGeneratorPrivate::solutionCB(const SolutionBase& s) {
}
ConnectingPrivate::ConnectingPrivate(Connecting* me, const std::string& name) : ComputeBasePrivate(me, name) {
starts_.reset(new Interface(std::bind(&ConnectingPrivate::newState<Interface::BACKWARD>, this, std::placeholders::_1,
std::placeholders::_2)));
ends_.reset(new Interface(std::bind(&ConnectingPrivate::newState<Interface::FORWARD>, this, std::placeholders::_1,
std::placeholders::_2)));
starts_ = std::make_shared<Interface>(std::bind(&ConnectingPrivate::newState<Interface::BACKWARD>, this,
std::placeholders::_1, std::placeholders::_2));
ends_ = std::make_shared<Interface>(
std::bind(&ConnectingPrivate::newState<Interface::FORWARD>, this, std::placeholders::_1, std::placeholders::_2));
}
InterfaceFlags ConnectingPrivate::requiredInterface() const {
@ -736,58 +755,120 @@ ConnectingPrivate::StatePair ConnectingPrivate::make_pair<Interface::FORWARD>(In
return StatePair(second, first);
}
template <Interface::Direction other>
void ConnectingPrivate::newState(Interface::iterator it, bool updated) {
if (updated) { // many pairs might be affected: resort
if (it->priority().pruned())
// remove all pending pairs involving this state
pending.remove_if([it](const StatePair& p) { return std::get<opposite<other>()>(p) == it; });
else
// TODO(v4hn): If a state becomes reenabled, this skips all previously removed pairs, right?
pending.sort();
} else { // new state: insert all pairs with other interface
assert(it->priority().enabled()); // new solutions are feasible, aren't they?
InterfacePtr other_interface = pullInterface(other);
for (Interface::iterator oit = other_interface->begin(), oend = other_interface->end(); oit != oend; ++oit) {
// Don't re-enable states that are marked DISABLED
if (static_cast<Connecting*>(me_)->compatible(*it, *oit)) {
// re-enable the opposing state oit if its status is FAILED
if (oit->priority().status() == InterfaceState::Status::FAILED)
oit->owner()->updatePriority(&*oit,
InterfaceState::Priority(oit->priority(), InterfaceState::Status::ENABLED));
pending.insert(make_pair<other>(it, oit));
template <Interface::Direction dir>
void ConnectingPrivate::newState(Interface::iterator it, Interface::UpdateFlags updated) {
auto parent_pimpl = parent()->pimpl();
// disable current interface to break loop (jumping back and forth between both interfaces)
// this will be checked by notifyEnabled() below
Interface::DisableNotify disable_source_interface(*pullInterface<dir>());
if (updated) {
if (updated.testFlag(Interface::STATUS) && // only perform these costly operations if needed
pullInterface<opposite<dir>()>()->notifyEnabled()) // suppressing recursive loop?
{
// If status has changed, propagate the update to the opposite side
auto status = it->priority().status();
if (status == InterfaceState::Status::PRUNED) // PRUNED becomes ARMED on opposite side
status = InterfaceState::Status::ARMED; // (only for pending state pairs)
for (const auto& candidate : this->pending) {
if (std::get<opposite<dir>()>(candidate) != it) // only consider pairs with source state == state
continue;
auto oit = std::get<dir>(candidate); // opposite target state
auto ostatus = oit->priority().status();
if (ostatus != status) {
if (status != InterfaceState::Status::ENABLED) {
// quicker check for hasPendingOpposites(): search in it->owner() for an enabled alternative
bool cancel = false; // if found, cancel propagation of new status
for (const auto alternative : *it->owner())
if ((cancel = alternative->priority().enabled()))
break;
if (cancel)
continue;
}
// pass creator=nullptr to skip hasPendingOpposites() check
parent_pimpl->setStatus<opposite<dir>()>(nullptr, nullptr, &*oit, status);
}
}
}
// many pairs will have changed priorities: resort pending list
pending.sort();
} else { // new state: insert all pairs with other interface
assert(it->priority().enabled()); // new solutions are feasible, aren't they?
InterfacePtr other_interface = pullInterface<dir>();
bool have_enabled_opposites = false;
// other interface states to re-enable (post-poned because otherwise order in other_interface changes during loop)
std::vector<Interface::iterator> oit_to_enable;
for (Interface::iterator oit = other_interface->begin(), oend = other_interface->end(); oit != oend; ++oit) {
if (!static_cast<Connecting*>(me_)->compatible(*it, *oit))
continue;
// re-enable the opposing state oit (and its associated solution branch) if its status is ARMED
// https://github.com/moveit/moveit_task_constructor/pull/309#issuecomment-974636202
if (oit->priority().status() == InterfaceState::Status::ARMED) {
oit_to_enable.push_back(oit);
have_enabled_opposites = true;
}
if (oit->priority().enabled())
have_enabled_opposites = true;
// Remember all pending states, regardless of their status!
pending.insert(make_pair<dir>(it, oit));
}
// actually re-enable other interface states, which were scheduled for re-enabling above
for (Interface::iterator oit : oit_to_enable)
parent_pimpl->setStatus<opposite<dir>()>(me(), &*it, &*oit, InterfaceState::Status::ENABLED);
if (!have_enabled_opposites) // prune new state and associated branch if necessary
// pass creator=nullptr to skip hasPendingOpposites() check as we did this here already
parent_pimpl->setStatus<dir>(nullptr, nullptr, &*it, InterfaceState::Status::ARMED);
}
// std::cerr << name_ << ": ";
// printPendingPairs(std::cerr);
// std::cerr << std::endl;
#if 0
auto& os = std::cerr;
for (auto d : { Interface::FORWARD, Interface::BACKWARD }) {
if (d == Interface::FORWARD)
os << " " << std::setw(10) << std::left << this->name();
else
os << std::setw(12) << std::right << "";
if (dir != d)
os << (updated ? " !" : " +");
else
os << " ";
os << d << " " << this->pullInterface(d) << ": " << *this->pullInterface(d) << '\n';
}
os << std::setw(15) << " ";
os << pendingPairsPrinter() << '\n';
#endif
}
// Check whether there are pending feasible states that could connect to source.
// If not, we exhausted all solution candidates for source and thus should mark it as failure.
// Check whether there are pending feasible states (other than source) that could connect to target.
// If not, we exhausted all solution candidates for target and thus should mark it as failure.
template <Interface::Direction dir>
inline bool ConnectingPrivate::hasPendingOpposites(const InterfaceState* source) const {
inline bool ConnectingPrivate::hasPendingOpposites(const InterfaceState* source, const InterfaceState* target) const {
for (const auto& candidate : this->pending) {
static_assert(Interface::FORWARD == 0, "This code assumes FORWARD=0, BACKWARD=1. Don't change their order!");
const auto src = std::get<dir>(candidate);
static_assert(Interface::BACKWARD == 1, "This code assumes FORWARD=0, BACKWARD=1. Don't change their order!");
const auto tgt = std::get<opposite<dir>()>(candidate);
static_assert(Interface::FORWARD == 0 && Interface::BACKWARD == 1,
"This code assumes FORWARD=0, BACKWARD=1. Don't change their order!");
const InterfaceState* src = &*std::get<dir>(candidate);
const InterfaceState* tgt = &*std::get<opposite<dir>()>(candidate);
if (&*src == source && tgt->priority().enabled())
if (tgt == target && src != source && src->priority().enabled())
return true;
// early stopping when only infeasible pairs are to come
if (!std::get<0>(candidate)->priority().enabled())
if (!std::get<0>(candidate)->priority().enabled() || !std::get<1>(candidate)->priority().enabled())
break;
}
return false;
}
// explicitly instantiate templates for both directions
template bool ConnectingPrivate::hasPendingOpposites<Interface::FORWARD>(const InterfaceState* source) const;
template bool ConnectingPrivate::hasPendingOpposites<Interface::BACKWARD>(const InterfaceState* source) const;
template bool ConnectingPrivate::hasPendingOpposites<Interface::FORWARD>(const InterfaceState* start,
const InterfaceState* end) const;
template bool ConnectingPrivate::hasPendingOpposites<Interface::BACKWARD>(const InterfaceState* end,
const InterfaceState* start) const;
bool ConnectingPrivate::canCompute() const {
// ROS_DEBUG_STREAM("canCompute " << name() << ": " << pendingPairsPrinter());
// Do we still have feasible pending state pairs?
return !pending.empty() && pending.front().first->priority().enabled() &&
pending.front().second->priority().enabled();
@ -801,25 +882,17 @@ void ConnectingPrivate::compute() {
static_cast<Connecting*>(me_)->compute(from, to);
}
std::ostream& ConnectingPrivate::printPendingPairs(std::ostream& os) const {
static const char* red = "\033[31m";
static const char* reset = "\033[m";
for (const auto& candidate : pending) {
if (!candidate.first->priority().enabled() || !candidate.second->priority().enabled())
os << " " << red;
// find indeces of InterfaceState pointers in start/end Interfaces
unsigned int first = 0, second = 0;
std::find_if(starts()->begin(), starts()->end(), [&](const InterfaceState* s) {
++first;
return &*candidate.first == s;
});
std::find_if(ends()->begin(), ends()->end(), [&](const InterfaceState* s) {
++second;
return &*candidate.second == s;
});
os << first << ":" << second << " ";
std::ostream& operator<<(std::ostream& os, const PendingPairsPrinter& p) {
const auto* impl = p.instance_;
const char* reset = InterfaceState::colorForStatus(3);
for (const auto& candidate : impl->pending) {
size_t first = getIndex(*impl->starts(), candidate.first);
size_t second = getIndex(*impl->ends(), candidate.second);
os << InterfaceState::colorForStatus(candidate.first->priority().status()) << first << reset << ":"
<< InterfaceState::colorForStatus(candidate.second->priority().status()) << second << reset << " ";
}
os << reset;
if (impl->pending.empty())
os << "---";
return os;
}
@ -835,40 +908,33 @@ bool Connecting::compatible(const InterfaceState& from_state, const InterfaceSta
const planning_scene::PlanningSceneConstPtr& from = from_state.scene();
const planning_scene::PlanningSceneConstPtr& to = to_state.scene();
if (from->getWorld()->size() != to->getWorld()->size()) {
ROS_DEBUG_STREAM_NAMED("Connecting", name() << ": different number of collision objects");
auto false_with_debug = [](auto... args) {
ROS_DEBUG_STREAM_NAMED("Connecting", fmt::format(args...));
return false;
}
};
if (from->getWorld()->size() != to->getWorld()->size())
return false_with_debug("{}: different number of collision objects", name());
// both scenes should have the same set of collision objects, at the same location
for (const auto& from_object_pair : *from->getWorld()) {
const std::string& from_object_name = from_object_pair.first;
const collision_detection::World::ObjectPtr& from_object = from_object_pair.second;
const collision_detection::World::ObjectConstPtr& to_object = to->getWorld()->getObject(from_object_name);
if (!to_object) {
ROS_DEBUG_STREAM_NAMED("Connecting", name() << ": object missing: " << from_object_name);
return false;
}
if (!to_object)
return false_with_debug("{}: object missing: {}", name(), from_object_name);
#if MOVEIT_HAS_OBJECT_POSE
if (!(from_object->pose_.matrix() - to_object->pose_.matrix()).isZero(1e-4)) {
ROS_DEBUG_STREAM_NAMED("Connecting", name() << ": different object pose: " << from_object_name);
return false; // transforms do not match
}
#endif
if (!(from_object->pose_.matrix() - to_object->pose_.matrix()).isZero(1e-4))
return false_with_debug("{}: different object pose: {}", name(), from_object_name);
if (from_object->shape_poses_.size() != to_object->shape_poses_.size()) {
ROS_DEBUG_STREAM_NAMED("Connecting", name() << ": different object shapes: " << from_object_name);
return false; // shapes not matching
}
if (from_object->shape_poses_.size() != to_object->shape_poses_.size())
return false_with_debug("{}: different object shapes: {}", name(), from_object_name);
for (auto from_it = from_object->shape_poses_.cbegin(), from_end = from_object->shape_poses_.cend(),
to_it = to_object->shape_poses_.cbegin();
from_it != from_end; ++from_it, ++to_it)
if (!(from_it->matrix() - to_it->matrix()).isZero(1e-4)) {
ROS_DEBUG_STREAM_NAMED("Connecting", name() << ": different shape pose: " << from_object_name);
return false; // transforms do not match
}
if (!(from_it->matrix() - to_it->matrix()).isZero(1e-4))
return false_with_debug("{}: different shape pose: {}", name(), from_object_name);
}
// Also test for attached objects which have a different storage
@ -876,47 +942,32 @@ bool Connecting::compatible(const InterfaceState& from_state, const InterfaceSta
std::vector<const moveit::core::AttachedBody*> to_attached;
from->getCurrentState().getAttachedBodies(from_attached);
to->getCurrentState().getAttachedBodies(to_attached);
if (from_attached.size() != to_attached.size()) {
ROS_DEBUG_STREAM_NAMED("Connecting", name() << ": different number of objects");
return false;
}
if (from_attached.size() != to_attached.size())
return false_with_debug("{}: different number of objects", name());
for (const moveit::core::AttachedBody* from_object : from_attached) {
auto it = std::find_if(to_attached.cbegin(), to_attached.cend(),
[from_object](const moveit::core::AttachedBody* object) {
return object->getName() == from_object->getName();
});
if (it == to_attached.cend()) {
ROS_DEBUG_STREAM_NAMED("Connecting", name() << ": object missing: " << from_object->getName());
return false;
}
const moveit::core::AttachedBody* to_object = *it;
if (from_object->getAttachedLink() != to_object->getAttachedLink()) {
ROS_DEBUG_STREAM_NAMED("Connecting", name() << ": different attach links: " << from_object->getName()
<< " attached to " << from_object->getAttachedLinkName() << " / "
<< to_object->getAttachedLinkName());
return false; // links not matching
}
if (from_object->getShapes().size() != to_object->getShapes().size()) {
ROS_DEBUG_STREAM_NAMED("Connecting", name() << ": different object shapes: " << from_object->getName());
return false; // shapes not matching
}
if (it == to_attached.cend())
return false_with_debug("{}: object missing: {}", name(), from_object->getName());
const moveit::core::AttachedBody* to_object = *it;
if (from_object->getAttachedLink() != to_object->getAttachedLink())
return false_with_debug("{}: different attach links: {} attached to {} vs. {}", //
name(), from_object->getName(), //
from_object->getAttachedLink()->getName(), to_object->getAttachedLink()->getName());
if (from_object->getShapes().size() != to_object->getShapes().size())
return false_with_debug("{}: different object shapes: {}", name(), from_object->getName());
#if MOVEIT_HAS_OBJECT_POSE
auto from_it = from_object->getShapePosesInLinkFrame().cbegin();
auto from_end = from_object->getShapePosesInLinkFrame().cend();
auto to_it = to_object->getShapePosesInLinkFrame().cbegin();
#else
auto from_it = from_object->getFixedTransforms().cbegin();
auto from_end = from_object->getFixedTransforms().cend();
auto to_it = to_object->getFixedTransforms().cbegin();
#endif
for (; from_it != from_end; ++from_it, ++to_it)
if (!(from_it->matrix() - to_it->matrix()).isZero(1e-4)) {
ROS_DEBUG_STREAM_NAMED("Connecting",
name() << ": different pose of attached object shape: " << from_object->getName());
return false; // transforms do not match
}
if (!(from_it->matrix() - to_it->matrix()).isZero(1e-4))
return false_with_debug("{}: different pose of attached object shape: {}", name(), from_object->getName());
}
return true;
}

View File

@ -6,11 +6,14 @@ add_library(${PROJECT_NAME}_stages
${PROJECT_INCLUDE}/stages/fixed_state.h
${PROJECT_INCLUDE}/stages/fixed_cartesian_poses.h
${PROJECT_INCLUDE}/stages/generate_pose.h
${PROJECT_INCLUDE}/stages/generate_random_pose.h
${PROJECT_INCLUDE}/stages/generate_grasp_pose.h
${PROJECT_INCLUDE}/stages/generate_place_pose.h
${PROJECT_INCLUDE}/stages/compute_ik.h
${PROJECT_INCLUDE}/stages/passthrough.h
${PROJECT_INCLUDE}/stages/noop.h
${PROJECT_INCLUDE}/stages/predicate_filter.h
${PROJECT_INCLUDE}/stages/limit_solutions.h
${PROJECT_INCLUDE}/stages/connect.h
${PROJECT_INCLUDE}/stages/move_to.h
@ -26,11 +29,13 @@ add_library(${PROJECT_NAME}_stages
fixed_state.cpp
fixed_cartesian_poses.cpp
generate_pose.cpp
generate_random_pose.cpp
generate_grasp_pose.cpp
generate_place_pose.cpp
compute_ik.cpp
passthrough.cpp
predicate_filter.cpp
limit_solutions.cpp
connect.cpp
move_to.cpp
@ -39,7 +44,7 @@ add_library(${PROJECT_NAME}_stages
simple_grasp.cpp
pick.cpp
)
target_link_libraries(${PROJECT_NAME}_stages ${PROJECT_NAME} ${catkin_LIBRARIES})
target_link_libraries(${PROJECT_NAME}_stages PUBLIC ${PROJECT_NAME} ${catkin_LIBRARIES})
add_library(${PROJECT_NAME}_stage_plugins
plugins.cpp

View File

@ -37,7 +37,7 @@
#include <moveit/task_constructor/stages/compute_ik.h>
#include <moveit/task_constructor/storage.h>
#include <moveit/task_constructor/marker_tools.h>
#include <moveit/task_constructor/moveit_compat.h>
#include <moveit/task_constructor/fmt_p.h>
#include <moveit/planning_scene/planning_scene.h>
#include <moveit/robot_state/conversions.h>
@ -63,6 +63,7 @@ ComputeIK::ComputeIK(const std::string& name, Stage::pointer&& child) : WrapperB
p.declare<bool>("ignore_collisions", false);
p.declare<double>("min_solution_distance", 0.1,
"minimum distance between seperate IK solutions for the same target");
p.declare<moveit_msgs::Constraints>("constraints", moveit_msgs::Constraints(), "additional constraints to obey");
// ik_frame and target_pose are read from the interface
p.declare<geometry_msgs::PoseStamped>("ik_frame", "frame to be moved towards goal pose");
@ -84,18 +85,27 @@ void ComputeIK::setTargetPose(const Eigen::Isometry3d& pose, const std::string&
}
// found IK solutions
using IKSolutions = std::vector<std::vector<double>>;
struct IKSolution
{
std::vector<double> joint_positions;
collision_detection::CollisionResult::ContactMap contacts;
bool collision_free;
bool satisfies_constraints;
};
using IKSolutions = std::vector<IKSolution>;
namespace {
// ??? TODO: provide callback methods in PlanningScene class / probably not very useful here though...
// TODO: move into MoveIt core, lift active_components_only_ from fcl to common interface
bool isTargetPoseCollidingInEEF(const planning_scene::PlanningSceneConstPtr& scene,
robot_state::RobotState& robot_state, Eigen::Isometry3d pose,
const robot_model::LinkModel* link,
moveit::core::RobotState& robot_state, Eigen::Isometry3d pose,
const moveit::core::LinkModel* link, const moveit::core::JointModelGroup* jmg = nullptr,
collision_detection::CollisionResult* collision_result = nullptr) {
// consider all rigidly connected parent links as well
const robot_model::LinkModel* parent = robot_model::RobotModel::getRigidlyConnectedParentLinkModel(link);
const moveit::core::LinkModel* parent = moveit::core::RobotModel::getRigidlyConnectedParentLinkModel(link);
if (parent != link) // transform pose into pose suitable to place parent
pose = pose * robot_state.getGlobalLinkTransform(link).inverse() * robot_state.getGlobalLinkTransform(parent);
@ -109,10 +119,10 @@ bool isTargetPoseCollidingInEEF(const planning_scene::PlanningSceneConstPtr& sce
while (parent) {
pending_links.push_back(&parent->getName());
link = parent;
const robot_model::JointModel* joint = link->getParentJointModel();
const moveit::core::JointModel* joint = link->getParentJointModel();
parent = joint->getParentLinkModel();
if (joint->getType() != robot_model::JointModel::FIXED) {
if (joint->getType() != moveit::core::JointModel::FIXED) {
for (const std::string* name : pending_links)
acm.setDefaultEntry(*name, true);
pending_links.clear();
@ -123,13 +133,15 @@ bool isTargetPoseCollidingInEEF(const planning_scene::PlanningSceneConstPtr& sce
collision_detection::CollisionRequest req;
collision_detection::CollisionResult result;
req.contacts = (collision_result != nullptr);
if (jmg)
req.group_name = jmg->getName();
collision_detection::CollisionResult& res = collision_result ? *collision_result : result;
scene->checkCollision(req, res, robot_state, acm);
return res.collision;
}
std::string listCollisionPairs(const collision_detection::CollisionResult::ContactMap& contacts,
const std::string& separator) {
const std::string& separator = ", ") {
std::string result;
for (const auto& contact : contacts) {
if (!result.empty())
@ -239,16 +251,23 @@ void ComputeIK::compute() {
const moveit::core::JointModelGroup* jmg = nullptr;
std::string msg;
auto report_failure = [&s, this](const std::string& msg) {
planning_scene::PlanningScenePtr scene = s.start()->scene()->diff();
SubTrajectory solution;
solution.markAsFailure(msg);
spawn(InterfaceState(scene), std::move(solution));
};
if (!validateEEF(props, robot_model, eef_jmg, &msg)) {
ROS_WARN_STREAM_NAMED("ComputeIK", msg);
report_failure(msg);
return;
}
if (!validateGroup(props, robot_model, eef_jmg, jmg, &msg)) {
ROS_WARN_STREAM_NAMED("ComputeIK", msg);
report_failure(msg);
return;
}
if (!eef_jmg && !jmg) {
ROS_WARN_STREAM_NAMED("ComputeIK", "Neither eef nor group are well defined");
report_failure("Neither eef nor group are well defined");
return;
}
properties().property("timeout").setDefaultValue(jmg->getDefaultIKTimeout());
@ -262,8 +281,7 @@ void ComputeIK::compute() {
tf2::fromMsg(target_pose_msg.pose, target_pose);
if (target_pose_msg.header.frame_id != scene->getPlanningFrame()) {
if (!scene->knowsFrameTransform(target_pose_msg.header.frame_id)) {
ROS_WARN_STREAM_NAMED("ComputeIK",
"Unknown reference frame for target pose: " << target_pose_msg.header.frame_id);
report_failure(fmt::format("Unknown reference frame for target pose: '{}'", target_pose_msg.header.frame_id));
return;
}
// transform target_pose w.r.t. planning frame
@ -271,14 +289,14 @@ void ComputeIK::compute() {
}
// determine IK link from ik_frame
const robot_model::LinkModel* link = nullptr;
const moveit::core::LinkModel* link = nullptr;
geometry_msgs::PoseStamped ik_pose_msg;
const boost::any& value = props.get("ik_frame");
if (value.empty()) { // property undefined
// determine IK link from eef/group
if (!(link = eef_jmg ? robot_model->getLinkModel(eef_jmg->getEndEffectorParentGroup().second) :
jmg->getOnlyOneEndEffectorTip())) {
ROS_WARN_STREAM_NAMED("ComputeIK", "Failed to derive IK target link");
report_failure("Failed to derive IK target link");
return;
}
ik_pose_msg.header.frame_id = link->getName();
@ -289,12 +307,12 @@ void ComputeIK::compute() {
tf2::fromMsg(ik_pose_msg.pose, ik_pose);
if (!scene->getCurrentState().knowsFrameTransform(ik_pose_msg.header.frame_id)) {
ROS_WARN_STREAM_NAMED("ComputeIK", "ik frame unknown in robot: '" << ik_pose_msg.header.frame_id << "'");
report_failure(fmt::format("ik frame unknown in robot: '{}'", ik_pose_msg.header.frame_id));
return;
}
ik_pose = scene->getCurrentState().getFrameTransform(ik_pose_msg.header.frame_id) * ik_pose;
link = utils::getRigidlyConnectedParentLinkModel(scene->getCurrentState(), ik_pose_msg.header.frame_id);
link = scene->getCurrentState().getRigidlyConnectedParentLinkModel(ik_pose_msg.header.frame_id);
// transform target pose such that ik frame will reach there if link does
target_pose = target_pose * ik_pose.inverse() * scene->getCurrentState().getFrameTransform(link->getName());
@ -302,31 +320,33 @@ void ComputeIK::compute() {
// validate placed link for collisions
collision_detection::CollisionResult collisions;
robot_state::RobotState sandbox_state{ scene->getCurrentState() };
moveit::core::RobotState sandbox_state{ scene->getCurrentState() };
bool colliding =
!ignore_collisions && isTargetPoseCollidingInEEF(scene, sandbox_state, target_pose, link, &collisions);
!ignore_collisions && isTargetPoseCollidingInEEF(scene, sandbox_state, target_pose, link, jmg, &collisions);
// markers used for failures
std::deque<visualization_msgs::Marker> failure_markers;
// frames at target pose and ik frame
rviz_marker_tools::appendFrame(failure_markers, target_pose_msg, 0.1, "target frame");
rviz_marker_tools::appendFrame(failure_markers, ik_pose_msg, 0.1, "ik frame");
std::deque<visualization_msgs::Marker> frame_markers;
rviz_marker_tools::appendFrame(frame_markers, target_pose_msg, 0.1, "target frame");
rviz_marker_tools::appendFrame(frame_markers, ik_pose_msg, 0.1, "ik frame");
// end-effector markers
std::deque<visualization_msgs::Marker> eef_markers;
// visualize placed end-effector
auto appender = [&failure_markers](visualization_msgs::Marker& marker, const std::string& /*name*/) {
auto appender = [&eef_markers](visualization_msgs::Marker& marker, const std::string& /*name*/) {
marker.ns = "ik target";
marker.color.a *= 0.5;
failure_markers.push_back(marker);
eef_markers.push_back(marker);
};
const auto& links_to_visualize = moveit::core::RobotModel::getRigidlyConnectedParentLinkModel(link)
->getParentJointModel()
->getDescendantLinkModels();
if (colliding) {
SubTrajectory solution;
std::copy(frame_markers.begin(), frame_markers.end(), std::back_inserter(solution.markers()));
generateCollisionMarkers(sandbox_state, appender, links_to_visualize);
std::copy(failure_markers.begin(), failure_markers.end(), std::back_inserter(solution.markers()));
std::copy(eef_markers.begin(), eef_markers.end(), std::back_inserter(solution.markers()));
solution.markAsFailure();
// TODO: visualize collisions
solution.setComment(s.comment() + " eef in collision: " + listCollisionPairs(collisions.contacts, ", "));
solution.setComment(s.comment() + " eef in collision: " + listCollisionPairs(collisions.contacts));
utils::addCollisionMarkers(solution.markers(), scene->getPlanningFrame(), collisions.contacts);
auto colliding_scene{ scene->diff() };
colliding_scene->setCurrentState(sandbox_state);
spawn(InterfaceState(colliding_scene), std::move(solution));
@ -338,7 +358,7 @@ void ComputeIK::compute() {
std::vector<double> compare_pose;
const std::string& compare_pose_name = props.get<std::string>("default_pose");
if (!compare_pose_name.empty()) {
robot_state::RobotState compare_state(robot_model);
moveit::core::RobotState compare_state(robot_model);
compare_state.setToDefaultValues(jmg, compare_pose_name);
compare_state.copyJointGroupPositions(jmg, compare_pose);
} else
@ -346,19 +366,38 @@ void ComputeIK::compute() {
double min_solution_distance = props.get<double>("min_solution_distance");
kinematic_constraints::KinematicConstraintSet constraint_set(robot_model);
constraint_set.add(props.get<moveit_msgs::Constraints>("constraints"), scene->getTransforms());
IKSolutions ik_solutions;
auto is_valid = [scene, ignore_collisions, min_solution_distance,
&ik_solutions](robot_state::RobotState* state, const robot_model::JointModelGroup* jmg,
auto is_valid = [scene, ignore_collisions, min_solution_distance, &constraint_set = std::as_const(constraint_set),
&ik_solutions](moveit::core::RobotState* state, const moveit::core::JointModelGroup* jmg,
const double* joint_positions) {
for (const auto& sol : ik_solutions) {
if (jmg->distance(joint_positions, sol.data()) < min_solution_distance)
if (jmg->distance(joint_positions, sol.joint_positions.data()) < min_solution_distance)
return false; // too close to already found solution
}
state->setJointGroupPositions(jmg, joint_positions);
ik_solutions.emplace_back();
state->copyJointGroupPositions(jmg, ik_solutions.back());
state->update();
return ignore_collisions || !scene->isStateColliding(*state, jmg->getName());
ik_solutions.emplace_back();
auto& solution{ ik_solutions.back() };
state->copyJointGroupPositions(jmg, solution.joint_positions);
// validate constraints
solution.satisfies_constraints = constraint_set.decide(*state).satisfied;
// check for collisions
collision_detection::CollisionRequest req;
collision_detection::CollisionResult res;
req.contacts = true;
req.max_contacts = 1;
req.group_name = jmg->getName();
scene->checkCollision(req, res, *state);
solution.collision_free = ignore_collisions || !res.collision;
solution.contacts = std::move(res.contacts);
return solution.satisfies_constraints && solution.collision_free;
};
uint32_t max_ik_solutions = props.get<uint32_t>("max_ik_solutions");
@ -367,8 +406,10 @@ void ComputeIK::compute() {
double remaining_time = timeout();
auto start_time = std::chrono::steady_clock::now();
while (ik_solutions.size() < max_ik_solutions && remaining_time > 0) {
if (tried_current_state_as_seed)
if (tried_current_state_as_seed) {
sandbox_state.setToRandomPositions(jmg);
sandbox_state.update();
}
tried_current_state_as_seed = true;
size_t previous = ik_solutions.size();
@ -384,24 +425,28 @@ void ComputeIK::compute() {
planning_scene::PlanningScenePtr solution_scene = scene->diff();
SubTrajectory solution;
solution.setComment(s.comment());
std::copy(frame_markers.begin(), frame_markers.end(), std::back_inserter(solution.markers()));
// frames at target pose and ik frame
rviz_marker_tools::appendFrame(solution.markers(), target_pose_msg, 0.1, "ik frame");
rviz_marker_tools::appendFrame(solution.markers(), ik_pose_msg, 0.1, "ik frame");
if (succeeded && i + 1 == ik_solutions.size())
if (ik_solutions[i].collision_free && ik_solutions[i].satisfies_constraints)
// compute cost as distance to compare_pose
solution.setCost(s.cost() + jmg->distance(ik_solutions.back().data(), compare_pose.data()));
else // found an IK solution, but this was not valid
solution.markAsFailure();
solution.setCost(s.cost() + jmg->distance(ik_solutions[i].joint_positions.data(), compare_pose.data()));
else if (!ik_solutions[i].collision_free) { // solution was in collision
solution.markAsFailure("Collision between " + listCollisionPairs(ik_solutions[i].contacts));
utils::addCollisionMarkers(solution.markers(), scene->getPlanningFrame(), ik_solutions[i].contacts);
} else if (!ik_solutions[i].satisfies_constraints) { // solution was violating constraints
solution.markAsFailure("Constraints violated");
}
// set scene's robot state
robot_state::RobotState& solution_state = solution_scene->getCurrentStateNonConst();
solution_state.setJointGroupPositions(jmg, ik_solutions[i].data());
moveit::core::RobotState& solution_state = solution_scene->getCurrentStateNonConst();
solution_state.setJointGroupPositions(jmg, ik_solutions[i].joint_positions.data());
solution_state.update();
InterfaceState state(solution_scene);
forwardProperties(*s.start(), state);
// ik target link placement
std::copy(eef_markers.begin(), eef_markers.end(), std::back_inserter(solution.markers()));
spawn(std::move(state), std::move(solution));
}
@ -418,9 +463,17 @@ void ComputeIK::compute() {
solution.markAsFailure();
solution.setComment(s.comment() + " no IK found");
std::copy(frame_markers.begin(), frame_markers.end(), std::back_inserter(solution.markers()));
// ik target link placement
std::copy(failure_markers.begin(), failure_markers.end(), std::back_inserter(solution.markers()));
std_msgs::ColorRGBA tint_color;
tint_color.r = 1.0;
tint_color.g = 0.0;
tint_color.b = 0.0;
tint_color.a = 0.5;
for (auto& marker : eef_markers)
marker.color = tint_color;
std::copy(eef_markers.begin(), eef_markers.end(), std::back_inserter(solution.markers()));
spawn(InterfaceState(scene), std::move(solution));
}

View File

@ -38,9 +38,13 @@
#include <moveit/task_constructor/stages/connect.h>
#include <moveit/task_constructor/merge.h>
#include <moveit/planning_scene/planning_scene.h>
#include <moveit/task_constructor/cost_terms.h>
#include <moveit/task_constructor/fmt_p.h>
#include <moveit/planning_scene/planning_scene.h>
#include <moveit/trajectory_processing/time_optimal_trajectory_generation.h>
using namespace trajectory_processing;
namespace moveit {
namespace task_constructor {
@ -52,8 +56,12 @@ Connect::Connect(const std::string& name, const GroupPlannerVector& planners) :
auto& p = properties();
p.declare<MergeMode>("merge_mode", WAYPOINTS, "merge mode");
p.declare<double>("max_distance", 1e-2,
"maximally accepted joint configuration distance between trajectory endpoint and goal state");
p.declare<moveit_msgs::Constraints>("path_constraints", moveit_msgs::Constraints(),
"constraints to maintain during trajectory");
properties().declare<TimeParameterizationPtr>("merge_time_parameterization",
std::make_shared<TimeOptimalTrajectoryGeneration>());
}
void Connect::reset() {
@ -88,7 +96,7 @@ void Connect::init(const core::RobotModelConstPtr& robot_model) {
try {
merged_jmg_.reset(task_constructor::merge(groups));
} catch (const std::runtime_error& e) {
ROS_INFO_STREAM_NAMED("Connect", this->name() << ": " << e.what() << ". Disabling merging.");
ROS_INFO_STREAM_NAMED("Connect", fmt::format("{}: {}. Disabling merging.", this->name(), e.what()));
}
}
@ -119,8 +127,8 @@ bool Connect::compatible(const InterfaceState& from_state, const InterfaceState&
Eigen::Map<const Eigen::VectorXd> positions_from(from.getJointPositions(jm), num);
Eigen::Map<const Eigen::VectorXd> positions_to(to.getJointPositions(jm), num);
if (!(positions_from - positions_to).isZero(1e-4)) {
ROS_INFO_STREAM_NAMED("Connect", "Deviation in joint " << jm->getName() << ": [" << positions_from.transpose()
<< "] != [" << positions_to.transpose() << "]");
ROS_INFO_STREAM_NAMED("Connect", fmt::format("Deviation in joint {}: [{}] != [{}]", jm->getName(),
positions_from.transpose(), positions_to.transpose()));
return false;
}
}
@ -131,6 +139,7 @@ void Connect::compute(const InterfaceState& from, const InterfaceState& to) {
const auto& props = properties();
double timeout = this->timeout();
MergeMode mode = props.get<MergeMode>("merge_mode");
double max_distance = props.get<double>("max_distance");
const auto& path_constraints = props.get<moveit_msgs::Constraints>("path_constraints");
const moveit::core::RobotState& final_goal_state = to.scene()->getCurrentState();
@ -141,23 +150,35 @@ void Connect::compute(const InterfaceState& from, const InterfaceState& to) {
intermediate_scenes.push_back(start);
bool success = false;
bool has_potential_collisions = false;
std::string comment = "No planners specified";
std::vector<double> positions;
for (const GroupPlannerVector::value_type& pair : planner_) {
// set intermediate goal state
planning_scene::PlanningScenePtr end = start->diff();
const moveit::core::JointModelGroup* jmg = final_goal_state.getJointModelGroup(pair.first);
final_goal_state.copyJointGroupPositions(jmg, positions);
robot_state::RobotState& goal_state = end->getCurrentStateNonConst();
moveit::core::RobotState& goal_state = end->getCurrentStateNonConst();
goal_state.setJointGroupPositions(jmg, positions);
goal_state.update();
intermediate_scenes.push_back(end);
robot_trajectory::RobotTrajectoryPtr trajectory;
success = pair.second->plan(start, end, jmg, timeout, trajectory, path_constraints);
auto result = pair.second->plan(start, end, jmg, timeout, trajectory, path_constraints);
success = bool(result);
sub_trajectories.push_back(trajectory); // include failed trajectory
if (!success)
if (!success) {
comment = result.message;
has_potential_collisions = trajectory && utils::hints_at_collisions(result);
break;
}
if (trajectory->getLastWayPoint().distance(goal_state, jmg) > max_distance) {
success = false;
comment = "Trajectory end-point deviates too much from goal state";
break;
}
// continue from reached state
start = end;
@ -168,8 +189,15 @@ void Connect::compute(const InterfaceState& from, const InterfaceState& to) {
solution = merge(sub_trajectories, intermediate_scenes, from.scene()->getCurrentState());
if (!solution) // success == false or merging failed: store sequentially
solution = makeSequential(sub_trajectories, intermediate_scenes, from, to);
if (!success) // error during sequential planning
solution->markAsFailure();
if (!success) { // error already during sequential planning
solution->markAsFailure(comment);
if (has_potential_collisions) {
// add collision markers for last (failed) trajectory segment
auto sequence = std::dynamic_pointer_cast<SolutionSequence>(solution);
auto trajectory = dynamic_cast<const SubTrajectory*>(sequence->solutions().back())->trajectory();
utils::addCollisionMarkers(solution->markers(), *trajectory, start);
}
}
connect(from, to, solution);
}
@ -219,7 +247,8 @@ SubTrajectoryPtr Connect::merge(const std::vector<robot_trajectory::RobotTraject
auto jmg = merged_jmg_.get();
assert(jmg);
robot_trajectory::RobotTrajectoryPtr trajectory = task_constructor::merge(sub_trajectories, state, jmg);
auto timing = properties().get<TimeParameterizationPtr>("merge_time_parameterization");
robot_trajectory::RobotTrajectoryPtr trajectory = task_constructor::merge(sub_trajectories, state, jmg, *timing);
if (!trajectory)
return SubTrajectoryPtr();

Some files were not shown because too many files have changed in this diff Show More