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_Triangulate |
35  aiProcess_JoinIdenticalVertices |
36  aiProcess_SortByPType |
37  aiProcess_OptimizeGraph |
38  aiProcess_OptimizeMeshes);
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_Triangulate |
77  aiProcess_JoinIdenticalVertices |
78  aiProcess_SortByPType |
79  aiProcess_OptimizeGraph |
80  aiProcess_OptimizeMeshes);
81  if (envScene != nullptr)
82  {
83  if (!envScene->HasMeshes())
84  {
85  OMPL_ERROR("There is no mesh specified in the indicated environment resource: %s", env.c_str());
86  importerEnv_.resize(p);
87  }
88  }
89  else
90  {
91  OMPL_ERROR("Unable to load environment scene: %s", env.c_str());
92  importerEnv_.resize(p);
93  }
94 
95  if (p < importerEnv_.size())
96  {
97  computeGeometrySpecification();
98  return true;
99  }
100 
101  return false;
102 }
103 
105 {
106  base::RealVectorBounds bounds(3);
107 
108  for (const auto & i : importerEnv_)
109  {
110  std::vector<aiVector3D> vertices;
111  scene::extractVertices(i->GetScene(), vertices);
112  scene::inferBounds(bounds, vertices, factor_, add_);
113  }
114 
115  if (mtype_ == Motion_2D)
116  {
117  bounds.low.resize(2);
118  bounds.high.resize(2);
119  }
120 
121  return bounds;
122 }
123 
124 const ompl::app::GeometrySpecification& ompl::app::RigidBodyGeometry::getGeometrySpecification() const
125 {
126  return geom_;
127 }
128 
129 void ompl::app::RigidBodyGeometry::computeGeometrySpecification()
130 {
131  validitySvc_.reset();
132  geom_.obstacles.clear();
133  geom_.obstaclesShift.clear();
134  geom_.robot.clear();
135  geom_.robotShift.clear();
136 
137  for (auto & i : importerEnv_)
138  geom_.obstacles.push_back(i->GetScene());
139 
140  for (unsigned int i = 0 ; i < importerRobot_.size() ; ++i)
141  {
142  geom_.robot.push_back(importerRobot_[i]->GetScene());
143  aiVector3D c = getRobotCenter(i);
144  if (mtype_ == Motion_2D)
145  c[2] = 0.0;
146  geom_.robotShift.push_back(c);
147  }
148 }
149 
150 aiVector3D ompl::app::RigidBodyGeometry::getRobotCenter(unsigned int robotIndex) const
151 {
152  aiVector3D s(0.0, 0.0, 0.0);
153  if (robotIndex >= importerRobot_.size())
154  throw Exception("Robot " + std::to_string(robotIndex) + " not found.");
155 
156  scene::sceneCenter(importerRobot_[robotIndex]->GetScene(), s);
157  return s;
158 }
159 
161 {
162  if (ctype != ctype_)
163  {
164  ctype_ = ctype;
165  if (validitySvc_)
166  {
167  validitySvc_.reset ();
168  }
169 
170  assert (!validitySvc_);
171  }
172 }
173 
175 {
176  if (validitySvc_)
177  return validitySvc_;
178 
179  GeometrySpecification geom = getGeometrySpecification();
180 
181  switch (ctype_)
182  {
183 #if OMPL_HAS_PQP
184  case PQP:
185  if (mtype_ == Motion_2D)
186  validitySvc_ = std::make_shared<PQPStateValidityChecker<Motion_2D>>(si, geom, se, selfCollision);
187  else
188  validitySvc_ = std::make_shared<PQPStateValidityChecker<Motion_3D>>(si, geom, se, selfCollision);
189  break;
190 #endif
191  case FCL:
192  if (mtype_ == Motion_2D)
193  validitySvc_ = std::make_shared<FCLStateValidityChecker<Motion_2D>>(si, geom, se, selfCollision);
194  else
195  validitySvc_ = std::make_shared<FCLStateValidityChecker<Motion_3D>>(si, geom, se, selfCollision);
196  break;
197 
198  default:
199  OMPL_ERROR("Unexpected collision checker type (%d) encountered", ctype_);
200  };
201 
202  return validitySvc_;
203 }
double add_
The value to add to inferred environment bounds (default 0)
std::vector< double > low
Lower bound.
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< std::shared_ptr< Assimp::Importer > > importerEnv_
Instance of assimp importer used to load environment.
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.
double factor_
The factor to multiply inferred environment bounds by (default 1)
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 PQP.
base::RealVectorBounds inferEnvironmentBounds() const
Given the representation of an environment, infer its bounds. The bounds will be 2-dimensional when p...
std::vector< double > high
Upper bound.
CollisionChecker ctype_
Value containing the type of collision checking to use.
A shared pointer wrapper for ompl::base::SpaceInformation.
CollisionChecker
Enumeration of the possible collision checker types.
base::StateValidityCheckerPtr validitySvc_
Instance of the state validity checker for collision checking.
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.
GeometrySpecification geom_
Object containing mesh data for robot and environment.
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.