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 #include <assimp/scene.h>
22 #include <assimp/Importer.hpp>
23 #include <string>
24 #include <vector>
25 
26 namespace ompl
27 {
28 
30  namespace app
31  {
33  enum CollisionChecker
34  { PQP, FCL };
35 
36  class RigidBodyGeometry
37  {
38  public:
39 
41  explicit
44  RigidBodyGeometry(MotionModel mtype, CollisionChecker ctype) : mtype_(mtype), factor_(1.0), add_(0.0), ctype_(ctype)
45  {
46  }
47 
51  explicit
52  RigidBodyGeometry(MotionModel mtype) : mtype_(mtype), factor_(1.0), add_(0.0), ctype_(FCL)
53  {
54  }
55 
56  virtual ~RigidBodyGeometry() = default;
57 
58  MotionModel getMotionModel() const
59  {
60  return mtype_;
61  }
62 
63  CollisionChecker getCollisionCheckerType() const
64  {
65  return ctype_;
66  }
67 
68  bool hasEnvironment() const
69  {
70  return !importerEnv_.empty();
71  }
72 
73  bool hasRobot() const
74  {
75  return !importerRobot_.empty();
76  }
77 
78  unsigned int getLoadedRobotCount() const
79  {
80  return importerRobot_.size();
81  }
82 
84  aiVector3D getRobotCenter(unsigned int robotIndex) const;
85 
89  virtual bool setEnvironmentMesh(const std::string &env);
90 
94  virtual bool addEnvironmentMesh(const std::string &env);
95 
98  virtual bool setRobotMesh(const std::string &robot);
99 
102  virtual bool addRobotMesh(const std::string &robot);
103 
105  virtual void setStateValidityCheckerType (CollisionChecker ctype);
106 
108  const base::StateValidityCheckerPtr& allocStateValidityChecker(const base::SpaceInformationPtr &si, const GeometricStateExtractor &se, bool selfCollision);
109 
110  const GeometrySpecification& getGeometrySpecification() const;
111 
116  void setBoundsFactor(double factor)
117  {
118  factor_ = factor;
119  }
120 
122  double getBoundsFactor() const
123  {
124  return factor_;
125  }
126 
131  void setBoundsAddition(double add)
132  {
133  add_ = add;
134  }
135 
137  double getBoundsAddition() const
138  {
139  return add_;
140  }
141 
147 
148  protected:
149 
150  void computeGeometrySpecification();
151 
152  MotionModel mtype_;
153 
155  double factor_;
156 
158  double add_;
159 
161  std::vector< std::shared_ptr<Assimp::Importer> > importerEnv_;
162 
164  std::vector< std::shared_ptr<Assimp::Importer> > importerRobot_;
165 
168 
170  base::StateValidityCheckerPtr validitySvc_;
171 
174 
175  };
176 
177  }
178 }
179 
180 #endif
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, CollisionChecker ctype)
Constructor expects a state space that can represent a rigid body.
virtual void setStateValidityCheckerType(CollisionChecker ctype)
Change the type of collision checking for the rigid body.
MotionModel
Specify whether bodies are moving in 2D or bodies moving in 3D.
virtual bool setRobotMesh(const std::string &robot)
This function specifies the name of the CAD file representing the robot (robot). Returns 1 on success...
virtual bool setEnvironmentMesh(const std::string &env)
This function specifies the name of the CAD file representing the environment (env)....
std::vector< std::shared_ptr< Assimp::Importer > > importerEnv_
Instance of assimp importer used to load environment.
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)....
CollisionChecker
Enumeration of the possible collision checker types.
aiVector3D getRobotCenter(unsigned int robotIndex) const
Get the robot's center (average of all the vertices of all its parts)
std::vector< std::shared_ptr< Assimp::Importer > > importerRobot_
Instance of assimp importer used to load robot.
virtual bool addEnvironmentMesh(const std::string &env)
This function specifies the name of the CAD file representing a part of the environment (env)....
double getBoundsAddition() const
Get the data set by setBoundsAddition()
double getBoundsFactor() const
Get the data set by setBoundsFactor()
base::StateValidityCheckerPtr validitySvc_
Instance of the state validity checker for collision checking.
GeometrySpecification geom_
Object containing mesh data for robot and environment.
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...
CollisionChecker ctype_
Value containing the type of collision checking to use.
double factor_
The factor to multiply inferred environment bounds by (default 1)
double add_
The value to add to inferred environment bounds (default 0)
Main namespace. Contains everything in this library.
Definition: AppBase.h:20
The lower and upper bounds for an Rn space.