Connect: allow multiple groups to be processed in series

This commit is contained in:
Robert Haschke 2018-03-12 23:32:56 +01:00
parent 8811045f89
commit 77442c0eed
5 changed files with 91 additions and 28 deletions

View File

@ -73,16 +73,10 @@ int main(int argc, char** argv){
stage->setMaxPenetration(0.1); stage->setMaxPenetration(0.1);
t.add(std::move(stage)); t.add(std::move(stage));
} }
{
auto move = std::make_unique<stages::MoveTo>("open gripper", pipeline);
move->restrictDirection(stages::MoveTo::FORWARD);
move->properties().property("group").configureInitFrom(Stage::PARENT, "gripper");
move->setGoal("open");
t.add(std::move(move));
}
{ {
auto move = std::make_unique<stages::Connect>("move to object", pipeline); stages::Connect::GroupPlannerVector planners = {{"left_hand", pipeline}, {"left_arm", pipeline}};
auto move = std::make_unique<stages::Connect>("connect", planners);
move->properties().configureInitFrom(Stage::PARENT); move->properties().configureInitFrom(Stage::PARENT);
t.add(std::move(move)); t.add(std::move(move));
} }

View File

@ -351,6 +351,8 @@ public:
trajectory.setCost(cost); trajectory.setCost(cost);
connect(from, to, std::move(trajectory)); connect(from, to, std::move(trajectory));
} }
void newSolution(const InterfaceState& from, const InterfaceState& to, SolutionBase& solution);
}; };
} } } }

View File

@ -32,7 +32,7 @@
* POSSIBILITY OF SUCH DAMAGE. * POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/ *********************************************************************/
/* Authors: Michael Goerner, Robert Haschke /* Authors: Robert Haschke, Michael Goerner
Desc: Connect arbitrary states by motion planning Desc: Connect arbitrary states by motion planning
*/ */
@ -47,7 +47,8 @@ namespace moveit { namespace task_constructor { namespace stages {
class Connect : public Connecting { class Connect : public Connecting {
public: public:
Connect(std::string name, const solvers::PlannerInterfacePtr &planner); typedef std::vector<std::pair<std::string, solvers::PlannerInterfacePtr>> GroupPlannerVector;
Connect(std::string name, const GroupPlannerVector& planners);
void setTimeout(const ros::Duration& timeout){ void setTimeout(const ros::Duration& timeout){
setProperty("timeout", timeout.toSec()); setProperty("timeout", timeout.toSec());
@ -57,11 +58,20 @@ public:
setProperty("path_constraints", std::move(path_constraints)); setProperty("path_constraints", std::move(path_constraints));
} }
void init(const moveit::core::RobotModelConstPtr& robot_model); void reset() override;
bool compute(const InterfaceState &from, const InterfaceState &to); void init(const moveit::core::RobotModelConstPtr& robot_model) override;
bool compute(const InterfaceState &from, const InterfaceState &to) override;
size_t numSolutions() const override { return solutions_.size(); }
size_t numFailures() const override { return 0; }
void processSolutions(const SolutionProcessor &processor) const override;
void processFailures(const SolutionProcessor &processor) const override { return; }
protected: protected:
solvers::PlannerInterfacePtr planner_; GroupPlannerVector planner_;
std::list<SubTrajectory> subsolutions_;
std::list<SolutionSequence> solutions_;
std::list<InterfaceState> states_;
}; };
} } } } } }

View File

@ -655,11 +655,13 @@ void Connecting::reset()
} }
void Connecting::connect(const InterfaceState& from, const InterfaceState& to, SubTrajectory&& t) { void Connecting::connect(const InterfaceState& from, const InterfaceState& to, SubTrajectory&& t) {
auto impl = pimpl(); newSolution(from, to, pimpl()->addTrajectory(std::move(t)));
SubTrajectory& trajectory = impl->addTrajectory(std::move(t)); }
trajectory.setStartState(from);
trajectory.setEndState(to); void Connecting::newSolution(const InterfaceState& from, const InterfaceState& to, SolutionBase& solution) {
impl->newSolution(trajectory); solution.setStartState(from);
solution.setEndState(to);
pimpl()->newSolution(solution);
} }
std::ostream& operator<<(std::ostream& os, const Stage& stage) { std::ostream& operator<<(std::ostream& os, const Stage& stage) {

View File

@ -41,9 +41,9 @@
namespace moveit { namespace task_constructor { namespace stages { namespace moveit { namespace task_constructor { namespace stages {
Connect::Connect(std::string name, const solvers::PlannerInterfacePtr& planner) Connect::Connect(std::string name, const GroupPlannerVector& planners)
: Connecting(name) : Connecting(name)
, planner_(planner) , planner_(planners)
{ {
auto& p = properties(); auto& p = properties();
p.declare<double>("timeout", 10.0, "planning timeout"); p.declare<double>("timeout", 10.0, "planning timeout");
@ -52,25 +52,80 @@ Connect::Connect(std::string name, const solvers::PlannerInterfacePtr& planner)
"constraints to maintain during trajectory"); "constraints to maintain during trajectory");
} }
void Connect::reset()
{
Connecting::reset();
solutions_.clear();
subsolutions_.clear();
states_.clear();
}
void Connect::init(const core::RobotModelConstPtr& robot_model) void Connect::init(const core::RobotModelConstPtr& robot_model)
{ {
Connecting::init(robot_model); Connecting::init(robot_model);
planner_->init(robot_model);
InitStageException errors;
if (planner_.empty())
errors.push_back(*this, "empty set of groups");
for (const GroupPlannerVector::value_type& pair : planner_) {
if (!robot_model->hasJointModelGroup(pair.first))
errors.push_back(*this, "invalid group: " + pair.first);
else if (!pair.second)
errors.push_back(*this, "invalid planner for group: " + pair.first);
else
pair.second->init(robot_model);
}
if (errors)
throw errors;
} }
bool Connect::compute(const InterfaceState &from, const InterfaceState &to) { bool Connect::compute(const InterfaceState &from, const InterfaceState &to) {
const auto& props = properties(); const auto& props = properties();
const std::string& group = props.get<std::string>("group");
double timeout = props.get<double>("timeout"); double timeout = props.get<double>("timeout");
const moveit::core::JointModelGroup* jmg = from.scene()->getRobotModel()->getJointModelGroup(group); const auto& path_constraints = props.get<moveit_msgs::Constraints>("path_constraints");
robot_trajectory::RobotTrajectoryPtr trajectory; SolutionSequence::container_type subsolutions;
if (!planner_->plan(from.scene(), to.scene(), jmg, timeout, trajectory, planning_scene::PlanningScenePtr start = from.scene()->diff();
props.get<moveit_msgs::Constraints>("path_constraints"))) const moveit::core::RobotState& goal_state = to.scene()->getCurrentState();
return false;
connect(from, to, trajectory); for (const GroupPlannerVector::value_type& pair : planner_) {
// set intermediate goal state
planning_scene::PlanningScenePtr end = start->diff();
const moveit::core::JointModelGroup *jmg = goal_state.getJointModelGroup(pair.first);
std::vector<double> positions;
goal_state.copyJointGroupPositions(jmg, positions);
end->getCurrentStateNonConst().setJointGroupPositions(jmg, positions);
robot_trajectory::RobotTrajectoryPtr trajectory;
bool success = pair.second->plan(start, to.scene(), jmg, timeout, trajectory, path_constraints);
// store solution
auto inserted = subsolutions_.insert(subsolutions_.end(), SubTrajectory(trajectory));
inserted->setCreator(pimpl_);
// push back solution pointer
subsolutions.push_back(&*inserted);
// provide meaningful start/end states
states_.emplace_back(InterfaceState(start));
subsolutions_.back().setStartState(states_.back());
states_.emplace_back(InterfaceState(end));
subsolutions_.back().setEndState(states_.back());
if (!success) return false;
// continue from reached state
start = end;
}
solutions_.emplace_back(SolutionSequence(std::move(subsolutions), 0.0));
newSolution(from, to, solutions_.back());
return true; return true;
} }
void Connect::processSolutions(const Stage::SolutionProcessor& processor) const
{
}
} } } } } }