路径规划[3/4]：simplifySolution、interpolateSolution

ancientcc

公式1（计算、两状态点/路点之间距离）

：该规划组内有个可变关节。

：状态点的第个关节值。

double JointModelGroup::distance(const double* state1, const double* state2) const
{
  double d = 0.0;
  for (std::size_t i = 0; i < active_joint_model_vector_.size(); ++i)
    d += active_joint_model_vector_[i]->getDistanceFactor() *
         active_joint_model_vector_[i]->distance(state1 + active_joint_model_start_index_[i],
                                                 state2 + active_joint_model_start_index_[i]);
  return d;
}

double RevoluteJointModel::distance(const double* values1, const double* values2) const
{
  if (continuous_)
  {
    double d = fmod(fabs(values1[0] - values2[0]), 2.0 * boost::math::constants::pi<double>());
    return (d > boost::math::constants::pi<double>()) ? 2.0 * boost::math::constants::pi<double>() - d : d;
  }
  else
    return fabs(values1[0] - values2[0]);
}

JointModelGroup::distance是对应该公式的代码。示例：规划组有3个可变关节，(-0.56000, 0.79000, -0.44999)， (0.26203, 1.31634, -0.98562)，则是1.884。

  • waypoint。路点，值就是状态点State，只是要用在路径中，给个别名而已。存储在path_，path_是分段的，路点就是这些段的边界点。
  • longestValidSegment_表示该规划组一次可移动的最大距离。值依赖两个变量。一是<joint>内的<limit>，用于计算该规划组最大可移动距离maxExtent_，二是param中/move_group/<robot_arm>/longest_valid_segment_fraction，表示一次可移动最大距离占maxExtent_的比率。
  • simplifySolution功能是让path_中路点尽可做到单方向、平滑变化。
  • interpolateSolution功能是在路径中补插路点，保证相邻路点距离不会大于longestValidSegment_。
  • 除、，RRTConnect::solve生成的路点是随机的，最终生成的路径可能和它们没有关系。

RRTConnect::solve结束后，规化出的路径存放在pdef_->solutions_->solutions_[0].path_。让看下变量中二个solutions_。

class ProblemDefinition::PlannerSolutionSet
{
public:
    void add(const PlannerSolution &s)
    {
        std::lock_guard<std::mutex> slock(lock_);
        int index = solutions_.size();
        solutions_.push_back(s);
        solutions_.back().index_ = index;
        std::sort(solutions_.begin(), solutions_.end());
    }

RRTConnect::solve会调用add，把规划出的路径放入solutions_。一次规划成功，只调用一次add，因而对RRTConnect算法，solutions_.size()最大值是1。

    PathPtr getTopSolution()
    {
        std::lock_guard<std::mutex> slock(lock_);
        PathPtr copy;
        if (!solutions_.empty())
            copy = solutions_[0].path_;
        return copy;
    }

    ...

private:
    std::vector<PlannerSolution> solutions_;
    std::mutex lock_;
};

第一个solutions_类型是PlannerSolutionSet，第二个solutions_是它内中成员，类型std::vector<PlannerSolution>。这里有个疑问，RRTConnect::solve规划出的路径只用一个PlannerSolution就够了，为什么要用std::vector？——我的理解：RRTConnect是众多规划算法中一种，用它时，整个过程只要一个就够了，但其它算法运行过程中可能会临时需要多个。对那些算法，要多个是临时的，结束后还是一个就够了，它存放在索引0。为得到这个真正需要的PlannerSolution，于是PlannerSolutionSet提供了方法getTopSolution()。

struct PlannerSolution
{
    ......
    PathPtr path_;
};

在PlannerSolution，具体存储路径是的成员path_。RRTConnect是基于Geometric方式的路径规划算法，path_类型是个从Path派生的PathGeometric。

为直观描述，这里附上个运行实例。示例中机械臂有3个可变关节：a、b、c，RRTConnect::solve结束后，路径中有4个路点。

states_[0](-0.56000, 0.79000, -0.45)
states_[1](0.26203, 1.31634, -0.98562)
states_[2](0.19253, 0.96723, -0.72422)
states_[3](-0.00000, 0, -0.00000)

其中第一个states_[0]是初始State，记为。最后一个states_[3]是目标State，记为。中间[1]和[2]是RRTConnect::solve找出的路点。

simplifySolution、interpolateSolution是啥功能？

让看下solve找出的两个路点。对关节b，初始0.79，目标0。中间两个分别是1.31634、0.96723，如果关节是可以由0.79直接旋转到0，那让它由初始0.79转到1.31634，然后转到0.96723，再转到目标0，明显是个坏方案。至少如果中间是0.5、0.3啥的，就比它好多了。同理关节a、关节c都有这问题。

path_中有4个路点，拆分成了三段。很显然，RRTConnect::solve拆分出的段不是个好方案，须要进行优化，衡量优化好坏标准就是由到时，1）尽可能一个方向变化，2）尽可能平滑。这正是simplifySolution函数的目标。

经过simplifySolution后，path_尽可能一个方向、平滑变化了，但当中路点个数毕竟有限。会存在两个相邻路点距离大于关节号称的一次旋转最大距离longestValidSegment_，这时就要插补路点，保证相邻路点距离不会大于longestValidSegment_。这正是interpolateSolution函数的目标。

 

一、SimpleSetup::simplifySolution

功能是让path_中路点尽可做到单方向、平滑变化。生成的结果依旧存在path_。

void ompl::geometric::SimpleSetup::simplifySolution(const base::PlannerTerminationCondition &ptc)
{
    if (pdef_)
    {
        const base::PathPtr &p = pdef_->getSolutionPath();

此时p就是RRTConnect::solve结束前赋给的值。对示例，p->states_.size()是4。

state[0](-0.56000, 0.79000, -0.45)

state[1](0.26203, 1.31634, -0.98562)

state[2](0.19253, 0.96723, -0.72422)

state[3](-0.00000, 0, -0.00000)

        if (p)
        {
            time::point start = time::now();
            auto &path = static_cast<PathGeometric &>(*p);
            std::size_t numStates = path.getStateCount();

虽然path和p指向的是同一个东西，但类型一个派生类PathGeometric一个基类Path，而且p只读，path则可读写。对示例，numStates值4。

            psk_->simplify(path, ptc);

psk_类型是PathSimplifierPtr，调用它的路径平滑函数simplify，该函数执行后，作为输入的path同时存储着平滑后的结果

            simplifyTime_ = time::seconds(time::now() - start);
            OMPL_INFORM("SimpleSetup: Path simplification took %f seconds and changed from %d to %d states",
                        simplifyTime_, numStates, path.getStateCount());
            return;
        }
    }
    OMPL_WARN("No solution to simplify");
}

PathSimplifier::simplify有复杂逻辑，像PathSimplifier::shortcutPath。为直观理解，这里在相关地方加上SDL_log，给出一次simplify的输出日志。

INFO: {PathSimplifier::simplify}---path: [0/4](-0.56000, 0.79000, -0.44999),[1/4](0.26203, 1.31634, -0.98562),[2/4](0.19253, 0.96723, -0.72422),[3/4](-0.00000, 0, -0.00000)
INFO: {PathSimplifier::simplify}while#0, path: [0/4](-0.56000, 0.79000, -0.44999),[1/4](0.26203, 1.31634, -0.98562),[2/4](0.19253, 0.96723, -0.72422),[3/4](-0.00000, 0, -0.00000)

--执行第一次shortcutPath
INFO: {shortcutPath}---path: [0/4](-0.56000, 0.79000, -0.44999),[1/4](0.26203, 1.31634, -0.98562),[2/4](0.19253, 0.96723, -0.72422),[3/4](-0.00000, 0, -0.00000)
INFO: {PathSimplifier::shortcutPath}maxSteps: 4, maxEmptySteps: 4, rangeRatio: 0.33000, snapToVertex: 0.00500
INFO: {shortcutPath}pre for costs: (0, 1.884, 2.564, 4.448), dists: (0, 1.884, 2.564, 4.448)
INFO: {shortcutPath}for#[0/4] nochange: 0...
INFO: {shortcutPath}for#[1/4] nochange: 1...
INFO: {shortcutPath}for#[2/4] nochange: 2...
INFO: {shortcutPath}[2/4]s0{1, -1, 0.16518, (0.25055, 1.25867, -0.94244)}, s1{2, -1, 0.40550, (0.11446, 0.57502, -0.43055)}
INFO: {shortcutPath}[2/4]post a valid, path: [0/5](-0.56000, 0.79000, -0.44999),[1/5](0.26203, 1.31634, -0.98562),[2/5](0.25055, 1.25867, -0.94244),[3/5](0.11446, 0.57502, -0.43055),[4/5](-0.00000, 0, -0.00000)
INFO: {shortcutPath}[2/4]post a valid, costs: (0, 1.884, 1.99633, 3.32796, 4.448), dists: (0, 1.884, 1.99633, 3.32796, 4.448)
INFO: {shortcutPath}for#[3/4] nochange: 1...
INFO: {shortcutPath}[3/4]s0{0, -1, 0.90637, (0.18506, 1.26706, -0.93547)}, s1{2, -1, 0.15557, (0.22938, 1.15231, -0.86280)}
INFO: {shortcutPath}[3/4]post a valid, path: [0/5](-0.56000, 0.79000, -0.44999),[1/5](0.18506, 1.26706, -0.93547),[2/5](0.22938, 1.15231, -0.86280),[3/5](0.11446, 0.57502, -0.43055),[4/5](-0.00000, 0, -0.00000)
INFO: {shortcutPath}[3/4]post a valid, costs: (0, 1.70761, 1.93934, 3.0638, 4.18383), dists: (0, 1.70761, 1.93934, 3.0638, 4.18383)
INFO: ---{shortcutPath}X, path: [0/5](-0.56000, 0.79000, -0.44999),[1/5](0.18506, 1.26706, -0.93547),[2/5](0.22938, 1.15231, -0.86280),[3/5](0.11446, 0.57502, -0.43055),[4/5](-0.00000, 0, -0.00000), result: true

--执行第二次shortcutPath
INFO: {shortcutPath}---path: [0/5](-0.56000, 0.79000, -0.44999),[1/5](0.18506, 1.26706, -0.93547),[2/5](0.22938, 1.15231, -0.86280),[3/5](0.11446, 0.57502, -0.43055),[4/5](-0.00000, 0, -0.00000)
INFO: {PathSimplifier::shortcutPath}maxSteps: 5, maxEmptySteps: 5, rangeRatio: 0.33000, snapToVertex: 0.00500
INFO: {shortcutPath}pre for costs: (0, 1.70761, 1.93934, 3.0638, 4.18383), dists: (0, 1.70761, 1.93934, 3.0638, 4.18383)
INFO: {shortcutPath}for#[0/5] nochange: 0...
INFO: {shortcutPath}for#[1/5] nochange: 1...
INFO: {shortcutPath}for#[2/5] nochange: 2...
INFO: {shortcutPath}for#[3/5] nochange: 3...
INFO: {shortcutPath}[3/5]s0{1, -1, 0.38121, (0.20195, 1.22332, -0.90777)}, s1{2, -1, 0.25445, (0.20013, 1.00542, -0.75281)}
INFO: {shortcutPath}[3/5]post a valid, path: [0/6](-0.56000, 0.79000, -0.44999),[1/6](0.18506, 1.26706, -0.93547),[2/6](0.20195, 1.22332, -0.90777),[3/6](0.20013, 1.00542, -0.75281),[4/6](0.11446, 0.57502, -0.43055),[5/6](-0.00000, 0, -0.00000)
INFO: {shortcutPath}[3/5]post a valid, costs: (0, 1.70761, 1.79594, 2.17062, 3.00896, 4.12899), dists: (0, 1.70761, 1.79594, 2.17062, 3.00896, 4.12899)
INFO: {shortcutPath}for#[4/5] nochange: 1...
INFO: ---{shortcutPath}X, path: [0/6](-0.56000, 0.79000, -0.44999),[1/6](0.18506, 1.26706, -0.93547),[2/6](0.20195, 1.22332, -0.90777),[3/6](0.20013, 1.00542, -0.75281),[4/6](0.11446, 0.57502, -0.43055),[5/6](-0.00000, 0, -0.00000), result: true

--执行第三次shortcutPath
INFO: {shortcutPath}---path: [0/7](-0.56000, 0.79000, -0.44999),[1/7](0.18506, 1.26706, -0.93547),[2/7](0.20195, 1.22332, -0.90777),[3/7](0.20013, 1.00542, -0.75281),[4/7](0.11446, 0.57502, -0.43055),[5/7](0.06301, 0.31656, -0.23703),[6/7](-0.00000, 0, -0.00000)
INFO: {PathSimplifier::shortcutPath}maxSteps: 7, maxEmptySteps: 7, rangeRatio: 0.33000, snapToVertex: 0.00500
INFO: {shortcutPath}pre for costs: (0, 1.70761, 1.79594, 2.17062, 3.00896, 3.51239, 4.12899), dists: (0, 1.70761, 1.79594, 2.17062, 3.00896, 3.51239, 4.12899)
INFO: {shortcutPath}for#[0/7] nochange: 0...
INFO: {shortcutPath}for#[1/7] nochange: 1...
INFO: {shortcutPath}[1/7]s0{3, 3, 0, (0.20013, 1.00542, -0.75281)}, s1{4, -1, 0.42225, (0.09273, 0.46588, -0.34883)}
INFO: {shortcutPath}for#[2/7] nochange: 2...
INFO: {shortcutPath}[2/7]s0{2, -1, 0.35637, (0.20131, 1.14566, -0.85254)}, s1{3, -1, 0.25023, (0.17869, 0.89771, -0.67217)}
INFO: {shortcutPath}[2/7]post a valid, path: [0/8](-0.56000, 0.79000, -0.44999),[1/8](0.18506, 1.26706, -0.93547),[2/8](0.20195, 1.22332, -0.90777),[3/8](0.20131, 1.14566, -0.85254),[4/8](0.17869, 0.89771, -0.67217),[5/8](0.11446, 0.57502, -0.43055),[6/8](0.06301, 0.31656, -0.23703),[7/8](-0.00000, 0, -0.00000)
INFO: {shortcutPath}[2/7]post a valid, costs: (0, 1.70761, 1.79594, 1.92947, 2.3804, 3.00896, 3.51239, 4.12899), dists: (0, 1.70761, 1.79594, 1.92947, 2.3804, 3.00896, 3.51239, 4.12899)
INFO: {shortcutPath}for#[3/7] nochange: 1...
INFO: {shortcutPath}[3/7]s0{0, -1, 0.94411, (0.14343, 1.24040, -0.90834)}, s1{3, -1, 0.89425, (0.18109, 0.92393, -0.69124)}
INFO: {shortcutPath}[3/7]post a valid, path: [0/7](-0.56000, 0.79000, -0.44999),[1/7](0.14343, 1.24040, -0.90834),[2/7](0.18109, 0.92393, -0.69124),[3/7](0.17869, 0.89771, -0.67217),[4/7](0.11446, 0.57502, -0.43055),[5/7](0.06301, 0.31656, -0.23703),[6/7](-0.00000, 0, -0.00000)
INFO: {shortcutPath}[3/7]post a valid, costs: (0, 1.61218, 2.18341, 2.23109, 2.85964, 3.36307, 3.97968), dists: (0, 1.61218, 2.18341, 2.23109, 2.85964, 3.36307, 3.97968)
INFO: {shortcutPath}for#[4/7] nochange: 1...
INFO: {shortcutPath}[4/7]s0{1, -1, 0.62914, (0.16712, 1.04130, -0.77175)}, s1{3, -1, 0.84002, (0.12473, 0.62664, -0.46920)}
INFO: {shortcutPath}[4/7]post a valid, path: [0/7](-0.56000, 0.79000, -0.44999),[1/7](0.14343, 1.24040, -0.90834),[2/7](0.16712, 1.04130, -0.77175),[3/7](0.12473, 0.62664, -0.46920),[4/7](0.11446, 0.57502, -0.43055),[5/7](0.06301, 0.31656, -0.23703),[6/7](-0.00000, 0, -0.00000)
INFO: {shortcutPath}[4/7]post a valid, costs: (0, 1.61218, 1.97156, 2.73116, 2.83171, 3.33514, 3.95175), dists: (0, 1.61218, 1.97156, 2.73116, 2.83171, 3.33514, 3.95175)
INFO: {shortcutPath}for#[5/7] nochange: 1...
INFO: {shortcutPath}for#[6/7] nochange: 2...
INFO: ---{shortcutPath}X, path: [0/7](-0.56000, 0.79000, -0.44999),[1/7](0.14343, 1.24040, -0.90834),[2/7](0.16712, 1.04130, -0.77175),[3/7](0.12473, 0.62664, -0.46920),[4/7](0.11446, 0.57502, -0.43055),[5/7](0.06301, 0.31656, -0.23703),[6/7](-0.00000, 0, -0.00000), result: true

--执行第四次shortcutPath
INFO: {shortcutPath}---path: [0/7](-0.56000, 0.79000, -0.44999),[1/7](0.14343, 1.24040, -0.90834),[2/7](0.16712, 1.04130, -0.77175),[3/7](0.12473, 0.62664, -0.46920),[4/7](0.11446, 0.57502, -0.43055),[5/7](0.06301, 0.31656, -0.23703),[6/7](-0.00000, 0, -0.00000)
INFO: {PathSimplifier::shortcutPath}maxSteps: 7, maxEmptySteps: 7, rangeRatio: 0.33000, snapToVertex: 0.00500
INFO: {shortcutPath}pre for costs: (0, 1.61218, 1.97156, 2.73116, 2.83171, 3.33514, 3.95175), dists: (0, 1.61218, 1.97156, 2.73116, 2.83171, 3.33514, 3.95175)
INFO: {shortcutPath}for#[0/7] nochange: 0...
INFO: {shortcutPath}[0/7]s0{1, -1, 0.39677, (0.15283, 1.16140, -0.85415)}, s1{2, -1, 0.51946, (0.14510, 0.82590, -0.61459)}
INFO: {shortcutPath}[0/7]post a valid, path: [0/8](-0.56000, 0.79000, -0.44999),[1/8](0.14343, 1.24040, -0.90834),[2/8](0.15283, 1.16140, -0.85415),[3/8](0.14510, 0.82590, -0.61459),[4/8](0.12473, 0.62664, -0.46920),[5/8](0.11446, 0.57502, -0.43055),[6/8](0.06301, 0.31656, -0.23703),[7/8](-0.00000, 0, -0.00000)
INFO: {shortcutPath}[0/7]post a valid, costs: (0, 1.61218, 1.75478, 2.33756, 2.70258, 2.80313, 3.30656, 3.92316), dists: (0, 1.61218, 1.75478, 2.33756, 2.70258, 2.80313, 3.30656, 3.92316)
INFO: {shortcutPath}for#[1/7] nochange: 1...
INFO: {shortcutPath}[1/7]s0{0, -1, 0.56560, (-0.16213, 1.04475, -0.70924)}, s1{1, -1, 0.48271, (0.14797, 1.20227, -0.88218)}
INFO: {shortcutPath}[1/7]post a valid, path: [0/9](-0.56000, 0.79000, -0.44999),[1/9](-0.16213, 1.04475, -0.70924),[2/9](0.14797, 1.20227, -0.88218),[3/9](0.15283, 1.16140, -0.85415),[4/9](0.14510, 0.82590, -0.61459),[5/9](0.12473, 0.62664, -0.46920),[6/9](0.11446, 0.57502, -0.43055),[7/9](0.06301, 0.31656, -0.23703),[8/9](-0.00000, 0, -0.00000)
INFO: {shortcutPath}[1/7]post a valid, costs: (0, 0.911863, 1.55243, 1.62619, 2.20897, 2.57399, 2.67454, 3.17797, 3.79458), dists: (0, 0.911863, 1.55243, 1.62619, 2.20897, 2.57399, 2.67454, 3.17797, 3.79458)
INFO: {shortcutPath}for#[2/7] nochange: 1...
INFO: {shortcutPath}[2/7]s0{4, -1, 0.87356, (0.12731, 0.65183, -0.48758)}, s1{6, -1, 0.88609, (0.06887, 0.34600, -0.25907)}
INFO: {shortcutPath}[2/7]post a valid, path: [0/9](-0.56000, 0.79000, -0.44999),[1/9](-0.16213, 1.04475, -0.70924),[2/9](0.14797, 1.20227, -0.88218),[3/9](0.15283, 1.16140, -0.85415),[4/9](0.14510, 0.82590, -0.61459),[5/9](0.12731, 0.65183, -0.48758),[6/9](0.06887, 0.34600, -0.25907),[7/9](0.06301, 0.31656, -0.23703),[8/9](-0.00000, 0, -0.00000)
INFO: {shortcutPath}[2/7]post a valid, costs: (0, 0.911863, 1.55243, 1.62619, 2.20897, 2.52784, 3.12063, 3.17797, 3.79458), dists: (0, 0.911863, 1.55243, 1.62619, 2.20897, 2.52784, 3.12063, 3.17797, 3.79458)
INFO: {shortcutPath}for#[3/7] nochange: 1...
INFO: {shortcutPath}[3/7]s0{0, -1, 0.67273, (-0.29234, 0.96138, -0.62440)}, s1{2, 2, 0, (0.14797, 1.20227, -0.88218)}
INFO: {shortcutPath}[3/7]post a valid, path: [0/9](-0.56000, 0.79000, -0.44999),[1/9](-0.29234, 0.96138, -0.62440),[2/9](0.14797, 1.20227, -0.88218),[3/9](0.15283, 1.16140, -0.85415),[4/9](0.14510, 0.82590, -0.61459),[5/9](0.12731, 0.65183, -0.48758),[6/9](0.06887, 0.34600, -0.25907),[7/9](0.06301, 0.31656, -0.23703),[8/9](-0.00000, 0, -0.00000)
INFO: {shortcutPath}[3/7]post a valid, costs: (0, 0.613444, 1.55243, 1.62619, 2.20897, 2.52784, 3.12063, 3.17797, 3.79458), dists: (0, 0.613444, 1.55243, 1.62619, 2.20897, 2.52784, 3.12063, 3.17797, 3.79458)
INFO: {shortcutPath}for#[4/7] nochange: 1...
INFO: {shortcutPath}for#[5/7] nochange: 2...
INFO: {shortcutPath}for#[6/7] nochange: 3...
INFO: {shortcutPath}[6/7]s0{3, -1, 0.64656, (0.14783, 0.94448, -0.69926)}, s1{4, -1, 0.30002, (0.13976, 0.77367, -0.57648)}
INFO: {shortcutPath}[6/7]post a valid, path: [0/10](-0.56000, 0.79000, -0.44999),[1/10](-0.29234, 0.96138, -0.62440),[2/10](0.14797, 1.20227, -0.88218),[3/10](0.15283, 1.16140, -0.85415),[4/10](0.14783, 0.94448, -0.69926),[5/10](0.13976, 0.77367, -0.57648),[6/10](0.12731, 0.65183, -0.48758),[7/10](0.06887, 0.34600, -0.25907),[8/10](0.06301, 0.31656, -0.23703),[9/10](-0.00000, 0, -0.00000)
INFO: {shortcutPath}[6/7]post a valid, costs: (0, 0.613444, 1.55243, 1.62619, 2.003, 2.30464, 2.52784, 3.12063, 3.17797, 3.79458), dists: (0, 0.613444, 1.55243, 1.62619, 2.003, 2.30464, 2.52784, 3.12063, 3.17797, 3.79458)
INFO: ---{shortcutPath}X, path: [0/10](-0.56000, 0.79000, -0.44999),[1/10](-0.29234, 0.96138, -0.62440),[2/10](0.14797, 1.20227, -0.88218),[3/10](0.15283, 1.16140, -0.85415),[4/10](0.14783, 0.94448, -0.69926),[5/10](0.13976, 0.77367, -0.57648),[6/10](0.12731, 0.65183, -0.48758),[7/10](0.06887, 0.34600, -0.25907),[8/10](0.06301, 0.31656, -0.23703),[9/10](-0.00000, 0, -0.00000), result: true

--执行第五次shortcutPath
INFO: {shortcutPath}---path: [0/10](-0.56000, 0.79000, -0.44999),[1/10](-0.29234, 0.96138, -0.62440),[2/10](0.14797, 1.20227, -0.88218),[3/10](0.15283, 1.16140, -0.85415),[4/10](0.14783, 0.94448, -0.69926),[5/10](0.13976, 0.77367, -0.57648),[6/10](0.12731, 0.65183, -0.48758),[7/10](0.06887, 0.34600, -0.25907),[8/10](0.06301, 0.31656, -0.23703),[9/10](-0.00000, 0, -0.00000)
INFO: {PathSimplifier::shortcutPath}maxSteps: 10, maxEmptySteps: 10, rangeRatio: 0.33000, snapToVertex: 0.00500
INFO: {shortcutPath}pre for costs: (0, 0.613444, 1.55243, 1.62619, 2.003, 2.30464, 2.52784, 3.12063, 3.17797, 3.79458), dists: (0, 0.613444, 1.55243, 1.62619, 2.003, 2.30464, 2.52784, 3.12063, 3.17797, 3.79458)
INFO: {shortcutPath}for#[0/10] nochange: 0...
INFO: {shortcutPath}[0/10]s0{6, -1, 0.40334, (0.10374, 0.52848, -0.39541)}, s1{7, -1, 0.66119, (0.06499, 0.32653, -0.24450)}
INFO: {shortcutPath}[0/10]post a valid, path: [0/11](-0.56000, 0.79000, -0.44999),[1/11](-0.29234, 0.96138, -0.62440),[2/11](0.14797, 1.20227, -0.88218),[3/11](0.15283, 1.16140, -0.85415),[4/11](0.14783, 0.94448, -0.69926),[5/11](0.13976, 0.77367, -0.57648),[6/11](0.12731, 0.65183, -0.48758),[7/11](0.10374, 0.52848, -0.39541),[8/11](0.06499, 0.32653, -0.24450),[9/11](0.06301, 0.31656, -0.23703),[10/11](-0.00000, 0, -0.00000)
INFO: {shortcutPath}[0/10]post a valid, costs: (0, 0.613444, 1.55243, 1.62619, 2.003, 2.30464, 2.52784, 2.76694, 3.15854, 3.17797, 3.79458), dists: (0, 0.613444, 1.55243, 1.62619, 2.003, 2.30464, 2.52784, 2.76694, 3.15854, 3.17797, 3.79458)
INFO: {shortcutPath}for#[1/10] nochange: 1...
INFO: {shortcutPath}for#[2/10] nochange: 2...
INFO: {shortcutPath}[2/10]s0{0, -1, 0.08479, (-0.53730, 0.80453, -0.46478)}, s1{1, -1, 0.69363, (0.01307, 1.12847, -0.80320)}
INFO: {shortcutPath}[2/10]post a valid, path: [0/12](-0.56000, 0.79000, -0.44999),[1/12](-0.53730, 0.80453, -0.46478),[2/12](0.01307, 1.12847, -0.80320),[3/12](0.14797, 1.20227, -0.88218),[4/12](0.15283, 1.16140, -0.85415),[5/12](0.14783, 0.94448, -0.69926),[6/12](0.13976, 0.77367, -0.57648),[7/12](0.12731, 0.65183, -0.48758),[8/12](0.10374, 0.52848, -0.39541),[9/12](0.06499, 0.32653, -0.24450),[10/12](0.06301, 0.31656, -0.23703),[11/12](-0.00000, 0, -0.00000)
INFO: {shortcutPath}[2/10]post a valid, costs: (0, 0.0520145, 1.26476, 1.55243, 1.62619, 2.003, 2.30464, 2.52784, 2.76694, 3.15854, 3.17797, 3.79458), dists: (0, 0.0520145, 1.26476, 1.55243, 1.62619, 2.003, 2.30464, 2.52784, 2.76694, 3.15854, 3.17797, 3.79458)
INFO: {shortcutPath}for#[3/10] nochange: 1...
INFO: {shortcutPath}[3/10]s0{7, -1, 0.82997, (0.10775, 0.54945, -0.41109)}, s1{10, -1, 0.96691, (0.00208, 0.01047, -0.00784)}
INFO: {shortcutPath}[3/10]post a valid, path: [0/11](-0.56000, 0.79000, -0.44999),[1/11](-0.53730, 0.80453, -0.46478),[2/11](0.01307, 1.12847, -0.80320),[3/11](0.14797, 1.20227, -0.88218),[4/11](0.15283, 1.16140, -0.85415),[5/11](0.14783, 0.94448, -0.69926),[6/11](0.13976, 0.77367, -0.57648),[7/11](0.12731, 0.65183, -0.48758),[8/11](0.10775, 0.54945, -0.41109),[9/11](0.00208, 0.01047, -0.00784),[10/11](-0.00000, 0, -0.00000)
INFO: {shortcutPath}[3/10]post a valid, costs: (0, 0.0520145, 1.26476, 1.55243, 1.62619, 2.003, 2.30464, 2.52784, 2.72628, 3.77417, 3.79458), dists: (0, 0.0520145, 1.26476, 1.55243, 1.62619, 2.003, 2.30464, 2.52784, 2.72628, 3.77417, 3.79458)
INFO: {shortcutPath}for#[4/10] nochange: 1...
INFO: {shortcutPath}for#[5/10] nochange: 2...
INFO: {shortcutPath}[5/10]s0{1, -1, 0.36535, (-0.33622, 0.92288, -0.58843)}, s1{2, -1, 0.31488, (0.05555, 1.15171, -0.82807)}
INFO: {shortcutPath}[5/10]post a valid, path: [0/12](-0.56000, 0.79000, -0.44999),[1/12](-0.53730, 0.80453, -0.46478),[2/12](-0.33622, 0.92288, -0.58843),[3/12](0.05555, 1.15171, -0.82807),[4/12](0.14797, 1.20227, -0.88218),[5/12](0.15283, 1.16140, -0.85415),[6/12](0.14783, 0.94448, -0.69926),[7/12](0.13976, 0.77367, -0.57648),[8/12](0.12731, 0.65183, -0.48758),[9/12](0.10775, 0.54945, -0.41109),[10/12](0.00208, 0.01047, -0.00784),[11/12](-0.00000, 0, -0.00000)
INFO: {shortcutPath}[5/10]post a valid, costs: (0, 0.0520145, 0.495092, 1.35534, 1.55243, 1.62619, 2.003, 2.30464, 2.52784, 2.72628, 3.77417, 3.79458), dists: (0, 0.0520145, 0.495092, 1.35534, 1.55243, 1.62619, 2.003, 2.30464, 2.52784, 2.72628, 3.77417, 3.79458)
INFO: {shortcutPath}for#[6/10] nochange: 1...
INFO: {shortcutPath}[6/10]s0{6, -1, 0.26876, (0.14566, 0.89857, -0.66626)}, s1{7, -1, 0.21747, (0.13706, 0.74718, -0.55715)}
INFO: {shortcutPath}[6/10]post a valid, path: [0/13](-0.56000, 0.79000, -0.44999),[1/13](-0.53730, 0.80453, -0.46478),[2/13](-0.33622, 0.92288, -0.58843),[3/13](0.05555, 1.15171, -0.82807),[4/13](0.14797, 1.20227, -0.88218),[5/13](0.15283, 1.16140, -0.85415),[6/13](0.14783, 0.94448, -0.69926),[7/13](0.14566, 0.89857, -0.66626),[8/13](0.13706, 0.74718, -0.55715),[9/13](0.12731, 0.65183, -0.48758),[10/13](0.10775, 0.54945, -0.41109),[11/13](0.00208, 0.01047, -0.00784),[12/13](-0.00000, 0, -0.00000)
INFO: {shortcutPath}[6/10]post a valid, costs: (0, 0.0520145, 0.495092, 1.35534, 1.55243, 1.62619, 2.003, 2.08407, 2.35318, 2.52784, 2.72628, 3.77417, 3.79458), dists: (0, 0.0520145, 0.495092, 1.35534, 1.55243, 1.62619, 2.003, 2.08407, 2.35318, 2.52784, 2.72628, 3.77417, 3.79458)
INFO: {shortcutPath}for#[7/10] nochange: 1...
INFO: {shortcutPath}for#[8/10] nochange: 2...
INFO: {shortcutPath}[8/10]s0{1, -1, 0.32923, (-0.47110, 0.84349, -0.50549)}, s1{2, -1, 0.84696, (-0.00440, 1.11669, -0.79140)}
INFO: {shortcutPath}[8/10]post a valid, path: [0/14](-0.56000, 0.79000, -0.44999),[1/14](-0.53730, 0.80453, -0.46478),[2/14](-0.47110, 0.84349, -0.50549),[3/14](-0.00440, 1.11669, -0.79140),[4/14](0.05555, 1.15171, -0.82807),[5/14](0.14797, 1.20227, -0.88218),[6/14](0.15283, 1.16140, -0.85415),[7/14](0.14783, 0.94448, -0.69926),[8/14](0.14566, 0.89857, -0.66626),[9/14](0.13706, 0.74718, -0.55715),[10/14](0.12731, 0.65183, -0.48758),[11/14](0.10775, 0.54945, -0.41109),[12/14](0.00208, 0.01047, -0.00784),[13/14](-0.00000, 0, -0.00000)
INFO: {shortcutPath}[8/10]post a valid, costs: (0, 0.0520145, 0.19789, 1.22369, 1.35534, 1.55243, 1.62619, 2.003, 2.08407, 2.35318, 2.52784, 2.72628, 3.77417, 3.79458), dists: (0, 0.0520145, 0.19789, 1.22369, 1.35534, 1.55243, 1.62619, 2.003, 2.08407, 2.35318, 2.52784, 2.72628, 3.77417, 3.79458)
INFO: {shortcutPath}for#[9/10] nochange: 1...
INFO: {shortcutPath}[9/10]s0{1, -1, 0.46165, (-0.50674, 0.82252, -0.48358)}, s1{2, -1, 0.61720, (-0.18305, 1.01211, -0.68195)}
INFO: ---{shortcutPath}X, path: [0/14](-0.56000, 0.79000, -0.44999),[1/14](-0.53730, 0.80453, -0.46478),[2/14](-0.47110, 0.84349, -0.50549),[3/14](-0.00440, 1.11669, -0.79140),[4/14](0.05555, 1.15171, -0.82807),[5/14](0.14797, 1.20227, -0.88218),[6/14](0.15283, 1.16140, -0.85415),[7/14](0.14783, 0.94448, -0.69926),[8/14](0.14566, 0.89857, -0.66626),[9/14](0.13706, 0.74718, -0.55715),[10/14](0.12731, 0.65183, -0.48758),[11/14](0.10775, 0.54945, -0.41109),[12/14](0.00208, 0.01047, -0.00784),[13/14](-0.00000, 0, -0.00000), result: true

--执行第六次shortcutPath
INFO: {shortcutPath}---path: [0/14](-0.56000, 0.79000, -0.44999),[1/14](-0.53730, 0.80453, -0.46478),[2/14](-0.47110, 0.84349, -0.50549),[3/14](-0.00440, 1.11669, -0.79140),[4/14](0.05555, 1.15171, -0.82807),[5/14](0.14797, 1.20227, -0.88218),[6/14](0.15283, 1.16140, -0.85415),[7/14](0.14783, 0.94448, -0.69926),[8/14](0.14566, 0.89857, -0.66626),[9/14](0.13706, 0.74718, -0.55715),[10/14](0.12731, 0.65183, -0.48758),[11/14](0.10775, 0.54945, -0.41109),[12/14](0.00208, 0.01047, -0.00784),[13/14](-0.00000, 0, -0.00000)
INFO: {PathSimplifier::shortcutPath}maxSteps: 14, maxEmptySteps: 14, rangeRatio: 0.33000, snapToVertex: 0.00500
INFO: {shortcutPath}pre for costs: (0, 0.0520145, 0.19789, 1.22369, 1.35534, 1.55243, 1.62619, 2.003, 2.08407, 2.35318, 2.52784, 2.72628, 3.77417, 3.79458), dists: (0, 0.0520145, 0.19789, 1.22369, 1.35534, 1.55243, 1.62619, 2.003, 2.08407, 2.35318, 2.52784, 2.72628, 3.77417, 3.79458)
INFO: {shortcutPath}for#[0/14] nochange: 0...
INFO: {shortcutPath}[0/14]s0{6, -1, 0.25567, (0.15155, 1.10594, -0.81454)}, s1{9, -1, 0.66899, (0.13054, 0.68339, -0.51061)}
INFO: {shortcutPath}[0/14]post a valid, path: [0/13](-0.56000, 0.79000, -0.44999),[1/13](-0.53730, 0.80453, -0.46478),[2/13](-0.47110, 0.84349, -0.50549),[3/13](-0.00440, 1.11669, -0.79140),[4/13](0.05555, 1.15171, -0.82807),[5/13](0.14797, 1.20227, -0.88218),[6/13](0.15283, 1.16140, -0.85415),[7/13](0.15155, 1.10594, -0.81454),[8/13](0.13054, 0.68339, -0.51061),[9/13](0.12731, 0.65183, -0.48758),[10/13](0.10775, 0.54945, -0.41109),[11/13](0.00208, 0.01047, -0.00784),[12/13](-0.00000, 0, -0.00000)
INFO: {shortcutPath}[0/14]post a valid, costs: (0, 0.0520145, 0.19789, 1.22369, 1.35534, 1.55243, 1.62619, 1.72253, 2.47003, 2.52784, 2.72628, 3.77417, 3.79458), dists: (0, 0.0520145, 0.19789, 1.22369, 1.35534, 1.55243, 1.62619, 1.72253, 2.47003, 2.52784, 2.72628, 3.77417, 3.79458)
INFO: {shortcutPath}for#[1/14] nochange: 1...
INFO: {shortcutPath}for#[2/14] nochange: 2...
INFO: {shortcutPath}for#[3/14] nochange: 3...
INFO: {shortcutPath}for#[4/14] nochange: 4...
INFO: {shortcutPath}for#[5/14] nochange: 5...
INFO: {shortcutPath}[5/14]s0{6, -1, 0.41867, (0.15229, 1.13818, -0.83757)}, s1{10, -1, 0.04668, (0.10281, 0.52429, -0.39226)}
INFO: {shortcutPath}[5/14]post a valid, path: [0/11](-0.56000, 0.79000, -0.44999),[1/11](-0.53730, 0.80453, -0.46478),[2/11](-0.47110, 0.84349, -0.50549),[3/11](-0.00440, 1.11669, -0.79140),[4/11](0.05555, 1.15171, -0.82807),[5/11](0.14797, 1.20227, -0.88218),[6/11](0.15283, 1.16140, -0.85415),[7/11](0.15229, 1.13818, -0.83757),[8/11](0.10281, 0.52429, -0.39226),[9/11](0.00208, 0.01047, -0.00784),[10/11](-0.00000, 0, -0.00000)
INFO: {shortcutPath}[5/14]post a valid, costs: (0, 0.0520145, 0.19789, 1.22369, 1.35534, 1.55243, 1.62619, 1.66652, 2.77521, 3.77417, 3.79458), dists: (0, 0.0520145, 0.19789, 1.22369, 1.35534, 1.55243, 1.62619, 1.66652, 2.77521, 3.77417, 3.79458)
INFO: {shortcutPath}for#[6/14] nochange: 1...
INFO: {shortcutPath}for#[7/14] nochange: 2...
INFO: {shortcutPath}[7/14]s0{2, -1, 0.42203, (-0.27413, 0.95879, -0.62615)}, s1{6, 6, 0, (0.15283, 1.16140, -0.85415)}
INFO: {shortcutPath}[7/14]post a valid, path: [0/9](-0.56000, 0.79000, -0.44999),[1/9](-0.53730, 0.80453, -0.46478),[2/9](-0.47110, 0.84349, -0.50549),[3/9](-0.27413, 0.95879, -0.62615),[4/9](0.15283, 1.16140, -0.85415),[5/9](0.15229, 1.13818, -0.83757),[6/9](0.10281, 0.52429, -0.39226),[7/9](0.00208, 0.01047, -0.00784),[8/9](-0.00000, 0, -0.00000)
INFO: {shortcutPath}[7/14]post a valid, costs: (0, 0.0520145, 0.19789, 0.630818, 1.48839, 1.52873, 2.63741, 3.63637, 3.65678), dists: (0, 0.0520145, 0.19789, 0.630818, 1.48839, 1.52873, 2.63741, 3.63637, 3.65678)
INFO: {shortcutPath}for#[8/14] nochange: 1...
INFO: {shortcutPath}for#[9/14] nochange: 2...
INFO: {shortcutPath}[9/14]s0{2, -1, 0.26161, (-0.41957, 0.87366, -0.53706)}, s1{3, -1, 0.63236, (-0.00413, 1.08692, -0.77033)}
INFO: {shortcutPath}[9/14]post a valid, path: [0/10](-0.56000, 0.79000, -0.44999),[1/10](-0.53730, 0.80453, -0.46478),[2/10](-0.47110, 0.84349, -0.50549),[3/10](-0.41957, 0.87366, -0.53706),[4/10](-0.00413, 1.08692, -0.77033),[5/10](0.15283, 1.16140, -0.85415),[6/10](0.15229, 1.13818, -0.83757),[7/10](0.10281, 0.52429, -0.39226),[8/10](0.00208, 0.01047, -0.00784),[9/10](-0.00000, 0, -0.00000)
INFO: {shortcutPath}[9/14]post a valid, costs: (0, 0.0520145, 0.19789, 0.311151, 1.17312, 1.48839, 1.52873, 2.63741, 3.63637, 3.65678), dists: (0, 0.0520145, 0.19789, 0.311151, 1.17312, 1.48839, 1.52873, 2.63741, 3.63637, 3.65678)
INFO: {shortcutPath}for#[10/14] nochange: 1...
INFO: {shortcutPath}[10/14]s0{4, -1, 0.22625, (0.03137, 1.10377, -0.78929)}, s1{6, -1, 0.19222, (0.14278, 1.02018, -0.75197)}
INFO: {shortcutPath}[10/14]post a valid, path: [0/10](-0.56000, 0.79000, -0.44999),[1/10](-0.53730, 0.80453, -0.46478),[2/10](-0.47110, 0.84349, -0.50549),[3/10](-0.41957, 0.87366, -0.53706),[4/10](-0.00413, 1.08692, -0.77033),[5/10](0.03137, 1.10377, -0.78929),[6/10](0.14278, 1.02018, -0.75197),[7/10](0.10281, 0.52429, -0.39226),[8/10](0.00208, 0.01047, -0.00784),[9/10](-0.00000, 0, -0.00000)
INFO: {shortcutPath}[10/14]post a valid, costs: (0, 0.0520145, 0.19789, 0.311151, 1.17312, 1.24445, 1.47677, 2.37234, 3.37131, 3.39171), dists: (0, 0.0520145, 0.19789, 0.311151, 1.17312, 1.24445, 1.47677, 2.37234, 3.37131, 3.39171)
INFO: {shortcutPath}for#[11/14] nochange: 1...
INFO: {shortcutPath}[11/14]s0{5, -1, 0.91746, (0.13359, 1.02708, -0.75505)}, s1{6, -1, 0.60220, (0.11871, 0.72155, -0.53535)}
INFO: {shortcutPath}[11/14]post a valid, path: [0/11](-0.56000, 0.79000, -0.44999),[1/11](-0.53730, 0.80453, -0.46478),[2/11](-0.47110, 0.84349, -0.50549),[3/11](-0.41957, 0.87366, -0.53706),[4/11](-0.00413, 1.08692, -0.77033),[5/11](0.03137, 1.10377, -0.78929),[6/11](0.13359, 1.02708, -0.75505),[7/11](0.11871, 0.72155, -0.53535),[8/11](0.10281, 0.52429, -0.39226),[9/11](0.00208, 0.01047, -0.00784),[10/11](-0.00000, 0, -0.00000)
INFO: {shortcutPath}[11/14]post a valid, costs: (0, 0.0520145, 0.19789, 0.311151, 1.17312, 1.24445, 1.4576, 1.9977, 2.35395, 3.35292, 3.37332), dists: (0, 0.0520145, 0.19789, 0.311151, 1.17312, 1.24445, 1.4576, 1.9977, 2.35395, 3.35292, 3.37332)
INFO: {shortcutPath}for#[12/14] nochange: 1...
INFO: {shortcutPath}for#[13/14] nochange: 2...
INFO: {shortcutPath}[13/14]s0{1, -1, 0.43011, (-0.50882, 0.82129, -0.48229)}, s1{3, -1, 0.30705, (-0.29201, 0.93914, -0.60868)}
INFO: {shortcutPath}[13/14]post a valid, path: [0/11](-0.56000, 0.79000, -0.44999),[1/11](-0.53730, 0.80453, -0.46478),[2/11](-0.50882, 0.82129, -0.48229),[3/11](-0.29201, 0.93914, -0.60868),[4/11](-0.00413, 1.08692, -0.77033),[5/11](0.03137, 1.10377, -0.78929),[6/11](0.13359, 1.02708, -0.75505),[7/11](0.11871, 0.72155, -0.53535),[8/11](0.10281, 0.52429, -0.39226),[9/11](0.00208, 0.01047, -0.00784),[10/11](-0.00000, 0, -0.00000)
INFO: {shortcutPath}[13/14]post a valid, costs: (0, 0.0520145, 0.114758, 0.575819, 1.17312, 1.24445, 1.4576, 1.9977, 2.35395, 3.35292, 3.37332), dists: (0, 0.0520145, 0.114758, 0.575819, 1.17312, 1.24445, 1.4576, 1.9977, 2.35395, 3.35292, 3.37332)
INFO: ---{shortcutPath}X, path: [0/11](-0.56000, 0.79000, -0.44999),[1/11](-0.53730, 0.80453, -0.46478),[2/11](-0.50882, 0.82129, -0.48229),[3/11](-0.29201, 0.93914, -0.60868),[4/11](-0.00413, 1.08692, -0.77033),[5/11](0.03137, 1.10377, -0.78929),[6/11](0.13359, 1.02708, -0.75505),[7/11](0.11871, 0.72155, -0.53535),[8/11](0.10281, 0.52429, -0.39226),[9/11](0.00208, 0.01047, -0.00784),[10/11](-0.00000, 0, -0.00000), result: true

--结束了执行shortcutPath的while循环，path_增加到11个路点。关节b最大值降到了1.10377。
INFO: {PathSimplifier::simplify}while[1/5], path: [0/11](-0.56000, 0.79000, -0.44999),[1/11](-0.53730, 0.80453, -0.46478),[2/11](-0.50882, 0.82129, -0.48229),[3/11](-0.29201, 0.93914, -0.60868),[4/11](-0.00413, 1.08692, -0.77033),[5/11](0.03137, 1.10377, -0.78929),[6/11](0.13359, 1.02708, -0.75505),[7/11](0.11871, 0.72155, -0.53535),[8/11](0.10281, 0.52429, -0.39226),[9/11](0.00208, 0.01047, -0.00784),[10/11](-0.00000, 0, -0.00000)

INFO: {PathSimplifier::simplify}while[2/5], path: [0/41](-0.56000, 0.79000, -0.44999),[1/41](-0.55432, 0.79363, -0.45369),[2/41](-0.54865, 0.79726, -0.45739),[3/41](-0.54297, 0.80089, -0.46109),[4/41](-0.53730, 0.80453, -0.46478),[5/41](-0.53018, 0.80872, -0.46916),[6/41](-0.52306, 0.81291, -0.47354),[7/41](-0.50417, 0.82341, -0.48472),[8/41](-0.48528, 0.83392, -0.49590),[9/41](-0.44285, 0.85707, -0.52069),[10/41](-0.40042, 0.88021, -0.54549),[11/41](-0.34621, 0.90968, -0.57709),[12/41](-0.29201, 0.93914, -0.60868),[13/41](-0.22004, 0.97608, -0.64909),[14/41](-0.14807, 1.01303, -0.68951),[15/41](-0.09187, 1.04179, -0.72100),[16/41](-0.03568, 1.07055, -0.75249),[17/41](-0.01102, 1.08295, -0.76615),[18/41](0.01362, 1.09534, -0.77981),[19/41](0.02250, 1.09956, -0.78455),[20/41](0.03137, 1.10377, -0.78929),[21/41](0.05693, 1.08460, -0.78073),[22/41](0.08248, 1.06542, -0.77217),[23/41](0.10072, 1.03195, -0.75202),[24/41](0.11895, 0.99847, -0.73187),[25/41](0.12255, 0.93639, -0.68853),[26/41](0.12615, 0.87431, -0.64520),[27/41](0.12243, 0.79793, -0.59027),[28/41](0.11871, 0.72155, -0.53535),[29/41](0.11474, 0.67223, -0.49958),[30/41](0.11076, 0.62292, -0.46380),[31/41](0.10148, 0.55382, -0.41295),[32/41](0.09221, 0.48472, -0.36209),[33/41](0.07233, 0.37605, -0.28107),[34/41](0.05244, 0.26738, -0.20005),[35/41](0.03343, 0.17038, -0.12748),[36/41](0.01441, 0.07339, -0.05492),[37/41](0.00772, 0.03931, -0.02942),[38/41](0.00104, 0.00523, -0.00392),[39/41](0.00051, 0.00261, -0.00196),[40/41](-0.00000, 0, -0.00000)

--结束了smoothBSpline。经过smoothBSpline，path_路点增加到41个。执行此轮top-while的第一次reduceVertices。
INFO: {reduceVertices}---path.size(): 41
--初始点path_[0]可移动到目标点path_[40]，于是path_只留初始、目标两个路点就可以了。
INFO: {reduceVertices}si->checkMotion(...) true, path: [0/2](-0.56000, 0.79000, -0.44999),[1/2](-0.00000, 0, -0.00000)
INFO: ---{reduceVertices}X, path: [0/2](-0.56000, 0.79000, -0.44999),[1/2](-0.00000, 0, -0.00000), result: true

--经过第一次reduceVertices，path_就只剩初始、目标两个路点了。
INFO: {PathSimplifier::simplify}while[3/5], path: [0/2](-0.56000, 0.79000, -0.44999),[1/2](-0.00000, 0, -0.00000)
INFO: {PathSimplifier::simplify}while[4/5], path: [0/2](-0.56000, 0.79000, -0.44999),[1/2](-0.00000, 0, -0.00000)

--执行此轮top-while的第二次reduceVertices。由于path.getStateCount() < 3，此次reduceVertices啥也不干。
INFO: {reduceVertices}---path.size(): 2

INFO: {PathSimplifier::simplify}while[5/5], path: [0/2](-0.56000, 0.79000, -0.44999),[1/2](-0.00000, 0, -0.00000)
INFO: ---{PathSimplifier::simplify}X, path: [0/2](-0.56000, 0.79000, -0.44999),[1/2](-0.00000, 0, -0.00000)

1.1 PathSimplifier::simplify

功能是让path中路点尽可做到单方向、平滑变化。生成的结果依旧存在path。simplify执行一轮轮迭代，以下是一轮的迭代过程。对不少场景，一轮就够了。

  1. 最多执行六次shortcutPath。这种方法特点：随机定位出曲线中1或2个路点，删除它们，然后用两个能更平滑的路点进行替代。直观过程见图1。
  2. 执行smoothBSpline，在各路点间补插路点。B样条插值是相邻点均值替换原来值，换句话说，之前在path_中的路点值，smoothBSpline后可能会消失。
  3. 此轮的第一次reduceVertices，移除满足条件的路点。
  4. collapseCloseVertices。
  5. 此轮的第二次reduceVertices，移除满足条件的路点。

参数

  • @path[IN, OUT]。作为输入，存储着RRTConnect::solve生成的、需要平滑的路径；作为出输出，存储着平滑后路径。
  • @atLeastOnce[IN]。默认true。函数运行过程中会被修改。
bool ompl::geometric::PathSimplifier::simplify(PathGeometric &path, const base::PlannerTerminationCondition &ptc,
                                               bool atLeastOnce)
{
    SDL_Log("{PathSimplifier::simplify}---path: %s", PathGeometric_DebugString(path).c_str());
    if (path.getStateCount() < 3)
        return true;

    bool tryMore = true, valid = true;
    int my_times = 0;
    while ((ptc == false || atLeastOnce) && tryMore)
    {
        SDL_Log("{PathSimplifier::simplify}while#%i, path: %s", my_times ++, PathGeometric_DebugString(path).c_str());

为和后续内中出现的while作区分，这个while记为top-while。对示例，只需要执行一轮top-while。

        // if the space is metric, we can do some additional smoothing
        if ((ptc == false || atLeastOnce) && si_->getStateSpace()->isMetricSpace())
        {
            bool metricTryMore = true;
            unsigned int times = 0;
            do
            {
                bool shortcut = shortcutPath(path);  // split path segments, not just vertices
图1 shortcutPath

第一种平滑路径方法：shortcutPath。这种方法特点：随机定位出曲线中1或2个路点，删除它们，然后用两个能更平滑的路点进行替代。当删除是一个、增加的是两个时，path_路点数会加1。这种算法有个形像叫法：去凸点。图1中经过两次去凸点后，凸点1.31634被向下拉，曲线也就变平滑了。

                bool better_goal =
                    gsr_ ? findBetterGoal(path, ptc) : false;  // Try to connect the path to a closer goal

                metricTryMore = shortcut || better_goal;
            } while ((ptc == false || atLeastOnce) && ++times <= 5 && metricTryMore);

times值0、1、2、3、4、5时，都会进入while，因而对每轮top-while，最多会执行6次shortcutPath。示例经过shortcutPath后，path_增加到11个路点。关节b最大值下拉到了1.10377。

            SDL_Log("{PathSimplifier::simplify}while[1/5], path: %s", PathGeometric_DebugString(path).c_str());

            // smooth the path with BSpline interpolation
            if (ptc == false || atLeastOnce)
                smoothBSpline(path, 3, path.length() / 100.0);

第二种平滑路径方法：smoothBSpline。它在各路点间补插路点。但这个补充的密度并不高。示例经过smoothBSpline，path_路点增加到41个。B样条插值是相邻点均值替换原来值，换句话说，之前在path_中的路点值，smoothBSpline后可能会消失。

            if (ptc == false || atLeastOnce)
            {
                // we always run this if the metric-space algorithms were run.  In non-metric spaces this does not work.
                const std::pair<bool, bool> &p = path.checkAndRepair(magic::MAX_VALID_SAMPLE_ATTEMPTS);
                if (!p.second)
                {
                    valid = false;
                    OMPL_WARN("Solution path may slightly touch on an invalid region of the state space");
                }
                else if (!p.first)
                    OMPL_DEBUG("The solution path was slightly touching on an invalid region of the state space, but "
                               "it was "
                               "successfully fixed.");
            }
        }

        SDL_Log("{PathSimplifier::simplify}while[2/5], path: %s", PathGeometric_DebugString(path).c_str());

        // try a randomized step of connecting vertices
        if (ptc == false || atLeastOnce)
            tryMore = reduceVertices(path);

第三种平滑路径方法：reduceVertices。它要移除满足条件的路点。举个例子，存在、、，如果可以运动到，那意味着可以移除。作为特例，如果、就是可以移动的，那reduceVertices后，path_留这两个路点就行了，示例就属于这种情况。

        SDL_Log("{PathSimplifier::simplify}while[3/5], path: %s", PathGeometric_DebugString(path).c_str());

        // try to collapse close-by vertices
        if (ptc == false || atLeastOnce)
            collapseCloseVertices(path);

        SDL_Log("{PathSimplifier::simplify}while[4/5], path: %s", PathGeometric_DebugString(path).c_str());
        // try to reduce verices some more, if there is any point in doing so
        unsigned int times = 0;
        while ((ptc == false || atLeastOnce) && tryMore && ++times <= 5)
            tryMore = reduceVertices(path);

        if ((ptc == false || atLeastOnce) && si_->getStateSpace()->isMetricSpace())
        {
            SDL_Log("{PathSimplifier::simplify}while[5/5], path: %s", PathGeometric_DebugString(path).c_str());
            // we always run this if the metric-space algorithms were run.  In non-metric spaces this does not work.
            const std::pair<bool, bool> &p = path.checkAndRepair(magic::MAX_VALID_SAMPLE_ATTEMPTS);
            if (!p.second)
            {
                valid = false;
                OMPL_WARN("Solution path may slightly touch on an invalid region of the state space");
            }
            else if (!p.first)
                OMPL_DEBUG("The solution path was slightly touching on an invalid region of the state space, but it "
                           "was "
                           "successfully fixed.");
        }

        atLeastOnce = false;
    }
    SDL_Log("---{PathSimplifier::simplify}X, path: %s", PathGeometric_DebugString(path).c_str());
    return valid || path.check();
}

1.2 PathSimplifier::shortcutPath

拆分路径段，而不仅仅是顶点。目的：让分段更平滑。

  • @path[IN, OUT]。要拆分路径段。
  • @maxSteps。默认值0。
bool ompl::geometric::PathSimplifier::shortcutPath(PathGeometric &path, unsigned int maxSteps,
                                                   unsigned int maxEmptySteps, double rangeRatio, double snapToVertex)
{
    SDL_Log("{shortcutPath}---path: %s", PathGeometric_DebugString(path).c_str());
    if (path.getStateCount() < 3)
        return false;

    if (maxSteps == 0)
        maxSteps = path.getStateCount();

    if (maxEmptySteps == 0)
        maxEmptySteps = path.getStateCount();

    const base::SpaceInformationPtr &si = path.getSpaceInformation();
    std::vector<base::State *> &states = path.getStates();

    SDL_Log("{PathSimplifier::shortcutPath}maxSteps: %i, maxEmptySteps: %i, rangeRatio: %.5f, snapToVertex: %.5f", 
        (int)maxSteps, (int)maxEmptySteps, rangeRatio, snapToVertex);

    // costs[i] contains the cumulative cost of the path up to and including state i
    std::vector<base::Cost> costs(states.size(), obj_->identityCost());
    std::vector<double> dists(states.size(), 0.0);
    for (unsigned int i = 1; i < costs.size(); ++i)
    {
        costs[i] = obj_->combineCosts(costs[i - 1], obj_->motionCost(states[i - 1], states[i]));
        dists[i] = dists[i - 1] + si->distance(states[i - 1], states[i]);

motionCost用于计算机器人从states[i - 1]运动到states[i]的成本。如果没有自提供计算方法，像示例，用的就是计算distance的公式1。

对那些没提提供motionCost私有方法的，motionCost就是distance，因而costs和dists有着一样值。

    }
    
    // Sampled states closer than 'threshold' distance to any existing state in the path
    // are snapped to the close state
    double threshold = dists.back() * snapToVertex;

采样状态接近路径中任何现有状态的“阈值”距离将被捕捉到接近状态

    // The range (distance) of a single connection that will be attempted
    double rd = rangeRatio * dists.back();

    SDL_Log("{shortcutPath}pre for costs: %s, dists: %s", vector_Cost_DebugString(costs).c_str(), vector_double_DebugString(dists).c_str());

    base::State *temp0 = si->allocState();
    base::State *temp1 = si->allocState();
    bool result = false;
    unsigned int nochange = 0;
    // Attempt shortcutting maxSteps times or when no improvement is found after
    // maxEmptySteps attempts, whichever comes first
    for (unsigned int i = 0; i < maxSteps && nochange < maxEmptySteps; ++i, ++nochange)
    {
        SDL_Log("{shortcutPath}for#[%i/%i] nochange: %i...", (int)i, (int)maxSteps, (int)nochange);

        // Sample a random point anywhere along the path
        base::State *s0 = nullptr;
        int index0 = -1;
        double t0 = 0.0;
        double distTo0 = rng_.uniformReal(0.0, dists.back());  // sample a random point (p0) along the path
        auto pit =
            std::lower_bound(dists.begin(), dists.end(), distTo0);  // find the NEXT waypoint after the random point

pit类型std::vector<double>::iterator，std::lower_bound()作用是在区间内找到第一个大于等于distTo0的值的位置并返回，如果没找到就返回end()位置。

        int pos0 = pit == dists.end() ? dists.size() - 1 :
                                        pit - dists.begin();  // get the index of the NEXT waypoint after the point

        if (pos0 == 0 || dists[pos0] - distTo0 < threshold)  // snap to the NEXT waypoint
            index0 = pos0;
        else
        {
            while (pos0 > 0 && distTo0 < dists[pos0])
                --pos0;
            if (distTo0 - dists[pos0] < threshold)  // snap to the PREVIOUS waypoint
                index0 = pos0;
        }

        // Sample a random point within rd distance of the previously sampled point
        base::State *s1 = nullptr;
        int index1 = -1;
        double t1 = 0.0;
        double distTo1 =
            rng_.uniformReal(std::max(0.0, distTo0 - rd),
                             std::min(distTo0 + rd, dists.back()));   // sample a random point (distTo1) near s0
        pit = std::lower_bound(dists.begin(), dists.end(), distTo1);  // find the NEXT waypoint after the random point
        int pos1 = pit == dists.end() ? dists.size() - 1 :
                                        pit - dists.begin();  // get the index of the NEXT waypoint after the point

        if (pos1 == 0 || dists[pos1] - distTo1 < threshold)  // snap to the NEXT waypoint
            index1 = pos1;
        else
        {
            while (pos1 > 0 && distTo1 < dists[pos1])
                --pos1;
            if (distTo1 - dists[pos1] < threshold)  // snap to the PREVIOUS waypoint
                index1 = pos1;
        }

        // Don't waste time on points that are on the same path segment
        if (pos0 == pos1 || index0 == pos1 || index1 == pos0 || pos0 + 1 == index1 || pos1 + 1 == index0 ||
            (index0 >= 0 && index1 >= 0 && abs(index0 - index1) < 2))

distTo0、distTo1都是随机采的，会出现采的值没法优化情况，称为采空了。如果一次shortcutPath连续采空maxEmptySteps次，那shortcutPath就会返回false。这次flase有可能导致top-while不会再调用shortcutPath。于是会存在这么种情况，第一次shortcutPath连续采空maxEmptySteps次，导致此轮top-while就没让shortcutPath生效过。

            continue;

        // Get the state pointer for costTo0
        if (index0 >= 0)
        {
            s0 = states[index0];
        }
        else
        {
            t0 = (distTo0 - dists[pos0]) / (dists[pos0 + 1] - dists[pos0]);
            si->getStateSpace()->interpolate(states[pos0], states[pos0 + 1], t0, temp0);
            s0 = temp0;
        }

        // Get the state pointer for costTo1
        if (index1 >= 0)
        {
            s1 = states[index1];
        }
        else
        {
            t1 = (distTo1 - dists[pos1]) / (dists[pos1 + 1] - dists[pos1]);
            si->getStateSpace()->interpolate(states[pos1], states[pos1 + 1], t1, temp1);
            s1 = temp1;
        }

        // Check for validity between s0 and s1
        if (si->checkMotion(s0, s1))
        {
            if (pos0 > pos1)
            {
                std::swap(pos0, pos1);
                std::swap(index0, index1);
                std::swap(s0, s1);
                std::swap(t0, t1);
            }
            SDL_Log("{shortcutPath}[%i/%i]s0{%i, %i, %.5f, %s}, s1{%i, %i, %.5f, %s}", 
                (int)i, (int)maxSteps, pos0, index0, t0, get_state_str(s0).c_str(),
                pos1, index1, t1, get_state_str(s1).c_str());

            // Now that states are in the right order, make sure the cost actually decreases.
            base::Cost s0PartialCost = (index0 >= 0) ? obj_->identityCost() : obj_->motionCost(s0, states[pos0 + 1]);
            base::Cost s1PartialCost = (index1 >= 0) ? obj_->identityCost() : obj_->motionCost(states[pos1], s1);
            base::Cost alongPath = s0PartialCost;
            int posTemp = pos0 + 1;
            while (posTemp < pos1)
            {
                alongPath = obj_->combineCosts(alongPath, obj_->motionCost(states[posTemp], states[posTemp + 1]));
                posTemp++;
            }
            alongPath = obj_->combineCosts(alongPath, s1PartialCost);
            if (obj_->isCostBetterThan(alongPath, obj_->motionCost(s0, s1)))
            {
                // The cost along the path from state 0 to 1 is better than the straight line motion cost between the
                // two.
                continue;
            }
            // Otherwise, shortcut cost is better!

            // Modify the path with the new, shorter result
            if (index0 < 0 && index1 < 0)
            {
                if (pos0 + 1 == pos1)
                {
                    si->copyState(states[pos1], s0);
                    states.insert(states.begin() + pos1 + 1, si->cloneState(s1));
                }
                else
                {
                    if (freeStates_)
                        for (int j = pos0 + 2; j < pos1; ++j)
                            si->freeState(states[j]);
                    si->copyState(states[pos0 + 1], s0);
                    si->copyState(states[pos1], s1);
                    states.erase(states.begin() + pos0 + 2, states.begin() + pos1);
                }
            }
            else if (index0 >= 0 && index1 >= 0)
            {
                if (freeStates_)
                    for (int j = index0 + 1; j < index1; ++j)
                        si->freeState(states[j]);
                states.erase(states.begin() + index0 + 1, states.begin() + index1);
            }
            else if (index0 < 0 && index1 >= 0)
            {
                if (freeStates_)
                    for (int j = pos0 + 2; j < index1; ++j)
                        si->freeState(states[j]);
                si->copyState(states[pos0 + 1], s0);
                states.erase(states.begin() + pos0 + 2, states.begin() + index1);
            }
            else if (index0 >= 0 && index1 < 0)
            {
                if (freeStates_)
                    for (int j = index0 + 1; j < pos1; ++j)
                        si->freeState(states[j]);
                si->copyState(states[pos1], s1);
                states.erase(states.begin() + index0 + 1, states.begin() + pos1);
            }

shortcutPath要对path_发生实质影响了。增加新的路点，删除掉的路点。

            SDL_Log("{shortcutPath}[%i/%i]post a valid, path: %s", (int)i, (int)maxSteps, PathGeometric_DebugString(path).c_str());

            // fix the helper variables
            dists.resize(states.size(), 0.0);
            costs.resize(states.size(), obj_->identityCost());
            for (unsigned int j = pos0 + 1; j < costs.size(); ++j)
            {
                costs[j] = obj_->combineCosts(costs[j - 1], obj_->motionCost(states[j - 1], states[j]));
                dists[j] = dists[j - 1] + si->distance(states[j - 1], states[j]);
            }
            threshold = dists.back() * snapToVertex;
            rd = rangeRatio * dists.back();
            result = true;
            nochange = 0;

path_已经发生改变，须重新计算dists、costs、threshold、rd。

            SDL_Log("{shortcutPath}[%i/%i]post a valid, costs: %s, dists: %s", (int)i, (int)maxSteps, vector_Cost_DebugString(costs).c_str(), vector_double_DebugString(dists).c_str());
        }
    }

    si->freeState(temp1);
    si->freeState(temp0);
    SDL_Log("---{shortcutPath}X, path: %s, result: %s", PathGeometric_DebugString(path).c_str(), result? "true": "false");
    return result;
}

1.3 PathSimplifier::reduceVertices

对给定路径path，尝试从中移除路点，同时保持路径有效。这是一个迭代过程，试图在路径上进行找到“捷径”。尝试在路径上的非连续路点之间建立连接。如果连接成功，则通过删除中间的路点来缩短路径。如果对路径进行过修改，函数返回true。

bool ompl::geometric::PathSimplifier::reduceVertices(PathGeometric &path, unsigned int maxSteps,
                                                     unsigned int maxEmptySteps, double rangeRatio)
{
    SDL_Log("{reduceVertices}---path.size(): %i", (int)path.getStateCount());
    if (path.getStateCount() < 3)
        return false;

    if (maxSteps == 0)
        maxSteps = path.getStateCount();

    if (maxEmptySteps == 0)
        maxEmptySteps = path.getStateCount();

    bool result = false;
    unsigned int nochange = 0;
    const base::SpaceInformationPtr &si = path.getSpaceInformation();
    std::vector<base::State *> &states = path.getStates();

    if (si->checkMotion(states.front(), states.back()))
    {

如果初始点和移动到目标点，那路径只留、两个路点就可以了。这里有个疑问，为什么ompl不首先判断下这个checkMotion？一旦它为true，这里的simplifySolution岂不是可以省了？

        if (freeStates_)
            for (std::size_t i = 2; i < states.size(); ++i)
                si->freeState(states[i - 1]);
        std::vector<base::State *> newStates(2);
        newStates[0] = states.front();
        newStates[1] = states.back();
        states.swap(newStates);
        result = true;
        SDL_Log("{reduceVertices}si->checkMotion(...) true, path: %s", PathGeometric_DebugString(path).c_str());
    }
    else
        for (unsigned int i = 0; i < maxSteps && nochange < maxEmptySteps; ++i, ++nochange)
        {
            int count = states.size();
            int maxN = count - 1;
            int range = 1 + (int)(floor(0.5 + (double)count * rangeRatio));

            int p1 = rng_.uniformInt(0, maxN);
            int p2 = rng_.uniformInt(std::max(p1 - range, 0), std::min(maxN, p1 + range));
            if (abs(p1 - p2) < 2)
            {
                if (p1 < maxN - 1)
                    p2 = p1 + 2;
                else if (p1 > 1)
                    p2 = p1 - 2;
                else
                    continue;
            }

            if (p1 > p2)
                std::swap(p1, p2);

            if (si->checkMotion(states[p1], states[p2]))
            {
                if (freeStates_)
                    for (int j = p1 + 1; j < p2; ++j)
                        si->freeState(states[j]);
                states.erase(states.begin() + p1 + 1, states.begin() + p2);
                nochange = 0;
                result = true;
            }
        }
    SDL_Log("---{reduceVertices}X, path: %s, result: %s", PathGeometric_DebugString(path).c_str(), result? "true": "false");
    return result;
}

 

二、ModelBasedPlanningContext::interpolateSolution

计算每段内要再分要生成多少个点，并补插这些点。

void ompl_interface::ModelBasedPlanningContext::interpolateSolution()
{
  if (ompl_simple_setup_->haveSolutionPath())
  {
    og::PathGeometric& pg = ompl_simple_setup_->getSolutionPath();

    // Find the number of states that will be in the interpolated solution.
    // This is what interpolate() does internally.
    unsigned int eventual_states = 1;
    std::vector<ompl::base::State*> states = pg.getStates();
    for (size_t i = 0; i < states.size() - 1; i++)
    {
      eventual_states += ompl_simple_setup_->getStateSpace()->validSegmentCount(states[i], states[i + 1]);

for次数就是路径中段数，合计此个路径会有多少个点。

    }

    if (eventual_states < minimum_waypoint_count_)
    {
      // If that's not enough states, use the minimum amount instead.
      pg.interpolate(minimum_waypoint_count_);
    }
    else
    {
      // Interpolate the path to have as the exact states that are checked when validating motions.
      pg.interpolate();

补插path_，让含有eventual_states个点。针对示例，就是40个，即最后的pg.states_.size() = 40，每个pg.states[i]存储着一个路点，在值上它们就是要最终返回给client的路径点。

    }
  }
}

validSegmentCount用于计算该分段要生成多个点。结果只和该段的两个分界点，以及配置参数longestValidSegmentCountFactor_(1)、longestValidSegment_(0.0471)有关。

unsigned int ompl::base::StateSpace::validSegmentCount(const State *state1, const State *state2) const
{
    return longestValidSegmentCountFactor_ * (unsigned int)ceil(distance(state1, state2) / longestValidSegment_);
}

那longestValidSegment_是怎么来的？

void ompl::base::StateSpace::setup()
{
    maxExtent_ = getMaximumExtent();
    longestValidSegment_ = maxExtent_ * longestValidSegmentFraction_;
    ...
}

longestValidSegment_表示该规划组一次可移动的最大距离，值依赖两个变量。

  • maxExtent_：9.42。该规划组关节可旋转的最大范围。maxExtent_ = getMaximumExtent()。示例有a、b、c三个关节，每个关节范围都是3.14，那maxExtent_的值是3.14 * 3 = 9.42。它的值由urdf中<joint>内的<limit>决定。
  • longestValidSegmentFraction_: 0.005。一次可移动最大距离占maxExtent_的比率。初始值0.01。如果存在param变量/move_group/robot_arm/longest_valid_segment_fraction，像0.05，那会覆盖该值。即规划组robot_arm的一次关节移动不能超过范围的1/20。

经过interpolateSolution()，path_存储了client希望的路径。这个补插只和段分界有关，可认为RRTConnect::solve中sampleUniform随机生成的采样值，不影响最后生成的路径。

INFO: ------size: 40
INFO: [0/39] (-0.56,0.79,-0.45)
INFO: [1/39] (-0.545641,0.769744,-0.438462)
INFO: [2/39] (-0.531282,0.749487,-0.426923)
INFO: [3/39] (-0.516923,0.729231,-0.415385)
INFO: [4/39] (-0.502564,0.708974,-0.403847)
INFO: [5/39] (-0.488205,0.688718,-0.392308)
INFO: [6/39] (-0.473847,0.668462,-0.38077)
INFO: [7/39] (-0.459488,0.648205,-0.369232)
...
INFO: [32/39] (-0.100515,0.141795,-0.0807739)
INFO: [33/39] (-0.086156,0.121538,-0.0692355)
INFO: [34/39] (-0.0717971,0.101282,-0.0576972)
INFO: [35/39] (-0.0574382,0.0810256,-0.0461589)
INFO: [36/39] (-0.0430793,0.0607692,-0.0346206)
INFO: [37/39] (-0.0287204,0.0405128,-0.0230823)
INFO: [38/39] (-0.0143615,0.0202564,-0.011544)
INFO: [39/39] (-2.54839e-06,0,-5.63609e-06)
发布于 2022-05-29 21:36:41

全部评论: 0 条

    写评论: