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
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
v4hn
84f96ec74c
MoveRelative: Interpret direction relative to IKFrame
...
bugfix
2021-12-20 10:19:52 +01:00
Robert Haschke
b4a9e2033d
Stage::reset() should reset total_compute_time_ ( #310 )
2021-11-25 19:47:00 +03:00
v4hn
b675876d3b
add debug logs to pruning code
2021-11-16 22:52:06 +01:00
v4hn
3b835986e3
refactor logic to handle ik_frame
...
fallbacks and verification.
2021-11-12 14:02:46 +01:00
v4hn
f1fc447e3b
establish utils namespace
...
leaves us a place to put free helper functions
2021-11-10 14:36:58 +01:00
v4hn
48959c6806
add compat header to cmake
...
previous oversight
2021-11-10 14:35:25 +01:00
v4hn
ef27a6eb22
ComputeIK supports attached-object ik frame
2021-11-10 13:14:04 +01:00
v4hn
e1216aa8ab
MoveTo supports attached objects&subframes for ik frame
2021-11-10 13:14:04 +01:00
v4hn
86fe752d43
InterpolationPlanner: implement simple IK-based solver for pose targets
2021-11-10 13:12:21 +01:00
v4hn
dfe746b9bb
FixedState: add optional scene in constructor
2021-11-10 13:12:21 +01:00
Robert Haschke
d6f68f9254
Simplify code
...
We know that trajectory at least comprises the start state.
Thus, we don't need the sanity checks.
2021-11-10 12:11:39 +01:00
Michael Görner
7ec874572c
move to tf2_eigen everywhere ( #301 )
2021-10-18 23:34:29 +02:00
Robert Haschke
9da3a8055e
Optimize setStatus(): only escalate to parent interface at boundaries of a partial solution
...
In all other cases internalToExternalMap().find(s) will fail anyway.
2021-09-20 23:07:00 +02:00
v4hn
35560d897d
DISABLED -> PRUNED
...
This makes the semantics much clearer as states can only be disabled by pruning.
2021-09-20 23:07:00 +02:00
v4hn
d143dd7076
simplify exception handling
...
This could have been done already back when `runCompute` was introduced.
Wrapping the calls in try/catch comes from the previous implementation directly
calling `compute()`.
2021-09-20 23:07:00 +02:00
v4hn
1e10aaccfd
Connecting: add another ROS_DEBUG hint
...
These can facilitate debugging a lot for little overhead.
2021-09-20 23:07:00 +02:00
v4hn
40b00c61d2
DISABLED_FAILED -> FAILED
...
Failed states are *not* disabled, they just failed connecting (for now).
2021-09-20 23:07:00 +02:00
v4hn
3b9f6ee519
add todo
2021-09-20 23:07:00 +02:00
Robert Haschke
d0ab3c3703
Improve readability of internal-external bimap using tags ( #293 )
2021-09-16 13:52:34 +02:00
v4hn
06b3df9c30
split assert
...
so that it becomes obvious which condition triggered it.
2021-09-01 14:53:52 +02:00
v4hn
fa8d21741c
print debug message in runCompute
...
so that it prints from all containers
2021-09-01 14:53:14 +02:00
v4hn
f51f6eb982
address interface changes for object poses in MoveIt
...
Also include a check for the new object pose field in `Connecting::compatible()`.
2021-08-23 00:33:37 +02:00
v4hn
0273aec83e
remove dirty MOVEIT_MASTER-check
...
... in favor of checking version numbers.
Checking for one header was used for multiple independent things.
In theory we could do exact feature testing instead of using the next release number,
but in practice nobody cares about the individual commits between older releases.
2021-08-23 00:06:42 +02:00
v4hn
6b4eae9d51
ComputeIK: spawn failures with correct states
...
Otherwise all failures look correct in the introspection.
That was a stupid oversight at some point.
2021-08-21 22:15:21 +02:00
v4hn
ff86217f9a
do not modify scene in isTargetPoseColliding
...
Especially, do not create a custom scene at all.
The method only affects a RobotState.
Fixes https://github.com/ros-planning/moveit_task_constructor/issues/209
2021-08-21 22:15:12 +02:00
Captain Yoshi
64b65e4eca
GenerateGraspPose: Handle RobotState.msg as pregrasp property ( #275 )
...
Co-authored-by: v4hn <me@v4hn.de>
2021-06-15 07:51:18 +02:00
Michael Görner
84d668b0d5
Split ClassLoader plugin code from main libraries ( #271 )
...
to work around https://github.com/ros/class_loader/issues/131
2021-05-29 00:38:02 +02:00
j-kuehn
67fe4faa30
allow to consider specific joints only in cost terms
2021-05-25 15:47:18 +02:00
Robert Haschke
84ccd679d9
Fix compiler warnings
...
unused parameters and functions
2021-05-19 21:54:48 +02:00
j-kuehn
d71c645d83
add visualization for Point goals ( #264 )
...
- move visualization from `getPoseGoal` to `compute`
- create target frame from `target_eigen` to allow visualization from Pose and Point goals
2021-05-11 10:53:15 +02:00
j-kuehn
ba0b4f6146
Improve arrow visualization for MoveRelative stage ( #255 )
...
Implement visualization as red-green arrow
* overload makeArrow to allow creation with points
* create new function for visualization
* if no plan is found, construct arrow from green cylinder and red arrow
* adjust arrow construction for backward propagators
2021-05-03 21:29:05 +02:00
Robert Haschke
cfcb1f274e
Fix more clang-tidy issues
2021-04-24 02:12:24 +02:00
Robert Haschke
4fa8c10f44
clang-tidy auto-fixes
2021-04-24 02:03:24 +02:00
v4hn
4e09d78239
Implement pruning inside-to-outside of a container
...
- Remove public onNewFailure() interface
Moved to ContainerBasePrivate to reuse logic for serial and parallel containers.
- Add tests
2021-04-01 00:19:15 +02:00
v4hn
472a78800d
implement bimap for internal_external state map
...
Get hashing for inverted lookups, but incur
structural overhead.
Whether this is worth it depends on the number of mapped interface states
and the number of pruning/reactivation requests.
2021-03-30 15:19:49 +02:00
Robert Haschke
0e3ec0b6ec
fix constness
2021-03-30 15:19:49 +02:00
v4hn
106c138ef5
Implement pruning inside serial container
...
By inefficient inverse lookup.
Also add disabled test for the inverted inference (failure inside should prune outside)
2021-03-30 15:19:49 +02:00
Michael Görner
36d9d3e8c0
Rework PipelinePlanner creation ( #249 )
...
- Moved Task::createPlanner into PipelinePlanner::create
- Handle mutiple planner pipeline configs as introduced in https://github.com/ros-planning/moveit/pull/2127
2021-03-29 12:00:28 +02:00
Robert Haschke
4ee71881ae
Rework Pruning ( #221 )
2021-03-23 13:04:38 +01:00
Michael Görner
f2958a4a0b
Explicitly instantiate PropagatingEitherWay::send<> templates ( #246 )
...
Otherwise compiler was optimizing (inlining) them away.
2021-03-23 11:37:30 +01:00
v4hn
b7f80939ef
add more asserts to onNewSolution
...
Strictly speaking there is no need for the invariant,
but if something violates it a stage computed something it wasn't meant to.
2021-03-22 14:42:23 +01:00
v4hn
3b6c13e26b
only run propagators on enabled solutions
...
resolve simple pruning test
2021-03-22 14:42:23 +01:00
v4hn
be1c8292b5
add more (commented-out) debug statements
...
Also print generator symbol for completeness.
2021-03-22 14:42:23 +01:00
v4hn
e33962364d
resolve PruningMultiForward test
...
Every InterfaceState along the partial solution has to be disabled
for pruning, not just the ends that are currently relevant.
2021-03-22 14:42:23 +01:00
v4hn
a82b88355e
streamline trait parameters
...
state asked for a reference, but trajectories for a pointer.
2021-03-22 14:42:23 +01:00
v4hn
bca01e8aa7
rename Status values
...
Start and End are already used for an entirely different concept,
so if anyone ever wants to read this code, we should use new terms instead.
Because the source state is the disabled state that *failed* to extend,
triggering the whole subtree to be disabled, I went for the new terms
DISABLED and DISABLED_FAILED.
2021-03-22 14:42:23 +01:00
Robert Haschke
c0100ff775
Remove debug output
2021-03-22 14:42:23 +01:00
Robert Haschke
d3864596b6
Replace Priority::enabled() with status()
...
The key to pruning in the Connecting stage was the following:
- Don't remove states during pruning, but only disable them.
They might become re-enabled due to further input.
- Distinguish START and END sides of a disabled solution tree to break their symmetry.
The START side from where we started disabling, can be re-enabled by a new partner state in
Connecting, the END side must not. This was important as, otherwise, the states would simply
get re-enabled immediately. The END side only gets re-enabled if the START side actually
connects the whole solution branch.
2021-03-22 14:42:23 +01:00