ompl constrained plannerの理解

[HiroIshida]

project 関数はどう呼ばれているか

https://github.com/HiroIshida/snippets/tree/master/cpp/ext_example/ompl/read_code_constrained
glibc backtrace_symbolの出力

RRT/RRTConnectの場合

callstak for Hash: -680453616
SphereConstraint::project(Eigen::Ref<Eigen::Matrix<double, -1, 1, 0, -1, 1>, 0, Eigen::InnerStride<1> >) const
ompl::base::Constraint::project(ompl::base::State*) const
ompl::base::ProjectedStateSpace::discreteGeodesic(ompl::base::State const*, ompl::base::State const*, bool, std::vector<ompl::base::State*, std::allocatorompl::base::State* >) const
ompl::base::ConstrainedMotionValidator::checkMotion(ompl::base::State const, ompl::base::State const*) const
ompl::geometric::RRT::solve(ompl::base::PlannerTerminationCondition const&)
ompl::base::Planner::solve(double)
ompl::geometric::SimpleSetup::solve(double)
main

callstak for Hash: 361405613
SphereConstraint::project(Eigen::Ref<Eigen::Matrix<double, -1, 1, 0, -1, 1>, 0, Eigen::InnerStride<1> >) const
ompl::base::Constraint::project(ompl::base::State*) const
ompl::base::ProjectedStateSpace::discreteGeodesic(ompl::base::State const*, ompl::base::State const*, bool, std::vector<ompl::base::State*, std::allocatorompl::base::State* >) const
ompl::base::ConstrainedStateSpace::interpolate(ompl::base::State const, ompl::base::State const*, double, ompl::base::State*) const
ompl::geometric::RRT::solve(ompl::base::PlannerTerminationCondition const&)
ompl::base::Planner::solve(double)
ompl::geometric::SimpleSetup::solve(double)
main

callstak for Hash: -1343344934
SphereConstraint::project(Eigen::Ref<Eigen::Matrix<double, -1, 1, 0, -1, 1>, 0, Eigen::InnerStride<1> >) const
ompl::base::Constraint::project(ompl::base::State*) const
ompl::base::ProjectedStateSampler::sampleUniform(ompl::base::State*)
ompl::geometric::RRT::solve(ompl::base::PlannerTerminationCondition const&)
ompl::base::Planner::solve(double)
ompl::geometric::SimpleSetup::solve(double)
main

KPIECEの場合

callstak for Hash: -302607490
SphereConstraint::project(Eigen::Ref<Eigen::Matrix<double, -1, 1, 0, -1, 1>, 0, Eigen::InnerStride<1> >) const
ompl::base::Constraint::project(ompl::base::State*) const
ompl::base::ProjectedStateSpace::discreteGeodesic(ompl::base::State const*, ompl::base::State const*, bool, std::vector<ompl::base::State*, std::allocatorompl::base::State* >) const
ompl::base::ConstrainedMotionValidator::checkMotion(ompl::base::State const, ompl::base::State const*, std::pair<ompl::base::State*, double>&) const
ompl::geometric::KPIECE1::solve(ompl::base::PlannerTerminationCondition const&)
ompl::base::Planner::solve(double)
ompl::geometric::SimpleSetup::solve(double)
main
callstak for Hash: -33775606
SphereConstraint::project(Eigen::Ref<Eigen::Matrix<double, -1, 1, 0, -1, 1>, 0, Eigen::InnerStride<1> >) const
ompl::base::Constraint::project(ompl::base::State*) const
ompl::base::ProjectedStateSampler::sampleUniformNear(ompl::base::State*, ompl::base::State const*, double)
ompl::geometric::KPIECE1::solve(ompl::base::PlannerTerminationCondition const&)
ompl::base::Planner::solve(double)
ompl::geometric::SimpleSetup::solve(double)
main

[HiroIshida]

discreteGeodesicは何をしているのか?

bool ompl::base::ProjectedStateSpace::discreteGeodesic(const State *from, const State *to, bool interpolate,
                                                       std::vector<State *> *geodesic) const
{
    // Save a copy of the from state.
    if (geodesic != nullptr)
    {
        geodesic->clear();
        geodesic->push_back(cloneState(from));
    }

    const double tolerance = delta_;

    // No need to traverse the manifold if we are already there.
    double dist, step, total = 0;
    if ((dist = distance(from, to)) <= tolerance)
        return true;

    const double max = dist * lambda_;

    auto previous = cloneState(from);
    auto scratch = allocState();

    auto &&svc = si_->getStateValidityChecker();

    do
    {
        WrapperStateSpace::interpolate(previous, to, delta_ / dist, scratch);

        // Project new state onto constraint manifold
        if (!constraint_->project(scratch)                  // not on manifold
            || !(interpolate || svc->isValid(scratch))      // not valid
            || (step = distance(previous, scratch)) > lambda_ * delta_)  // deviated
            break;

        // Check if we have wandered too far
        total += step;
        if (total > max)
            break;

        // Check if we are no closer than before
        const double newDist = distance(scratch, to);
        if (newDist >= dist)
            break;

        dist = newDist;
        copyState(previous, scratch);

        // Store the new state
        if (geodesic != nullptr)
            geodesic->push_back(cloneState(scratch));

    } while (dist >= tolerance);

    freeState(scratch);
    freeState(previous);
    return dist <= tolerance;
}

[HiroIshida]

function　はどこで呼ばれているか

SphereConstraint::function(Eigen::Ref<Eigen::Matrix<double, -1, 1, 0, -1, 1> const, 0, Eigen::InnerStride<1> > const&, Eigen::Ref<Eigen::Matrix<double, -1, 1, 0, -1, 1>, 0, Eigen::InnerStride<1> >) const
ompl::base::Constraint::isSatisfied(Eigen::Ref<Eigen::Matrix<double, -1, 1, 0, -1, 1> const, 0, Eigen::InnerStride<1> > const&) const
ompl::base::Constraint::isSatisfied(ompl::base::State const*) const
ompl::base::ConstrainedMotionValidator::checkMotion(ompl::base::State const*, ompl::base::State const*) const
ompl::geometric::RRT::solve(ompl::base::PlannerTerminationCondition const&)
ompl::base::Planner::solve(double)
ompl::geometric::SimpleSetup::solve(double)
solve()
他にも, 当然project()の中でも呼ばれている

[HiroIshida]

Q: KPIECEでisSatisifed でdistを求めるが, distはconstrainedの場合どう定義されるか?

A: space informationの中で定義されている. ambient spaceから多様体への距離じゃなくて, 点同士の距離なのね

#0 std::__shared_ptr_access<ompl::base::SpaceInformation, (__gnu_cxx::_Lock_policy)2, false, false>::operator-> (this=0x555555dc30a0) at /usr/include/c++/13/bits/shared_ptr_base.h:1354
#1 0x0000555555b5c598 in ompl::base::GoalState::distanceGoal (this=0x555555dbe870, st=0x555555dc8d80)
at /home/h-ishida/misc/snippets/cpp/ext_example/ompl/read_code_constrained/ompl/src/ompl/base/goals/src/GoalState.cpp:48
#2 0x0000555555b5c41c in ompl::base::GoalRegion::isSatisfied (this=0x555555dbe870, st=0x555555dc8d80, distance=0x7fffffffd4d0)
at /home/h-ishida/misc/snippets/cpp/ext_example/ompl/read_code_constrained/ompl/src/ompl/base/goals/src/GoalRegion.cpp:54
#3 0x0000555555b31441 in ompl::geometric::KPIECE1::solve (this=0x555555dbe930, ptc=...)
at /home/h-ishida/misc/snippets/cpp/ext_example/ompl/read_code_constrained/ompl/src/ompl/geometric/planners/kpiece/src/KPIECE1.cpp:153
#4 0x0000555555adcd90 in ompl::base::Planner::solve (this=0x555555dbe930, solveTime=10) at /home/h-ishida/misc/snippets/cpp/ext_example/ompl/read_code_constrained/ompl/src/ompl/base/src/Planner.cpp:144
#5 0x0000555555b499b9 in ompl::geometric::SimpleSetup::solve (this=0x555555dbe5e0, time=10)
at /home/h-ishida/misc/snippets/cpp/ext_example/ompl/read_code_constrained/ompl/src/ompl/geometric/src/SimpleSetup.cpp:128
#6 0x0000555555a51786 in main () at /home/h-ishida/misc/snippets/cpp/ext_example/ompl/read_code_constrained/main.cpp:138

[HiroIshida]

KPIECEのどこを変更するか?

• uniformed samplingで, xstateを得る段階でprojectしている. これをやめて普通にサンプルするようにする. 新しいmethodを追加して nonProjectionSamplingみたいなの作る
• uniform sampling して, そこに向かっていくのではなく, それを一旦boxでcutしてやる.