mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
improve cost term tests
- add tests for nested containers - add descriptions to checks - refactor helper to support different containers
This commit is contained in:
parent
dcee7c408f
commit
28b98e989c
@ -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";
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user