RigidBodyGeometry.h
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 #ifndef OMPLAPP_GEOMETRY_RIGID_BODY_GEOMETRY_
14 #define OMPLAPP_GEOMETRY_RIGID_BODY_GEOMETRY_
15 
16 #include "omplapp/config.h"
17 #include "omplapp/geometry/GeometrySpecification.h"
18 #include <ompl/base/SpaceInformation.h>
19 #include <ompl/base/spaces/RealVectorBounds.h>
20 #include <memory>
21 #if OMPL_HAS_ASSIMP3
22 #include <assimp/scene.h>
23 #include <assimp/Importer.hpp>
24 #else
25 #include <assimp/aiScene.h>
26 #include <assimp/assimp.hpp>
27 #endif
28 #include <string>
29 #include <vector>
30 
31 namespace ompl
32 {
33 
35  namespace app
36  {
39  { PQP, FCL };
40 
42  {
43  public:
44 
46  explicit
49  RigidBodyGeometry(MotionModel mtype, CollisionChecker ctype) : mtype_(mtype), factor_(1.0), add_(0.0), ctype_(ctype)
50  {
51  }
52 
56  explicit
57  RigidBodyGeometry(MotionModel mtype) : mtype_(mtype), factor_(1.0), add_(0.0), ctype_(FCL)
58  {
59  }
60 
61  virtual ~RigidBodyGeometry() = default;
62 
63  MotionModel getMotionModel() const
64  {
65  return mtype_;
66  }
67 
68  CollisionChecker getCollisionCheckerType() const
69  {
70  return ctype_;
71  }
72 
73  bool hasEnvironment() const
74  {
75  return !importerEnv_.empty();
76  }
77 
78  bool hasRobot() const
79  {
80  return !importerRobot_.empty();
81  }
82 
83  unsigned int getLoadedRobotCount() const
84  {
85  return importerRobot_.size();
86  }
87 
89  aiVector3D getRobotCenter(unsigned int robotIndex) const;
90 
94  virtual bool setEnvironmentMesh(const std::string &env);
95 
99  virtual bool addEnvironmentMesh(const std::string &env);
100 
103  virtual bool setRobotMesh(const std::string &robot);
104 
107  virtual bool addRobotMesh(const std::string &robot);
108 
110  virtual void setStateValidityCheckerType (CollisionChecker ctype);
111 
113  const base::StateValidityCheckerPtr& allocStateValidityChecker(const base::SpaceInformationPtr &si, const GeometricStateExtractor &se, bool selfCollision);
114 
115  const GeometrySpecification& getGeometrySpecification() const;
116 
121  void setBoundsFactor(double factor)
122  {
123  factor_ = factor;
124  }
125 
127  double getBoundsFactor() const
128  {
129  return factor_;
130  }
131 
136  void setBoundsAddition(double add)
137  {
138  add_ = add;
139  }
140 
142  double getBoundsAddition() const
143  {
144  return add_;
145  }
146 
152 
153  protected:
154 
155  void computeGeometrySpecification();
156 
157  MotionModel mtype_;
158 
160  double factor_;
161 
163  double add_;
164 
166  std::vector< std::shared_ptr<Assimp::Importer> > importerEnv_;
167 
169  std::vector< std::shared_ptr<Assimp::Importer> > importerRobot_;
170 
173 
176 
179 
180  };
181 
182  }
183 }
184 
185 #endif
double getBoundsFactor() const
Get the data set by setBoundsFactor()
double add_
The value to add to inferred environment bounds (default 0)
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 getBoundsAddition() const
Get the data set by setBoundsAddition()
double factor_
The factor to multiply inferred environment bounds by (default 1)
Main namespace. Contains everything in this library.
Definition: AppBase.h:21
aiVector3D getRobotCenter(unsigned int robotIndex) const
Get the robot&#39;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 PQP.
base::RealVectorBounds inferEnvironmentBounds() const
Given the representation of an environment, infer its bounds. The bounds will be 2-dimensional when p...
CollisionChecker ctype_
Value containing the type of collision checking to use.
A shared pointer wrapper for ompl::base::SpaceInformation.
MotionModel
Specify whether bodies are moving in 2D or bodies moving in 3D.
CollisionChecker
Enumeration of the possible collision checker types.
base::StateValidityCheckerPtr validitySvc_
Instance of the state validity checker for collision checking.
void setBoundsFactor(double factor)
The bounds of the environment are inferred based on the axis-aligned bounding box for the objects in ...
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.
void setBoundsAddition(double add)
The bounds of the environment are inferred based on the axis-aligned bounding box for the objects in ...
RigidBodyGeometry(MotionModel mtype)
Constructor expects a state space that can represent a 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.
RigidBodyGeometry(MotionModel mtype, CollisionChecker ctype)
Constructor expects a state space that can represent a rigid body.