mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
compute cost in liftSolution as well
This commit is contained in:
parent
939f0b7256
commit
0e38730fd4
@ -201,10 +201,13 @@ class SubTrajectory;
|
|||||||
using CostTerm = std::function<double(const SubTrajectory&, std::string&)>;
|
using CostTerm = std::function<double(const SubTrajectory&, std::string&)>;
|
||||||
|
|
||||||
class StagePrivate;
|
class StagePrivate;
|
||||||
|
class ContainerBasePrivate;
|
||||||
/// abstract base class for solutions (primitive and sequences)
|
/// abstract base class for solutions (primitive and sequences)
|
||||||
class SolutionBase
|
class SolutionBase
|
||||||
{
|
{
|
||||||
friend StagePrivate; // for set[Start|End]StateUnsafe
|
friend StagePrivate; // for set[Start|End]StateUnsafe
|
||||||
|
friend ContainerBasePrivate;
|
||||||
|
|
||||||
public:
|
public:
|
||||||
virtual ~SolutionBase() = default;
|
virtual ~SolutionBase() = default;
|
||||||
|
|
||||||
|
|||||||
@ -120,6 +120,10 @@ void ContainerBasePrivate::copyState(Interface::iterator external, const Interfa
|
|||||||
|
|
||||||
void ContainerBasePrivate::liftSolution(const SolutionBasePtr& solution, const InterfaceState* internal_from,
|
void ContainerBasePrivate::liftSolution(const SolutionBasePtr& solution, const InterfaceState* internal_from,
|
||||||
const InterfaceState* internal_to) {
|
const InterfaceState* internal_to) {
|
||||||
|
solution->setCreator(me());
|
||||||
|
|
||||||
|
computeCost(*internal_from, *internal_to, *solution);
|
||||||
|
|
||||||
if (!storeSolution(solution))
|
if (!storeSolution(solution))
|
||||||
return;
|
return;
|
||||||
|
|
||||||
@ -139,8 +143,8 @@ void ContainerBasePrivate::liftSolution(const SolutionBasePtr& solution, const I
|
|||||||
InterfaceState* external_to = find_or_create_external(internal_to, created_to);
|
InterfaceState* external_to = find_or_create_external(internal_to, created_to);
|
||||||
|
|
||||||
// connect solution to start/end state
|
// connect solution to start/end state
|
||||||
solution->setStartState(*external_from);
|
solution->setStartStateUnsafe(*external_from);
|
||||||
solution->setEndState(*external_to);
|
solution->setEndStateUnsafe(*external_to);
|
||||||
|
|
||||||
// spawn created states in external interfaces
|
// spawn created states in external interfaces
|
||||||
if (created_from)
|
if (created_from)
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user