SE3MultiRigidBodyPlanning.cpp
1 /*********************************************************************
2 * Rice University Software Distribution License
3 *
4 * Copyright (c) 2012, Rice University
5 * All Rights Reserved.
6 *
7 * For a full description see the file named LICENSE.
8 *
9 *********************************************************************/
10 
11 /* Author: Ryan Luna */
12 
13 #include <omplapp/config.h>
14 #include <omplapp/apps/AppBase.h>
15 #include <omplapp/apps/SE3MultiRigidBodyPlanning.h>
16 #include <ompl/geometric/planners/rrt/RRTConnect.h>
17 
18 using namespace ompl;
19 
20 int main()
21 {
22  // plan for 2 rigid bodies in SE3
24 
25  // load the robot and the environment
26  std::string robot_fname = std::string(OMPLAPP_RESOURCE_DIR) + "/3D/cubicles_robot.dae";
27  std::string env_fname = std::string(OMPLAPP_RESOURCE_DIR) + "/3D/cubicles_env.dae";
28  setup.setRobotMesh(robot_fname.c_str()); // The first mesh should use setRobotMesh.
29  setup.addRobotMesh(robot_fname.c_str()); // Subsequent robot meshes MUST use addRobotMesh!
30  setup.setEnvironmentMesh(env_fname.c_str());
31 
32  // constructing start and goal states
33  base::ScopedState<base::CompoundStateSpace> start(setup.getSpaceInformation());
34  base::ScopedState<base::CompoundStateSpace> goal(setup.getSpaceInformation());
35 
37  // define start state (robot 1)
38  start1->setX(-4.96);
39  start1->setY(-40.62);
40  start1->setZ(70.57);
41  start1->rotation().setIdentity();
42 
43  // define goal state (robot 1)
45  goal1->setX(200.49);
46  goal1->setY(-40.62);
47  goal1->setZ(70.57);
48  goal1->rotation().setIdentity();
49 
51  // define start state (robot 2)
52  start2->setX(200.49);
53  start2->setY(-40.62);
54  start2->setZ(70.57);
55  start2->rotation().setIdentity();
56 
57  // define goal state (robot 2)
59  goal2->setX(-4.96);
60  goal2->setY(-40.62);
61  goal2->setZ(70.57);
62  goal2->rotation().setIdentity();
63 
64  // set the start & goal states
65  setup.setStartAndGoalStates(start, goal);
66 
67  // setting collision checking resolution to 1% of the space extent
68  setup.getSpaceInformation()->setStateValidityCheckingResolution(0.01);
69 
70  // use RRTConnect for planning
71  setup.setPlanner(std::make_shared<geometric::RRTConnect>(setup.getSpaceInformation()));
72 
73  // we call setup just so print() can show more information
74  setup.setup();
75  setup.print();
76 
77  // try to solve the problem
78  if (setup.solve(30))
79  {
80  if (setup.haveExactSolutionPath())
81  {
82  // simplify & print the solution
83  setup.simplifySolution();
84  setup.getSolutionPath().printAsMatrix(std::cout);
85  }
86  else
87  {
88  std::cout << "Exact solution not found" << std::endl;
89  }
90  }
91 
92  return 0;
93 }
Wrapper for ompl::app::RigidBodyPlanning that plans for multiple rigid bodies in SE3.
Definition of a scoped state.
Definition: ScopedState.h:56
A state in SE(3): position = (x, y, z), quaternion = (x, y, z, w)
Definition: SE3StateSpace.h:53
void setZ(double z)
Set the Z component of the state.
Main namespace. Contains everything in this library.
Definition: AppBase.h:21
void setY(double y)
Set the Y component of the state.
Definition: SE3StateSpace.h:97
const SO3StateSpace::StateType & rotation() const
Get the rotation component of the state.
Definition: SE3StateSpace.h:79
void setIdentity()
Set the state to identity – no rotation.
void setX(double x)
Set the X component of the state.
Definition: SE3StateSpace.h:91