Commit Graph

432 Commits

Author SHA1 Message Date
Robert Haschke
86093be94a Merge branch 'master' into wip-python-api 2022-01-07 16:20:00 +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
442d39ad3e Improve comments 2022-01-05 16:37:46 +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
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
e296bd7aed Simplify: job_has_solutions_
Just set a flag when we received a full solution
2021-11-25 07:37:28 +01:00
Robert Haschke
7237e81547 Rework FallbacksPrivate*
Further factorize and simplify FallbacksPrivate classes employing ideas from @v4hn.
The key difference between the variants his how they advance to the next job.
Thus, the only virtual method required is nextJob().
2021-11-25 07:36:32 +01:00
Robert Haschke
8dd8022ef9 Factorize implementation of FallbacksPrivate into 3 classes 2021-11-24 21:17:49 +01:00
Robert Haschke
e67b3252fc static TaskPrivate::swap() -> ContainerBasePrivate::operator=()
- Enable moving/swapping of other container impls (e.g. Fallbacks)
- Clarify (via move semantics) that content of source impl will be lost
- Get rid of friend declarations
2021-11-24 21:17:49 +01:00
Robert Haschke
2e63c154aa Reintroduce pending state 2021-11-22 01:39:45 +01:00
Robert Haschke
5c235ab580 debugging helper function 2021-11-22 01:39:45 +01:00
Robert Haschke
dcb6857f36 Simplify computePropagate()
- Drop variable current_external_state_
- Instead encode the info that the external state wasn't yet forwarded to any child via stage = children().cend()
- If all children have exhausted their solutions for this state, it is removed from the pending list
2021-11-22 01:39:45 +01:00
Robert Haschke
6653c4853a Rename: computeFromExternal -> computePropagate 2021-11-22 01:36:34 +01:00
Robert Haschke
b783173b27 GENERATE: return correct canCompute() result as early as possible
Moving to next child generator only in compute() requires an extra call
to canCompute() to notice the failure of the next generator(s).
2021-11-22 01:35:34 +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
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
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
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
66e141db7b Fix printChildrenInterfaces() 2021-11-19 09:42:30 +01:00
v4hn
22809c04a5 order external states 2021-11-15 09:29:09 +01:00
v4hn
9a583ab006 run only one compute step per call
Note that while this ensures other stages outside the Fallbacks container
can compute as well, it does not solve the problem internally.
A new incoming state will only ever be considered once
the current stage cannot compute any more.

We have no way of telling a child to compute for *a specific state* for now.
So once we copied a state to its interface we have to let it compute until
all possibilities are exhausted to detect whether or not it could generate a solution for it.
If we wouldn't do so, there were no way of knowing when to fall back
to the next child as long as the stage can still compute on *any* copied solution.
2021-11-15 09:29:09 +01:00
v4hn
5a9cfc50ea cleanup: get rid of superfluous parameter 2021-11-15 09:29:09 +01:00
v4hn
8719b1c3d6 Implement state-wise Fallbacks
Keep the previous logic around for Generator stages.
Note that this only makes sense for *pure* Generators and not for MonitoringGenerator,
because for the latter we would expect monitored solutions to be passed individually
(similar to pruning).
2021-11-15 09:29:09 +01:00
v4hn
f32e747645 add hook to ParallelContainerBase to customize state propagation 2021-11-15 09:29:09 +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
e1216aa8ab MoveTo supports attached objects&subframes for ik frame 2021-11-10 13:14:04 +01:00
v4hn
dfe746b9bb FixedState: add optional scene in constructor 2021-11-10 13:12:21 +01: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
40b00c61d2 DISABLED_FAILED -> FAILED
Failed states are *not* disabled, they just failed connecting (for now).
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
fa8d21741c print debug message in runCompute
so that it prints from all containers
2021-09-01 14:53:14 +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
062c127b86 better API comments for StageCallback 2021-06-15 09:49:05 +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
Robert Haschke
ced362f5f6 Merge branch 'master' into wip-python-api 2021-06-11 09:57:55 +02:00
v4hn
1f51124ae5 add missing virtual destructor to CostTerm
Correctly pointed out by the clang-tidy CI.
2021-06-04 14:19:52 +02:00