Commit Graph

586 Commits

Author SHA1 Message Date
Robert Haschke
7d5f9fe6e3 Fix clang-tidy warnings 2022-11-02 18:44:38 +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
64a8df1fde Merge CI fixes/improvements 2022-10-24 10:21:34 +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
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
28fb974adb Remove MoveIt compatibility code 2022-10-23 22:16:58 +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
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
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
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
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
4be448641f Improve debug output
- printChildrenInterfaces(): fix/add usage
- printPendingPairs(): full colorization according to status
2021-11-28 18:28:20 +01:00
Robert Haschke
b4a9e2033d
Stage::reset() should reset total_compute_time_ (#310) 2021-11-25 19:47:00 +03: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
c33b1967bc Handle updates on external states 2021-11-22 01:36:34 +01:00
Robert Haschke
e5b20ac11f Fix pruning
Pruning - if acting on the external state - needs to pass the current stage (this).
2021-11-22 01:36:34 +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
aef991602f Propagate either STATUS or PRIORITY updates into a container 2021-11-22 00:12:50 +01:00