2021-11-10 末端空间路径规划？

endStateSpace = SE(3) + SO(3)

末端空间规划的难点：

采样范围限制：平移范围和转动范围，statespace bounds？
逆解可达性和有效性限制，validity check：ik, collsion avoidance, distance

今天还在搞ompl末端空间轨迹规划
之前在Windows系统里已经写过三维刚体路径规划了，机械臂末端路径规划不考虑碰撞先规划路径的话其实就是个三维刚体，用Ubuntu不知道哪里又出问题了，planner->setup()一运行就崩了。。。

用simplesetup 和 without simplesetup都试过了，不行。。。

解决：statespaceptr 和 spaceinformationptr 用auto 类型或者 make_shared<> 得到会有问题！！！原理看看c++手册再弄清。。。

留空

将各个变量通过simplesetup 内的方法得到后即可顺利运行。

注意：vrep只能与一个远程终端保持联系，如果有未断开连接的终端窗口开着，那么新的连接就会失败，下午在Windows上用VS试了一下3dplanning，忘了关闭窗口，然后Ubuntu与vrep的连接就一直gethandle失败，可以连接，simxconnect 正常，通信是不行的。