RigidBodyGeometry.cpp
1 /*********************************************************************
2 * Rice University Software Distribution License
3 *
4 * Copyright (c) 2010, Rice University
5 * All Rights Reserved.
6 *
7 * For a full description see the file named LICENSE.
8 *
9 *********************************************************************/
10 
11 /* Author: Ioan Sucan */
12 
13 #include "omplapp/geometry/RigidBodyGeometry.h"
14 #if OMPL_HAS_PQP
15 #include "omplapp/geometry/detail/PQPStateValidityChecker.h"
16 #endif
17 #include "omplapp/geometry/detail/FCLStateValidityChecker.h"
18 
19 boost::filesystem::path ompl::app::RigidBodyGeometry::findMeshFile(const std::string& fname)
20 {
21  boost::filesystem::path path(fname);
22  if (boost::filesystem::exists(path))
23  return boost::filesystem::absolute(path);
24  if (path.is_absolute())
25  return {};
26  for (const auto &dir : meshPath_)
27  {
28  boost::filesystem::path candidate(dir / path);
29  if (boost::filesystem::exists(candidate))
30  return boost::filesystem::absolute(candidate);
31  }
32  return {};
33 }
34 
35 bool ompl::app::RigidBodyGeometry::setRobotMesh(const std::string &robot)
36 {
37  importerRobot_.clear();
38  computeGeometrySpecification();
39  return addRobotMesh(robot);
40 }
41 
42 bool ompl::app::RigidBodyGeometry::addRobotMesh(const std::string &robot)
43 {
44  assert(!robot.empty());
45  std::size_t p = importerRobot_.size();
46  importerRobot_.resize(p + 1);
47  importerRobot_[p] = std::make_shared<Assimp::Importer>();
48 
49  const boost::filesystem::path path = findMeshFile(robot);
50  if (path.empty())
51  OMPL_ERROR("File '%s' not found in mesh path.", robot.c_str());
52  const aiScene* robotScene = importerRobot_[p]->ReadFile(path.string().c_str(),
53  aiProcess_GenNormals |
54  aiProcess_Triangulate |
55  aiProcess_JoinIdenticalVertices |
56  aiProcess_SortByPType |
57  aiProcess_OptimizeGraph);
58  if (robotScene != nullptr)
59  {
60  if (!robotScene->HasMeshes())
61  {
62  OMPL_ERROR("There is no mesh specified in the indicated robot resource: %s", robot.c_str());
63  importerRobot_.resize(p);
64  }
65  }
66  else
67  {
68  OMPL_ERROR("Unable to load robot scene: %s", robot.c_str());
69  importerRobot_.resize(p);
70  }
71 
72  if (p < importerRobot_.size())
73  {
74  computeGeometrySpecification();
75  return true;
76  }
77  return false;
78 }
79 
81 {
82  importerEnv_.clear();
83  computeGeometrySpecification();
84  return addEnvironmentMesh(env);
85 }
86 
88 {
89  assert(!env.empty());
90  std::size_t p = importerEnv_.size();
91  importerEnv_.resize(p + 1);
92  importerEnv_[p] = std::make_shared<Assimp::Importer>();
93 
94  const boost::filesystem::path path = findMeshFile(env);
95  if (path.empty())
96  OMPL_ERROR("File '%s' not found in mesh path.", env.c_str());
97  const aiScene* envScene = importerEnv_[p]->ReadFile(path.string().c_str(),
98  aiProcess_GenNormals |
99  aiProcess_Triangulate |
100  aiProcess_JoinIdenticalVertices |
101  aiProcess_SortByPType |
102  aiProcess_OptimizeGraph);
103 
104  if (envScene != nullptr)
105  {
106  if (!envScene->HasMeshes())
107  {
108  OMPL_ERROR("There is no mesh specified in the indicated environment resource: %s", env.c_str());
109  importerEnv_.resize(p);
110  }
111  }
112  else
113  {
114  OMPL_ERROR("Unable to load environment scene: %s", env.c_str());
115  importerEnv_.resize(p);
116  }
117 
118  if (p < importerEnv_.size())
119  {
120  computeGeometrySpecification();
121  return true;
122  }
123 
124  return false;
125 }
126 
128 {
129  base::RealVectorBounds bounds(3);
130 
131  for (const auto & i : importerEnv_)
132  {
133  std::vector<aiVector3D> vertices;
134  scene::extractVertices(i->GetScene(), vertices);
135  scene::inferBounds(bounds, vertices, factor_, add_);
136  }
137 
138  if (mtype_ == Motion_2D)
139  {
140  bounds.low.resize(2);
141  bounds.high.resize(2);
142  }
143 
144  return bounds;
145 }
146 
147 const ompl::app::GeometrySpecification& ompl::app::RigidBodyGeometry::getGeometrySpecification() const
148 {
149  return geom_;
150 }
151 
152 void ompl::app::RigidBodyGeometry::computeGeometrySpecification()
153 {
154  validitySvc_.reset();
155  geom_.obstacles.clear();
156  geom_.obstaclesShift.clear();
157  geom_.robot.clear();
158  geom_.robotShift.clear();
159 
160  for (auto & i : importerEnv_)
161  geom_.obstacles.push_back(i->GetScene());
162 
163  for (unsigned int i = 0 ; i < importerRobot_.size() ; ++i)
164  {
165  geom_.robot.push_back(importerRobot_[i]->GetScene());
166  aiVector3D c = getRobotCenter(i);
167  if (mtype_ == Motion_2D)
168  c[2] = 0.0;
169  geom_.robotShift.push_back(c);
170  }
171 }
172 
173 aiVector3D ompl::app::RigidBodyGeometry::getRobotCenter(unsigned int robotIndex) const
174 {
175  aiVector3D s(0.0, 0.0, 0.0);
176  if (robotIndex >= importerRobot_.size())
177  throw Exception("Robot " + std::to_string(robotIndex) + " not found.");
178 
179  scene::sceneCenter(importerRobot_[robotIndex]->GetScene(), s);
180  return s;
181 }
182 
184 {
185  if (ctype != ctype_)
186  {
187  ctype_ = ctype;
188  if (validitySvc_)
189  {
190  validitySvc_.reset ();
191  }
192 
193  assert (!validitySvc_);
194  }
195 }
196 
197 const ompl::base::StateValidityCheckerPtr& ompl::app::RigidBodyGeometry::allocStateValidityChecker(const base::SpaceInformationPtr &si, const GeometricStateExtractor &se, bool selfCollision)
198 {
199  if (validitySvc_)
200  return validitySvc_;
201 
202  GeometrySpecification geom = getGeometrySpecification();
203 
204  switch (ctype_)
205  {
206 #if OMPL_HAS_PQP
207  case PQP:
208  if (mtype_ == Motion_2D)
209  validitySvc_ = std::make_shared<PQPStateValidityChecker<Motion_2D>>(si, geom, se, selfCollision);
210  else
211  validitySvc_ = std::make_shared<PQPStateValidityChecker<Motion_3D>>(si, geom, se, selfCollision);
212  break;
213 #endif
214  case FCL:
215  if (mtype_ == Motion_2D)
216  validitySvc_ = std::make_shared<FCLStateValidityChecker<Motion_2D>>(si, geom, se, selfCollision);
217  else
218  validitySvc_ = std::make_shared<FCLStateValidityChecker<Motion_3D>>(si, geom, se, selfCollision);
219  break;
220 
221  default:
222  OMPL_ERROR("Unexpected collision checker type (%d) encountered", ctype_);
223  };
224 
225  return validitySvc_;
226 }
The exception type for ompl.
Definition: Exception.h:47
aiVector3D getRobotCenter(unsigned int robotIndex) const
Get the robot's center (average of all the vertices of all its parts)
const base::StateValidityCheckerPtr & allocStateValidityChecker(const base::SpaceInformationPtr &si, const GeometricStateExtractor &se, bool selfCollision)
Allocate default state validity checker using FCL.
virtual void setStateValidityCheckerType(CollisionChecker ctype)
Change the type of collision checking for the rigid body.
virtual bool addRobotMesh(const std::string &robot)
This function specifies the name of the CAD file representing a part of the robot (robot)....
boost::filesystem::path findMeshFile(const std::string &fname)
return absolute path to mesh file if it exists and an empty path otherwise
virtual bool setRobotMesh(const std::string &robot)
This function specifies the name of the CAD file representing the robot (robot). Returns 1 on success...
base::RealVectorBounds inferEnvironmentBounds() const
Given the representation of an environment, infer its bounds. The bounds will be 2-dimensional when p...
virtual bool setEnvironmentMesh(const std::string &env)
This function specifies the name of the CAD file representing the environment (env)....
virtual bool addEnvironmentMesh(const std::string &env)
This function specifies the name of the CAD file representing a part of the environment (env)....
std::vector< boost::filesystem::path > meshPath_
Paths to search for mesh files if mesh file names do not correspond to absolute paths.
The lower and upper bounds for an Rn space.
A shared pointer wrapper for ompl::base::StateValidityChecker.
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
Definition: Console.h:64
CollisionChecker
Enumeration of the possible collision checker types.