37 #include <ompl/extensions/opende/OpenDESimpleSetup.h>
38 #include <ompl/base/goals/GoalRegion.h>
39 #include <ompl/config.h>
43 namespace ob = ompl::base;
44 namespace og = ompl::geometric;
45 namespace oc = ompl::control;
53 RigidBodyEnvironment(
void) : oc::OpenDEEnvironment()
58 virtual ~RigidBodyEnvironment(
void)
72 virtual void getControlBounds(std::vector<double> &lower, std::vector<double> &upper)
const
74 static double maxForce = 0.2;
88 dBodyAddForce(boxBody, control[0], control[1], control[2]);
96 virtual void setupContact(dGeomID , dGeomID , dContact &contact)
const
98 contact.surface.mode = dContactSoftCFM | dContactApprox1;
99 contact.surface.mu = 0.9;
100 contact.surface.soft_cfm = 0.2;
111 void createWorld(
void);
114 void destroyWorld(
void);
118 void setPlanningParameters(
void);
151 double dx = fabs(pos[0] - 30);
152 double dy = fabs(pos[1] - 55);
153 double dz = fabs(pos[2] - 35);
154 return sqrt(dx * dx + dy * dy + dz * dz);
165 RigidBodyStateProjectionEvaluator(
const ob::StateSpace *space) : ob::ProjectionEvaluator(space)
176 cellSizes_.resize(3);
185 projection[0] = pos[0];
186 projection[1] = pos[1];
187 projection[2] = pos[2];
205 double dx = fabs(p1[0] - p2[0]);
206 double dy = fabs(p1[1] - p2[1]);
207 double dz = fabs(p1[2] - p2[2]);
208 return sqrt(dx * dx + dy * dy + dz * dz);
220 int main(
int,
char **)
229 RigidBodyStateSpace *stateSpace =
new RigidBodyStateSpace(env);
236 ss.setGoal(
ob::GoalPtr(
new RigidBodyGoal(ss.getSpaceInformation())));
241 stateSpace->setVolumeBounds(bounds);
245 stateSpace->setLinearVelocityBounds(bounds);
246 stateSpace->setAngularVelocityBounds(bounds);
252 ss.getSolutionPath().asGeometric().printAsMatrix(std::cout);
272 void RigidBodyEnvironment::createWorld(
void)
277 bodyWorld = dWorldCreate();
278 space = dHashSpaceCreate(0);
280 dWorldSetGravity(bodyWorld, 0, 0, -0.981);
286 dMassSetBox(&m, 1, lx, ly, lz);
288 boxGeom = dCreateBox(space, lx, ly, lz);
289 boxBody = dBodyCreate(bodyWorld);
290 dBodySetMass(boxBody, &m);
291 dGeomSetBody(boxGeom, boxBody);
296 setPlanningParameters();
299 void RigidBodyEnvironment::destroyWorld(
void)
301 dSpaceDestroy(space);
302 dWorldDestroy(bodyWorld);
305 void RigidBodyEnvironment::setPlanningParameters(
void)
309 collisionSpaces_.push_back(space);
310 stateBodies_.push_back(boxBody);
313 minControlSteps_ = 10;
314 maxControlSteps_ = 500;
Create the set of classes typically needed to solve a control problem when forward propagation is com...
virtual void applyControl(const double *control) const =0
Application of a control. This function sets the forces/torques/velocities for bodies in the simulati...
A boost shared pointer wrapper for ompl::base::StateSpace.
virtual double distanceGoal(const State *st) const =0
Compute the distance to the goal (heuristic). This function is the one used in computing the distance...
CompoundState StateType
Define the type of state allocated by this state space.
virtual unsigned int getDimension(void) const =0
Return the dimension of the projection defined by this evaluator.
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.
void registerDefaultProjection(const ProjectionEvaluatorPtr &projection)
Register the default projection for this state space.
virtual void setupContact(dGeomID geom1, dGeomID geom2, dContact &contact) const
Parameters to set when contacts are created between geom1 and geom2.
const T * as(void) const
Cast this instance to a desired type.
virtual void registerProjections(void)
Register the projections for this state space. Usually, this is at least the default projection...
This class contains the OpenDE constructs OMPL needs to know about when planning. ...
State space representing OpenDE states.
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...
A boost shared pointer wrapper for ompl::base::ProjectionEvaluator.
Representation of a space in which planning can be performed. Topology specific sampling, interpolation and distance are defined.
boost::numeric::ublas::vector< double > EuclideanProjection
The datatype for state projections. This class contains a real vector.
Definition of an abstract state.
virtual double distance(const State *state1, const State *state2) const
Computes distance between two states. This function satisfies the properties of a metric if isMetricS...
virtual void project(const State *state, EuclideanProjection &projection) const =0
Compute the projection as an array of double values.
The lower and upper bounds for an Rn space.
virtual unsigned int getControlDimension(void) const =0
Number of parameters (double values) needed to specify a control input.
Definition of a goal region.
A boost shared pointer wrapper for ompl::base::Goal.
virtual void defaultCellSizes(void)
Set the default cell dimensions for this projection. The default implementation of this function is e...
Abstract definition for a class computing projections to Rn. Implicit integer grids are imposed on th...
A boost shared pointer wrapper for ompl::control::OpenDEEnvironment.