Beispiel #1
0
ompl::control::OpenDEStateSpace::OpenDEStateSpace(OpenDEEnvironmentPtr env, double positionWeight, double linVelWeight,
                                                  double angVelWeight, double orientationWeight)
  : env_(std::move(env))
{
    setName("OpenDE" + getName());
    type_ = base::STATE_SPACE_TYPE_COUNT + 1;
    for (unsigned int i = 0; i < env_->stateBodies_.size(); ++i)
    {
        std::string body = ":B" + std::to_string(i);

        addSubspace(std::make_shared<base::RealVectorStateSpace>(3), positionWeight);  // position
        components_.back()->setName(components_.back()->getName() + body + ":position");

        addSubspace(std::make_shared<base::RealVectorStateSpace>(3), linVelWeight);  // linear velocity
        components_.back()->setName(components_.back()->getName() + body + ":linvel");

        addSubspace(std::make_shared<base::RealVectorStateSpace>(3), angVelWeight);  // angular velocity
        components_.back()->setName(components_.back()->getName() + body + ":angvel");

        addSubspace(std::make_shared<base::SO3StateSpace>(), orientationWeight);  // orientation
        components_.back()->setName(components_.back()->getName() + body + ":orientation");
    }
    lock();
    setDefaultBounds();
}
ompl::control::OpenDEStateSpace::OpenDEStateSpace(const OpenDEEnvironmentPtr &env,
                                                  double positionWeight, double linVelWeight, double angVelWeight, double orientationWeight) :
    base::CompoundStateSpace(), env_(env)
{
    setName("OpenDE" + getName());
    type_ = base::STATE_SPACE_TYPE_COUNT + 1;
    for (unsigned int i = 0 ; i < env_->stateBodies_.size() ; ++i)
    {
        std::string body = ":B" + boost::lexical_cast<std::string>(i);

        addSubspace(base::StateSpacePtr(new base::RealVectorStateSpace(3)), positionWeight); // position
        components_.back()->setName(components_.back()->getName() + body + ":position");

        addSubspace(base::StateSpacePtr(new base::RealVectorStateSpace(3)), linVelWeight);   // linear velocity
        components_.back()->setName(components_.back()->getName() + body + ":linvel");

        addSubspace(base::StateSpacePtr(new base::RealVectorStateSpace(3)), angVelWeight);   // angular velocity
        components_.back()->setName(components_.back()->getName() + body + ":angvel");

        addSubspace(base::StateSpacePtr(new base::SO3StateSpace()), orientationWeight);      // orientation
        components_.back()->setName(components_.back()->getName() + body + ":orientation");
    }
    lock();
    setDefaultBounds();
}
 PatchedSE3StateSpace() : CompoundStateSpace()
 {
     setName("PatchedSE3" + getName());
     type_ = STATE_SPACE_SE3;
     addSubspace(StateSpacePtr(new RealVectorStateSpace(3)), 1.0);
     addSubspace(StateSpacePtr(new PatchedSO3StateSpace()), 4.0);
     lock();
 }
 KinematicChainSpace(unsigned int numLinks, double linkLength, Environment *env = nullptr)
     : ompl::base::CompoundStateSpace(), linkLength_(linkLength), environment_(env)
 {
     for (unsigned int i = 0; i < numLinks; ++i)
         addSubspace(ompl::base::StateSpacePtr(new ompl::base::SO2StateSpace()), 1.);
     lock();
 }
 OMPLRNStateSpace::OMPLRNStateSpace(unsigned int dim, const Server_ptr &server,
     OMPLProblem_ptr &prob)
     : OMPLBaseStateSpace(dim, server, prob)
 {
   setName("OMPLRNStateSpace");
   addSubspace(
       ompl::base::StateSpacePtr(new ompl::base::RealVectorStateSpace(dim)),
       1.0);
   ompl::base::RealVectorBounds bounds(dim);
   for (int i = 0; i < dim; i++)
   {
     bounds.setHigh(i, prob->getBounds()[i + dim]);
     bounds.setLow(i, prob->getBounds()[i]);
   }
   getSubspace(0)->as<ob::RealVectorStateSpace>()->setBounds(bounds);
   lock();
 }