mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-09-27 00:29:13 +08:00
cleanup / renaming
* Rename pruneInterface() -> resolveInterface() * Rename accepted (interface) -> expected * Improve exception strings
This commit is contained in:
parent
aa732d8c66
commit
499fcfb04b
@ -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);
|
||||
|
@ -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;
|
||||
|
@ -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:
|
||||
|
@ -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);
|
||||
|
@ -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());
|
||||
|
@ -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
46
demo/scripts/grasp.py
Normal 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
65
demo/scripts/pick.py
Normal 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
53
notes.org
Normal 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
28
pick+place.org
Normal 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
|
||||
|
Loading…
Reference in New Issue
Block a user