merge trajectories

This commit is contained in:
Robert Haschke 2018-04-05 19:56:36 +02:00
parent 03092e7e45
commit 55e3aae30a
2 changed files with 80 additions and 13 deletions

View File

@ -70,8 +70,15 @@ public:
void processSolutions(const SolutionProcessor &processor) const override; void processSolutions(const SolutionProcessor &processor) const override;
void processFailures(const SolutionProcessor &processor) const override { return; } void processFailures(const SolutionProcessor &processor) const override { return; }
protected:
SolutionBase* storeSequential(const std::vector<robot_trajectory::RobotTrajectoryPtr>& sub_trajectories,
const std::vector<planning_scene::PlanningScenePtr>& intermediate_scenes);
robot_trajectory::RobotTrajectoryPtr merge(const std::vector<robot_trajectory::RobotTrajectoryPtr>& sub_trajectories,
const std::vector<planning_scene::PlanningScenePtr>& intermediate_scenes);
protected: protected:
GroupPlannerVector planner_; GroupPlannerVector planner_;
moveit::core::JointModelGroupPtr merged_jmg_;
std::list<SubTrajectory> subsolutions_; std::list<SubTrajectory> subsolutions_;
std::list<SolutionSequence> solutions_; std::list<SolutionSequence> solutions_;
std::list<InterfaceState> states_; std::list<InterfaceState> states_;

View File

@ -37,6 +37,7 @@
*/ */
#include <moveit/task_constructor/stages/connect.h> #include <moveit/task_constructor/stages/connect.h>
#include <moveit/task_constructor/merge.h>
#include <moveit/planning_scene/planning_scene.h> #include <moveit/planning_scene/planning_scene.h>
namespace moveit { namespace task_constructor { namespace stages { namespace moveit { namespace task_constructor { namespace stages {
@ -55,6 +56,7 @@ Connect::Connect(std::string name, const GroupPlannerVector& planners)
void Connect::reset() void Connect::reset()
{ {
Connecting::reset(); Connecting::reset();
merged_jmg_.reset();
solutions_.clear(); solutions_.clear();
subsolutions_.clear(); subsolutions_.clear();
states_.clear(); states_.clear();
@ -68,13 +70,25 @@ void Connect::init(const core::RobotModelConstPtr& robot_model)
if (planner_.empty()) if (planner_.empty())
errors.push_back(*this, "empty set of groups"); errors.push_back(*this, "empty set of groups");
size_t num_joints = 0;
std::vector<const moveit::core::JointModelGroup*> groups;
for (const GroupPlannerVector::value_type& pair : planner_) { for (const GroupPlannerVector::value_type& pair : planner_) {
if (!robot_model->hasJointModelGroup(pair.first)) if (!robot_model->hasJointModelGroup(pair.first))
errors.push_back(*this, "invalid group: " + pair.first); errors.push_back(*this, "invalid group: " + pair.first);
else if (!pair.second) else if (!pair.second)
errors.push_back(*this, "invalid planner for group: " + pair.first); errors.push_back(*this, "invalid planner for group: " + pair.first);
else else {
pair.second->init(robot_model); pair.second->init(robot_model);
auto jmg = robot_model->getJointModelGroup(pair.first);
groups.push_back(jmg);
num_joints += jmg->getJointModels().size();
}
}
if (!errors && groups.size() >= 2) {
merged_jmg_.reset(task_constructor::merge(groups));
if (merged_jmg_->getJointModels().size() != num_joints)
errors.push_back(*this, "overlapping joint groups");
} }
if (errors) if (errors)
@ -119,46 +133,92 @@ bool Connect::compute(const InterfaceState &from, const InterfaceState &to) {
double timeout = props.get<double>("timeout"); double timeout = props.get<double>("timeout");
const auto& path_constraints = props.get<moveit_msgs::Constraints>("path_constraints"); const auto& path_constraints = props.get<moveit_msgs::Constraints>("path_constraints");
SolutionSequence::container_type subsolutions; std::vector<robot_trajectory::RobotTrajectoryPtr> sub_trajectories;
planning_scene::PlanningScenePtr start = from.scene()->diff(); planning_scene::PlanningScenePtr start = from.scene()->diff();
const moveit::core::RobotState& goal_state = to.scene()->getCurrentState(); const moveit::core::RobotState& goal_state = to.scene()->getCurrentState();
std::vector<planning_scene::PlanningScenePtr> intermediate_scenes;
intermediate_scenes.push_back(start);
std::vector<double> positions;
for (const GroupPlannerVector::value_type& pair : planner_) { for (const GroupPlannerVector::value_type& pair : planner_) {
// set intermediate goal state // set intermediate goal state
planning_scene::PlanningScenePtr end = start->diff(); planning_scene::PlanningScenePtr end = start->diff();
intermediate_scenes.push_back(end);
const moveit::core::JointModelGroup *jmg = goal_state.getJointModelGroup(pair.first); const moveit::core::JointModelGroup *jmg = goal_state.getJointModelGroup(pair.first);
std::vector<double> positions;
goal_state.copyJointGroupPositions(jmg, positions); goal_state.copyJointGroupPositions(jmg, positions);
end->getCurrentStateNonConst().setJointGroupPositions(jmg, positions); end->getCurrentStateNonConst().setJointGroupPositions(jmg, positions);
robot_trajectory::RobotTrajectoryPtr trajectory; robot_trajectory::RobotTrajectoryPtr trajectory;
bool success = pair.second->plan(start, to.scene(), jmg, timeout, trajectory, path_constraints); if (!pair.second->plan(start, to.scene(), jmg, timeout, trajectory, path_constraints))
break;
// store solution sub_trajectories.push_back(trajectory);
auto inserted = subsolutions_.insert(subsolutions_.end(), SubTrajectory(trajectory)); // continue from reached state
start = end;
}
SolutionBase* solution = nullptr;
if (sub_trajectories.size() != planner_.size()) { // error during sequential planning
if (!storeFailures())
return false;
sub_trajectories.push_back(robot_trajectory::RobotTrajectoryPtr());
solution = storeSequential(sub_trajectories, intermediate_scenes);
// mark solution as failure
solution->setCost(std::numeric_limits<double>::infinity());
} else {
robot_trajectory::RobotTrajectoryPtr t = merge(sub_trajectories, intermediate_scenes);
if (t) {
connect(from, to, SubTrajectory(t));
return true;
}
// merging failed, store sequentially
solution = storeSequential(sub_trajectories, intermediate_scenes);
}
newSolution(from, to, *solution);
return !solution->isFailure();
}
SolutionBase* Connect::storeSequential(const std::vector<robot_trajectory::RobotTrajectoryPtr>& sub_trajectories,
const std::vector<planning_scene::PlanningScenePtr>& intermediate_scenes)
{
assert(sub_trajectories.size() + 1 == intermediate_scenes.size());
auto scene_it = intermediate_scenes.begin();
planning_scene::PlanningScenePtr start = *scene_it;
SolutionSequence::container_type sub_solutions;
for (const auto &sub : sub_trajectories) {
planning_scene::PlanningScenePtr end = *++scene_it;
auto inserted = subsolutions_.insert(subsolutions_.end(), SubTrajectory(sub));
inserted->setCreator(pimpl_); inserted->setCreator(pimpl_);
// push back solution pointer // push back solution pointer
subsolutions.push_back(&*inserted); sub_solutions.push_back(&*inserted);
// provide meaningful start/end states // provide meaningful start/end states
states_.emplace_back(InterfaceState(start)); states_.emplace_back(InterfaceState(start));
subsolutions_.back().setStartState(states_.back()); subsolutions_.back().setStartState(states_.back());
states_.emplace_back(InterfaceState(end)); states_.emplace_back(InterfaceState(end));
subsolutions_.back().setEndState(states_.back()); subsolutions_.back().setEndState(states_.back());
if (!success) return false;
// continue from reached state
start = end; start = end;
} }
solutions_.emplace_back(SolutionSequence(std::move(subsolutions), 0.0)); solutions_.emplace_back(SolutionSequence(std::move(sub_solutions)));
newSolution(from, to, solutions_.back()); return &solutions_.back();
return true; }
robot_trajectory::RobotTrajectoryPtr Connect::merge(const std::vector<robot_trajectory::RobotTrajectoryPtr>& sub_trajectories,
const std::vector<planning_scene::PlanningScenePtr>& intermediate_scenes)
{
auto jmg = merged_jmg_.get();
robot_trajectory::RobotTrajectoryPtr trajectory = task_constructor::merge(sub_trajectories, jmg);
return trajectory;
} }
void Connect::processSolutions(const Stage::SolutionProcessor& processor) const void Connect::processSolutions(const Stage::SolutionProcessor& processor) const
{ {
} }
} } } } } }