OMPL 入门Tutorial 4:使用现有的采样器和创建新的(Using existing samplers and creating new ones)

以下内容为OMPL官方Tutorial Using existing samplers and creating new ones的翻译

可选择的状态采样器

有两种采样器听起来相同但扮演着不同的角色:状态空间采样器(ompl::base::StateSampler)和有效状态采样器(ompl::base::ValidStateSampler)。对于每一种状态空间,都需要一个对应的ompl::base::StateSampler类来允许从状态空间均匀采样、在一个状态的附近采样和用高斯分布采样。 有效状态采样器将状态空间采样器作为一个低级的初始值。一般来说,他们用合适的状态空间采样器生成一系列状态采样点直到找到一个有效的状态或者超出最大迭代次数。下面列出了一些预定义的ompl::base::ValidStateSampler类

  • ompl::base::UniformValidStateSampler: 这是默认的采样器。他只是调用状态空间采样器来生成均匀的采样直到找到一个有效的状态或者超出最大迭代次数。
  • ompl::base::ObstacleBasedValidStateSampler: 这个采样器先试图找到一个无效的sample和一个有效的sample;然后增量式的从有效采样点向无效采样点插值。他返回最后一个有效的状态。这样的采样器的用处在于:在狭窄通道的环境下,障碍物附近会有更大的几率找到有效的采样。在求解运动规划的问题中,寻找这样的采样点一般是关键的问题。
  • ompl::base::GaussianValidStateSampler: 这一采样器想解决的问题与上一个相同,但是是以另一种方式。他重复地生成状态对(pairs of states)。第一个是均匀采样随机得到的,而第二个是根据以第一个为中心的高斯分布采样得到的。如果一个采样是有效的而另一个是无效的,那就返回有效的。如果都是有效或都是无效的,则会生成新的状态对,重复直到超过最大迭代次数。
  • ompl::base::MaximizeClearanceValidStateSampler: 这个采样器有点像第一个,但一旦他找到一个有效的状态,就会尝试寻找其他有更大的空间(clearance)的有效状态。最后返回的采样是有最大的clearance的状态。

接下来演示了如何定义一个规划器来使用上述的采样器之一和如何写你自己的有效状态采样器。代码样例来自于StateSampling.cpp演示程序,该代码也有python版本

使用一个已有的采样器

我们不会直接在SimpleSetup或者SpaceInformation类中直接设置采样器的类型,因为每一个线程都需要他自己拥有采样器的备份。我们需要定义ompl::base::ValidStateSamplerAllocator,一个“接收指向 ompl::base::SpaceInformation的指针,返回 ompl::base::ValidStateSamplerPtr”的函数。这个函数也可以在返回之前根据具体的状态空间信息配置有效的状态采样器。下面这个简单的样例演示了如何用ObstacleBasedValidStateSampler

namespace ob = ompl::base;
namespace og = ompl::geometric;
ob::ValidStateSamplerPtr allocOBValidStateSampler(const ob::SpaceInformation *si)
{
    // 可以在这里实现任何额外的setup/configuration,但在ObstacleBasedValidStateSampler中没有需要额外配置的,所以这里没有任何额外设置
    return std::make_shared<ob::ObstacleBasedValidStateSampler>(si);
}

其他的setup步骤,比如定义起始状态末尾状态被省略掉了。

创建一个新的可行状态采样器

现在有很多种启发函数被应用于改进状态的采样。采样的质量可以通过例如与最近障碍物的距离,或者从一个状态的“可见性”来评判。有两种常见的情况可以利用特定问题的信息:

  • 如何你用一个返回值是与最近障碍物距离、或者距离函数的梯度的碰撞检测器,那么久需要创建一个新的有效状态采样器来利用这一信息
  • 如果你可以直接将状态可行约束融合到采样中(而非用标准有效状态采样器的拒绝采样的方案),那么这将使得算法表现大幅提升。下方给出了示例

下面的代码中我们为一个3D点规划在以原点为中心的立方体中的运动。有一个长方体障碍物。由于状态区域方便描述,我们可以直接在free space采样

namespace ob = ompl::base;
namespace og = ompl::geometric;
 
// 这是一个根据问题定制的采样器,自动的生成有效的状态;他不需要调用SpaceInformation::isValid。这是一个受约束采样的例子。如果你可以明确地描述设置的有效状态并可以从中采样,那么这将会比从整个状态空间随机生成采样点再检查可行性更高效。
class MyValidStateSampler : public ob::ValidStateSampler
{
public:
    MyValidStateSampler(const ob::SpaceInformation *si) : ValidStateSampler(si)
    {
        name_ = "my sampler";
    }
    // 在R^3状态空间的有效部分采样
    // 有效的状态满足以下约束
    // -1<= x,y,z <=1
    // if .25 <= z <= .5, then |x|>.8 and |y|>.8
    bool sample(ob::State *state) override
    {
        double* val = static_cast<ob::RealVectorStateSpace::StateType*>(state)->values;
        double z = rng_.uniformReal(-1,1);
 
        if (z>.25 && z<.5)
        {
            double x = rng_.uniformReal(0,1.8), y = rng_.uniformReal(0,.2);
            switch(rng_.uniformInt(0,3))
            {
                case 0: val[0]=x-1;  val[1]=y-1;  break;
                case 1: val[0]=x-.8; val[1]=y+.8; break;
                case 2: val[0]=y-1;  val[1]=x-1;  break;
                case 3: val[0]=y+.8; val[1]=x-.8; break;
            }
        }
        else
        {
            val[0] = rng_.uniformReal(-1,1);
            val[1] = rng_.uniformReal(-1,1);
        }
        val[2] = z;
        assert(si_->isValid(state));
        return true;
    }
    // 在下面的例子中不需要这个
    bool sampleNear(ob::State* /*state*/, const ob::State* /*near*/, const double /*distance*/) override
    {
        throw ompl::Exception("MyValidStateSampler::sampleNear", "not implemented");
        return false;
    }
protected:
    ompl::RNG rng_;
};

我们用与上述描述的相似的方法定义一个有效状态分配器

ob::ValidStateSamplerPtr allocMyValidStateSampler(const ob::SpaceInformation *si)
{
    return std::make_shared<MyValidStateSampler>(si);
}

最后,使用这个采样器

ss.getSpaceInformation()->setValidStateSamplerAllocator(allocMyValidStateSampler);
  • 0
    点赞
  • 5
    收藏
    觉得还不错? 一键收藏
  • 2
    评论
评论 2
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值