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 bool ompl::app::RigidBodyGeometry::setRobotMesh(const std::string &robot)
20 {
21  importerRobot_.clear();
22  computeGeometrySpecification();
23  return addRobotMesh(robot);
24 }
25 
26 bool ompl::app::RigidBodyGeometry::addRobotMesh(const std::string &robot)
27 {
28  assert(!robot.empty());
29  std::size_t p = importerRobot_.size();
30  importerRobot_.resize(p + 1);
31  importerRobot_[p] = std::make_shared<Assimp::Importer>();
32 
33  const aiScene* robotScene = importerRobot_[p]->ReadFile(robot.c_str(),
34  aiProcess_GenNormals |
35  aiProcess_Triangulate |
36  aiProcess_JoinIdenticalVertices |
37  aiProcess_SortByPType |
38  aiProcess_OptimizeGraph);
39  if (robotScene != nullptr)
40  {
41  if (!robotScene->HasMeshes())
42  {
43  OMPL_ERROR("There is no mesh specified in the indicated robot resource: %s", robot.c_str());
44  importerRobot_.resize(p);
45  }
46  }
47  else
48  {
49  OMPL_ERROR("Unable to load robot scene: %s", robot.c_str());
50  importerRobot_.resize(p);
51  }
52 
53  if (p < importerRobot_.size())
54  {
55  computeGeometrySpecification();
56  return true;
57  }
58  return false;
59 }
60 
62 {
63  importerEnv_.clear();
64  computeGeometrySpecification();
65  return addEnvironmentMesh(env);
66 }
67 
69 {
70  assert(!env.empty());
71  std::size_t p = importerEnv_.size();
72  importerEnv_.resize(p + 1);
73  importerEnv_[p] = std::make_shared<Assimp::Importer>();
74 
75  const aiScene* envScene = importerEnv_[p]->ReadFile(env.c_str(),
76  aiProcess_GenNormals |
77  aiProcess_Triangulate |
78  aiProcess_JoinIdenticalVertices |
79  aiProcess_SortByPType |
80  aiProcess_OptimizeGraph);
81 
82  if (envScene != nullptr)
83  {
84  if (!envScene->HasMeshes())
85  {
86  OMPL_ERROR("There is no mesh specified in the indicated environment resource: %s", env.c_str());
87  importerEnv_.resize(p);
88  }
89  }
90  else
91  {
92  OMPL_ERROR("Unable to load environment scene: %s", env.c_str());
93  importerEnv_.resize(p);
94  }
95 
96  if (p < importerEnv_.size())
97  {
98  computeGeometrySpecification();
99  return true;
100  }
101 
102  return false;
103 }
104 
106 {
107  base::RealVectorBounds bounds(3);
108 
109  for (const auto & i : importerEnv_)
110  {
111  std::vector<aiVector3D> vertices;
112  scene::extractVertices(i->GetScene(), vertices);
113  scene::inferBounds(bounds, vertices, factor_, add_);
114  }
115 
116  if (mtype_ == Motion_2D)
117  {
118  bounds.low.resize(2);
119  bounds.high.resize(2);
120  }
121 
122  return bounds;
123 }
124 
125 const ompl::app::GeometrySpecification& ompl::app::RigidBodyGeometry::getGeometrySpecification() const
126 {
127  return geom_;
128 }
129 
130 void ompl::app::RigidBodyGeometry::computeGeometrySpecification()
131 {
132  validitySvc_.reset();
133  geom_.obstacles.clear();
134  geom_.obstaclesShift.clear();
135  geom_.robot.clear();
136  geom_.robotShift.clear();
137 
138  for (auto & i : importerEnv_)
139  geom_.obstacles.push_back(i->GetScene());
140 
141  for (unsigned int i = 0 ; i < importerRobot_.size() ; ++i)
142  {
143  geom_.robot.push_back(importerRobot_[i]->GetScene());
144  aiVector3D c = getRobotCenter(i);
145  if (mtype_ == Motion_2D)
146  c[2] = 0.0;
147  geom_.robotShift.push_back(c);
148  }
149 }
150 
151 aiVector3D ompl::app::RigidBodyGeometry::getRobotCenter(unsigned int robotIndex) const
152 {
153  aiVector3D s(0.0, 0.0, 0.0);
154  if (robotIndex >= importerRobot_.size())
155  throw Exception("Robot " + std::to_string(robotIndex) + " not found.");
156 
157  scene::sceneCenter(importerRobot_[robotIndex]->GetScene(), s);
158  return s;
159 }
160 
162 {
163  if (ctype != ctype_)
164  {
165  ctype_ = ctype;
166  if (validitySvc_)
167  {
168  validitySvc_.reset ();
169  }
170 
171  assert (!validitySvc_);
172  }
173 }
174 
176 {
177  if (validitySvc_)
178  return validitySvc_;
179 
180  GeometrySpecification geom = getGeometrySpecification();
181 
182  switch (ctype_)
183  {
184 #if OMPL_HAS_PQP
185  case PQP:
186  if (mtype_ == Motion_2D)
187  validitySvc_ = std::make_shared<PQPStateValidityChecker<Motion_2D>>(si, geom, se, selfCollision);
188  else
189  validitySvc_ = std::make_shared<PQPStateValidityChecker<Motion_3D>>(si, geom, se, selfCollision);
190  break;
191 #endif
192  case FCL:
193  if (mtype_ == Motion_2D)
194  validitySvc_ = std::make_shared<FCLStateValidityChecker<Motion_2D>>(si, geom, se, selfCollision);
195  else
196  validitySvc_ = std::make_shared<FCLStateValidityChecker<Motion_3D>>(si, geom, se, selfCollision);
197  break;
198 
199  default:
200  OMPL_ERROR("Unexpected collision checker type (%d) encountered", ctype_);
201  };
202 
203  return validitySvc_;
204 }
virtual bool addEnvironmentMesh(const std::string &env)
This function specifies the name of the CAD file representing a part of the environment (env)...
virtual bool setEnvironmentMesh(const std::string &env)
This function specifies the name of the CAD file representing the environment (env). Returns 1 on success, 0 on failure.
A shared pointer wrapper for ompl::base::StateValidityChecker.
aiVector3D getRobotCenter(unsigned int robotIndex) const
Get the robot&#39;s center (average of all the vertices of all its parts)
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
Definition: Console.h:64
const base::StateValidityCheckerPtr & allocStateValidityChecker(const base::SpaceInformationPtr &si, const GeometricStateExtractor &se, bool selfCollision)
Allocate default state validity checker using FCL.
base::RealVectorBounds inferEnvironmentBounds() const
Given the representation of an environment, infer its bounds. The bounds will be 2-dimensional when p...
A shared pointer wrapper for ompl::base::SpaceInformation.
CollisionChecker
Enumeration of the possible collision checker types.
The exception type for ompl.
Definition: Exception.h:46
virtual bool addRobotMesh(const std::string &robot)
This function specifies the name of the CAD file representing a part of the robot (robot)...
The lower and upper bounds for an Rn space.
virtual void setStateValidityCheckerType(CollisionChecker ctype)
Change the type of collision checking for the rigid body.
virtual bool setRobotMesh(const std::string &robot)
This function specifies the name of the CAD file representing the robot (robot). Returns 1 on success...
std::vector< std::shared_ptr< Assimp::Importer > > importerRobot_
Instance of assimp importer used to load robot.