ompl::base::ScopedState<> ompl::app::DynamicCarPlanning::getDefaultStartState() const
{
base::ScopedState<base::CompoundStateSpace> s(getStateSpace());
base::SE2StateSpace::StateType& pose = *s->as<base::SE2StateSpace::StateType>(0);
base::RealVectorStateSpace::StateType& vel = *s->as<base::RealVectorStateSpace::StateType>(1);
aiVector3D c = getRobotCenter(0);
pose.setX(c.x);
pose.setY(c.y);
pose.setYaw(0.);
vel.values[0] = 0.;
vel.values[1] = 0.;
return s;
}
ompl::base::ScopedState<> ompl::app::SE3MultiRigidBodyPlanning::getDefaultStartState(void) const
{
base::ScopedState<> st(getStateSpace());
base::CompoundStateSpace::StateType* c_st = st.get()->as<base::CompoundStateSpace::StateType>();
for (unsigned int i = 0; i < n_; ++i)
{
aiVector3D s = getRobotCenter(i);
base::SE3StateSpace::StateType* sub = c_st->as<base::SE3StateSpace::StateType>(i);
sub->setX(s.x);
sub->setY(s.y);
sub->setZ(s.z);
sub->rotation().setIdentity();
}
return st;
}
void ompl::app::RigidBodyGeometry::computeGeometrySpecification()
{
validitySvc_.reset();
geom_.obstacles.clear();
geom_.obstaclesShift.clear();
geom_.robot.clear();
geom_.robotShift.clear();
for (auto & i : importerEnv_)
geom_.obstacles.push_back(i->GetScene());
for (unsigned int i = 0 ; i < importerRobot_.size() ; ++i)
{
geom_.robot.push_back(importerRobot_[i]->GetScene());
aiVector3D c = getRobotCenter(i);
if (mtype_ == Motion_2D)
c[2] = 0.0;
geom_.robotShift.push_back(c);
}
}