mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
fix trajectory merging: initialize from well-defined RobotState
This commit is contained in:
parent
c553275276
commit
b22217deab
@ -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.
|
* 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,
|
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);
|
||||||
|
|
||||||
} }
|
} }
|
||||||
|
|||||||
@ -43,6 +43,10 @@
|
|||||||
|
|
||||||
#include <moveit_msgs/Constraints.h>
|
#include <moveit_msgs/Constraints.h>
|
||||||
|
|
||||||
|
namespace moveit { namespace core {
|
||||||
|
MOVEIT_CLASS_FORWARD(RobotState)
|
||||||
|
} }
|
||||||
|
|
||||||
namespace moveit { namespace task_constructor { namespace stages {
|
namespace moveit { namespace task_constructor { namespace stages {
|
||||||
|
|
||||||
class Connect : public Connecting {
|
class Connect : public Connecting {
|
||||||
@ -73,7 +77,8 @@ protected:
|
|||||||
SolutionBase* storeSequential(const std::vector<robot_trajectory::RobotTrajectoryConstPtr>& sub_trajectories,
|
SolutionBase* storeSequential(const std::vector<robot_trajectory::RobotTrajectoryConstPtr>& sub_trajectories,
|
||||||
const std::vector<planning_scene::PlanningScenePtr>& intermediate_scenes);
|
const std::vector<planning_scene::PlanningScenePtr>& intermediate_scenes);
|
||||||
robot_trajectory::RobotTrajectoryPtr merge(const std::vector<robot_trajectory::RobotTrajectoryConstPtr>& sub_trajectories,
|
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:
|
protected:
|
||||||
GroupPlannerVector planner_;
|
GroupPlannerVector planner_;
|
||||||
|
|||||||
@ -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,
|
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)
|
if (sub_trajectories.size() <= 1)
|
||||||
throw std::runtime_error("Expected multiple sub solutions");
|
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());
|
groups.reserve(sub_trajectories.size());
|
||||||
|
|
||||||
// sanity checks: all sub solutions must share the same robot model and use disjoint joint sets
|
// 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 max_num_joints = 0; // maximum number of joints in sub groups
|
||||||
size_t num_joints = 0; // sum of joints in all 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;
|
std::vector<double> values;
|
||||||
values.reserve(max_num_joints);
|
values.reserve(max_num_joints);
|
||||||
|
|
||||||
|
auto merged_state = std::make_shared<robot_state::RobotState>(base_state);
|
||||||
while (true) {
|
while (true) {
|
||||||
bool finished = true;
|
bool finished = true;
|
||||||
size_t index = merged_traj->getWayPointCount();
|
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) {
|
for (const robot_trajectory::RobotTrajectoryConstPtr& sub : sub_trajectories) {
|
||||||
if (index >= sub->getWayPointCount())
|
if (index >= sub->getWayPointCount())
|
||||||
continue; // no more waypoints in this sub solution
|
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);
|
const robot_state::RobotState& sub_state = sub->getWayPoint(index);
|
||||||
sub_state.copyJointGroupPositions(sub->getGroup(), values);
|
sub_state.copyJointGroupPositions(sub->getGroup(), values);
|
||||||
merged_state->setJointGroupPositions(sub->getGroup(), values);
|
merged_state->setJointGroupPositions(sub->getGroup(), values);
|
||||||
|
merged_state->update();
|
||||||
}
|
}
|
||||||
if (finished)
|
if (finished)
|
||||||
break;
|
break;
|
||||||
|
|
||||||
// add waypoint without timing
|
// add waypoint without timing
|
||||||
merged_state->update();
|
|
||||||
merged_traj->addSuffixWayPoint(merged_state, 0.0);
|
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;
|
return merged_traj;
|
||||||
}
|
}
|
||||||
|
|||||||
@ -176,7 +176,7 @@ bool Connect::compute(const InterfaceState &from, const InterfaceState &to) {
|
|||||||
// mark solution as failure
|
// mark solution as failure
|
||||||
solution->setCost(std::numeric_limits<double>::infinity());
|
solution->setCost(std::numeric_limits<double>::infinity());
|
||||||
} else {
|
} else {
|
||||||
robot_trajectory::RobotTrajectoryPtr t = merge(sub_trajectories, intermediate_scenes);
|
robot_trajectory::RobotTrajectoryPtr t = merge(sub_trajectories, intermediate_scenes, from.scene()->getCurrentState());
|
||||||
if (t) {
|
if (t) {
|
||||||
connect(from, to, SubTrajectory(t));
|
connect(from, to, SubTrajectory(t));
|
||||||
return true;
|
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,
|
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();
|
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;
|
return trajectory;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user