OpenDEEnvironment.h
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2010, Rice University
5 * All rights reserved.
6 *
7 * Redistribution and use in source and binary forms, with or without
8 * modification, are permitted provided that the following conditions
9 * are met:
10 *
11 * * Redistributions of source code must retain the above copyright
12 * notice, this list of conditions and the following disclaimer.
13 * * Redistributions in binary form must reproduce the above
14 * copyright notice, this list of conditions and the following
15 * disclaimer in the documentation and/or other materials provided
16 * with the distribution.
17 * * Neither the name of the Rice University nor the names of its
18 * contributors may be used to endorse or promote products derived
19 * from this software without specific prior written permission.
20 *
21 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 * POSSIBILITY OF SUCH DAMAGE.
33 *********************************************************************/
34 
35 /* Author: Ioan Sucan */
36 
37 #ifndef OMPL_EXTENSION_OPENDE_ENVIRONMENT_
38 #define OMPL_EXTENSION_OPENDE_ENVIRONMENT_
39 
40 #include "ompl/config.h"
41 #if OMPL_EXTENSION_ODE == 0
42 #error OpenDE extension not built
43 #endif
44 
45 #include "ompl/util/ClassForward.h"
46 
47 #include <ode/ode.h>
48 #include <vector>
49 #include <string>
50 #include <map>
51 #include <mutex>
52 
53 namespace ompl
54 {
55  namespace control
56  {
58 
59  OMPL_CLASS_FORWARD(OpenDEEnvironment);
61 
66  class OpenDEEnvironment
67  {
68  public:
70  dWorldID world_{nullptr};
71 
73  std::vector<dSpaceID> collisionSpaces_;
74 
78  std::vector<dBodyID> stateBodies_;
79 
81  std::map<dGeomID, std::string> geomNames_;
82 
85  bool verboseContacts_{false};
86 
88  dJointGroupID contactGroup_;
89 
91  unsigned int maxContacts_{3};
92 
94  double stepSize_{0.05};
95 
97  unsigned int maxControlSteps_{100};
98 
100  unsigned int minControlSteps_{5};
101 
103  mutable std::mutex mutex_;
104 
105  OpenDEEnvironment()
106  {
107  contactGroup_ = dJointGroupCreate(0);
108  }
109 
110  virtual ~OpenDEEnvironment()
111  {
112  if (contactGroup_ != nullptr)
113  dJointGroupDestroy(contactGroup_);
114  }
115 
117  virtual unsigned int getControlDimension() const = 0;
118 
120  virtual void getControlBounds(std::vector<double> &lower, std::vector<double> &upper) const = 0;
121 
125  virtual void applyControl(const double *control) const = 0;
126 
131  virtual bool isValidCollision(dGeomID geom1, dGeomID geom2, const dContact &contact) const;
132 
136  virtual unsigned int getMaxContacts(dGeomID geom1, dGeomID geom2) const;
137 
139  virtual void setupContact(dGeomID geom1, dGeomID geom2, dContact &contact) const;
140 
142  std::string getGeomName(dGeomID geom) const;
143 
145  void setGeomName(dGeomID geom, const std::string &name);
146  };
147  }
148 }
149 
150 #endif
double stepSize_
The simulation step size.
virtual void getControlBounds(std::vector< double > &lower, std::vector< double > &upper) const =0
Get the control bounds – the bounding box in which to sample controls.
unsigned int minControlSteps_
The minimum number of times a control is applies in sequence.
unsigned int maxControlSteps_
The maximum number of times a control is applies in sequence.
virtual unsigned int getControlDimension() const =0
Number of parameters (double values) needed to specify a control input.
virtual bool isValidCollision(dGeomID geom1, dGeomID geom2, const dContact &contact) const
Decide whether a collision is a valid one or not. In some cases, collisions between some bodies can b...
std::vector< dBodyID > stateBodies_
The set of bodies that need to be considered part of the state when planning. This is not necessarily...
std::mutex mutex_
Lock to use when performing simulations in the world. (OpenDE simulations are NOT thread safe)
std::string getGeomName(dGeomID geom) const
Get the name of a body.
dWorldID world_
The OpenDE world where the simulation is performed.
dJointGroupID contactGroup_
The group of joints where contacts are created.
virtual void applyControl(const double *control) const =0
Application of a control. This function sets the forces/torques/velocities for bodies in the simulati...
unsigned int maxContacts_
The maximum number of contacts to create between two bodies when a collision occurs.
virtual unsigned int getMaxContacts(dGeomID geom1, dGeomID geom2) const
Get the maximum number of contacts to set up between two colliding geoms. By default,...
void setGeomName(dGeomID geom, const std::string &name)
Set the name of a body.
virtual void setupContact(dGeomID geom1, dGeomID geom2, dContact &contact) const
Parameters to set when contacts are created between geom1 and geom2.
std::vector< dSpaceID > collisionSpaces_
The set of spaces where contacts need to be evaluated before simulation takes place.
bool verboseContacts_
Issue debug messages when contacts are found. Default is false. This should only be used for debuggin...
std::map< dGeomID, std::string > geomNames_
Optional map of names given to geoms. This is useful when collision checking is verbose.
Main namespace. Contains everything in this library.