Commit Graph

858 Commits

Author SHA1 Message Date
Robert Haschke
f1acfa2f7e
Merge pull request #151: Various fixes 2020-04-03 11:58:19 +02:00
v4hn
00331414a6 merge: update states only once per waypoint 2020-04-03 11:36:05 +02:00
Robert Haschke
8834ce18f7 simplify trajectory merging
avoid code duplication: reuse merge(std::vector<JointModelGroup*>)
2020-04-03 11:36:05 +02:00
Robert Haschke
8debe68f99 fix joint_interpolation
- return a trajectory in any case (even if there is no motion needed)
- check feasability of goal pose
2020-04-03 11:36:05 +02:00
v4hn
07f62aa72f add override in Task
I very much considered just removing the protected inheritance again,
but it would add unnecessary code duplication.

Take note, the overriding `insert` function bypasses the Wrapper and directly forwards to the wrapped container.
This is somewhat dirty and could be an issue for anyone inheriting from `Task`.
2020-04-01 15:05:20 +02:00
v4hn
7cd82d5c04 resolve clang warnings & suggestions 2020-04-01 15:05:20 +02:00
v4hn
8578a64ac0 correct sign for StageCallback type
backward compatible, the function type implicitly converts from previous int definitions.
2020-04-01 15:05:20 +02:00
v4hn
cc86dd8a74 rename Task::erase -> eraseTaskCallback
`erase` is too general for a container class,
when it's not actually about erasing children or the container itself.
2020-04-01 15:05:20 +02:00
v4hn
c046ec605d streamline add/insert interfaces for Task/Container
`add` falls back to `insert` for both structures,
but `add` throws exceptions and does not provide a return value.

`insert` provides standard STL container access.
2020-04-01 15:05:20 +02:00
v4hn
df8a783239 enforce one-parent policy in StagePrivate
So new containers cannot get this wrong by accident.
2020-04-01 15:05:20 +02:00
Michael Görner
90f5d22e83
do not export inline functions (#147)
This makes sure that code using `pimpl()` will not compile out of the box unless the _p header is included.
2020-03-29 20:47:26 +02:00
Michael Görner
0eae0e652e
build tests using gmock via catkin_add_gmock (#148)
Otherwise these suddenly started to fail for me on Lunar Linux...
2020-03-20 20:33:33 +01:00
v4hn
8f56d4ae79 avoid copy from const id reference
This mustn't compile as users got confused about it:
```
Task t;
t.id() = "foobar";
```
2020-03-05 12:08:59 +01:00
Robert Haschke
9ee653453d Container: abort traversal with false return value of processor
So far, returning false from the processor function, just skipped further traversing the current child (depth-wise).
Now, traversal is completely aborted, even not traversing the remaining siblings of the current child.
Having a single boolean return value, we cannot distinguish both cases.
We need the new behaviour for 8061945c15bea22e8f8899c987bc28e3542885aa.
2020-03-03 17:57:06 +01:00
Mark Moll
0d6f79050a don't install PIMPL header files (#119) 2020-03-03 17:21:54 +01:00
Markus Vieth
534d520a04
Add return value to Task::execute (#136) 2020-02-20 11:31:51 +01:00
Robert Haschke
d10565cabf fix memory access issue
Fixes #132.
2020-02-18 09:01:00 +01:00
Aris Synodinos
8ac9957b55
Added C++11 flags for MOVEIT_MASTER check (#129) 2020-02-12 21:59:33 +01:00
Robert Haschke
463486257b Merge #120: Add computation timing 2020-02-06 16:18:49 +01:00
Jafar Abdi
fa9b177575 transmit and display computation in rviz 2020-02-06 16:18:20 +01:00
Jafar Abdi
aa11b66824 use runCompute() 2020-02-06 16:17:55 +01:00
Jafar Abdi
89fea00b33 Stage::runCompute() to measure computation time 2020-02-06 16:17:04 +01:00
Robert Haschke
5a85343490 Merge branch master into wip-python-api 2020-01-11 09:17:32 +01:00
Robert Haschke
ba815cff34 Task: don't publish solutions by default
Having multiple solutions, automatic publishing of intermediate solutions is confusing.
One never knows, which one is the final one. If desired, the user should setup a hook for this.
2020-01-11 09:00:34 +01:00
Henning Kayser
c1f6ca8e84 Expose SolutionCallback API in Task (#121) 2020-01-11 08:33:57 +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
v4hn
233d63aa7a avoid deprecation warnings of MoveIt master branch
Use new methods
- moveit::core::isEmpty
- moveit::core::CartesianInterpolator::computeCartesianPath
2019-12-13 11:29:02 +01:00
Robert Haschke
acddc268ed compatibility to MoveIt master branch 2019-11-27 17:43:57 +01:00
Robert Haschke
3887b32ba1 fix MoveRelative::getJointStateFromOffset
Fix for #114: Accessing variable indexes was screwed.
2019-11-26 21:08:31 +01:00
Aris Synodinos
145bec1ed3 Added cost calculation in move_relative (#108) 2019-11-05 17:38:25 +01:00
Robert Haschke
facdc8e278 update RobotState in getJointStateGoal() 2019-11-02 22:02:56 +08:00
Robert Haschke
1ba4bc4508
MoveTo/MoveRelative: report failure on invalid trajectories (#107) 2019-10-22 12:06:11 +02:00
Robert Haschke
fd9462c27f Merge branch master into wip-python-api 2019-07-26 11:11:41 +02:00
Robert Haschke
975491fba9 Fixup: adding cost calculations to connect and move_to 2019-07-26 11:05:52 +02:00
Robert Haschke
4225671ae4 PipelinePlanner: always forward trajectory
(also in case of failure)
2019-07-19 13:45:07 +02:00
Robert Haschke
6ff1a5162f Fixup: adding cost calculations to connect and move_to
Need to handle nullptr trajectories.
2019-07-19 13:45:07 +02:00
Michael Görner
f7b259259e Introduce clang-format (#102)
* fix catkin_lint issues
* introduce clang-format config and apply it
2019-07-18 11:04:35 +02:00
Tyler Weaver
7ca49bc29b adding cost calculations to connect and move_to 2019-07-12 20:29:08 +02:00
v4hn
0b47224ef1 micro-fix whitespace
Let's use clang-format...
2019-07-12 13:51:21 +02:00
Robert Haschke
874994bdb5 test packages are required 2019-07-12 09:35:48 +02:00
Robert Haschke
95455c2a26 fix Task::operator=(Task&&)
To move a task instance to another one, it's not sufficient to swap all task members,
but we also need to adapt all back pointers, i.e. me_ and parent_ pointers of children,
to point to the (swapped) task instances.
2019-06-04 14:20:47 +02:00
Robert Haschke
26f679bd8e relax assertion 2019-05-16 18:12:20 +02:00
Robert Haschke
6cf0cbb4c9 improve comments 2019-05-16 18:02:08 +02:00
Michael Görner
267c214288 Overload setGoal to accept map of joint values as argument (#87) 2019-05-10 14:41:46 +02:00
Robert Haschke
fee73bcd78 Merge branch 'fix-interface-deduction' into master 2019-05-03 01:53:57 +02:00
Robert Haschke
11fb15fd0c fix clang-tidy warnings
- virtual functions used in constructor / destructor
- captured variable in lambda expression not used
- unhandled enums in switch
2019-05-03 01:53:17 +02:00
Robert Haschke
102c80dda1 ParallelContainer: fix interface error reporting
need to separately check for start/end interface
2019-05-03 01:06:56 +02:00
Robert Haschke
d9a38f46ab SerialContainer: fix nested interface resolution
We need to distinguish two cases for how the interface of a nested serial container is determined:
1. from its children
2. from its (outer) context

As long as the interface is not fully resolved, requiredInterface() returns UNKNOWN.
After pruning, the first/last child's interface is remembered and reported instead.
2019-05-03 01:06:56 +02:00
Robert Haschke
fd1aa38a39 fix unit test definitions 2019-05-03 01:06:56 +02:00
Robert Haschke
135951f690 replace assertion by exception 2019-05-03 01:06:56 +02:00
Robert Haschke
f402cdec5e rename [INPUT|OUTPUT]_IF_MASK -> [START|END]_IF_MASK 2019-05-03 01:06:56 +02:00
Robert Haschke
28703af384 improve comments 2019-05-03 01:06:56 +02:00
Robert Haschke
ea75070155 cherry-pick changes from boost-python branch
stages: default arguments for constructors
properties: make iterator public
2019-05-02 14:55:34 +02:00
Robert Haschke
d4dfcf962b FixedCartesianPoses 2019-05-02 14:55:34 +02:00
Robert Haschke
cef6eb64bb ContainerBase::remove(Stage* child) 2019-05-02 14:55:34 +02:00
Robert Haschke
c62938a1c4 SimpleGrasp: only use generator if it's defined
UnGrasp doesn't need a grasp generator
2019-05-02 14:55:34 +02:00
Robert Haschke
29ecec7403 vary place pose depending on object shape
- boxes, cylinders: flip up/down, rotate about world's z
- spheres: rotate about world's z
2019-05-02 14:55:34 +02:00
Robert Haschke
168ff6f3f4 use simple InterpolationPlanner to open/close gripper 2019-05-02 14:55:34 +02:00
Robert Haschke
4063a3ca29 fix SimpleUnGrasp
- ComputeIK needs to be first sub stage for both, grasping and ungrasping
- correctly re-enable object collision checking
2019-05-02 14:55:34 +02:00
Robert Haschke
0fe18799d9 GeneratePlacePose 2019-05-02 14:55:34 +02:00
Robert Haschke
6de1c8dbe5 ContainerBase::findChild() 2019-05-02 14:55:34 +02:00
Robert Haschke
39427f89bf Merge branches 'fix-interface-deduction' (#84), 'compute-ik-default-timeout' (#80),
'fix-visualization' (#76), and master-improvements (#81)
2019-03-11 14:09:03 +01:00
Robert Haschke
c56c7294e4 fix pruning
Never augment already derived interfaces, only prune!
2019-03-11 13:54:54 +01:00
Robert Haschke
48d93e803d improve SerialContainerPrivate::pruneInterface 2019-03-11 13:54:54 +01:00
Robert Haschke
905b6a3b0c move validateConnectivity() from ContainerBase to StagePrivate
... to allow specific stage types (PropagatingEitherWay) implementing their own validation
No need for a public interface.
2019-03-11 13:54:54 +01:00
Robert Haschke
10c7a9cfd7 prune UNKNOWN *and* PROPAGATE_BOTHWAYS
If PropagatingEitherWay's interface is not met in *both* directions (but only one),
in BOTHWAY mode, issue a warning. Otherwise handle both, AUTO and BOTHWAY mode,
in the same fashion when resolving interfaces.

TODO: move validateConnectivity() in StagePrivate.
default action = default action from ContainerBase.
PropagatingEitherWay: issue warning for case above
2019-03-11 13:54:54 +01:00
Robert Haschke
f831fe5483 fix comments + typos 2019-03-11 13:54:54 +01:00
Robert Haschke
7d25c55978 Container: more unit tests for interface detection / validation 2019-03-11 13:54:54 +01:00
Robert Haschke
d741f36ee3 fixup! improve error msg for mismatching container/child interfaces
We need to consider input and output interfaces separately.
Also, use console output symbols (<- / -> / <->)
2019-03-11 13:54:54 +01:00
Robert Haschke
1e953db649 non-const Property::value() 2019-03-10 17:55:46 +01:00
Robert Haschke
36df4f9975 MoveTo/MoveRelative: reduce default timeout to 1s 2019-03-10 17:55:15 +01:00
Robert Haschke
0304c22f7d ComputeIK: allow attached body as ik_frame 2019-03-10 17:50:45 +01: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
Robert Haschke
c72a6cddac ComputeIK: auto-configure default timeout from JMG's default
TODO: actually set the default value but not the current value!
2019-02-21 00:53:26 +01:00
Robert Haschke
579c336558 Connecting: also check that attached objects match 2019-02-21 00:51:57 +01:00
Robert Haschke
e885c272fa Merger: skip empty sub trajectories for merging 2019-02-21 00:50:25 +01:00
Robert Haschke
1047ef08a4 ComputeIK: fix typo 2019-02-21 00:47:52 +01:00
Robert Haschke
2da9e0d722 rework solution msgs
- to allow solution wrappers (WrappedSolution, SolutionSequence)
  to transmit their comment and markers as well
- introduced new SolutionInfo.msg,
  which is the info common to solution wrappers and actual SubTrajectories
2019-02-12 13:57:12 +01:00
Robert Haschke
527ec8edaa correctly sort upstream_solutions_ in ComputeIK and GeneratePose
Reworked cost_queue to correctly sort pointer-like types.
Added unittests for new ValueOrPointeeLess<T> less operator, ordered<T>, and rviz cost ordering.
2019-02-12 03:05:36 +01:00
v4hn
8392ed5ad3 generate IK solutions incrementally
This is not a good approach.
The same can be achieved by generating targets incrementally.
The better approach, to generate IK solutions incrementally,
has to maintain previous solutions for each target.
2019-02-10 08:17:34 +01:00
Robert Haschke
1e0a9401e7 Merge PR #72: rviz property visualization 2019-02-10 06:25:25 +01:00
Robert Haschke
e71376743c yamp-cpp parsing 2019-02-10 06:21:51 +01:00
Robert Haschke
a1c81f1236 YAML property serialization
- switch from ROS serialization/deserialization to YAML
- no native C++ deserialization for ROS msg types available
- drop Propert::print()
2019-02-10 06:21:51 +01:00
Robert Haschke
4ca794cb86 unify property handling in LocalTaskModel and RemoteTaskModel 2019-02-10 06:21:51 +01:00
Robert Haschke
71099c68d4 Task: fix RobotModelPtr leak
On Task::clear() also need to reset introspection's cache.
2019-02-09 01:00:29 +01:00
Robert Haschke
a1f7c5d1c6 improve error msg for mismatching container/child interfaces 2019-02-06 10:29:10 +01:00
Robert Haschke
81cc0aecc6 Property::type_index -> boost::typeindex::type_info
avoid decl expressions, explicit public typedef
2019-01-17 23:54:35 +01:00
Robert Haschke
062cfe0b60 PropertyTypeRegistry to store serialization/deserialization functions 2019-01-17 23:54:35 +01:00
Robert Haschke
ae75c3aa7f Eigen::Affine3 -> Eigen::Isometry3 2018-11-28 08:14:44 +01:00
v4hn
cd9115483b let marker_ns default to stage name
The stages can still decide on their namespaces on their own,
but markerNS() at least provides a stage-specific name they can use.
2018-11-09 10:50:48 +01:00
v4hn
3176aff30f add accessors for marker_ns property 2018-11-09 10:50:40 +01:00
v4hn
ac3b56a160 document timeout() helpers 2018-11-09 10:50:33 +01:00
v4hn
6b4b4ca624 ComputeIK: threshold for new solutions as property 2018-11-09 10:50:14 +01:00
v4hn
b379ba2268 fix broken rvalue-forward & SerialContainer assert
Release mode builds previously produced broken solutions with too many entries,
debug build triggered the assert

container.cpp:334: assert(solution.empty())

The standard guarantees std::vector(&&a) leaves a.empty() == true,
so the logic there is fine as long as subsolutions is actually
used for move-construction.
2018-11-09 10:47:51 +01:00
Robert Haschke
b9a86eb5be use task id as default name of top-level stage 2018-11-08 12:55:45 +01:00
v4hn
a86eca9ada predicate filter: fix documentation 2018-11-02 15:51:24 +01:00
v4hn
cc90d5f3f2 add PredicateFilter 2018-11-02 11:53:09 +01:00
Robert Haschke
5af54b5a21 GenerateGraspPose: spawn failure if object unknown 2018-11-02 11:36:00 +01:00
Robert Haschke
0f31cdee2e move storeFailures() into public Stage API 2018-11-02 11:36:00 +01:00
Robert Haschke
915151fdd2 modify ps: add convenience functions 2018-11-02 11:21:48 +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
v4hn
e5fc8b62b4 MoveRelative: possibly update last waypoint before transform lookup
Apparently this RobotState is not necessarily updated,
this broke some pipeline testing over here.
2018-10-25 00:12:46 +02:00
v4hn
38cc4c6936 add comment to subtrajectory 2018-10-25 00:12:46 +02:00
v4hn
80adcb9e24 add comments to wrapped solutions 2018-10-24 11:53:14 +02:00
v4hn
73b35ef2bf do not send failures to parent
It is enough for us to investigate these locally
2018-10-24 11:47:42 +02:00
v4hn
932497ce6d do not pass failures on to monitors
They should not work with them and this
removes the need for the usual `if(s.isFailure) return`.
2018-10-24 11:44:26 +02:00
Robert Haschke
d6714e89ab fixup! rename "goal" to "direction" 2018-10-23 21:09:07 +02:00
Robert Haschke
b511110ae6 remove dummy file 2018-10-23 01:30:33 +02:00
v4hn
9740179f7f Merge branches 'cleanup-planner-interface', 'connect', 'fix-visualization' and 'rviz-createMarker' into master 2018-10-22 17:31:01 +02:00
v4hn
7fcef54a0e fixup! add name to Connecting DEBUG output
This is important to differentiate between different Connect stages.
2018-10-22 17:28:52 +02:00
v4hn
13ad4d58f1 fixup! rename "goal" to "direction"
"Goal" implies a motion to a target configuration.
MoveRelative explicitly does not do that.

"Direction" is usually not used for rotations,
but perfectly valid to describe them.

I merged 544f574166
together with the first version of this rename
that got force-pushed because it was incomplete.
2018-10-22 17:07:00 +02:00
v4hn
a05a76fd8c fixup! add properties for conditional debug output
in MoveIt planner.

These can be quite helpful.
I agree, we really don't need them enabled by default.
2018-10-22 16:59:24 +02:00
Robert Haschke
70e32cb0b3 Connect: skip initial PlanningScene::diff() 2018-10-21 00:27:50 +02:00
Robert Haschke
18bf5246ed cleanup PlannerInterface
- remove group + timeout properties: they are passed as arguments to plan()
- move max_velocity_scaling_factor, max_acceleration_scaling_factor to PlannerInterface base class
2018-10-21 00:02:10 +02:00
Robert Haschke
b5b58ea0ca Introspection::solutionFromId() 2018-10-20 23:33:54 +02:00
Robert Haschke
02ed4d3579 Connect: verbose debugging output in case of state incompatibilities 2018-10-20 23:32:43 +02:00
Robert Haschke
c6f985be3a Connect: allow different merge modes
for now:
SEQUENTIAL (no merging)
WAYPOINTS (naive)
2018-10-20 23:32:43 +02:00
Robert Haschke
ba88c59201
Merge pull request #54 from ros-planning/pr-capability
ExecuteTaskSolution capability
2018-10-20 23:25:48 +02:00
Robert Haschke
1e39da2388 fix stage "FixCollisionObjects"
... correction wasn't initialized to zero, resulting in random NaNs
2018-10-20 22:59:27 +02:00
Robert Haschke
a0a9738a00 update PlanningScene's RobotState before storing it 2018-10-20 22:02:05 +02:00
Robert Haschke
e894d8bce2 fix caching of PlanningPipeline ptrs
Need to reset cache if corresponding RobotModel was destroyed.
To this end, we cannot simply use the RobotModel's name.
2018-10-20 21:50:05 +02:00
Robert Haschke
f548b7edcc PipelinePlanner: disable publishing of plan request + computed path 2018-10-20 12:21:18 +02:00
Robert Haschke
36fe4c98be reduce copying of shared ptrs 2018-10-20 12:21:18 +02:00
Robert Haschke
d648a4091e guard use of introspection_ pointer 2018-10-20 02:38:11 +02:00
Robert Haschke
af6f7b12bf pick_pa10: fix initialization of RobotState
only a subset of joints was initialized
2018-10-20 02:38:11 +02:00
Robert Haschke
e9b16ba7c0 fix destruction order 2018-10-20 02:38:11 +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
v4hn
1a4f6c0e57 keep RobotModelLoader around
Otherwise the robot_model_ does not remain valid
2018-10-17 01:55:28 +02:00
Robert Haschke
a745f55ab7 fix compiler warnings in release mode 2018-10-17 01:55:28 +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
1787a66d37 fix compiler warnings 2018-10-16 23:14:58 +02:00
Robert Haschke
f1764d0de4 fix tests 2018-10-16 23:14:58 +02:00
Robert Haschke
940543a063 fixup! allow preemption of Task::plan() 2018-10-16 23:14:58 +02:00
Robert Haschke
b5bc97b3eb Task::execute() 2018-09-25 22:17:42 +02:00
Robert Haschke
7bd9bcf972 allow preemption of Task::plan() 2018-09-25 22:16:45 +02:00
Robert Haschke
e5e291d86f FixCollisions stage 2018-09-25 22:16:45 +02:00
Robert Haschke
a7b74673ce PlannerInterface for joint-space interpolation 2018-09-25 22:16:45 +02:00
Robert Haschke
4860462076 ComputeIK: forward child solution's comment 2018-09-25 16:25:22 +02:00
Robert Haschke
c70db65578 fixup! MoveTo: store goal as any type 2018-09-25 16:25:13 +02:00
Robert Haschke
ca9d50e7d8 fix compiler warnings 2018-09-25 13:30:53 +02:00
Robert Haschke
f2573e9bfd fix include order
Local headers should be preferred over those from underlay.
Consequently use target_include_directories() to properly define include order.
2018-09-25 13:30:53 +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
Robert Haschke
ff2474f262 pa10 doesn't need move_group 2018-06-03 12:36:12 +02:00
Robert Haschke
932ab5eba3 cast demos as integration unit tests
- moved demos from demo to test folder
- run them as unittest, checking range of solutions
2018-06-03 12:36:01 +02:00
Robert Haschke
3215880b98 Merge branches 'move-to', 'simple-grasp' and 'properties' into master
These branches only work together:
- MoveTo / MoveRelative: common handling of "goal" property
- properties: required changes to allow for multiple inheritance
- generalize SimpleGrasp / GenerateGraspPose
2018-06-03 11:38:50 +02:00
Robert Haschke
2e46082521 remove Container::exposePropertiesOfChild() 2018-06-03 11:36:50 +02:00
Robert Haschke
157caac385 generalize SimpleGrasp / GenerateGraspPose
- move "pregrasp", "grasp" property from SimpleGrasp to GenerateGraspPose
- Container::exposePropertiesOfChild: decouple exposure from inheritance
2018-06-03 11:36:50 +02:00
Robert Haschke
544f574166 MoveRelative: store goal as any type 2018-06-03 09:22:06 +02:00
Robert Haschke
00260d62b5 MoveTo: store goal as any type 2018-06-03 09:22:06 +02:00
Robert Haschke
941dee7fc1 exposeTo: const method 2018-06-03 09:22:06 +02:00
Robert Haschke
abc4c5de50 PropertyMap: allow any type 2018-06-03 09:18:37 +02:00
Robert Haschke
a8431ce661 Container::exposePropertiesOfChild: allow skipping of undefined props 2018-06-03 06:46:12 +02:00
Robert Haschke
0dab499533 Stage: generally allow forwarding of interface properties 2018-06-03 06:46:12 +02:00
Robert Haschke
92296e020f SimpleGrasp(GraspGenerator)
allow any GraspGenerator stage that provides "pregrasp" and "grasp"
postures as well as a target_pose for grasping.
2018-06-03 06:46:12 +02:00
Robert Haschke
beffe27987 improve property debugging 2018-06-03 06:42:11 +02:00
Robert Haschke
cbb2cd69f7 property inheritance: both from PARENT and INTERFACE
- source_id -> source_flags: bits indicating configured paths
- initializers, e.g. fromName(), should throw
- ignore undeclared errors during inheritance
- on undefined error, reset the value to None
- override value only if previously set by lower-priority source
  MANUAL > CURRENT > PARENT > INTERFACE
2018-06-03 06:36:15 +02:00
Robert Haschke
abea2ee82b reuse boost::any's type_index type 2018-06-02 17:46:37 +02:00
Robert Haschke
2e9932cfbc fix: don't report config issues as successful (but empty) solutions 2018-06-02 17:46:28 +02:00
v4hn
381210f43d compatibility with boost 1.54
This is required for ROS indigo on ubuntu 14.04
2018-06-01 15:02:11 +02:00
Michael Görner
5739bf39d9 use correct frame in pick (#52) 2018-06-01 14:55:23 +02:00
v4hn
0091c76208 compute ik: ignore collisions skips early eef check 2018-06-01 11:34:07 +02:00
v4hn
4026661bfe Rewrite and relax approx comparisons
This is more readable.

Adjusting the threshold to 1e-4 is required to allow for tolerances
in potential sampling planner steps in between.
2018-06-01 11:32:29 +02:00
v4hn
bd1edcbde7 add max_solutions param to plan()
For big problems you just don't want *all* solutions
2018-06-01 11:13:44 +02:00
Robert Haschke
ac1ccba883 minor formatting changes (#51) 2018-05-31 18:01:30 +02:00
Robert Haschke
b35aba6e19 validate merged trajectories 2018-05-30 21:23:34 +02:00
Robert Haschke
941e9df737 declare properties "timeout", "marker_ns" for all stages 2018-05-30 21:21:14 +02:00
Robert Haschke
c2dd28abae rework storing of solutions
- solutions_, failures_ as SolutionBaseConstPtrs in StagePrivate
- replace processSolutions() / processFailures() by direct const-access to storage containers
- generic sendForward(), sendBackward(), spawn(), connect() methods in StagePrivate
- reuse StagePrivate's sendForward(), sendBackward(), spawn() in containers

- store created InterfaceStates in StagePrivate::states_
- Interface: ordered<InterfaceState*> (only store pointers)
  allows for common handling of states of valid and failure solutions

- remove additional state+solution storages
- containers: internal->external state mapping as InterfaceState* -> InterfaceState*
2018-05-30 21:02:50 +02:00
v4hn
ae6b86c7d7 remove void line
?
2018-05-30 21:02:19 +02:00
Robert Haschke
5878e8ab6b correctly reset FixedState 2018-05-30 21:02:03 +02:00
Robert Haschke
52fdf30433 simplify compute() API
- remove bool return value
- always create a solution trajectory, also in case of failures
- success/failure determined from solution.isFailure()

minor adjustments during cherry-pick
2018-05-30 21:01:34 +02:00
Robert Haschke
eb50aaed0e SolutionBase: rename 'name' to 'comment' 2018-05-30 20:25:42 +02:00
Robert Haschke
84dec07565 MoveTo/MoveRelative: generic IK target frame
...instead of simple link name
2018-05-30 20:25:07 +02:00
v4hn
495c80350b Merge branches 'joint_pose', 'move-relative-joints', 'basic-merger' and 'compute-ik' 2018-05-30 19:35:58 +02:00
v4hn
015b1466f4 MoveRel: handle unknown links with hard failure 2018-05-30 19:09:57 +02:00
v4hn
56c9128d99 MoveTo: cleaner strict handling of invalid properties 2018-05-30 18:53:39 +02:00
v4hn
b8e6e5e2fa MoveTo: reduce scope of try-catch 2018-05-30 18:44:26 +02:00
v4hn
bbd274da96 MoveTo: simplify getJointStateGoal 2018-05-30 18:42:31 +02:00
Robert Haschke
b7b7a916df basic merger functionality 2018-05-18 17:44:55 +02:00
Robert Haschke
5e8117720e ignore failures for further processing 2018-05-18 17:44:43 +02:00
Robert Haschke
abb68da083 forward properties 2018-05-18 17:44:43 +02:00
Robert Haschke
ce10d96c5c ComputeIK: report collision pairs 2018-05-18 17:44:43 +02:00
Robert Haschke
41e64ed7e1 remove std::map serialization 2018-05-18 17:44:30 +02:00
Robert Haschke
40ca35085a cleanup 2018-05-18 17:44:30 +02:00
llach
9a7aa4dad7 Pick: added setter for relative joint lift motion 2018-05-18 17:44:30 +02:00
llach
d0c8f7f819 MoveRelative: added relative joint space goals to 2018-05-18 17:44:30 +02:00
llach
0779c255df added stream serialization for std::map<std::string, T> 2018-05-18 17:44:30 +02:00
Robert Haschke
9e19f2187b moved implementations of property setters to header 2018-05-18 17:44:17 +02:00
Robert Haschke
17e487be06 cleanup
- indentation: space -> tabs
- only consider joints of JMG
2018-05-18 17:44:17 +02:00
llach
2be3ee611b adapted MoveTo api change. joint_pose -> named_joint_pose 2018-05-18 17:44:17 +02:00
llach
55470062c9 MoveTo: converting named joint poses to robot state msg in init; property names refactoring 2018-05-18 17:44:17 +02:00
llach
97c2312d67 MoveTo: can now take RobotState msg as goal 2018-05-18 17:44:17 +02:00
Robert Haschke
19fa7349d3 Task: only accept containers 2018-05-18 17:42:34 +02:00
Robert Haschke
e3ca832497 Property: silently return empty string when serialization is not supported 2018-05-18 17:42:34 +02:00
Robert Haschke
2106c51c9d Property: provide a fallback serialize() implementation
... in case operator<< is not defined for type T
2018-04-28 22:13:52 +02:00
Robert Haschke
ae3b9126bd get rid off empty JointState errors 2018-04-27 12:07:27 +02:00
Robert Haschke
ecf8029f75 add timing to merged trajectories 2018-04-27 12:06:59 +02:00
Robert Haschke
872e3cc004 unittest for Priority comparisons 2018-04-27 12:04:55 +02:00
Robert Haschke
8fb47e99e8 update ordering on any change of priority 2018-04-27 12:04:55 +02:00
Robert Haschke
67b6dcb49b move implementation into cpp file 2018-04-27 12:04:55 +02:00
v4hn
bbb64f2f8c inf cost states always go last
Also update sorted interface when state becomes inf or get's new cost.
2018-04-27 12:04:55 +02:00
Robert Haschke
728c1b40e2 Merge branches 'travis', 'fixes', 'visualization' and 'todos' 2018-04-15 12:01:19 +02:00
Robert Haschke
b8a4653f31 relax equality condition for joint values for inactive groups 2018-04-15 11:55:45 +02:00
Robert Haschke
5c525f8729 consider v4hn's comment 2018-04-15 11:55:45 +02:00
Robert Haschke
433db67cf9 only attempt to merge if there is something to merge 2018-04-15 11:55:45 +02:00
Robert Haschke
90b0b006eb more todo 2018-04-15 11:26:00 +02:00
Robert Haschke
6d6f185870 feedback 2018-04-15 11:26:00 +02:00
v4hn
ade42456b8 list of random todos 2018-04-15 11:26:00 +02:00
Robert Haschke
51e1dda6ec minor improvements: variable names, comments 2018-04-10 01:52:40 +02:00
Robert Haschke
564f3b0514 fix MoveRelative
Ignore success of planner_->plan() when min_distance is specified (and >= 0).
In this case, compute the achieved distance myself.
2018-04-10 01:52:40 +02:00
Robert Haschke
525ac10bbf std::string -> const std::string& 2018-04-10 01:52:40 +02:00
Robert Haschke
f2c0c7bf90 rename enableCollision -> allowCollision 2018-04-10 01:52:40 +02:00
Robert Haschke
fee7352961 renamed demo: plan_pick_trixi -> plan_pick_pr2 2018-04-10 01:52:40 +02:00
Robert Haschke
a044b719ac removed old stages: Move, Gripper, CartesianPositionMotion 2018-04-10 01:52:40 +02:00
Robert Haschke
b22217deab fix trajectory merging: initialize from well-defined RobotState 2018-04-10 01:52:40 +02:00
Robert Haschke
c553275276 SolutionSequence::fillMessage: ignore sub solutions with same creator as parent 2018-04-10 01:52:40 +02:00
Robert Haschke
556d9e1a5e Merge branches 'robot-model', 'task-move-constructor' and 'const-robot-trajectory' 2018-04-09 21:11:19 +02:00
Robert Haschke
2b79f15244 SubTrajectory: promise to not modify encapsulated RobotTrajectory 2018-04-09 21:10:18 +02:00
Robert Haschke
7463621f56 move assignment operator 2018-04-09 21:10:08 +02:00
Robert Haschke
7ebc4b2c7e Task: move constructor 2018-04-09 21:10:08 +02:00
Robert Haschke
35b35cc17e enable introspection only if ROS was initialized 2018-04-09 21:09:49 +02:00
Robert Haschke
bd60e6f65d Task::setRobotModel() / Task::loadRobotModel() 2018-04-09 21:09:49 +02:00
Robert Haschke
70065c98e7 Merge branches 'master' 'generate-pose' and 'connect' 2018-04-09 21:08:45 +02:00
Robert Haschke
21242ea976 publish failures 2018-04-09 21:08:17 +02:00
Robert Haschke
047a3f7db4 report duplicate joints, accept fixed joints as duplicates 2018-04-09 21:08:17 +02:00
Robert Haschke
55e3aae30a merge trajectories 2018-04-09 21:08:17 +02:00
Robert Haschke
03092e7e45 utility functions to merge multiple RobotTrajectories 2018-04-08 02:36:55 +02:00
Robert Haschke
2c08d9080d Pick, Place as specializations of PickPlaceBase 2018-04-08 02:36:55 +02:00
Robert Haschke
ce5d7c63f0 SimpleGrasp, SimpleUnGrasp as specializations of SimpleGraspBase 2018-04-08 02:36:55 +02:00
Robert Haschke
97d2eb5c6a Pick: remove Connect 2018-04-08 02:36:55 +02:00
Robert Haschke
85e88eabd1 Connecting::compatible() to check compatibility of states 2018-04-08 02:36:55 +02:00
Robert Haschke
eee53a0d80 MoveTo: use moving frame markers 2018-04-05 22:54:51 +02:00
v4hn
1424b51f79 expose typed setters in Cartesian solver 2018-04-05 22:53:24 +02:00
Robert Haschke
77442c0eed Connect: allow multiple groups to be processed in series 2018-04-05 13:51:50 +02:00
Robert Haschke
8811045f89 setCreator() once in StagePrivate::newSolution() 2018-04-05 13:51:50 +02:00
Robert Haschke
9ceecd235f SerialSolution -> SolutionSequence 2018-04-05 13:51:50 +02:00
Robert Haschke
4d77390d77 validate existence of object frame in init() 2018-04-05 13:50:09 +02:00
Robert Haschke
9e1c3059e1 derive GenerateGraspPose from GeneratePose 2018-04-05 13:50:09 +02:00
Robert Haschke
3640928a76 postpone pose transformation 2018-04-05 13:50:09 +02:00
Robert Haschke
603696f294 add missing reset() 2018-04-05 13:50:09 +02:00
v4hn
76c6ba14fa add a simple GeneratePose stage
ComputeIK is a wrapper, so we can't just give it a pose to compute.
2018-04-05 13:50:09 +02:00
llach
a82a48bb70 fix PropertyMap::exposeTo: use other_name as new name for property (#40) 2018-04-05 13:47:23 +02:00
llach
124979e2a8 MoveRelative: adapt msgs to fit class name (#39) 2018-04-05 13:46:13 +02:00
Robert Haschke
205beea503
Merge pull request #28 from ros-planning/pr-path-constraints
implement path constraints
2018-03-26 09:54:02 +02:00
Robert Haschke
2ca68e6645 actually implement path constraints for CartesianPath planner
... validating constraints
2018-03-26 09:50:17 +02:00
v4hn
d7d80c3499 implement path constraints for Move* 2018-03-26 09:50:17 +02:00
v4hn
80ae01dcca expose timeout property as typed setter 2018-03-26 09:50:17 +02:00
v4hn
8af44cefc3 add path constraints to planner API
and use it in the Connect class.

The cartesian planner ignores the constraints for now.
2018-03-26 09:50:17 +02:00
Robert Haschke
f0dc02e2c3 ContainerBasePrivate::position() -> childByIndex() 2018-03-26 00:24:25 +02:00
Robert Haschke
84f5c481be ContainerBasePrivate::position(): generically handle shifting 2018-03-26 00:24:25 +02:00
Robert Haschke
ff0ee407ff remove tool_to_grasp_tf from SimpleGrasp 2018-03-25 06:07:53 +02:00
Robert Haschke
54e4d8eed7 remove tool_to_grasp_tf from GenerateGraspPose 2018-03-25 06:07:53 +02:00
Robert Haschke
12e134ad09 added disclaimer 2018-03-25 06:07:53 +02:00
Robert Haschke
893cb25b5b fix isTargetPoseColliding
- must not use new sandbox_scene (we want to display the place eef)
- jmg not needed anymore
2018-03-25 06:07:53 +02:00
Robert Haschke
c98f10478e frame marker at ik frame 2018-03-25 06:07:53 +02:00
Robert Haschke
5b6a02d105 rename reference frame -> ik frame 2018-03-25 06:07:53 +02:00
Robert Haschke
f497112513 consider reference_frame 2018-03-25 06:04:09 +02:00
Robert Haschke
7531b8ad54 allow arbitrary reference frame for target_pose and ik frame 2018-03-25 06:04:09 +02:00
Robert Haschke
4fcac84d76 remove tests that do not test anything 2018-03-25 06:04:09 +02:00
Robert Haschke
9e5e098339 unittests
- provide simple hard-coded robot model
- test ComputeIK::init()
2018-03-25 06:04:09 +02:00
Robert Haschke
f4adaa949a validate available properties during init() 2018-03-25 06:04:09 +02:00
Robert Haschke
5a55a5949b Merge branches 'bug-fixes', 'gui' and 'pick-stage' 2018-03-24 07:32:20 +01:00
Robert Haschke
a4fed6a18a MoveTo Cartesian: create marker frame at current and goal pose 2018-03-24 06:16:10 +01:00
Robert Haschke
cb38a87a73 CartesianPath: return a partial trajectory in case of failure 2018-03-24 06:16:10 +01:00
Robert Haschke
81d88911a9 cosmetic fixes 2018-03-23 17:18:56 +01:00
Robert Haschke
2331c9aef2 pick: expose IK link frame as eef_frame 2018-03-23 17:18:56 +01:00
Robert Haschke
1c61fb3b85 expose solvers 2018-03-23 17:18:56 +01:00
llach
ea6cc4b6bf moved "attach object" from "pick" to "grasp" stage 2018-03-23 17:18:56 +01:00
Robert Haschke
ceaf896bf8 position scene node w.r.t. fixed frame 2018-03-23 11:22:23 +01:00
Robert Haschke
c2590636b1 fixup! SerialContainer: don't consider failures for solutions 2018-03-23 11:22:23 +01:00
Robert Haschke
862ac37cd6 fix ContainerBasePrivate::copyState(): don't copy (again) on update 2018-03-23 11:22:23 +01:00
Robert Haschke
b3af170663 reset num_failures_ 2018-03-23 11:22:23 +01:00
Robert Haschke
6b912c3531 MoveTo Cartesian Point: bug fix 2018-03-23 11:22:23 +01:00
Robert Haschke
6d859840c3 fix interface detection for nested SerialContainers 2018-03-23 09:18:21 +01:00
Robert Haschke
159beeb044 fix SerialContainer::canCompute() 2018-03-23 09:18:21 +01:00
Robert Haschke
bff6cc569c pick: reusable stage for picking up an object 2018-02-25 23:14:52 +01:00
Robert Haschke
d731e943f1 Merge branch 'parallel-container' 2018-02-25 23:12:35 +01:00
Robert Haschke
9545f92678 GenerateGraspPose::reset() 2018-02-25 23:11:55 +01:00
Robert Haschke
12582f6bbc SerialContainer: don't consider failures for solutions 2018-02-25 20:01:43 +01:00
Robert Haschke
99adfaf454 fix ParallelContainers' init() 2018-02-25 13:22:26 +01:00
Robert Haschke
dcf23f6147 relax ParallelContainer's validateConnectivity() 2018-02-25 13:22:26 +01:00
Robert Haschke
bcacda8507 implement ParallelContainer's pruneInterface() 2018-02-25 13:22:26 +01:00
eirtech
0dc4f5c53e ParallelContainer's requiredInterface() 2018-02-25 13:22:26 +01:00
eirtech
3e4aa19246 ParallelContainer: unittests 2018-02-25 13:22:14 +01:00
Robert Haschke
a2fd5fb7b8 more constexpr InterfaceFlags 2018-02-25 11:59:51 +01:00
Robert Haschke
a24039846a Merge branches 'analyze-property-errors' and 'fixes' 2018-02-24 13:45:51 +01:00
Robert Haschke
94bb50ddc1 fixup! PropertyMap: different exception types 2018-02-24 13:42:18 +01:00
v4hn
30de5e1a8c add catch-rethrow for property initialization
The property doesn't know its own name, so we add it here
2018-02-24 13:42:18 +01:00
v4hn
78b86bb65f add convenience overloads for setting properties 2018-02-24 13:42:18 +01:00
Robert Haschke
e9d1742337 ContainerBase::exposePropertiesOfChild 2018-02-24 13:42:18 +01:00
Robert Haschke
8152614d33 properties: init from source if current value is not defined
- reset(): reset current value to empty, not default
- value(): return current value, or - if not defined - the default
- initialize from source if current value is undefined,
  don't care about default value
2018-02-24 13:42:18 +01:00
Robert Haschke
0c8c298450 declare by std::type_index (instead of std::type_info) 2018-02-24 13:42:18 +01:00
Robert Haschke
6444229877 Stage::reportPropertyError 2018-02-24 13:42:18 +01:00
Robert Haschke
da9cdb5cf6 PropertyMap: different exception types
undeclared
undefined
type_error
2018-02-24 13:42:18 +01:00
v4hn
7caed29cb1 elaborate runtime_error in InterfaceState 2018-02-24 13:42:12 +01:00
v4hn
9709fd687d ComputeIK: update state to avoid dirty transforms 2018-02-24 13:42:12 +01:00
v4hn
69d3d8bd37 InitStageException::what(): distinguish stage/stages 2018-02-24 13:42:12 +01:00
Robert Haschke
cc8bcd3fdc become agnostic to urdfdom's shared_ptr types 2018-02-24 13:42:12 +01:00
Robert Haschke
07e99461fc conditionally normalize angular 2018-02-24 13:42:12 +01:00
Robert Haschke
cf54024379 fix derivation of propagation direction from connect stage 2018-02-24 13:42:12 +01:00
Robert Haschke
aee2525382 fix ModifyPlanningScene: pass stage properties to callback 2018-02-24 13:42:12 +01:00
Robert Haschke
b767e359e9 GenerateGraspPose: require pregrasp pose 2018-02-24 13:42:12 +01:00
Robert Haschke
6a3456f961 unittest ContainerBasePrivate::position() 2018-02-24 13:42:12 +01:00
Robert Haschke
9cd6efe46f Stage::init(PlanningScene) -> Stage::init(RobotModel) 2018-02-24 13:37:50 +01:00
v4hn
2a2d406368 remove restriction to watch only generators 2018-02-24 13:37:50 +01:00
Robert Haschke
d035089c10 MonitoringGenerator 2018-02-24 13:37:50 +01:00
Robert Haschke
8624224d6b FixedState generator 2018-02-24 13:37:41 +01:00
llach
e1281130ac CurrentState: fetch scene via get_planning_scene service 2018-02-24 13:37:41 +01:00
v4hn
4f2b38f02d add missing headers 2018-02-21 14:56:51 +01:00
Robert Haschke
cf76352d2d Merge branches 'fix-containers' and 'fix-priority-updates' 2018-02-20 08:07:48 +01:00
Robert Haschke
ba75f7c835 fixed remaining unittest 2018-02-20 08:06:23 +01:00
Robert Haschke
e368fd9948 avoid accidental overwrite of InterfaceState 2018-02-19 09:05:56 +01:00
Robert Haschke
373d7f204f update priorities of all interface states along a (partial) solution path
if a parallel container is involved somewhere in the middle, it will
again access these states, e.g. planning alternative solutions
2018-02-19 09:05:56 +01:00
Robert Haschke
1eacea6b35 reset InterfaceState::owner_ if state is removed from Interface 2018-02-19 09:05:56 +01:00
Robert Haschke
35f1540369 recursive interface auto-detection
trigger auto-detection from top (task) level:
only there we now for sure the accepted interface
2018-02-19 01:05:56 +01:00
Robert Haschke
f45a4e3e2c improve validation
- postpone pruning / interface auto-detection to top task-level
  only the task state knows for sure, that it requires its wrapped child to push to both ends
- perform connectivity validation only after pruning
  only then, we the interfaces are completely determined
2018-02-19 01:05:56 +01:00
Robert Haschke
a523333ef6 basic auto-detection of interfaces for propagating stages
works if propagation direction can be derived from a generator or
connecting stage within the sequence

start-to-end propagation through whole serial container still fails
2018-02-18 21:23:12 +01:00
Robert Haschke
1a0b9b36ee fix connection creation
Establishing the interface connections, we face a chicken-egg-problem:
To establish a connection, a predecessors/successors pull interface is
assigned to the current's stage push interface.
However, propagating stages (in auto-detection mode) can only create
their pull interfaces if the corresponding, opposite-side push interface
is present already (because that's the mechanism to determine the supported
propagation directions).

Hence, we need to resolve this by performing two sweeps:
- initialization, assuming both propagation directions should be supported,
  thus generating both pull interfaces, i.e. providing the egg
- stripping down the interfaces to the actual context
  This context is provided by two stages pushing from both ends
  into a (potentially long) sequence of propagating stages (tbd).

Contributions of this PR:
- PropagatingEitherWay: explicitly distinguish AUTO from BOTHWAYS interface
  AUTO: auto-derive interface from provided push interfaces
  BOTHWAYS: explicitly require both directions
- SerialContainer: (better, but not yet perfect) validation of connectivity
- ParallelContainer: determine interface from what children offer
2018-02-18 21:23:12 +01:00
Robert Haschke
ec2b06292e extended unittest for serial connection validation 2018-02-18 14:03:57 +01:00
Robert Haschke
a4feb705d0 cleanup operator<<(ostream, *)
- basic implementation for StagePrivate
- implementation for Stage calls this
- implementation for ContainerBase recursively calls this
- implementation for Task added
2018-02-18 13:58:09 +01:00
Robert Haschke
1f4264c8c8 PropagatingEitherWayPrivate: count failures in both directions 2018-02-16 16:01:05 +01:00
Robert Haschke
2ea463fff4 replace std::cout with ros console 2018-02-16 16:01:05 +01:00
Robert Haschke
acffd39091 SerialContainer: fix priority propagation
- traverse all (also partial) solution paths
- and update priority at both ends
- remove pending state pairs if cost increased to infinity
2018-02-16 16:01:05 +01:00
Robert Haschke
469c6ccede MoveRelative: fix marker arrow 2018-02-16 11:37:57 +01:00
Robert Haschke
dd0d004052 cleanup ComputeIK
- correctly check collision for target pose before doing IK
- visualize failed collision check / failed IK
2018-02-16 09:55:34 +01:00
Robert Haschke
deb09b7dfd GenerateGraspPose: marker cleanup
- remove arrow marker (approach direction isn't hard-coded anymore)
- consider all rigidly attached parent links for display
- add grasp frame
2018-02-14 17:50:02 +01:00
Robert Haschke
a8cf2dc844 GenerateGraspPose: correctly check for existence of link frames 2018-02-13 19:29:12 +01:00
Robert Haschke
7e66c24822 marker tools: allow vector of LinkModel* 2018-02-13 19:29:12 +01:00
Robert Haschke
b1fac6eed0 MoveRelative: allow zero min_distance 2018-02-13 19:29:12 +01:00
Robert Haschke
0b8bf2b8b7 example: twist motion 2018-02-13 00:01:01 +01:00
Robert Haschke
ca3bce4669 local planning scene 2018-02-12 23:58:49 +01:00
Robert Haschke
da9711a85c example using new features 2018-02-12 23:56:10 +01:00
Robert Haschke
d053e037eb cmake cleanup: group source files 2018-02-12 23:55:34 +01:00
Robert Haschke
4be373592a Merge branches 'cleanup', 'wip-modular-planning', 'wip-containers', 'wip-gui' and 'wip-modify-ps' 2018-02-12 23:54:51 +01:00
Robert Haschke
724f0c3014 modular planning
Separate planning approaches (using MoveIt pipeline or computeCartesianPath) from stages.
This allows to reuse planning in various stages without code duplication.

Reworked stages:
Move -> Connect
Gripper + CartesianPositionMotion -> MoveTo
CartesianPositionMotion -> MoveRelative
2018-02-12 23:43:47 +01:00
Robert Haschke
0d338a7f5f visualize collisions 2018-02-12 23:40:33 +01:00
Robert Haschke
f35a6bc05d FixCollisionObjects stage 2018-02-12 23:40:33 +01:00
Robert Haschke
f006f0f3d6 reduce code bloat using SFINAE template selection 2018-02-12 23:40:33 +01:00
Robert Haschke
381edf2d22 ModifyPlanningScene stage
- attach / detach objects to robot
- enable / disable collision pairs
- works in either direction (FORWARD +  BACKWARD)
2018-02-12 23:40:33 +01:00
Robert Haschke
449ccf55c6 merge WrapperBase + Wrapper into WrapperBase
Wrapper is not restricted to generator-type stage anymore.
2018-02-12 23:39:46 +01:00
Robert Haschke
032a4e23c1 derive WrapperBase from ParallelContainerBase 2018-02-12 23:39:46 +01:00
Robert Haschke
1ab2d596fb ContainerBasePrivate::liftSolution() 2018-02-12 23:39:46 +01:00
Robert Haschke
919be24d16 implement ParallelContainerBase 2018-02-12 23:39:46 +01:00
Robert Haschke
a3a8f1a9d2 improved directional (forward/backward) access to interfaces
- TraverseDirection -> Interface::Direction
- StagePrivate: pullInterface(), pushInterface()
- trajectories<dir>(SolutionBase) -> SolutionBase::trajectories<dir>()
2018-02-12 23:39:46 +01:00
Robert Haschke
73f509c3ba simplify usage of ContainerBasePrivate::copyState() 2018-02-12 23:39:46 +01:00
Robert Haschke
eaaaf07214 exploit default argument for Interface constructor 2018-02-12 23:39:46 +01:00
Robert Haschke
cf71d2f0d2 unify use of buffer interfaces in containers
All containers need to buffer their children's sendBackward/sendForward states.
2018-02-12 23:39:46 +01:00
Robert Haschke
9bdfcc8c23 expose Task::initScene()
... to allow use of local scene
2018-02-12 23:37:19 +01:00
Robert Haschke
e9d0a05f75 default arg in header 2018-02-12 23:37:19 +01:00
Robert Haschke
3a4dc6755b Merge branches 'fix-remote-task-model', 'wip-cost-ordering' and 'wip-properties' 2018-02-12 23:35:01 +01:00
Robert Haschke
fe2c4f312e serialize stage properties 2018-02-12 22:45:15 +01:00
Robert Haschke
5ff78f653e signal callback function
... to allow for syncing with rviz::Property
2018-02-12 22:45:15 +01:00
Robert Haschke
c3eead0115 allow only a single inititialization source 2018-02-12 22:45:15 +01:00
Robert Haschke
5c43d700a8 update state priorities in a container-specific fashion
Reorder interface list on priority updates.
This requires the InterfaceState to store a pointer to the owning Interface.
2018-02-12 22:44:18 +01:00
Robert Haschke
facd6438c9 Interface: order states by priority
Priority is (depth of solutions, accumulated cost along trace).
2018-02-12 22:44:18 +01:00
Robert Haschke
d0351698a6 single map internal_to_external
There is no need to distinguish between starts and ends when mapping
states, because start/end states need to be disjoint sets.
2018-02-12 22:44:18 +01:00
Robert Haschke
e6af069139 SerialContainer: traverse solutions w/o stopping stage
Always traverse from current solution to the start/end of a complete
path and only call the callback once for the whole trace.
2018-02-12 22:44:18 +01:00
Robert Haschke
6a62f6be76 default argument for NotifyFunction in Interface constructor 2018-02-12 22:44:18 +01:00
Robert Haschke
244a7d147b sort serial solutions before insertion 2018-02-12 22:36:58 +01:00
Robert Haschke
29577515a0 cost ordering for solutions 2018-02-12 22:36:56 +01:00
Robert Haschke
c84fa4cdd6 cost ordered API 2018-02-12 22:36:12 +01:00
Robert Haschke
1403fc9830 fixed some clang warnings 2018-02-12 22:36:12 +01:00
Robert Haschke
2184450bd6 count all failures for statistics 2018-02-12 22:36:12 +01:00
Robert Haschke
7a13d79bc4 example setting solution markers 2018-02-12 22:36:12 +01:00
Robert Haschke
97bb6ef2c6 allow handling of failures
- store "failure" solutions to facilitate debugging
- Introspection assigns solution IDs as soon as they are created in a stage
  Thus, solution IDs represent their creation order.
  In contrast, the order of publishing (in StageStatistics) should
  represent the cost order.
- Storing failures is disabled if Introspection is not available.
2018-02-12 22:35:58 +01:00
Robert Haschke
57cd11e3a9 Wrapper::compute: return true if new solutions are found 2018-02-12 22:33:39 +01:00
Robert Haschke
38f7696acf ComputeBase::addTrajectory -> ComputeBasePrivate::addTrajectory
Forbid indirect access to trajectories_.
Only official compute classes have access.
2018-02-12 22:33:39 +01:00
Robert Haschke
5c1cad14df Merge branches 'fixes', 'wip-drag-n-drop', 'wip-ikstage' and 'wip-marker-vis'
resolved conflicts:
core/include/moveit/task_constructor/container.h
core/src/container.cpp
core/src/stages/generate_grasp_pose.cpp
visualization/motion_planning_tasks/src/task_display.cpp
visualization/motion_planning_tasks/src/task_panel_p.h
2018-02-04 21:36:01 +01:00
Robert Haschke
17683b12c9 more exported stage plugins: CurrentState 2018-02-04 21:30:25 +01:00
Robert Haschke
736136dd9f collision test for ee group 2018-02-04 09:01:45 +01:00
Robert Haschke
3fd096c8e6 fix IK stage: insert different solutions as different scenes 2018-02-04 09:01:45 +01:00
Robert Haschke
57d907908d better names for frame and transform variables 2018-02-04 09:01:45 +01:00
Robert Haschke
fe05894709 renamed GenerateGraspPose::setGraspFrame() to setToolToGraspTF() 2018-02-04 09:01:45 +01:00
Robert Haschke
0918ad6897 implement IK as separate stage
... wrapping another stage
2018-02-04 09:01:45 +01:00
Robert Haschke
b6959170db derive WrapperBase from ContainerBase
While ParallelContainers can directly promote their child solutions as
their own, a Wrapper needs to modify those solutions. Hence, Wrappers
should be derived directly from Container.
2018-02-04 09:01:45 +01:00
Robert Haschke
e4ad7a0753 removed Stage::validate()
... only was checking for implies(a, a) which is always true
2018-02-04 09:01:45 +01:00
Robert Haschke
104e52eb48 promise to not modify solutions 2018-02-04 09:01:45 +01:00
Robert Haschke
f8952bb712 introducing process_id
The task id was not unique enough to distinguish different tasks.
When a task publisher is killed and restarted, it usually comes up with
the same task id. However, visualization doesn't notice this change and
get confused / crashes when receiving task statistics and solutions.
2018-02-04 09:01:40 +01:00
Robert Haschke
01d43e3f3b marker_tools 2018-02-04 09:00:54 +01:00
Robert Haschke
0e8dd85ae6 allow stages to access the created SubTrajectory
to add markers, set name, etc.
2018-02-04 09:00:54 +01:00
Robert Haschke
e9363919e8 Merge branch 'wip-properties' 2018-02-04 08:59:35 +01:00
Robert Haschke
41ec46964a more unit tests 2018-02-04 08:56:11 +01:00
Robert Haschke
3ab899a9a8 cleanup error handling
throw std::logic_error on type errors
throw std::runtime_error on undeclared property
don't expose generic PropertyMap::declare()
2018-02-04 08:56:11 +01:00
Robert Haschke
c9e3be08d0 fix initialization order of properties
First from INTERFACE, second from PARENT.

INTERFACE initialization only makes sense for Propagating stages.
Connecting stages should ensure that interfaces define identical
properties which is not possible with boost::any.
2018-02-04 08:56:11 +01:00
Robert Haschke
4db7e8eb88 definition of PropertyInitializerSource moved to Stage 2018-02-04 08:56:11 +01:00
Robert Haschke
f6253c46a4 generalize initialization source from enum to int 2018-02-04 08:56:11 +01:00
Robert Haschke
59fe1e7860 separate setValue() and setCurrentValue()
setValue() also updates the default value.
reset() reset to the default value.
setCurrentValue() only updates the current value, keeping current default.
Thus setCurrentValue() can be reverted (to default) using reset().
2018-02-04 08:55:58 +01:00
Robert Haschke
d62b85572a remove property name from InitializerFunction's signature 2018-02-03 21:45:40 +01:00
Robert Haschke
0d6dbee215 initFrom() -> configureInitFrom() + performInitFrom()
Use different function names for different semantics.
2018-02-03 21:45:40 +01:00
Robert Haschke
32a0de6bf3 countDefined(): count number of defined properties 2018-02-03 21:45:40 +01:00
Robert Haschke
f2b688d1f0 generalize GenerateGraspPose
replacing scalar graspOffset and hard-coded Euler angles with arbitrary graspFrame
2018-02-03 21:45:40 +01:00
Robert Haschke
587dcaebb2 ur5 example: use properties 2017-12-13 14:19:20 +01:00
Robert Haschke
7d8f8dfc8d PropertyMap 2017-12-13 14:19:20 +01:00
v4hn
d7719e721f add full license information
so dull... but for the matter of completeness
2017-12-12 21:24:32 +01:00
v4hn
9fca79e48b reduce catkin_lint complains in core 2017-12-08 20:55:30 +01:00
Robert Haschke
0246d60de9 DisplaySolution: struct to unify all data corresponding to a sub trajectory 2017-11-30 18:15:47 +01:00
Robert Haschke
1bc485658e Merge branch 'wip-visualization' into wip-refactor 2017-11-24 13:40:43 +01:00
Robert Haschke
b48ba91a75 cmake compatbility to 2.8.12 2017-11-24 13:37:50 +01:00
Robert Haschke
9032dabca5 navigate solutions 2017-11-24 11:24:18 +01:00
Robert Haschke
957b10a7a8 don't export SerialContainer as pluginlib class
... it's imported as builtin
2017-11-20 00:07:27 +01:00