cleanup / renaming

* Rename pruneInterface() -> resolveInterface()
* Rename accepted (interface) -> expected
* Improve exception strings
This commit is contained in:
Robert Haschke 2020-04-10 19:53:25 +02:00
parent aa732d8c66
commit 499fcfb04b
11 changed files with 251 additions and 47 deletions

View File

@ -106,7 +106,7 @@ public:
void validateConnectivity() const override;
// Containers derive their required interface from their children
// UNKNOWN until pruneInterface was called
// UNKNOWN until resolveInterface was called
InterfaceFlags requiredInterface() const override { return required_interface_; }
// forward these methods to the public interface for containers
@ -139,7 +139,7 @@ protected:
auto& internalToExternalMap() { return internal_to_external_; }
const auto& internalToExternalMap() const { return internal_to_external_; }
// set in pruneInterface()
// set in resolveInterface()
InterfaceFlags required_interface_;
private:
@ -170,7 +170,7 @@ public:
SerialContainerPrivate(SerialContainer* me, const std::string& name);
// called by parent asking for pruning of this' interface
void pruneInterface(InterfaceFlags accepted) override;
void resolveInterface(InterfaceFlags expected) override;
// validate connectivity of chain
void validateConnectivity() const override;
@ -215,7 +215,7 @@ public:
ParallelContainerBasePrivate(ParallelContainerBase* me, const std::string& name);
// called by parent asking for pruning of this' interface
void pruneInterface(InterfaceFlags accepted) override;
void resolveInterface(InterfaceFlags expected) override;
void validateConnectivity() const override;
@ -251,7 +251,7 @@ public:
typedef std::function<void(SubTrajectory&&)> Spawner;
MergerPrivate(Merger* me, const std::string& name);
void pruneInterface(InterfaceFlags accepted) override;
void resolveInterface(InterfaceFlags expected) override;
void onNewPropagateSolution(const SolutionBase& s);
void onNewGeneratorSolution(const SolutionBase& s);

View File

@ -71,15 +71,11 @@ public:
/// actually configured interface of this stage (only valid after init())
InterfaceFlags interfaceFlags() const;
/** Interface required by this stage
*
* If the interface is unknown (because it is auto-detected from context), return UNKNOWN */
/// Retrieve interface required by this stage, UNKNOWN if auto-detected from context
virtual InterfaceFlags requiredInterface() const = 0;
/** Prune interface to comply with the given propagation direction
*
* PropagatingEitherWay uses this in restrictDirection() */
virtual void pruneInterface(InterfaceFlags /* accepted */) {}
/// Resolve interface/propagation direction to comply with the given external interface
virtual void resolveInterface(InterfaceFlags /* expected */) {}
/// validate connectivity of children (after init() was done)
virtual void validateConnectivity() const;
@ -220,8 +216,8 @@ public:
InterfaceFlags requiredInterface() const override;
// initialize pull interfaces for given propagation directions
void initInterface(PropagatingEitherWay::Direction dir);
// prune interface to the given propagation direction
void pruneInterface(InterfaceFlags accepted) override;
// resolve interface to the given propagation direction
void resolveInterface(InterfaceFlags expected) override;
bool canCompute() const override;
void compute() override;

View File

@ -422,23 +422,23 @@ void SerialContainerPrivate::validateInterface(const StagePrivate& child, Interf
}
// called by parent asking for pruning of this' interface
void SerialContainerPrivate::pruneInterface(InterfaceFlags accepted) {
void SerialContainerPrivate::resolveInterface(InterfaceFlags expected) {
// we need to have some children to do the actual work
if (children().empty())
throw InitStageException(*me(), "no children");
if (!(accepted & START_IF_MASK))
if (!(expected & START_IF_MASK))
throw InitStageException(*me(), "unknown start interface");
Stage& first = *children().front();
Stage& last = *children().back();
// FIRST child
first.pimpl()->pruneInterface(accepted & START_IF_MASK);
first.pimpl()->resolveInterface(expected & START_IF_MASK);
// connect first child's (start) push interface
setChildsPushBackwardInterface(first.pimpl());
// validate that first child's and this container's start interfaces match
validateInterface<START_IF_MASK>(*first.pimpl(), accepted);
validateInterface<START_IF_MASK>(*first.pimpl(), expected);
// connect first child's (start) pull interface
if (const InterfacePtr& target = first.pimpl()->starts())
starts_.reset(new Interface(
@ -448,14 +448,14 @@ void SerialContainerPrivate::pruneInterface(InterfaceFlags accepted) {
for (auto it = ++children().begin(), previous_it = children().begin(); it != children().end(); ++it, ++previous_it) {
StagePrivate* child_impl = (**it).pimpl();
StagePrivate* previous_impl = (**previous_it).pimpl();
child_impl->pruneInterface(invert(previous_impl->requiredInterface()) & START_IF_MASK);
child_impl->resolveInterface(invert(previous_impl->requiredInterface()) & START_IF_MASK);
connect(*previous_impl, *child_impl);
}
// connect last child's (end) push interface
setChildsPushForwardInterface(last.pimpl());
// validate that last child's and this container's end interfaces match
validateInterface<END_IF_MASK>(*last.pimpl(), accepted);
validateInterface<END_IF_MASK>(*last.pimpl(), expected);
// connect last child's (end) pull interface
if (const InterfacePtr& target = last.pimpl()->ends())
ends_.reset(new Interface(
@ -552,7 +552,7 @@ void WrappedSolution::fillMessage(moveit_task_constructor_msgs::Solution& soluti
ParallelContainerBasePrivate::ParallelContainerBasePrivate(ParallelContainerBase* me, const std::string& name)
: ContainerBasePrivate(me, name) {}
void ParallelContainerBasePrivate::pruneInterface(InterfaceFlags accepted) {
void ParallelContainerBasePrivate::resolveInterface(InterfaceFlags expected) {
// we need to have some children to do the actual work
if (children().empty())
throw InitStageException(*me(), "no children");
@ -563,8 +563,8 @@ void ParallelContainerBasePrivate::pruneInterface(InterfaceFlags accepted) {
for (const Stage::pointer& child : children()) {
try {
auto child_impl = child->pimpl();
child_impl->pruneInterface(accepted);
validateInterfaces(*child_impl, accepted, first);
child_impl->resolveInterface(expected);
validateInterfaces(*child_impl, expected, first);
// initialize push connections of children according to their demands
setChildsPushForwardInterface(child_impl);
setChildsPushBackwardInterface(child_impl);
@ -579,16 +579,16 @@ void ParallelContainerBasePrivate::pruneInterface(InterfaceFlags accepted) {
throw exceptions;
// States received by the container need to be copied to all children's pull interfaces.
if (accepted & READS_START)
if (expected & READS_START)
starts().reset(new Interface([this](Interface::iterator external, bool updated) {
this->onNewExternalState(Interface::FORWARD, external, updated);
}));
if (accepted & READS_END)
if (expected & READS_END)
ends().reset(new Interface([this](Interface::iterator external, bool updated) {
this->onNewExternalState(Interface::BACKWARD, external, updated);
}));
required_interface_ = accepted;
required_interface_ = expected;
}
void ParallelContainerBasePrivate::validateInterfaces(const StagePrivate& child, InterfaceFlags& external,
@ -746,8 +746,8 @@ void Fallbacks::onNewSolution(const SolutionBase& s) {
MergerPrivate::MergerPrivate(Merger* me, const std::string& name) : ParallelContainerBasePrivate(me, name) {}
void MergerPrivate::pruneInterface(InterfaceFlags accepted) {
ContainerBasePrivate::pruneInterface(accepted);
void MergerPrivate::resolveInterface(InterfaceFlags expected) {
ContainerBasePrivate::resolveInterface(expected);
switch (interfaceFlags()) {
case PROPAGATE_FORWARDS:
case PROPAGATE_BACKWARDS:

View File

@ -117,7 +117,7 @@ void StagePrivate::validateConnectivity() const {
InterfaceFlags required = requiredInterface();
InterfaceFlags actual = interfaceFlags();
if ((required & actual) != required) {
boost::format desc("interface %1% %2% does not satisfy required interface %3% %4%");
boost::format desc("actual interface %1% %2% does not match required interface %3% %4%");
desc % flowSymbol<START_IF_MASK>(actual) % flowSymbol<END_IF_MASK>(actual);
desc % flowSymbol<START_IF_MASK>(required) % flowSymbol<END_IF_MASK>(required);
throw InitStageException(*me(), desc.str());
@ -420,24 +420,24 @@ void PropagatingEitherWayPrivate::initInterface(PropagatingEitherWay::Direction
}
}
void PropagatingEitherWayPrivate::pruneInterface(InterfaceFlags accepted) {
if (accepted == UNKNOWN)
void PropagatingEitherWayPrivate::resolveInterface(InterfaceFlags expected) {
if (expected == UNKNOWN)
throw InitStageException(*me(), "cannot initialize to unknown interface");
auto dir = PropagatingEitherWay::AUTO;
if ((accepted & START_IF_MASK) == READS_START || (accepted & END_IF_MASK) == WRITES_NEXT_START)
if ((expected & START_IF_MASK) == READS_START || (expected & END_IF_MASK) == WRITES_NEXT_START)
dir = PropagatingEitherWay::FORWARD;
else if ((accepted & START_IF_MASK) == WRITES_PREV_END || (accepted & END_IF_MASK) == READS_END)
else if ((expected & START_IF_MASK) == WRITES_PREV_END || (expected & END_IF_MASK) == READS_END)
dir = PropagatingEitherWay::BACKWARD;
else {
boost::format desc("propagator cannot handle external interface %1% %2%");
desc % flowSymbol<START_IF_MASK>(accepted) % flowSymbol<END_IF_MASK>(accepted);
boost::format desc("propagator cannot satisfy expected interface %1% %2%");
desc % flowSymbol<START_IF_MASK>(expected) % flowSymbol<END_IF_MASK>(expected);
throw InitStageException(*me(), desc.str());
}
if (configured_dir_ != PropagatingEitherWay::AUTO && dir != configured_dir_) {
boost::format desc("configured interface (%1% %2%) does not match external one (%3% %4%)");
boost::format desc("configured interface (%1% %2%) does not match expected one (%3% %4%)");
desc % flowSymbol<START_IF_MASK>(required_interface_) % flowSymbol<END_IF_MASK>(required_interface_);
desc % flowSymbol<START_IF_MASK>(accepted) % flowSymbol<END_IF_MASK>(accepted);
desc % flowSymbol<START_IF_MASK>(expected) % flowSymbol<END_IF_MASK>(expected);
throw InitStageException(*me(), desc.str());
}
initInterface(dir);

View File

@ -261,7 +261,7 @@ void Task::init() {
// and *afterwards* initialize all children recursively
stages()->init(impl->robot_model_);
// task expects its wrapped child to push to both ends, this triggers interface resolution
stages()->pimpl()->pruneInterface(InterfaceFlags({ GENERATE }));
stages()->pimpl()->resolveInterface(InterfaceFlags({ GENERATE }));
// provide introspection instance to all stages
impl->setIntrospection(impl->introspection_.get());

View File

@ -200,19 +200,19 @@ protected:
append(container, types);
try {
container.init(robot_model);
// prune interfaces based on provided external interface (start, end)
InterfaceFlags accepted;
// resolve interfaces based on provided external interface (start, end)
InterfaceFlags expected;
if (start)
accepted |= WRITES_PREV_END;
expected |= WRITES_PREV_END;
else
accepted |= READS_START;
expected |= READS_START;
if (end)
accepted |= WRITES_NEXT_START;
expected |= WRITES_NEXT_START;
else
accepted |= READS_END;
expected |= READS_END;
container.pimpl()->pruneInterface(accepted);
container.pimpl()->resolveInterface(expected);
container.pimpl()->validateConnectivity();
if (!expect_failure)
return; // as expected
@ -456,9 +456,9 @@ TEST_F(SerialTest, interface_detection) {
EXPECT_EQ(container.pimpl()->interfaceFlags(), InterfaceFlags(CONNECT));
// derive propagation direction from outer interface
EXPECT_INIT_SUCCESS(false, true, ANY); // should be pruned to FW
EXPECT_INIT_SUCCESS(false, true, ANY); // should be resolved to FW
EXPECT_EQ(container.pimpl()->interfaceFlags(), InterfaceFlags(PROPAGATE_FORWARDS));
EXPECT_INIT_SUCCESS(true, false, ANY); // should be pruned to BW
EXPECT_INIT_SUCCESS(true, false, ANY); // should be resolved to BW
EXPECT_EQ(container.pimpl()->interfaceFlags(), InterfaceFlags(PROPAGATE_BACKWARDS));
EXPECT_INIT_SUCCESS(false, true, ANY, ANY); // -> ->

46
demo/scripts/grasp.py Normal file
View File

@ -0,0 +1,46 @@
#! /usr/bin/env python
# -*- coding: utf-8 -*-
from std_msgs.msg import Header
from geometry_msgs.msg import TwistStamped, Twist, Vector3Stamped, Vector3, PoseStamped, Pose, Point, Quaternion
from moveit.task_constructor import core, stages
import moveit_commander
import rospy
import numpy
from moveit.python_tools import roscpp_init
roscpp_init("mtc_tutorial")
rospy.init_node('mtc_tutorial_py', anonymous=False)
def create_object():
scene = moveit_commander.PlanningSceneInterface(synchronous=True)
scene.remove_world_object();
pose = PoseStamped()
pose.header.frame_id = "world"
pose.pose.position.x = 0.5
pose.pose.position.y = numpy.random.uniform(-0.1, 0.1)
pose.pose.position.z = 0.16
pose.pose.orientation.w = 1.0
scene.add_box('object', pose, size=(0.05, 0.05, 0.2))
group = "panda_arm"
eef_frame = "panda_link8"
# Cartesian interpolation planner
cartesian = core.CartesianPath()
task = core.Task()
# start from current robot state
task.add(stages.CurrentState("current state"))
move = stages.MoveTo("to object", cartesian)
move.group = group
header = Header(frame_id = "object")
move.setGoal(PoseStamped(header=header, pose=Pose(position=Point(0,0,0.18),orientation=Quaternion(0.92388, -0.38268, 0, 0))))
task.add(move)
create_object()
if task.plan():
task.publish(task.solutions[0])
rospy.spin()

65
demo/scripts/pick.py Normal file
View File

@ -0,0 +1,65 @@
#! /usr/bin/env python
# -*- coding: utf-8 -*-
from std_msgs.msg import Header
from geometry_msgs.msg import TwistStamped, Twist, Vector3Stamped, Vector3, PoseStamped, Pose, Point, Quaternion
from moveit.task_constructor import core, stages
import moveit_commander
import rospy
import numpy
from moveit.python_tools import roscpp_init
roscpp_init("mtc_tutorial")
rospy.init_node('mtc_tutorial_py', anonymous=False)
group = 'panda_arm'
eef = 'panda_hand'
eef_frame = "panda_link8"
sampling_planner = core.JointInterpolationPlanner()
cartesian_planner = core.CartesianPath()
task = core.Task()
task.properties.update({'group': group, 'eef': eef, 'hand': eef, 'hand_grasping_frame': eef, 'ik_frame': eef_frame})
currstate = stages.CurrentState('current state')
#task.add(currstate) # Adding it to the task results in error for argument types in setMonitoredStage in GenerateGraspPose
open_hand = stages.MoveTo("open hand", sampling_planner)
open_hand.group = eef
open_hand.setGoal('open')
task.add(open_hand)
connect = stages.Connect('move to pick', [(group, sampling_planner)])
connect.timeout = 5
connect.properties.configureInitFrom(core.PARENT)
task.add(connect)
grasp = core.SerialContainer('pick object')
task.properties.exposeTo(grasp.properties, ['eef', 'hand', 'group', 'ik_frame'])
grasp.properties.configureInitFrom(core.PARENT, ['eef', 'hand', 'group', 'ik_frame'])
approach_object = stages.MoveRelative("approach_object", cartesian_planner)
approach_object.properties.update({'marker_ns': 'approach_object', 'link': eef_frame})
approach_object.properties.configureInitFrom(core.PARENT, ['group'])
approach_object.min_distance = 0.01
approach_object.max_distance = 0.1
print(approach_object.properties.__getitem__('group')) # Why is this None? how to get properties from within SerialContainer?
approach_object.setDirection(TwistStamped(header=Header(frame_id = eef_frame), twist=Twist(linear=Vector3(0,0,0.1))))
grasp.insert(approach_object)
generatepose = stages.GenerateGraspPose('generate grasp pose')
generatepose.properties.configureInitFrom(core.PARENT)
generatepose.properties.update({'marker_ns': 'grasp_pose'})
generatepose.pregrasp = 'open'
generatepose.object = 'base'
generatepose.angle_delta = numpy.pi/12
generatepose.setMonitoredStage(currstate)
# To be continued
task.add(grasp)
if task.plan():
task.publish(task.solutions[0])
rospy.spin()

53
notes.org Normal file
View File

@ -0,0 +1,53 @@
* scheduling
There are 2 levels of scheduling:
- choosing which state or state pair should be scheduled for execution
- choosing which stage should be scheduled for execution
Main goal is to find small-cost solutions fast.
To this end, we need to find a solution at all, i.e. connect start to end.
Second, we prefer small-cost solutions.
First sort by length of trajectory, second by sum of trajectory costs.
** approach
- scheduling priority for stage (default / assigned by user)
- fallback: sort stages by priority
- evaluate success rate
- expected costs of solutions
- expected computing time
- Container::schedule() -> return ordered list of stages to execute
* containers / interfaces
Basic stages have unique solutions connecting start-end.
Containers allow for several solutions connecting start-end:
- serial: several pathes might exist
- parallel: inherent
* notation
push vs pull connections
* TODO
** modify-ps
- std::function API
- replace templates with sfniae
** eef collision
- requires modification in MoveIt
- disable warnings vs. only consider relevant links (separate function?)
** MoveTo / MoveRelative
- interface: pass goal constraints
- fill goal constraints during setup?
requires transforms to be specified relative to current robot pose
** properties
- fix plan_pick_pa10: forward grasp property
- global type registry for serialization/deserialization functions
- use bits as enums to allow for configureInitFrom(PARENT | INTERFACE)
** incremental GraspPoseGenerator
in pending_ list, store the current angular value
** TaskModels
- move scene_, display_context_ to BaseTaskModel
- use registered property creators for RemoteTaskModel too
- deserialize property
- call creator function

28
pick+place.org Normal file
View File

@ -0,0 +1,28 @@
* capability params
- overall planning timeout
- max number of solutions
** Pick
- possible_grasps -> fixed list
- instantiate GraspProvider+Task on each call (eventually cache)
- object
- key-value parameter map (passed on to Action Server)
* GraspProvider params
- eef (hand_group, arm_group, parent link)
- hand_group: default from arm_group
- arm_group: default from hand_group
- grasp frame (PoseStamped rel. zu Link fest zu Tip-Link im Arm)
- key-value parameter map (passed on to Action Server)
- grasp config
- action server name (with default)
* PlaceProvider params
- PlaceLocation[]
- surface_name: sample PlaceLocations, disable collision with table during IK
* GraspMsg
- grasp frame?
- no-collision links (during closing)
- no-collision

16
test.py Normal file
View File

@ -0,0 +1,16 @@
import moveit.task_constructor as mtc
o=mtc.MyObject(42)
a=o
#print o
#print a
mtc.access(o)
mtc.access(o)
mtc.consume(o)
mtc.access(o)
mtc.access(o)
#print o
#print a
del o
del a
print "done"