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();
}