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 TRAJECTORY_DURATION{ 9.0 };
class GeneratorMockup : public Generator
{
PlanningScenePtr ps;
@ -30,7 +32,10 @@ public:
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; }
@ -39,7 +44,6 @@ public:
spawn(std::move(state), STAGE_COST);
}
};
MOVEIT_CLASS_FORWARD(GeneratorMockup);
class ConnectMockup : public Connecting
{
@ -51,21 +55,32 @@ class ConnectMockup : public Connecting
connect(from, to, solution);
}
};
MOVEIT_CLASS_FORWARD(GeneratorMockup);
class ForwardMockup : public PropagatingForward
{
PlanningScenePtr ps;
public:
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 {
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);
InterfaceState to(from);
sendForward(from, std::move(to), std::move(solution));
};
};
MOVEIT_CLASS_FORWARD(GeneratorMockup);
class BackwardMockup : public PropagatingBackward
{
@ -79,9 +94,9 @@ class BackwardMockup : public PropagatingBackward
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;
InterfacePtr dummy;
@ -89,15 +104,13 @@ class SerialContainerStandalone : public SerialContainer
InterfaceStatePtr state_start, state_end;
public:
SerialContainerStandalone(const moveit::core::RobotModelConstPtr& robot)
: SerialContainer()
, robot(robot)
, dummy(std::make_shared<Interface>())
, ps(new planning_scene::PlanningScene(robot)) {}
Standalone(const moveit::core::RobotModelConstPtr& robot)
: T(), robot(robot), dummy(std::make_shared<Interface>()), ps(new planning_scene::PlanningScene(robot)) {}
// reset and prepare for a compute step
void prepare() {
reset();
auto impl{ this->pimpl() };
this->reset();
state_start.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 };
ContainerBase* b;
while ((b = dynamic_cast<ContainerBase*>(s))) {
while ((b = dynamic_cast<ContainerBase*>(s)))
s = &(*b->pimpl()->children().front());
}
InterfaceFlags start_flags{ s->pimpl()->requiredInterface() & START_IF_MASK };
s = this;
while ((b = dynamic_cast<ContainerBase*>(s))) {
while ((b = dynamic_cast<ContainerBase*>(s)))
s = &(*b->pimpl()->children().back());
}
InterfaceFlags end_flags{ s->pimpl()->requiredInterface() & END_IF_MASK };
InterfaceFlags flags{ start_flags | end_flags };
pimpl()->setPrevEnds(flags & WRITES_PREV_END ? dummy : nullptr);
pimpl()->setNextStarts(flags & WRITES_NEXT_START ? dummy : nullptr);
SerialContainer::init(robot);
pimpl()->resolveInterface(flags);
impl->setPrevEnds(flags & WRITES_PREV_END ? dummy : nullptr);
impl->setNextStarts(flags & WRITES_NEXT_START ? dummy : nullptr);
T::init(robot);
impl->resolveInterface(flags);
// feed interfaces as required for one computation
if (flags & READS_START) {
pimpl()->starts()->add(*state_start);
impl->starts()->add(*state_start);
}
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) {
clear();
add(std::move(stage));
void computeWithStages(std::initializer_list<StageUniquePtr> stages) {
this->clear();
// initializer_list offers only const access (a known shortcoming in C++)
for (const auto& stage : stages) {
this->add(const_cast<StageUniquePtr&&>(stage));
}
prepare();
runCompute();
}
void computeWithStageCost(StageUniquePtr&& stage, const CostTerm& cost_term) {
stage->setCostTerm(cost_term);
computeWithStage(std::move(stage));
void computeWithStageCost(std::initializer_list<StageUniquePtr> stages, const CostTerm& cost_term) {
this->setCostTerm(nullptr);
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);
const moveit::core::RobotModelConstPtr robot{ getModel() };
SerialContainerStandalone container(robot);
Standalone<SerialContainer> container(robot);
// by default return the cost the stage declared
container.computeWithStage(std::make_unique<GeneratorMockup>());
EXPECT_EQ(container.solutions().front()->cost(), STAGE_COST);
container.computeWithStages({ std::make_unique<GeneratorMockup>() });
EXPECT_EQ(container.solutions().front()->cost(), STAGE_COST) << "return cost of stage by default";
// nullptr is the explicit default
container.computeWithStageCost(std::make_unique<GeneratorMockup>(), nullptr);
EXPECT_EQ(container.solutions().front()->cost(), STAGE_COST);
container.computeWithStageCost({ std::make_unique<GeneratorMockup>() }, nullptr);
EXPECT_EQ(container.solutions().front()->cost(), STAGE_COST) << "nullptr cost term forwards cost";
// custom cost overwrites the stage cost
cost::Constant constant_cost{ 1.0 };
container.computeWithStageCost(std::make_unique<GeneratorMockup>(), constant_cost);
EXPECT_EQ(container.solutions().front()->cost(), constant_cost.cost);
container.computeWithStageCost({ std::make_unique<GeneratorMockup>() }, constant_cost);
EXPECT_EQ(container.solutions().front()->cost(), constant_cost.cost) << "custom cost overwrites stage cost";
}
// test all primitive stage types
TEST(CostTerm, StageTypes) {
ros::console::set_logger_level(ROSCONSOLE_ROOT_LOGGER_NAME, ros::console::levels::Fatal);
moveit::core::RobotModelPtr robot{ getModel() };
SerialContainerStandalone container(robot);
Standalone<SerialContainer> container(robot);
const cost::Constant constant{ 1.0 };
@ -182,72 +200,127 @@ TEST(CostTerm, StageTypes) {
// cont.computeWithStageCost(std::make_unique<GeneratorMockup>(), constant);
// EXPECT_EQ(cont.solutions().front()->cost(), constant.cost);
container.computeWithStageCost(std::make_unique<ConnectMockup>(), constant);
EXPECT_EQ(container.solutions().front()->cost(), constant.cost);
container.computeWithStageCost({ std::make_unique<ConnectMockup>() }, constant);
EXPECT_EQ(container.solutions().front()->cost(), constant.cost) << "custom cost works for connect";
container.computeWithStageCost(std::make_unique<ForwardMockup>(), constant);
EXPECT_EQ(container.solutions().front()->cost(), constant.cost);
container.computeWithStageCost({ std::make_unique<ForwardMockup>() }, constant);
EXPECT_EQ(container.solutions().front()->cost(), constant.cost) << "custom cost works for forward propagator";
container.computeWithStageCost(std::make_unique<BackwardMockup>(), constant);
EXPECT_EQ(container.solutions().front()->cost(), constant.cost);
container.computeWithStageCost({ std::make_unique<BackwardMockup>() }, constant);
EXPECT_EQ(container.solutions().front()->cost(), constant.cost) << "custom cost works for backward propagator";
}
// Forward uses cost from solution
TEST(CostTerm, ForwardUsesCost) {
ros::console::set_logger_level(ROSCONSOLE_ROOT_LOGGER_NAME, ros::console::levels::Fatal);
moveit::core::RobotModelPtr robot{ getModel() };
SerialContainerStandalone container(robot);
Standalone<stages::Forward> container(robot);
auto stage{ std::make_unique<BackwardMockup>() };
cost::Constant constant_stage{ 84.0 };
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&>(*forward_ptr->solutions().front()) };
EXPECT_EQ(wrapped.cost(), constant_stage.cost);
EXPECT_EQ(wrapped.wrapped()->cost(), constant_stage.cost);
auto& wrapped{ dynamic_cast<const WrappedSolution&>(*container.solutions().front()) };
EXPECT_EQ(wrapped.cost(), constant_stage.cost) << "Forward forwards children's cost";
EXPECT_EQ(wrapped.wrapped()->cost(), constant_stage.cost) << "Child cost equals Forward cost";
}
// Forward can overwrite cost from solution
TEST(CostTerm, ForwardOverwritesCost) {
ros::console::set_logger_level(ROSCONSOLE_ROOT_LOGGER_NAME, ros::console::levels::Fatal);
moveit::core::RobotModelPtr robot{ getModel() };
SerialContainerStandalone container(robot);
Standalone<stages::Forward> container(robot);
auto stage{ std::make_unique<BackwardMockup>() };
cost::Constant constant_stage{ 84.0 };
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 };
forward->setCostTerm(constant_forward);
container.setCostTerm(constant_forward);
container.computeWithStage(std::move(forward));
auto& wrapped{ dynamic_cast<const WrappedSolution&>(*forward_ptr->solutions().front()) };
EXPECT_EQ(wrapped.cost(), constant_forward.cost);
EXPECT_EQ(wrapped.wrapped()->cost(), constant_stage.cost);
// Forward can modify cost from solution
container.computeWithStages({ std::move(stage) });
auto& wrapped{ dynamic_cast<const WrappedSolution&>(*container.solutions().front()) };
EXPECT_EQ(wrapped.cost(), constant_forward.cost) << "Forward can apply custom cost";
EXPECT_EQ(wrapped.wrapped()->cost(), constant_stage.cost) << "child's cost is not affected";
}
// Forward can modify cost from solution
TEST(CostTerm, ForwardCanModifyCost) {
ros::console::set_logger_level(ROSCONSOLE_ROOT_LOGGER_NAME, ros::console::levels::Fatal);
moveit::core::RobotModelPtr robot{ getModel() };
SerialContainerStandalone container(robot);
Standalone<stages::Forward> container(robot);
auto stage{ std::make_unique<BackwardMockup>() };
cost::Constant constant{ 8.0 };
stage->setCostTerm(constant);
auto forward{ std::make_unique<stages::Forward>("forward", std::move(stage)) };
auto* forward_ptr{ forward.get() };
forward->setCostTerm([](auto&& s) { return s.cost() * s.cost(); });
container.setCostTerm([](auto&& s) { return s.cost() * s.cost(); });
container.computeWithStage(std::move(forward));
auto& wrapped{ dynamic_cast<const WrappedSolution&>(*forward_ptr->solutions().front()) };
EXPECT_EQ(wrapped.cost(), constant.cost * constant.cost);
EXPECT_EQ(wrapped.wrapped()->cost(), constant.cost);
container.computeWithStages({ std::move(stage) });
auto& wrapped{ dynamic_cast<const WrappedSolution&>(*container.solutions().front()) };
EXPECT_EQ(wrapped.cost(), constant.cost * constant.cost) << "Forward can compute cost based on child";
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";
}
}