fix trajectory merging: initialize from well-defined RobotState

This commit is contained in:
Robert Haschke 2018-04-10 01:44:56 +02:00
parent c553275276
commit b22217deab
4 changed files with 19 additions and 10 deletions

View File

@ -64,6 +64,6 @@ bool findDuplicates(const std::vector<const moveit::core::JointModelGroup*>& gro
* For now, only the trajectory path is considered. Timings, velocities, etc. are ignored.
*/
robot_trajectory::RobotTrajectoryPtr merge(const std::vector<robot_trajectory::RobotTrajectoryConstPtr>& sub_trajectories,
moveit::core::JointModelGroup*& merged_group);
const moveit::core::RobotState& base_state, moveit::core::JointModelGroup*& merged_group);
} }

View File

@ -43,6 +43,10 @@
#include <moveit_msgs/Constraints.h>
namespace moveit { namespace core {
MOVEIT_CLASS_FORWARD(RobotState)
} }
namespace moveit { namespace task_constructor { namespace stages {
class Connect : public Connecting {
@ -73,7 +77,8 @@ protected:
SolutionBase* storeSequential(const std::vector<robot_trajectory::RobotTrajectoryConstPtr>& sub_trajectories,
const std::vector<planning_scene::PlanningScenePtr>& intermediate_scenes);
robot_trajectory::RobotTrajectoryPtr merge(const std::vector<robot_trajectory::RobotTrajectoryConstPtr>& sub_trajectories,
const std::vector<planning_scene::PlanningScenePtr>& intermediate_scenes);
const std::vector<planning_scene::PlanningScenePtr>& intermediate_scenes,
const moveit::core::RobotState& state);
protected:
GroupPlannerVector planner_;

View File

@ -89,7 +89,7 @@ bool findDuplicates(const std::vector<const moveit::core::JointModelGroup*>& gro
}
robot_trajectory::RobotTrajectoryPtr merge(const std::vector<robot_trajectory::RobotTrajectoryConstPtr>& sub_trajectories,
moveit::core::JointModelGroup*& merged_group)
const robot_state::RobotState& base_state, moveit::core::JointModelGroup*& merged_group)
{
if (sub_trajectories.size() <= 1)
throw std::runtime_error("Expected multiple sub solutions");
@ -101,7 +101,7 @@ robot_trajectory::RobotTrajectoryPtr merge(const std::vector<robot_trajectory::R
groups.reserve(sub_trajectories.size());
// sanity checks: all sub solutions must share the same robot model and use disjoint joint sets
const moveit::core::RobotModelConstPtr& robot_model = sub_trajectories[0]->getRobotModel();
const moveit::core::RobotModelConstPtr& robot_model = base_state.getRobotModel();
size_t max_num_joints = 0; // maximum number of joints in sub groups
size_t num_joints = 0; // sum of joints in all sub groups
@ -148,11 +148,11 @@ robot_trajectory::RobotTrajectoryPtr merge(const std::vector<robot_trajectory::R
std::vector<double> values;
values.reserve(max_num_joints);
auto merged_state = std::make_shared<robot_state::RobotState>(base_state);
while (true) {
bool finished = true;
size_t index = merged_traj->getWayPointCount();
auto merged_state = index == 0 ? std::make_shared<robot_state::RobotState>(robot_model)
: std::make_shared<robot_state::RobotState>(merged_traj->getLastWayPoint());
for (const robot_trajectory::RobotTrajectoryConstPtr& sub : sub_trajectories) {
if (index >= sub->getWayPointCount())
continue; // no more waypoints in this sub solution
@ -161,13 +161,15 @@ robot_trajectory::RobotTrajectoryPtr merge(const std::vector<robot_trajectory::R
const robot_state::RobotState& sub_state = sub->getWayPoint(index);
sub_state.copyJointGroupPositions(sub->getGroup(), values);
merged_state->setJointGroupPositions(sub->getGroup(), values);
merged_state->update();
}
if (finished)
break;
// add waypoint without timing
merged_state->update();
merged_traj->addSuffixWayPoint(merged_state, 0.0);
// create new RobotState for next waypoint
merged_state = std::make_shared<robot_state::RobotState>(*merged_state);
}
return merged_traj;
}

View File

@ -176,7 +176,7 @@ bool Connect::compute(const InterfaceState &from, const InterfaceState &to) {
// mark solution as failure
solution->setCost(std::numeric_limits<double>::infinity());
} else {
robot_trajectory::RobotTrajectoryPtr t = merge(sub_trajectories, intermediate_scenes);
robot_trajectory::RobotTrajectoryPtr t = merge(sub_trajectories, intermediate_scenes, from.scene()->getCurrentState());
if (t) {
connect(from, to, SubTrajectory(t));
return true;
@ -219,10 +219,12 @@ SolutionBase* Connect::storeSequential(const std::vector<robot_trajectory::Robot
}
robot_trajectory::RobotTrajectoryPtr Connect::merge(const std::vector<robot_trajectory::RobotTrajectoryConstPtr>& sub_trajectories,
const std::vector<planning_scene::PlanningScenePtr>& intermediate_scenes)
const std::vector<planning_scene::PlanningScenePtr>& intermediate_scenes,
const moveit::core::RobotState& state)
{
auto jmg = merged_jmg_.get();
robot_trajectory::RobotTrajectoryPtr trajectory = task_constructor::merge(sub_trajectories, jmg);
robot_trajectory::RobotTrajectoryPtr trajectory = task_constructor::merge(sub_trajectories, state, jmg);
// TODO: check merged trajectory for collisions
return trajectory;
}