mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
Connect: allow multiple groups to be processed in series
This commit is contained in:
parent
8811045f89
commit
77442c0eed
@ -73,16 +73,10 @@ int main(int argc, char** argv){
|
||||
stage->setMaxPenetration(0.1);
|
||||
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);
|
||||
t.add(std::move(move));
|
||||
}
|
||||
|
||||
@ -351,6 +351,8 @@ public:
|
||||
trajectory.setCost(cost);
|
||||
connect(from, to, std::move(trajectory));
|
||||
}
|
||||
|
||||
void newSolution(const InterfaceState& from, const InterfaceState& to, SolutionBase& solution);
|
||||
};
|
||||
|
||||
} }
|
||||
|
||||
@ -32,7 +32,7 @@
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*********************************************************************/
|
||||
|
||||
/* Authors: Michael Goerner, Robert Haschke
|
||||
/* Authors: Robert Haschke, Michael Goerner
|
||||
Desc: Connect arbitrary states by motion planning
|
||||
*/
|
||||
|
||||
@ -47,7 +47,8 @@ namespace moveit { namespace task_constructor { namespace stages {
|
||||
|
||||
class Connect : public Connecting {
|
||||
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){
|
||||
setProperty("timeout", timeout.toSec());
|
||||
@ -57,11 +58,20 @@ public:
|
||||
setProperty("path_constraints", std::move(path_constraints));
|
||||
}
|
||||
|
||||
void init(const moveit::core::RobotModelConstPtr& robot_model);
|
||||
bool compute(const InterfaceState &from, const InterfaceState &to);
|
||||
void reset() override;
|
||||
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:
|
||||
solvers::PlannerInterfacePtr planner_;
|
||||
GroupPlannerVector planner_;
|
||||
std::list<SubTrajectory> subsolutions_;
|
||||
std::list<SolutionSequence> solutions_;
|
||||
std::list<InterfaceState> states_;
|
||||
};
|
||||
|
||||
} } }
|
||||
|
||||
@ -655,11 +655,13 @@ void Connecting::reset()
|
||||
}
|
||||
|
||||
void Connecting::connect(const InterfaceState& from, const InterfaceState& to, SubTrajectory&& t) {
|
||||
auto impl = pimpl();
|
||||
SubTrajectory& trajectory = impl->addTrajectory(std::move(t));
|
||||
trajectory.setStartState(from);
|
||||
trajectory.setEndState(to);
|
||||
impl->newSolution(trajectory);
|
||||
newSolution(from, to, pimpl()->addTrajectory(std::move(t)));
|
||||
}
|
||||
|
||||
void Connecting::newSolution(const InterfaceState& from, const InterfaceState& to, SolutionBase& solution) {
|
||||
solution.setStartState(from);
|
||||
solution.setEndState(to);
|
||||
pimpl()->newSolution(solution);
|
||||
}
|
||||
|
||||
std::ostream& operator<<(std::ostream& os, const Stage& stage) {
|
||||
|
||||
@ -41,9 +41,9 @@
|
||||
|
||||
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)
|
||||
, planner_(planner)
|
||||
, planner_(planners)
|
||||
{
|
||||
auto& p = properties();
|
||||
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");
|
||||
}
|
||||
|
||||
void Connect::reset()
|
||||
{
|
||||
Connecting::reset();
|
||||
solutions_.clear();
|
||||
subsolutions_.clear();
|
||||
states_.clear();
|
||||
}
|
||||
|
||||
void Connect::init(const core::RobotModelConstPtr& 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) {
|
||||
const auto& props = properties();
|
||||
const std::string& group = props.get<std::string>("group");
|
||||
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;
|
||||
if (!planner_->plan(from.scene(), to.scene(), jmg, timeout, trajectory,
|
||||
props.get<moveit_msgs::Constraints>("path_constraints")))
|
||||
return false;
|
||||
SolutionSequence::container_type subsolutions;
|
||||
planning_scene::PlanningScenePtr start = from.scene()->diff();
|
||||
const moveit::core::RobotState& goal_state = to.scene()->getCurrentState();
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
void Connect::processSolutions(const Stage::SolutionProcessor& processor) const
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
} } }
|
||||
|
||||
Loading…
Reference in New Issue
Block a user