Commit Graph

515 Commits

Author SHA1 Message Date
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