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)
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  else
59  return false;
60 }
61 
63 {
64  importerEnv_.clear();
65  computeGeometrySpecification();
66  return addEnvironmentMesh(env);
67 }
68 
70 {
71  assert(!env.empty());
72  std::size_t p = importerEnv_.size();
73  importerEnv_.resize(p + 1);
74  importerEnv_[p] = std::make_shared<Assimp::Importer>();
75 
76  const aiScene* envScene = importerEnv_[p]->ReadFile(env.c_str(),
77  aiProcess_Triangulate |
78  aiProcess_JoinIdenticalVertices |
79  aiProcess_SortByPType |
80  aiProcess_OptimizeGraph |
81  aiProcess_OptimizeMeshes);
82  if (envScene)
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  else
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 }
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.