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]);
91 virtual bool isValidCollision(dGeomID geom1, dGeomID geom2,
const dContact& contact)
const
96 virtual void setupContact(dGeomID geom1, dGeomID geom2, 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);
254 std::vector<ob::State*> &states = p.
getStates();
255 for (
unsigned int i = 0 ; i < states.size() ; ++i)
257 const double *pos = states[i]->as<
ob::CompoundState>()->as<ob::RealVectorStateSpace::StateType>(0)->values;
258 std::cout << pos[0] <<
" " << pos[1] <<
" " << pos[2] << std::endl;
280 void RigidBodyEnvironment::createWorld(
void)
285 bodyWorld = dWorldCreate();
286 space = dHashSpaceCreate(0);
288 dWorldSetGravity(bodyWorld, 0, 0, -0.981);
294 dMassSetBox(&m, 1, lx, ly, lz);
296 boxGeom = dCreateBox(space, lx, ly, lz);
297 boxBody = dBodyCreate(bodyWorld);
298 dBodySetMass(boxBody, &m);
299 dGeomSetBody(boxGeom, boxBody);
304 setPlanningParameters();
307 void RigidBodyEnvironment::destroyWorld(
void)
309 dSpaceDestroy(space);
310 dWorldDestroy(bodyWorld);
313 void RigidBodyEnvironment::setPlanningParameters(
void)
317 collisionSpaces_.push_back(space);
318 stateBodies_.push_back(boxBody);
321 minControlSteps_ = 10;
322 maxControlSteps_ = 500;