improve cost term tests

- add tests for nested containers
- add descriptions to checks
- refactor helper to support different containers
This commit is contained in:
v4hn 2020-08-21 20:36:21 +02:00
parent dcee7c408f
commit 28b98e989c

View File

@ -16,6 +16,8 @@ using namespace planning_scene;
const double STAGE_COST{ 7.0 }; const double STAGE_COST{ 7.0 };
const double TRAJECTORY_DURATION{ 9.0 };
class GeneratorMockup : public Generator class GeneratorMockup : public Generator
{ {
PlanningScenePtr ps; PlanningScenePtr ps;
@ -30,7 +32,10 @@ public:
pimpl()->setNextStarts(next); pimpl()->setNextStarts(next);
} }
void init(const moveit::core::RobotModelConstPtr& robot_model) override { ps.reset(new PlanningScene(robot_model)); } void init(const moveit::core::RobotModelConstPtr& robot_model) override {
ps.reset(new PlanningScene(robot_model));
Generator::init(robot_model);
}
bool canCompute() const override { return true; } bool canCompute() const override { return true; }
@ -39,7 +44,6 @@ public:
spawn(std::move(state), STAGE_COST); spawn(std::move(state), STAGE_COST);
} }
}; };
MOVEIT_CLASS_FORWARD(GeneratorMockup);
class ConnectMockup : public Connecting class ConnectMockup : public Connecting
{ {
@ -51,21 +55,32 @@ class ConnectMockup : public Connecting
connect(from, to, solution); connect(from, to, solution);
} }
}; };
MOVEIT_CLASS_FORWARD(GeneratorMockup);
class ForwardMockup : public PropagatingForward class ForwardMockup : public PropagatingForward
{ {
PlanningScenePtr ps;
public:
using PropagatingForward::PropagatingForward; using PropagatingForward::PropagatingForward;
void init(const moveit::core::RobotModelConstPtr& robot_model) override {
ps.reset(new PlanningScene(robot_model));
PropagatingForward::init(robot_model);
}
void computeForward(const InterfaceState& from) override { void computeForward(const InterfaceState& from) override {
SubTrajectory solution; SubTrajectory solution;
auto traj{ std::make_shared<robot_trajectory::RobotTrajectory>(ps->getRobotModel(),
ps->getRobotModel()->getJointModelGroups()[0]) };
traj->addSuffixWayPoint(ps->getCurrentState(), 0.0);
traj->addSuffixWayPoint(ps->getCurrentState(), TRAJECTORY_DURATION);
solution.setTrajectory(traj);
solution.setCost(STAGE_COST); solution.setCost(STAGE_COST);
InterfaceState to(from); InterfaceState to(from);
sendForward(from, std::move(to), std::move(solution)); sendForward(from, std::move(to), std::move(solution));
}; };
}; };
MOVEIT_CLASS_FORWARD(GeneratorMockup);
class BackwardMockup : public PropagatingBackward class BackwardMockup : public PropagatingBackward
{ {
@ -79,9 +94,9 @@ class BackwardMockup : public PropagatingBackward
sendBackward(std::move(from), to, std::move(solution)); sendBackward(std::move(from), to, std::move(solution));
}; };
}; };
MOVEIT_CLASS_FORWARD(GeneratorMockup);
class SerialContainerStandalone : public SerialContainer template <typename T>
class Standalone : public T
{ {
moveit::core::RobotModelConstPtr robot; moveit::core::RobotModelConstPtr robot;
InterfacePtr dummy; InterfacePtr dummy;
@ -89,15 +104,13 @@ class SerialContainerStandalone : public SerialContainer
InterfaceStatePtr state_start, state_end; InterfaceStatePtr state_start, state_end;
public: public:
SerialContainerStandalone(const moveit::core::RobotModelConstPtr& robot) Standalone(const moveit::core::RobotModelConstPtr& robot)
: SerialContainer() : T(), robot(robot), dummy(std::make_shared<Interface>()), ps(new planning_scene::PlanningScene(robot)) {}
, robot(robot)
, dummy(std::make_shared<Interface>())
, ps(new planning_scene::PlanningScene(robot)) {}
// reset and prepare for a compute step // reset and prepare for a compute step
void prepare() { void prepare() {
reset(); auto impl{ this->pimpl() };
this->reset();
state_start.reset(new InterfaceState(ps, InterfaceState::Priority(1, 0.0))); state_start.reset(new InterfaceState(ps, InterfaceState::Priority(1, 0.0)));
state_end.reset(new InterfaceState(ps, InterfaceState::Priority(1, 0.0))); state_end.reset(new InterfaceState(ps, InterfaceState::Priority(1, 0.0)));
@ -106,46 +119,55 @@ public:
Stage* s{ this }; Stage* s{ this };
ContainerBase* b; ContainerBase* b;
while ((b = dynamic_cast<ContainerBase*>(s))) { while ((b = dynamic_cast<ContainerBase*>(s)))
s = &(*b->pimpl()->children().front()); s = &(*b->pimpl()->children().front());
}
InterfaceFlags start_flags{ s->pimpl()->requiredInterface() & START_IF_MASK }; InterfaceFlags start_flags{ s->pimpl()->requiredInterface() & START_IF_MASK };
s = this; s = this;
while ((b = dynamic_cast<ContainerBase*>(s))) { while ((b = dynamic_cast<ContainerBase*>(s)))
s = &(*b->pimpl()->children().back()); s = &(*b->pimpl()->children().back());
}
InterfaceFlags end_flags{ s->pimpl()->requiredInterface() & END_IF_MASK }; InterfaceFlags end_flags{ s->pimpl()->requiredInterface() & END_IF_MASK };
InterfaceFlags flags{ start_flags | end_flags }; InterfaceFlags flags{ start_flags | end_flags };
pimpl()->setPrevEnds(flags & WRITES_PREV_END ? dummy : nullptr); impl->setPrevEnds(flags & WRITES_PREV_END ? dummy : nullptr);
pimpl()->setNextStarts(flags & WRITES_NEXT_START ? dummy : nullptr); impl->setNextStarts(flags & WRITES_NEXT_START ? dummy : nullptr);
SerialContainer::init(robot); T::init(robot);
pimpl()->resolveInterface(flags); impl->resolveInterface(flags);
// feed interfaces as required for one computation // feed interfaces as required for one computation
if (flags & READS_START) { if (flags & READS_START) {
pimpl()->starts()->add(*state_start); impl->starts()->add(*state_start);
} }
if (flags & READS_END) { if (flags & READS_END) {
pimpl()->ends()->add(*state_end); impl->ends()->add(*state_end);
} }
} }
void runCompute() { pimpl()->runCompute(); } void runCompute() { this->pimpl()->runCompute(); }
void computeWithStage(StageUniquePtr&& stage) { void computeWithStages(std::initializer_list<StageUniquePtr> stages) {
clear(); this->clear();
add(std::move(stage));
// initializer_list offers only const access (a known shortcoming in C++)
for (const auto& stage : stages) {
this->add(const_cast<StageUniquePtr&&>(stage));
}
prepare(); prepare();
runCompute(); runCompute();
} }
void computeWithStageCost(StageUniquePtr&& stage, const CostTerm& cost_term) { void computeWithStageCost(std::initializer_list<StageUniquePtr> stages, const CostTerm& cost_term) {
stage->setCostTerm(cost_term); this->setCostTerm(nullptr);
computeWithStage(std::move(stage)); for (const auto& stage : stages)
const_cast<StageUniquePtr&&>(stage)->setCostTerm(cost_term);
computeWithStages(stages);
}
void computeWithContainerCost(std::initializer_list<StageUniquePtr> stages, const CostTerm& cost_term) {
this->setCostTerm(cost_term);
computeWithStages(stages);
} }
}; };
@ -153,28 +175,24 @@ TEST(CostTerm, CostOverwrite) {
ros::console::set_logger_level(ROSCONSOLE_ROOT_LOGGER_NAME, ros::console::levels::Fatal); ros::console::set_logger_level(ROSCONSOLE_ROOT_LOGGER_NAME, ros::console::levels::Fatal);
const moveit::core::RobotModelConstPtr robot{ getModel() }; const moveit::core::RobotModelConstPtr robot{ getModel() };
SerialContainerStandalone container(robot); Standalone<SerialContainer> container(robot);
// by default return the cost the stage declared container.computeWithStages({ std::make_unique<GeneratorMockup>() });
container.computeWithStage(std::make_unique<GeneratorMockup>()); EXPECT_EQ(container.solutions().front()->cost(), STAGE_COST) << "return cost of stage by default";
EXPECT_EQ(container.solutions().front()->cost(), STAGE_COST);
// nullptr is the explicit default container.computeWithStageCost({ std::make_unique<GeneratorMockup>() }, nullptr);
container.computeWithStageCost(std::make_unique<GeneratorMockup>(), nullptr); EXPECT_EQ(container.solutions().front()->cost(), STAGE_COST) << "nullptr cost term forwards cost";
EXPECT_EQ(container.solutions().front()->cost(), STAGE_COST);
// custom cost overwrites the stage cost
cost::Constant constant_cost{ 1.0 }; cost::Constant constant_cost{ 1.0 };
container.computeWithStageCost(std::make_unique<GeneratorMockup>(), constant_cost); container.computeWithStageCost({ std::make_unique<GeneratorMockup>() }, constant_cost);
EXPECT_EQ(container.solutions().front()->cost(), constant_cost.cost); EXPECT_EQ(container.solutions().front()->cost(), constant_cost.cost) << "custom cost overwrites stage cost";
} }
// test all primitive stage types
TEST(CostTerm, StageTypes) { TEST(CostTerm, StageTypes) {
ros::console::set_logger_level(ROSCONSOLE_ROOT_LOGGER_NAME, ros::console::levels::Fatal); ros::console::set_logger_level(ROSCONSOLE_ROOT_LOGGER_NAME, ros::console::levels::Fatal);
moveit::core::RobotModelPtr robot{ getModel() }; moveit::core::RobotModelPtr robot{ getModel() };
SerialContainerStandalone container(robot); Standalone<SerialContainer> container(robot);
const cost::Constant constant{ 1.0 }; const cost::Constant constant{ 1.0 };
@ -182,72 +200,127 @@ TEST(CostTerm, StageTypes) {
// cont.computeWithStageCost(std::make_unique<GeneratorMockup>(), constant); // cont.computeWithStageCost(std::make_unique<GeneratorMockup>(), constant);
// EXPECT_EQ(cont.solutions().front()->cost(), constant.cost); // EXPECT_EQ(cont.solutions().front()->cost(), constant.cost);
container.computeWithStageCost(std::make_unique<ConnectMockup>(), constant); container.computeWithStageCost({ std::make_unique<ConnectMockup>() }, constant);
EXPECT_EQ(container.solutions().front()->cost(), constant.cost); EXPECT_EQ(container.solutions().front()->cost(), constant.cost) << "custom cost works for connect";
container.computeWithStageCost(std::make_unique<ForwardMockup>(), constant); container.computeWithStageCost({ std::make_unique<ForwardMockup>() }, constant);
EXPECT_EQ(container.solutions().front()->cost(), constant.cost); EXPECT_EQ(container.solutions().front()->cost(), constant.cost) << "custom cost works for forward propagator";
container.computeWithStageCost(std::make_unique<BackwardMockup>(), constant); container.computeWithStageCost({ std::make_unique<BackwardMockup>() }, constant);
EXPECT_EQ(container.solutions().front()->cost(), constant.cost); EXPECT_EQ(container.solutions().front()->cost(), constant.cost) << "custom cost works for backward propagator";
} }
// Forward uses cost from solution
TEST(CostTerm, ForwardUsesCost) { TEST(CostTerm, ForwardUsesCost) {
ros::console::set_logger_level(ROSCONSOLE_ROOT_LOGGER_NAME, ros::console::levels::Fatal);
moveit::core::RobotModelPtr robot{ getModel() }; moveit::core::RobotModelPtr robot{ getModel() };
SerialContainerStandalone container(robot); Standalone<stages::Forward> container(robot);
auto stage{ std::make_unique<BackwardMockup>() }; auto stage{ std::make_unique<BackwardMockup>() };
cost::Constant constant_stage{ 84.0 }; cost::Constant constant_stage{ 84.0 };
stage->setCostTerm(constant_stage); stage->setCostTerm(constant_stage);
auto forward{ std::make_unique<stages::Forward>("forward", std::move(stage)) };
auto* forward_ptr{ forward.get() };
container.computeWithStage(std::move(forward)); container.computeWithStages({ std::move(stage) });
ASSERT_NE(nullptr, dynamic_cast<const WrappedSolution*>(&(*forward_ptr->solutions().front()))); auto& wrapped{ dynamic_cast<const WrappedSolution&>(*container.solutions().front()) };
EXPECT_EQ(wrapped.cost(), constant_stage.cost) << "Forward forwards children's cost";
auto& wrapped{ dynamic_cast<const WrappedSolution&>(*forward_ptr->solutions().front()) }; EXPECT_EQ(wrapped.wrapped()->cost(), constant_stage.cost) << "Child cost equals Forward cost";
EXPECT_EQ(wrapped.cost(), constant_stage.cost);
EXPECT_EQ(wrapped.wrapped()->cost(), constant_stage.cost);
} }
// Forward can overwrite cost from solution
TEST(CostTerm, ForwardOverwritesCost) { TEST(CostTerm, ForwardOverwritesCost) {
ros::console::set_logger_level(ROSCONSOLE_ROOT_LOGGER_NAME, ros::console::levels::Fatal);
moveit::core::RobotModelPtr robot{ getModel() }; moveit::core::RobotModelPtr robot{ getModel() };
SerialContainerStandalone container(robot); Standalone<stages::Forward> container(robot);
auto stage{ std::make_unique<BackwardMockup>() }; auto stage{ std::make_unique<BackwardMockup>() };
cost::Constant constant_stage{ 84.0 }; cost::Constant constant_stage{ 84.0 };
stage->setCostTerm(constant_stage); stage->setCostTerm(constant_stage);
auto forward{ std::make_unique<stages::Forward>("forward", std::move(stage)) };
auto* forward_ptr{ forward.get() };
cost::Constant constant_forward{ 72.0 }; cost::Constant constant_forward{ 72.0 };
forward->setCostTerm(constant_forward); container.setCostTerm(constant_forward);
container.computeWithStage(std::move(forward)); container.computeWithStages({ std::move(stage) });
auto& wrapped{ dynamic_cast<const WrappedSolution&>(*forward_ptr->solutions().front()) }; auto& wrapped{ dynamic_cast<const WrappedSolution&>(*container.solutions().front()) };
EXPECT_EQ(wrapped.cost(), constant_forward.cost); EXPECT_EQ(wrapped.cost(), constant_forward.cost) << "Forward can apply custom cost";
EXPECT_EQ(wrapped.wrapped()->cost(), constant_stage.cost); EXPECT_EQ(wrapped.wrapped()->cost(), constant_stage.cost) << "child's cost is not affected";
// Forward can modify cost from solution
} }
// Forward can modify cost from solution
TEST(CostTerm, ForwardCanModifyCost) { TEST(CostTerm, ForwardCanModifyCost) {
ros::console::set_logger_level(ROSCONSOLE_ROOT_LOGGER_NAME, ros::console::levels::Fatal);
moveit::core::RobotModelPtr robot{ getModel() }; moveit::core::RobotModelPtr robot{ getModel() };
SerialContainerStandalone container(robot); Standalone<stages::Forward> container(robot);
auto stage{ std::make_unique<BackwardMockup>() }; auto stage{ std::make_unique<BackwardMockup>() };
cost::Constant constant{ 8.0 }; cost::Constant constant{ 8.0 };
stage->setCostTerm(constant); stage->setCostTerm(constant);
auto forward{ std::make_unique<stages::Forward>("forward", std::move(stage)) }; container.setCostTerm([](auto&& s) { return s.cost() * s.cost(); });
auto* forward_ptr{ forward.get() };
forward->setCostTerm([](auto&& s) { return s.cost() * s.cost(); });
container.computeWithStage(std::move(forward)); container.computeWithStages({ std::move(stage) });
auto& wrapped{ dynamic_cast<const WrappedSolution&>(*forward_ptr->solutions().front()) }; auto& wrapped{ dynamic_cast<const WrappedSolution&>(*container.solutions().front()) };
EXPECT_EQ(wrapped.cost(), constant.cost * constant.cost); EXPECT_EQ(wrapped.cost(), constant.cost * constant.cost) << "Forward can compute cost based on child";
EXPECT_EQ(wrapped.wrapped()->cost(), constant.cost); EXPECT_EQ(wrapped.wrapped()->cost(), constant.cost) << "child's cost is not affected";
}
TEST(CostTerm, CompositeSolutions) {
ros::console::set_logger_level(ROSCONSOLE_ROOT_LOGGER_NAME, ros::console::levels::Fatal);
Standalone<SerialContainer> container{ getModel() };
{
auto s1{ std::make_unique<ForwardMockup>() };
auto s2{ std::make_unique<ForwardMockup>() };
container.computeWithStages({ std::move(s1), std::move(s2) });
EXPECT_EQ(container.solutions().front()->cost(), 2 * STAGE_COST) << "by default stage costs are added";
}
{
auto s1{ std::make_unique<ForwardMockup>() };
cost::Constant constant{ 1.0 };
s1->setCostTerm(constant);
auto s2{ std::make_unique<ForwardMockup>() };
container.computeWithStages({ std::move(s1), std::move(s2) });
EXPECT_EQ(container.solutions().front()->cost(), STAGE_COST + constant.cost)
<< "mix of explicit and implicit cost terms works";
}
{
auto s1{ std::make_unique<ForwardMockup>() };
auto s2{ std::make_unique<ForwardMockup>() };
auto c1{ std::make_unique<SerialContainer>() };
cost::Constant constant1{ 1.0 };
s1->setCostTerm(constant1);
s2->setCostTerm(constant1);
c1->add(std::move(s1));
c1->add(std::move(s2));
auto s3{ std::make_unique<ForwardMockup>() };
cost::Constant constant2{ 9.0 };
s3->setCostTerm(constant2);
container.computeWithStages({ std::move(c1), std::move(s3) });
EXPECT_EQ(container.solutions().front()->cost(), 2 * constant1.cost + constant2.cost)
<< "cost aggregation works across multiple levels";
}
}
TEST(CostTerm, CompositeSolutionsContainerCost) {
ros::console::set_logger_level(ROSCONSOLE_ROOT_LOGGER_NAME, ros::console::levels::Fatal);
Standalone<SerialContainer> container{ getModel() };
auto s1{ std::make_unique<ForwardMockup>() };
auto s1_ptr{ s1.get() };
auto s2{ std::make_unique<ForwardMockup>() };
auto c1{ std::make_unique<SerialContainer>() };
c1->add(std::move(s1));
c1->add(std::move(s2));
auto s3{ std::make_unique<ForwardMockup>() };
container.setCostTerm(cost::TrajectoryDuration);
container.computeWithStages({ std::move(c1), std::move(s3) });
EXPECT_EQ(container.solutions().front()->cost(), 3 * TRAJECTORY_DURATION)
<< "container cost term overwrites stage costs";
EXPECT_EQ(s1_ptr->solutions().front()->cost(), STAGE_COST) << "child cost is not affected";
}
} }