mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
Merge branches 'robot-model', 'task-move-constructor' and 'const-robot-trajectory'
This commit is contained in:
commit
556d9e1a5e
@ -63,7 +63,7 @@ bool findDuplicates(const std::vector<const moveit::core::JointModelGroup*>& gro
|
|||||||
* or created on the fly. This JMG needs to stay alive during the lifetime of the trajectory.
|
* or created on the fly. This JMG needs to stay alive during the lifetime of the trajectory.
|
||||||
* 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::RobotTrajectoryPtr>& sub_trajectories,
|
robot_trajectory::RobotTrajectoryPtr merge(const std::vector<robot_trajectory::RobotTrajectoryConstPtr>& sub_trajectories,
|
||||||
core::JointModelGroup*& merged_group);
|
moveit::core::JointModelGroup*& merged_group);
|
||||||
|
|
||||||
} }
|
} }
|
||||||
|
|||||||
@ -197,7 +197,7 @@ protected:
|
|||||||
Stage(const Stage&) = delete;
|
Stage(const Stage&) = delete;
|
||||||
|
|
||||||
protected:
|
protected:
|
||||||
StagePrivate* const pimpl_; // constness guarantees one initial write
|
StagePrivate* pimpl_;
|
||||||
};
|
};
|
||||||
std::ostream& operator<<(std::ostream& os, const Stage& stage);
|
std::ostream& operator<<(std::ostream& os, const Stage& stage);
|
||||||
|
|
||||||
@ -245,21 +245,6 @@ public:
|
|||||||
const InterfaceState& to,
|
const InterfaceState& to,
|
||||||
SubTrajectory&& trajectory);
|
SubTrajectory&& trajectory);
|
||||||
|
|
||||||
void sendForward(const InterfaceState& from,
|
|
||||||
InterfaceState&& to,
|
|
||||||
SubTrajectory&& trajectory,
|
|
||||||
double cost) {
|
|
||||||
trajectory.setCost(cost);
|
|
||||||
sendForward(from, std::move(to), std::move(trajectory));
|
|
||||||
}
|
|
||||||
void sendBackward(InterfaceState&& from,
|
|
||||||
const InterfaceState& to,
|
|
||||||
SubTrajectory&& trajectory,
|
|
||||||
double cost) {
|
|
||||||
trajectory.setCost(cost);
|
|
||||||
sendBackward(std::move(from), to, std::move(trajectory));
|
|
||||||
}
|
|
||||||
|
|
||||||
protected:
|
protected:
|
||||||
// constructor for use in derived classes
|
// constructor for use in derived classes
|
||||||
PropagatingEitherWay(PropagatingEitherWayPrivate* impl);
|
PropagatingEitherWay(PropagatingEitherWayPrivate* impl);
|
||||||
|
|||||||
@ -70,9 +70,9 @@ public:
|
|||||||
void processFailures(const SolutionProcessor &processor) const override;
|
void processFailures(const SolutionProcessor &processor) const override;
|
||||||
|
|
||||||
protected:
|
protected:
|
||||||
SolutionBase* storeSequential(const std::vector<robot_trajectory::RobotTrajectoryPtr>& 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::RobotTrajectoryPtr>& 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);
|
||||||
|
|
||||||
protected:
|
protected:
|
||||||
|
|||||||
@ -242,8 +242,9 @@ private:
|
|||||||
/// SubTrajectory connects interface states of ComputeStages
|
/// SubTrajectory connects interface states of ComputeStages
|
||||||
class SubTrajectory : public SolutionBase {
|
class SubTrajectory : public SolutionBase {
|
||||||
public:
|
public:
|
||||||
SubTrajectory(const robot_trajectory::RobotTrajectoryPtr& trajectory = robot_trajectory::RobotTrajectoryPtr())
|
SubTrajectory(const robot_trajectory::RobotTrajectoryConstPtr& trajectory = robot_trajectory::RobotTrajectoryConstPtr(),
|
||||||
: SolutionBase(), trajectory_(trajectory)
|
double cost = 0.0)
|
||||||
|
: SolutionBase(nullptr, cost), trajectory_(trajectory)
|
||||||
{}
|
{}
|
||||||
|
|
||||||
robot_trajectory::RobotTrajectoryConstPtr trajectory() const { return trajectory_; }
|
robot_trajectory::RobotTrajectoryConstPtr trajectory() const { return trajectory_; }
|
||||||
@ -254,7 +255,7 @@ public:
|
|||||||
|
|
||||||
private:
|
private:
|
||||||
// actual trajectory, might be empty
|
// actual trajectory, might be empty
|
||||||
robot_trajectory::RobotTrajectoryPtr trajectory_;
|
robot_trajectory::RobotTrajectoryConstPtr trajectory_;
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|
||||||
|
|||||||
@ -64,12 +64,14 @@ MOVEIT_CLASS_FORWARD(Task)
|
|||||||
*/
|
*/
|
||||||
class Task : protected WrapperBase {
|
class Task : protected WrapperBase {
|
||||||
public:
|
public:
|
||||||
Task(const std::string& id = "",
|
|
||||||
Stage::pointer &&container = std::make_unique<SerialContainer>("task pipeline"));
|
|
||||||
static planning_pipeline::PlanningPipelinePtr createPlanner(const moveit::core::RobotModelConstPtr &model,
|
static planning_pipeline::PlanningPipelinePtr createPlanner(const moveit::core::RobotModelConstPtr &model,
|
||||||
const std::string &ns = "move_group",
|
const std::string &ns = "move_group",
|
||||||
const std::string &planning_plugin_param_name = "planning_plugin",
|
const std::string &planning_plugin_param_name = "planning_plugin",
|
||||||
const std::string &adapter_plugins_param_name = "request_adapters");
|
const std::string &adapter_plugins_param_name = "request_adapters");
|
||||||
|
Task(const std::string& id = "",
|
||||||
|
Stage::pointer &&container = std::make_unique<SerialContainer>("task pipeline"));
|
||||||
|
Task(Task &&other);
|
||||||
|
Task& operator=(Task&& other);
|
||||||
~Task();
|
~Task();
|
||||||
|
|
||||||
std::string id() const;
|
std::string id() const;
|
||||||
|
|||||||
@ -88,7 +88,7 @@ bool findDuplicates(const std::vector<const moveit::core::JointModelGroup*>& gro
|
|||||||
return duplicates.size() > 0;
|
return duplicates.size() > 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
robot_trajectory::RobotTrajectoryPtr merge(const std::vector<robot_trajectory::RobotTrajectoryPtr>& sub_trajectories,
|
robot_trajectory::RobotTrajectoryPtr merge(const std::vector<robot_trajectory::RobotTrajectoryConstPtr>& sub_trajectories,
|
||||||
moveit::core::JointModelGroup*& merged_group)
|
moveit::core::JointModelGroup*& merged_group)
|
||||||
{
|
{
|
||||||
if (sub_trajectories.size() <= 1)
|
if (sub_trajectories.size() <= 1)
|
||||||
@ -105,7 +105,7 @@ robot_trajectory::RobotTrajectoryPtr merge(const std::vector<robot_trajectory::R
|
|||||||
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
|
||||||
|
|
||||||
for (const robot_trajectory::RobotTrajectoryPtr& sub : sub_trajectories) {
|
for (const robot_trajectory::RobotTrajectoryConstPtr& sub : sub_trajectories) {
|
||||||
if (sub->getRobotModel() != robot_model)
|
if (sub->getRobotModel() != robot_model)
|
||||||
throw std::runtime_error("subsolutions refer to multiple robot models");
|
throw std::runtime_error("subsolutions refer to multiple robot models");
|
||||||
|
|
||||||
@ -153,7 +153,7 @@ robot_trajectory::RobotTrajectoryPtr merge(const std::vector<robot_trajectory::R
|
|||||||
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)
|
auto merged_state = index == 0 ? std::make_shared<robot_state::RobotState>(robot_model)
|
||||||
: std::make_shared<robot_state::RobotState>(merged_traj->getLastWayPoint());
|
: std::make_shared<robot_state::RobotState>(merged_traj->getLastWayPoint());
|
||||||
for (const robot_trajectory::RobotTrajectoryPtr& 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
|
||||||
|
|
||||||
|
|||||||
@ -189,7 +189,7 @@ bool CartesianPositionMotion::computeForward(const InterfaceState &from){
|
|||||||
= std::make_shared<robot_trajectory::RobotTrajectory>(robot_state.getRobotModel(), jmg);
|
= std::make_shared<robot_trajectory::RobotTrajectory>(robot_state.getRobotModel(), jmg);
|
||||||
for( auto& tp : trajectory_steps )
|
for( auto& tp : trajectory_steps )
|
||||||
traj->addSuffixWayPoint(tp, 0.0);
|
traj->addSuffixWayPoint(tp, 0.0);
|
||||||
sendForward(from, InterfaceState(result_scene), traj);
|
sendForward(from, InterfaceState(result_scene), SubTrajectory(traj));
|
||||||
}
|
}
|
||||||
|
|
||||||
return succeeded;
|
return succeeded;
|
||||||
@ -256,7 +256,7 @@ bool CartesianPositionMotion::computeBackward(const InterfaceState &to){
|
|||||||
robot_trajectory::RobotTrajectoryPtr traj= std::make_shared<robot_trajectory::RobotTrajectory>(robot_state.getRobotModel(), jmg);
|
robot_trajectory::RobotTrajectoryPtr traj= std::make_shared<robot_trajectory::RobotTrajectory>(robot_state.getRobotModel(), jmg);
|
||||||
for( auto& tp : trajectory_steps )
|
for( auto& tp : trajectory_steps )
|
||||||
traj->addPrefixWayPoint(tp, 0.0);
|
traj->addPrefixWayPoint(tp, 0.0);
|
||||||
sendBackward(InterfaceState(result_scene), to, traj);
|
sendBackward(InterfaceState(result_scene), to, SubTrajectory(traj));
|
||||||
}
|
}
|
||||||
|
|
||||||
return succeeded;
|
return succeeded;
|
||||||
|
|||||||
@ -141,7 +141,7 @@ 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");
|
||||||
|
|
||||||
std::vector<robot_trajectory::RobotTrajectoryPtr> sub_trajectories;
|
std::vector<robot_trajectory::RobotTrajectoryConstPtr> 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();
|
||||||
|
|
||||||
@ -188,7 +188,7 @@ bool Connect::compute(const InterfaceState &from, const InterfaceState &to) {
|
|||||||
return !solution->isFailure();
|
return !solution->isFailure();
|
||||||
}
|
}
|
||||||
|
|
||||||
SolutionBase* Connect::storeSequential(const std::vector<robot_trajectory::RobotTrajectoryPtr>& sub_trajectories,
|
SolutionBase* Connect::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)
|
||||||
{
|
{
|
||||||
assert(sub_trajectories.size() + 1 == intermediate_scenes.size());
|
assert(sub_trajectories.size() + 1 == intermediate_scenes.size());
|
||||||
@ -217,7 +217,7 @@ SolutionBase* Connect::storeSequential(const std::vector<robot_trajectory::Robot
|
|||||||
return &solutions_.back();
|
return &solutions_.back();
|
||||||
}
|
}
|
||||||
|
|
||||||
robot_trajectory::RobotTrajectoryPtr Connect::merge(const std::vector<robot_trajectory::RobotTrajectoryPtr>& 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)
|
||||||
{
|
{
|
||||||
auto jmg = merged_jmg_.get();
|
auto jmg = merged_jmg_.get();
|
||||||
|
|||||||
@ -145,7 +145,7 @@ bool Gripper::computeForward(const InterfaceState &from){
|
|||||||
|
|
||||||
if (!compute(from, to, trajectory, cost, FORWARD))
|
if (!compute(from, to, trajectory, cost, FORWARD))
|
||||||
return false;
|
return false;
|
||||||
sendForward(from, InterfaceState(to), trajectory, cost);
|
sendForward(from, InterfaceState(to), SubTrajectory(trajectory, cost));
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -157,7 +157,7 @@ bool Gripper::computeBackward(const InterfaceState &to)
|
|||||||
|
|
||||||
if (!compute(to, from, trajectory, cost, BACKWARD))
|
if (!compute(to, from, trajectory, cost, BACKWARD))
|
||||||
return false;
|
return false;
|
||||||
sendBackward(InterfaceState(from), to, trajectory, cost);
|
sendBackward(InterfaceState(from), to, SubTrajectory(trajectory, cost));
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|||||||
@ -65,13 +65,13 @@ void ModifyPlanningScene::enableCollisions(const std::string &first, const movei
|
|||||||
}
|
}
|
||||||
|
|
||||||
bool ModifyPlanningScene::computeForward(const InterfaceState &from){
|
bool ModifyPlanningScene::computeForward(const InterfaceState &from){
|
||||||
sendForward(from, apply(from, false), robot_trajectory::RobotTrajectoryPtr());
|
sendForward(from, apply(from, false), SubTrajectory());
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
|
||||||
bool ModifyPlanningScene::computeBackward(const InterfaceState &to)
|
bool ModifyPlanningScene::computeBackward(const InterfaceState &to)
|
||||||
{
|
{
|
||||||
sendBackward(apply(to, true), to, robot_trajectory::RobotTrajectoryPtr());
|
sendBackward(apply(to, true), to, SubTrajectory());
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|||||||
@ -95,7 +95,7 @@ bool Move::compute(const InterfaceState &from, const InterfaceState &to) {
|
|||||||
return false;
|
return false;
|
||||||
|
|
||||||
// finish stage
|
// finish stage
|
||||||
connect(from, to, res.trajectory_);
|
connect(from, to, SubTrajectory(res.trajectory_));
|
||||||
|
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
|||||||
@ -60,6 +60,22 @@ Task::Task(const std::string& id, ContainerBase::pointer &&container)
|
|||||||
enableIntrospection(true);
|
enableIntrospection(true);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
Task::Task(Task&& other)
|
||||||
|
: WrapperBase(std::string(), std::make_unique<SerialContainer>())
|
||||||
|
{
|
||||||
|
*this = std::move(other);
|
||||||
|
}
|
||||||
|
|
||||||
|
Task& Task::operator=(Task&& other)
|
||||||
|
{
|
||||||
|
id_ = std::move(other.id_);
|
||||||
|
robot_model_ = std::move(other.robot_model_);
|
||||||
|
introspection_ = std::move(other.introspection_);
|
||||||
|
task_cbs_ = std::move(other.task_cbs_);
|
||||||
|
std::swap(pimpl_, other.pimpl_);
|
||||||
|
return *this;
|
||||||
|
}
|
||||||
|
|
||||||
planning_pipeline::PlanningPipelinePtr
|
planning_pipeline::PlanningPipelinePtr
|
||||||
Task::createPlanner(const robot_model::RobotModelConstPtr& model, const std::string& ns,
|
Task::createPlanner(const robot_model::RobotModelConstPtr& model, const std::string& ns,
|
||||||
const std::string& planning_plugin_param_name,
|
const std::string& planning_plugin_param_name,
|
||||||
|
|||||||
@ -1,5 +1,6 @@
|
|||||||
#include <moveit/task_constructor/container_p.h>
|
#include <moveit/task_constructor/container_p.h>
|
||||||
#include <moveit/task_constructor/stage_p.h>
|
#include <moveit/task_constructor/stage_p.h>
|
||||||
|
#include <moveit/task_constructor/task.h>
|
||||||
|
|
||||||
#include "gtest_value_printers.h"
|
#include "gtest_value_printers.h"
|
||||||
#include <gtest/gtest.h>
|
#include <gtest/gtest.h>
|
||||||
@ -450,3 +451,19 @@ TEST_F(ParallelTest, init_mismatching) {
|
|||||||
EXPECT_INIT_FAILURE(false, true, BOTH, GEN);
|
EXPECT_INIT_FAILURE(false, true, BOTH, GEN);
|
||||||
EXPECT_INIT_FAILURE(true, true, BOTH, GEN);
|
EXPECT_INIT_FAILURE(true, true, BOTH, GEN);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
TEST(Task, move) {
|
||||||
|
Task t1("foo");
|
||||||
|
t1.add(std::make_unique<GeneratorMockup>());
|
||||||
|
t1.add(std::make_unique<GeneratorMockup>());
|
||||||
|
EXPECT_EQ(t1.stages()->numChildren(), 2);
|
||||||
|
|
||||||
|
Task t2 = std::move(t1);
|
||||||
|
EXPECT_EQ(t2.stages()->numChildren(), 2);
|
||||||
|
EXPECT_EQ(t1.stages()->numChildren(), 0);
|
||||||
|
|
||||||
|
t1 = std::move(t2);
|
||||||
|
EXPECT_EQ(t1.stages()->numChildren(), 2);
|
||||||
|
EXPECT_EQ(t2.stages()->numChildren(), 0);
|
||||||
|
}
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user