一、安裝OMPL Installation
1. 建議直接下載對應操作系統平臺的shell腳本文件進行安裝即可：install-ompl-ubuntu.sh
2. 接下來修改腳本的運行權限並執行安裝
chmod u+x install-ompl-ubuntu.sh ./install-ompl-ubuntu.sh --app # 建議安裝過程使用--app選項，安裝後能包含app程序，同時能夠包括C++的庫函數，Python庫
3. 耐心等待安裝，由於此腳本需要在github上下載相關待編譯的源代碼，速度可能較慢，或者自行切換使用VPN安裝下載。
二、ompl_app簡單使用
1. 首先需要下載ompl-app源代碼文件，其中包括了我們需要的能夠加載的3D或者2D地圖以及運動物體：/omplapp/resources
2. 在Ubuntu系統終端直接運行omplapp即可啓動omplapp軟件進行基本的算法測試：omplapp
3. 更加詳細的操作步驟請參考官方文檔：OMPL_Primer.pdf
三、在官方網站首頁上可以找到Tutorials & Demo的入門(SourceCode-Download)
1. 2D空間中剛體的幾何路徑規劃-Python
a) 可用空間檢查器的設計
def isStateValid(state): # Some arbitrary condition on the state (note that thanks to # dynamic type checking we can just call getX() and do not need # to convert state to an SE2State.) return not (state.getX() < .4 and state.getX() > -.3 and state.getY() < .2 and state.getY() > -.2)
b) 空間規劃結果及可用空間可視化-MATLAB
%% OMPL 2D Data Viewer close all PUnValidState = [[-0.3,-0.2];[-0.3,0.2];[0.4,0.2];[0.4,-0.2]]; PState = [[-0.0584209 -0.71908];[-0.571242 -0.246387];[-0.445504 0.135975];[-0.793787 0.445143]]; % [[0.5 -0.816236];[0.427809 0.0547745];[0.38762 0.27163];[0.236824 0.52535];[-0.5 0.101018]]; figure, line(PUnValidState(:,1),PUnValidState(:,2)) patch(PUnValidState(:,1),PUnValidState(:,2),'red') hold on plot(PState(:,1),PState(:,2))
可視化結果：
c) 全部代碼-Python3
1 #!/usr/bin/env python 2 3 4 5 # Author: Mark Moll 6 7 try: 8 from ompl import base as ob 9 from ompl import geometric as og 10 except ImportError: 11 # if the ompl module is not in the PYTHONPATH assume it is installed in a 12 # subdirectory of the parent directory called "py-bindings." 13 from os.path import abspath, dirname, join 14 import sys 15 sys.path.insert(0, join(dirname(dirname(abspath(__file__))), 'py-bindings')) 16 from ompl import base as ob 17 from ompl import geometric as og 18 19 def isStateValid(state): 20 # Some arbitrary condition on the state (note that thanks to 21 # dynamic type checking we can just call getX() and do not need 22 # to convert state to an SE2State.) 23 return state.getX() < .6 24 25 def planWithSimpleSetup(): 26 # create an SE2 state space 27 space = ob.SE2StateSpace() 28 29 # set lower and upper bounds 30 bounds = ob.RealVectorBounds(2) 31 bounds.setLow(-1) 32 bounds.setHigh(1) 33 space.setBounds(bounds) 34 35 # create a simple setup object 36 ss = og.SimpleSetup(space) 37 ss.setStateValidityChecker(ob.StateValidityCheckerFn(isStateValid)) 38 39 start = ob.State(space) 40 # we can pick a random start state... 41 start.random() 42 # ... or set specific values 43 start().setX(.5) 44 45 goal = ob.State(space) 46 # we can pick a random goal state... 47 goal.random() 48 # ... or set specific values 49 goal().setX(-.5) 50 51 ss.setStartAndGoalStates(start, goal) 52 53 # this will automatically choose a default planner with 54 # default parameters 55 solved = ss.solve(1.0) 56 57 if solved: 58 # try to shorten the path 59 ss.simplifySolution() 60 # print the simplified path 61 print(ss.getSolutionPath()) 62 63 64 def planTheHardWay(): 65 # create an SE2 state space 66 space = ob.SE2StateSpace() 67 # set lower and upper bounds 68 bounds = ob.RealVectorBounds(2) 69 bounds.setLow(-1) 70 bounds.setHigh(1) 71 space.setBounds(bounds) 72 # construct an instance of space information from this state space 73 si = ob.SpaceInformation(space) 74 # set state validity checking for this space 75 si.setStateValidityChecker(ob.StateValidityCheckerFn(isStateValid)) 76 # create a random start state 77 start = ob.State(space) 78 start.random() 79 # create a random goal state 80 goal = ob.State(space) 81 goal.random() 82 # create a problem instance 83 pdef = ob.ProblemDefinition(si) 84 # set the start and goal states 85 pdef.setStartAndGoalStates(start, goal) 86 # create a planner for the defined space 87 planner = og.RRTConnect(si) 88 # set the problem we are trying to solve for the planner 89 planner.setProblemDefinition(pdef) 90 # perform setup steps for the planner 91 planner.setup() 92 # print the settings for this space 93 print(si.settings()) 94 # print the problem settings 95 print(pdef) 96 # attempt to solve the problem within one second of planning time 97 solved = planner.solve(1.0) 98 99 if solved: 100 # get the goal representation from the problem definition (not the same as the goal state) 101 # and inquire about the found path 102 path = pdef.getSolutionPath() 103 print("Found solution:\n%s" % path) 104 else: 105 print("No solution found") 106 107 108 if __name__ == "__main__": 109 planWithSimpleSetup() 110 print("") 111 planTheHardWay()
使用python執行
python3 RigidBodyPlanning.py
運行結果：
Info: RRTConnect: Starting planning with 1 states already in datastructure Info: RRTConnect: Created 6 states (3 start + 3 goal) Found solution: Geometric path with 5 states Compound state [ RealVectorState [-0.758775 -0.337708] SO2State [0.637024] ] Compound state [ RealVectorState [-0.585243 0.179672] SO2State [1.3053] ] Compound state [ RealVectorState [0.0897389 0.464516] SO2State [1.59975] ] Compound state [ RealVectorState [0.160821 0.230265] SO2State [1.68043] ] Compound state [ RealVectorState [0.862616 0.277856] SO2State [2.03331] ]
3. 3D空間中剛體的幾何路徑規劃-Cpp
a) 可用空間檢查器的設計
1 bool isStateValid(const ob::State *state) 2 { 3 // cast the abstract state type to the type we expect 4 const auto *se3state = state->as<ob::SE3StateSpace::StateType>(); 5 6 // extract the first component of the state and cast it to what we expect 7 const auto *pos = se3state->as<ob::RealVectorStateSpace::StateType>(0); 8 9 // extract the second component of the state and cast it to what we expect 10 const auto *rot = se3state->as<ob::SO3StateSpace::StateType>(1); 11 12 #if 1 13 // check validity of state defined by pos & rot 14 // pos->operator[0]= 0.1; 15 // std::cout << "Valid Position X Checkout:" << pos->values[0] << std::endl; // PrintOut the RealVectorStateSpace's X Axis. 16 // std::cout << "Valid Position Y Checkout:" << pos->values[1] << std::endl; // PrintOut the RealVectorStateSpace's Y Axis. 17 // std::cout << "Valid Position Z Checkout:" << pos->values[2] << std::endl; // PrintOut the RealVectorStateSpace's Z Axis. 18 19 // std::cout << "Valid Gesture Roll Checkout:" << rot->x << std::endl; // PrintOut the SO3StateSpace's Roll Angle. 20 // rot->setAxisAngle(0,0,0.5,0.8); 21 // std::cout << "Valid Gesture Yaw Checkout:" << rot->y << std::endl; // PrintOut the SO3StateSpace's Yaw Angle. 22 // std::cout << "Valid Gesture Pitch Checkout:" << rot->z << std::endl; // PrintOut the SO3StateSpace's Pitch Angle. 23 24 if((pos->values[0] < 0.7 && pos->values[0] > -0.3) && (pos->values[1] < 0.7 && pos->values[1] > -0.4) && (pos->values[2] < 0.6 && pos->values[2] > -0.2)) 25 { 26 return false; 27 } 28 else 29 { 30 return true; 31 } 32 #endif 33 34 // return a value that is always true but uses the two variables we define, so we avoid compiler warnings 35 // return (const void*)rot != (const void*)pos; 36 }
上述三維空間檢查器僅僅對物體位置進行了基本的檢查，對於物體的姿態未進行檢查，位置檢測爲三維笛卡爾座標系空間檢查，而對於姿態則是使用四元數來進行表示。上述檢查器構建了一個三維空間矩形的障礙物。
b) 空間規劃結果及可用空間可視化-MATLAB
1 close all 2 PUnValidStateXRange = linspace(-0.3,0.7,50); 3 PUnValidStateYRange = linspace(-0.4,0.7,50); 4 PUnValidStateZRange = linspace(-0.2,0.6,50); 5 MeshZ = [PUnValidStateXRange;PUnValidStateYRange;PUnValidStateZRange]; 6 PState = [[-0.9 -0.9 -0.9];[-0.730959 -0.434943 -0.477706];[-0.483787 -0.690545 -0.372619];[-0.182592 -0.119642 -0.411141]; 7 [0.0530512 -0.272759 -0.651536];[0.113694 -0.336638 -0.621017];[0.264269 -0.439495 -0.16104];[0.494128 -0.596511 0.541132]; 8 [0.632976 -0.251955 0.739708];[0.811015 0.126208 0.500853];[0.685227 0.363121 0.70494];[0.9 0.9 0.9]]; % [[0.5 -0.816236];[0.427809 0.0547745];[0.38762 0.27163];[0.236824 0.52535];[-0.5 0.101018]]; 9 figure, 10 % scatter3(PUnValidStateXRange,PUnValidStateYRange,PUnValidStateZRange, 'k') 11 plot3(PState(:,1),PState(:,2),PState(:,3),'r') 12 13 % xyz方向邊長 14 a=1; 15 b=1.1; 16 c=0.8; 17 18 startPos = [-0.3 -0.4 -0.2]; 19 20 % 8個頂點xyz座標 21 verts = [0 0 0;a 0 0;0 b 0;0 0 c; 22 a b c;0 b c;a 0 c;a b 0]; 23 24 verts = verts + startPos; 25 26 % 6個面頂點編號 27 faces = [1 2 7 4;1 3 6 4;1 2 8 3; 28 5 8 3 6;5 7 2 8;5 6 4 7]; 29 30 % 作圖 31 hold on; 32 patch('Faces',faces,'Vertices',verts,'Facecolor','none') 33 grid minor;%axis equal; 34 axis([-1 1 -1 1 -1 1]);
可視化結果：
c) 全部代碼-Cpp
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 #include <ompl/base/SpaceInformation.h> 38 #include <ompl/base/spaces/SE3StateSpace.h> 39 #include <ompl/geometric/planners/rrt/RRTConnect.h> 40 #include <ompl/geometric/SimpleSetup.h> 41 42 #include <ompl/config.h> 43 #include <iostream> 44 45 namespace ob = ompl::base; 46 namespace og = ompl::geometric; 47 48 bool isStateValid(const ob::State *state) 49 { 50 // cast the abstract state type to the type we expect 51 const auto *se3state = state->as<ob::SE3StateSpace::StateType>(); 52 53 // extract the first component of the state and cast it to what we expect 54 const auto *pos = se3state->as<ob::RealVectorStateSpace::StateType>(0); 55 56 // extract the second component of the state and cast it to what we expect 57 const auto *rot = se3state->as<ob::SO3StateSpace::StateType>(1); 58 59 #if 1 60 // check validity of state defined by pos & rot 61 // pos->operator[0]= 0.1; 62 // std::cout << "Valid Position X Checkout:" << pos->values[0] << std::endl; // PrintOut the RealVectorStateSpace's X Axis. 63 // std::cout << "Valid Position Y Checkout:" << pos->values[1] << std::endl; // PrintOut the RealVectorStateSpace's Y Axis. 64 // std::cout << "Valid Position Z Checkout:" << pos->values[2] << std::endl; // PrintOut the RealVectorStateSpace's Z Axis. 65 66 // std::cout << "Valid Gesture Roll Checkout:" << rot->x << std::endl; // PrintOut the SO3StateSpace's Roll Angle. 67 // rot->setAxisAngle(0,0,0.5,0.8); 68 // std::cout << "Valid Gesture Yaw Checkout:" << rot->y << std::endl; // PrintOut the SO3StateSpace's Yaw Angle. 69 // std::cout << "Valid Gesture Pitch Checkout:" << rot->z << std::endl; // PrintOut the SO3StateSpace's Pitch Angle. 70 71 if((pos->values[0] < 0.7 && pos->values[0] > -0.3) && (pos->values[1] < 0.7 && pos->values[1] > -0.4) && (pos->values[2] < 0.6 && pos->values[2] > -0.2)) 72 { 73 return false; 74 } 75 else 76 { 77 return true; 78 } 79 #endif 80 81 // return a value that is always true but uses the two variables we define, so we avoid compiler warnings 82 // return (const void*)rot != (const void*)pos; 83 } 84 85 void plan() 86 { 87 // construct the state space we are planning in 88 auto space(std::make_shared<ob::SE3StateSpace>()); 89 90 // set the bounds for the R^3 part of SE(3) 91 ob::RealVectorBounds bounds(3); 92 bounds.setLow(-1); 93 bounds.setHigh(1); 94 95 space->setBounds(bounds); 96 97 // construct an instance of space information from this state space 98 auto si(std::make_shared<ob::SpaceInformation>(space)); 99 100 // set state validity checking for this space 101 si->setStateValidityChecker(isStateValid); 102 103 // create a random start state 104 ob::ScopedState<> start(space); 105 start.random(); 106 std::cout << "################### Start ################" << std::endl; 107 start.print(); 108 start[0] = start[1] = start[2] = -0.9; 109 std::cout << "Start[0]" << start[0] << "," << start[1] << "," << start[2] << "," << std::endl; 110 std::cout << "Start[0]" << start[3] << "," << start[4] << "," << start[5] << "," << std::endl; 111 // start[0] = start[1] = start[2] = -0.9; 112 113 // create a random goal state 114 ob::ScopedState<> goal(space); 115 goal.random(); 116 goal[0] = goal[1] = goal[2] = 0.9; 117 std::cout << "################### Ended ################" << std::endl; 118 goal.print(); 119 // goal[0] = goal[1] = goal[2] = 0.9; 120 121 // create a problem instance 122 auto pdef(std::make_shared<ob::ProblemDefinition>(si)); 123 124 // set the start and goal states 125 pdef->setStartAndGoalStates(start, goal); 126 127 // create a planner for the defined space 128 auto planner(std::make_shared<og::RRTConnect>(si)); 129 130 // set the problem we are trying to solve for the planner 131 planner->setProblemDefinition(pdef); 132 133 // perform setup steps for the planner 134 planner->setup(); 135 136 137 // print the settings for this space 138 si->printSettings(std::cout); 139 140 // print the problem settings 141 pdef->print(std::cout); 142 143 // attempt to solve the problem within one second of planning time 144 ob::PlannerStatus solved = planner->ob::Planner::solve(10.0); 145 146 if (solved) 147 { 148 // get the goal representation from the problem definition (not the same as the goal state) 149 // and inquire about the found path 150 ob::PathPtr path = pdef->getSolutionPath(); 151 std::cout << "Found solution:" << std::endl; 152 153 // print the path to screen 154 path->print(std::cout); 155 } 156 else 157 std::cout << "No solution found" << std::endl; 158 } 159 160 void planWithSimpleSetup() 161 { 162 // construct the state space we are planning in 163 auto space(std::make_shared<ob::SE3StateSpace>()); 164 165 // set the bounds for the R^3 part of SE(3) 166 ob::RealVectorBounds bounds(3); 167 bounds.setLow(-1); 168 bounds.setHigh(1); 169 170 space->setBounds(bounds); 171 172 // define a simple setup class 173 og::SimpleSetup ss(space); 174 175 // set state validity checking for this space 176 ss.setStateValidityChecker([](const ob::State *state) { return isStateValid(state); }); 177 178 // create a random start state 179 ob::ScopedState<> start(space); 180 start.random(); 181 start[0] = 0; 182 start[1] = 0.8; 183 start[2] = 0; 184 185 // create a random goal state 186 ob::ScopedState<> goal(space); 187 goal.random(); 188 goal[0] = 0; 189 goal[1] = -0.8; 190 goal[2] = 0; 191 192 // set the start and goal states 193 ss.setStartAndGoalStates(start, goal); 194 195 // this call is optional, but we put it in to get more output information 196 ss.setup(); 197 ss.print(); 198 199 // attempt to solve the problem within one second of planning time 200 ob::PlannerStatus solved = ss.solve(10.0); 201 202 if (solved) 203 { 204 std::cout << "Found solution:" << std::endl; 205 // print the path to screen 206 ss.simplifySolution(); 207 ss.getSolutionPath().print(std::cout); 208 } 209 else 210 std::cout << "No solution found" << std::endl; 211 } 212 213 int main(int /*argc*/, char ** /*argv*/) 214 { 215 std::cout << "OMPL version: " << OMPL_VERSION << std::endl; 216 217 plan(); 218 219 std::cout << std::endl << std::endl; 220 221 planWithSimpleSetup(); 222 223 return 0; 224 }
編譯並執行
g++ -o main RigidBodyPlanning.cpp -lompl ./main
運行結果：
ubuntu@ubuntu-MS-7C88:~/SambaFolder/OMPL-Learning/ompl_learning/DemoCode$ ./main OMPL version: 1.2.1 ################### Start ################ Compound state [ RealVectorState [0.18123 -0.645509 0.524337] SO3State [0.057564 -0.292701 -0.834611 -0.463074] ] Start[0]-0.9,-0.9,-0.9, Start[0]0.057564,-0.292701,-0.834611, ################### Ended ################ Compound state [ RealVectorState [0.9 0.9 0.9] SO3State [-0.727838 -0.0505386 0.666869 -0.151602] ] Info: RRTConnect: Space information setup was not yet called. Calling now. Debug: RRTConnect: Planner range detected to be 1.006980 Settings for the state space 'SE3CompoundSpace0' - state validity check resolution: 1% - valid segment count factor: 1 - state space: Compound state space 'SE3CompoundSpace0' of dimension 6 (locked) [ Real vector state space 'RealVectorSpace1' of dimension 3 with bounds: - min: -1 -1 -1 - max: 1 1 1 of weight 1 SO(3) state space 'SO3Space2' (represented using quaternions) of weight 1 ] Registered projections: - <default> Projection of dimension 3 Cell sizes (computed defaults): [0.1 0.1 0.1] Declared parameters: longest_valid_segment_fraction = 0.01 projection.cellsize.0 = 0.10000000000000001 projection.cellsize.1 = 0.10000000000000001 projection.cellsize.2 = 0.10000000000000001 projection.cellsize_factor = valid_segment_count_factor = 1 Valid state sampler named uniform with parameters: nr_attempts = 100 Start states: Compound state [ RealVectorState [-0.9 -0.9 -0.9] SO3State [0.057564 -0.292701 -0.834611 -0.463074] ] Goal state, threshold = 2.22045e-16, memory address = 0x55f839a13250, state = Compound state [ RealVectorState [0.9 0.9 0.9] SO3State [-0.727838 -0.0505386 0.666869 -0.151602] ] OptimizationObjective = nullptr There are 0 solutions Info: RRTConnect: Starting planning with 1 states already in datastructure Info: RRTConnect: Created 12 states (7 start + 5 goal) Found solution: Geometric path with 10 states Compound state [ RealVectorState [-0.9 -0.9 -0.9] SO3State [0.057564 -0.292701 -0.834611 -0.463074] ] Compound state [ RealVectorState [-0.755399 -0.488916 -0.461162] SO3State [0.231568 -0.507052 -0.592245 -0.581825] ] Compound state [ RealVectorState [-0.300681 -0.20833 -0.107023] SO3State [0.395311 -0.434841 -0.748857 -0.306359] ] Compound state [ RealVectorState [-0.508495 -0.181512 0.187069] SO3State [-0.209415 -0.390755 -0.747933 -0.494016] ] Compound state [ RealVectorState [-0.509982 -0.233658 0.50644] SO3State [-0.587153 -0.529911 -0.217836 -0.571833] ] Compound state [ RealVectorState [-0.827995 -0.443582 0.695907] SO3State [-0.263542 -0.339063 0.0636545 -0.90085] ] Compound state [ RealVectorState [-0.437907 -0.227285 0.671053] SO3State [-0.395403 -0.230022 0.301636 -0.836518] ] Compound state [ RealVectorState [0.0872723 0.0639187 0.637593] SO3State [-0.514997 -0.0525998 0.575561 -0.633041] ] Compound state [ RealVectorState [0.612451 0.355122 0.604132] SO3State [-0.551049 0.133356 0.756119 -0.326873] ] Compound state [ RealVectorState [0.9 0.9 0.9] SO3State [-0.727838 -0.0505386 0.666869 -0.151602] ] Info: No planner specified. Using default. Info: LBKPIECE1: Attempting to use default projection. Debug: LBKPIECE1: Planner range detected to be 1.006980 Properties of the state space 'SE3CompoundSpace3' - signature: 6 5 6 1 3 3 3 - dimension: 6 - extent: 5.0349 - sanity checks for state space passed - probability of valid states: 0.872 - average length of a valid motion: 1.78183 - average number of samples drawn per second: sampleUniform()=5.09054e+06 sampleUniformNear()=3.12507e+06 sampleGaussian()=3.03362e+06 Settings for the state space 'SE3CompoundSpace3' - state validity check resolution: 1% - valid segment count factor: 1 - state space: Compound state space 'SE3CompoundSpace3' of dimension 6 (locked) [ Real vector state space 'RealVectorSpace4' of dimension 3 with bounds: - min: -1 -1 -1 - max: 1 1 1 of weight 1 SO(3) state space 'SO3Space5' (represented using quaternions) of weight 1 ] Registered projections: - <default> Projection of dimension 3 Cell sizes (computed defaults): [0.1 0.1 0.1] Declared parameters: longest_valid_segment_fraction = 0.01 projection.cellsize.0 = 0.10000000000000001 projection.cellsize.1 = 0.10000000000000001 projection.cellsize.2 = 0.10000000000000001 projection.cellsize_factor = valid_segment_count_factor = 1 Valid state sampler named uniform with parameters: nr_attempts = 100 Planner LBKPIECE1 specs: Multithreaded: No Reports approximate solutions: No Can optimize solutions: No Aware of the following parameters: border_fraction min_valid_path_fraction range Declared parameters for planner LBKPIECE1: border_fraction = 0.90000000000000002 min_valid_path_fraction = 0.5 range = 1.0069795883865302 Start states: Compound state [ RealVectorState [0 0.8 0] SO3State [0.0966151 0.600741 -0.0963739 0.78771] ] Goal state, threshold = 2.22045e-16, memory address = 0x55f839a0a110, state = Compound state [ RealVectorState [0 -0.8 0] SO3State [0.218671 0.951605 0.199975 0.0814875] ] OptimizationObjective = nullptr There are 0 solutions Info: LBKPIECE1: Starting planning with 1 states already in datastructure Info: LBKPIECE1: Created 327 (216 start + 111 goal) states in 321 cells (213 start (213 on boundary) + 108 goal (108 on boundary)) Info: Solution found in 0.004338 seconds Found solution: Debug: The solution path was slightly touching on an invalid region of the state space, but it was successfully fixed. Info: SimpleSetup: Path simplification took 0.000579 seconds and changed from 145 to 49 states Geometric path with 49 states Compound state [ RealVectorState [0 0.8 0] SO3State [0.0966151 0.600741 -0.0963739 0.78771] ] Compound state [ RealVectorState [-0.00108381 0.802151 -0.000333617] SO3State [0.0971981 0.605351 -0.0889379 0.78498] ] Compound state [ RealVectorState [-0.00216762 0.804301 -0.000667235] SO3State [0.0977729 0.609909 -0.0814944 0.782183] ] Compound state [ RealVectorState [-0.00325143 0.806452 -0.00100085] SO3State [0.0983394 0.614416 -0.0740441 0.77932] ] Compound state [ RealVectorState [-0.00433524 0.808602 -0.00133447] SO3State [0.0988977 0.618871 -0.0665875 0.776392] ] Compound state [ RealVectorState [-0.00638583 0.808301 -0.00151073] SO3State [0.0993195 0.620682 -0.0595546 0.775463] ] Compound state [ RealVectorState [-0.00843642 0.807999 -0.001687] SO3State [0.099736 0.622459 -0.0525186 0.774493] ] Compound state [ RealVectorState [-0.010487 0.807698 -0.00186326] SO3State [0.100147 0.624203 -0.0454798 0.773481] ] Compound state [ RealVectorState [-0.0125376 0.807396 -0.00203953] SO3State [0.100553 0.625914 -0.0384385 0.772427] ] Compound state [ RealVectorState [-0.0163061 0.806655 -0.00234395] SO3State [0.101258 0.628688 -0.0258106 0.770605] ] Compound state [ RealVectorState [-0.0200747 0.805914 -0.00264838] SO3State [0.101946 0.631354 -0.0131783 0.768652] ] Compound state [ RealVectorState [-0.0358327 0.802292 -0.00386699] SO3State [0.104567 0.640567 0.0387669 0.759761] ] Compound state [ RealVectorState [-0.0515908 0.79867 -0.0050856] SO3State [0.106888 0.647942 0.0906008 0.74869] ] Compound state [ RealVectorState [-0.0913278 0.789287 -0.00813259] SO3State [0.111346 0.65795 0.219417 0.71173] ] Compound state [ RealVectorState [-0.131065 0.779904 -0.0111796] SO3State [0.113791 0.656063 0.344267 0.661902] ] Compound state [ RealVectorState [-0.172057 0.770143 -0.0143142] SO3State [0.114155 0.641618 0.466396 0.598139] ] Compound state [ RealVectorState [-0.213049 0.760382 -0.0174488] SO3State [0.112329 0.61486 0.579574 0.522899] ] Compound state [ RealVectorState [-0.232571 0.755624 -0.0189304] SO3State [0.110701 0.597863 0.629563 0.48369] ] Compound state [ RealVectorState [-0.252093 0.750866 -0.0204119] SO3State [0.108594 0.578279 0.676827 0.442387] ] Compound state [ RealVectorState [-0.267202 0.736468 -0.0206945] SO3State [0.106613 0.555611 0.70539 0.42703] ] Compound state [ RealVectorState [-0.282311 0.72207 -0.020977] SO3State [0.104465 0.532071 0.732847 0.411002] ] Compound state [ RealVectorState [-0.346239 0.720743 -0.0688153] SO3State [0.105176 0.445799 0.799653 0.388272] ] Compound state [ RealVectorState [-0.337815 0.644706 -0.0200525] SO3State [0.095298 0.426435 0.811452 0.388094] ] Compound state [ RealVectorState [-0.371888 0.593882 -0.0192178] SO3State [0.0888149 0.355933 0.850842 0.376152] ] Compound state [ RealVectorState [-0.405961 0.543058 -0.0183831] SO3State [0.0817361 0.283045 0.884526 0.361688] ] Compound state [ RealVectorState [-0.414325 0.529883 -0.018168] SO3State [0.0797542 0.263811 0.892246 0.357687] ] Compound state [ RealVectorState [-0.422689 0.516707 -0.017953] SO3State [0.0777364 0.244458 0.899565 0.353526] ] Compound state [ RealVectorState [-0.427727 0.479233 -0.017394] SO3State [0.069378 0.18875 0.915835 0.347573] ] Compound state [ RealVectorState [-0.432766 0.441759 -0.0168351] SO3State [0.0607785 0.132387 0.928923 0.340413] ] Compound state [ RealVectorState [-0.431154 0.355687 -0.0155883] SO3State [0.0385692 0.00145237 0.945159 0.324321] ] Compound state [ RealVectorState [-0.429542 0.269616 -0.0143416] SO3State [0.0156595 -0.129509 0.944231 0.30234] ] Compound state [ RealVectorState [-0.424605 0.159245 -0.0127509] SO3State [-0.014451 -0.293694 0.917669 0.267242] ] Compound state [ RealVectorState [-0.419667 0.0488748 -0.0111603] SO3State [-0.0441308 -0.449127 0.86376 0.22418] ] Compound state [ RealVectorState [-0.401551 -0.028806 -0.010082] SO3State [-0.0662989 -0.550866 0.808658 0.19551] ] Compound state [ RealVectorState [-0.383435 -0.106487 -0.00900362] SO3State [-0.0874924 -0.644507 0.741668 0.163966] ] Compound state [ RealVectorState [-0.367446 -0.172223 -0.00809293] SO3State [-0.104536 -0.716402 0.676341 0.135662] ] Compound state [ RealVectorState [-0.351458 -0.23796 -0.00718225] SO3State [-0.120479 -0.780754 0.603893 0.10593] ] Compound state [ RealVectorState [-0.339723 -0.279807 -0.00660689] SO3State [-0.130217 -0.817487 0.554232 0.0870924] ] Compound state [ RealVectorState [-0.327989 -0.321655 -0.00603152] SO3State [-0.1394 -0.850733 0.502208 0.0678829] ] Compound state [ RealVectorState [-0.318382 -0.351558 -0.00562382] SO3State [-0.145788 -0.872283 0.463569 0.0545154] ] Compound state [ RealVectorState [-0.308775 -0.381462 -0.00521611] SO3State [-0.151859 -0.891934 0.423921 0.0410292] ] Compound state [ RealVectorState [-0.30517 -0.386574 -0.00515194] SO3State [-0.153119 -0.895104 0.416876 0.03946] ] Compound state [ RealVectorState [-0.301564 -0.391686 -0.00508777] SO3State [-0.15437 -0.898217 0.409805 0.0378883] ] Compound state [ RealVectorState [-0.280463 -0.4204 -0.00472967] SO3State [-0.161326 -0.914628 0.369549 0.0293757] ] Compound state [ RealVectorState [-0.259361 -0.449115 -0.00437158] SO3State [-0.167958 -0.9292 0.328549 0.0208041] ] Compound state [ RealVectorState [-0.203269 -0.525035 -0.00342564] SO3State [-0.183874 -0.958659 0.217164 -0.00188705] ] Compound state [ RealVectorState [-0.147176 -0.600955 -0.00247971] SO3State [-0.197208 -0.974655 0.10273 -0.0245517] ] Compound state [ RealVectorState [-0.0735882 -0.700477 -0.00123985] SO3State [-0.210477 -0.974883 -0.0492159 -0.0536665] ] Compound state [ RealVectorState [0 -0.8 0] SO3State [0.218671 0.951605 0.199975 0.0814875] ]
上述代碼基本參照SourceDemo中的相關代碼~