vrep中的运动规划(主要是针对机械臂)（未完）
1.0是什么
具体可以看看vrep中自带的几个demo，以及之前的博客
robot motion planning介绍
Vrep中支持的运动规划算法
Vrep中的RRT算法
还有大神冬木远景的博客：
V-rep学习笔记：机器人路径规划1 - 冬木远景
V-rep学习笔记：机器人路径规划2 - 冬木远景
A motion planning task allows to compute a trajectory (usually in the configuration space of the manipulator) from a start configuration to a goal configuration, by taking into account following main
items(主要考虑以下四个要素):
- the manipulator kinematics (机械臂的正逆运动学)
- the manipulator joint limits
- the manipulator self-collisions
- the collisions between manipulator and obstacles（机械臂的自我碰撞也是需要考虑的）
1.1一个运动规划任务可以分为以下两个步骤：
- Finding a goal configuration that matches a goal pose
- Finding a collision-free path from a start configuration to a goal configuration
相关图示：
1.2过程
- Finding a goal configuration( goal pose已知，寻找合适的goal configuration）
When the goal configuration is not known, but only a goal pose is known (i.e. a position and orientation of
the end-effector) then an appropriate manipulator configuration that satisfies the goal pose constraint has
to be found. There can be multiple (or even an infinitie number of) solutions:
不过有一点需要注意，找到的configuration并不一定需要完全符合goal pose，只要按照一定space metric计算方法，找到的configuration接近目标pose就可以了。
- Finding a collision-free path（找到一条无碰撞的路径）
the manipulator type, there can be multiple (or even an infinitie number of) solutions, mainly also because
the search algorithm is based on a randomized resolution method:
- Simplifying a found path （简化找到的路径）
计算得出的原始路径可能很复杂，而且没有满足一些既定要求比如说路径最短、能量最少等等，需要进行简化。
Finding a path involves trying to link the start configuration to the goal configuration via randomly generated configurations. The found trajectory is usually a succession of straight lines in the configuration space, and the output of the operation is a succession of path nodes (or path configurations) that are distant to each other by a maximum of D, which is a configuration space distance: 
A raw path is often not optimized and can rarely be used as-is. A simplification pass is usually required and involves trying to successively link path nodes via straight lines in the configuration space:
You should be aware that, depending on the selected parameters, the simplification procedure can be more lengthy than the path search in itself. For that reason a common practice to find an optimal path is to calculate several raw paths, then select the shortest path that will finally be simplified.
1.3伪代码示例
1. Starting with a manipulator start configuration, we want the end-effector to
move without collisions to a specific goal pose.
2. Search for a goal configuration that matches the goal pose
3. Search for a path between the start and goal configurations
4. Repeat above steps 2 & 3 n times.
5. Select the shortest calculated path, and simplify it 2.注意事项
2.1 机械臂运动学的选择
为了避免过约束，在vrep中逆运动学方法选择伪逆方法
The IK group should use an undamped resolution method (i.e. the pseudo inverse method), which means
that the IK element should not be overconstrained.
2.2 碰撞对象的设置
碰撞对象的检测主要有两类，一类是机械臂自身连杆之间的碰撞检测，二是机械臂和外界物理环境障碍物之间的碰撞检测。
collisionPairs: an array containing 2 entity handles for each collision pair. A collision pair is represented by a collider and a collidee, that will be tested against each other. The first pair could be used for robot self-collision testing, and a second pair could be used for robot-environment collision testing. The collider can be an object or a collection handle. The collidee can be an object or collection handle, or sim_handle_all, in which case the collider will be checked agains all other collidable objects in the scene. Can be NULL if collision checking is not required.
- self-collisions: define a collection that contains the collidable links of the manipulator.
- manipulator-environment collisions
- 选择1
you can temporarily make that object non-collidable (or non-measurable). It will then not be detected as an obstacle anymore, however, it will neither be seen as part of the manipulator (i.e. a motion planning task will not take into account possible collisions between the object and the environment).
- 选择2
you can also make that object temporarily part of the manipulator. This way, a motion planning task will take into account possible collisions between the object and the environment too. To do this, best is to define a manipulator collection as the tree including all objects of the manipulator starting at its base, then by attaching the object to the manipulator base (i.e. making the manipulator base parent of the object), the object will also be part of the manipulator collection.
————————————————————————————————————————————————
2.在vrep中的实现
2.1 vrep中使用OMPL进行运动规划
- Decide of a start and goal state. When the path planning object is a serial manipulator, a goal pose (or end-effector position/orientation) is often provided instead of a goal state. In that case function sim.getConfigForTipPose can be used to find one or several goal states that satisfy the provided goal pose.
- Create a path planning task with simOMPL.createTask.
- Select an algorithm with simOMPL.setAlgorithm.
- Create the required state space, which can be composed as a compound object: simOMPL.createStateSpace and simOMPL.setStateSpace.
- Specify which entities are not allowed to collide with simOMPL.setCollisionPairs.
- Specify the start and goal state with simOMPL.setStartState and simOMPL.setGoalState.
- Compute one or several paths with simOMPL.compute.
- Destroy the path planning task with simOMPL.destroyTask.
- Often, path planning is used in combination with inverse kinematics: in a pick-and-place task for instance, the final approach should usually be a straight-line path, which can be generated with sim.generateIkPath.
一下为代码实例：
findPath=function(startConfig,goalConfigs,cnt)
-- Here we do path planning between the specified start and goal configurations. We run the search cnt times,
-- and return the shortest path, and its length
--1.创建任务
local task=simOMPL.createTask('task')
--2.选择合适的算法 RRTConnect
simOMPL.setAlgorithm(task,simOMPL.Algorithm.RRTConnect)
--3.创建状态空间 每一个关节都要创建一个状态空间
local j1_space=simOMPL.createStateSpace('j1_space',simOMPL.StateSpaceType.joint_position,jh[1],{-170*math.pi/180},{170*math.pi/180},1)
local j2_space=simOMPL.createStateSpace('j2_space',simOMPL.StateSpaceType.joint_position,jh[2],{-120*math.pi/180},{120*math.pi/180},2)
local j3_space=simOMPL.createStateSpace('j3_space',simOMPL.StateSpaceType.joint_position,jh[3],{-170*math.pi/180},{170*math.pi/180},3)
local j4_space=simOMPL.createStateSpace('j4_space',simOMPL.StateSpaceType.joint_position,jh[4],{-120*math.pi/180},{120*math.pi/180},0)
local j5_space=simOMPL.createStateSpace('j5_space',simOMPL.StateSpaceType.joint_position,jh[5],{-170*math.pi/180},{170*math.pi/180},0)
local j6_space=simOMPL.createStateSpace('j6_space',simOMPL.StateSpaceType.joint_position,jh[6],{-120*math.pi/180},{120*math.pi/180},0)
local j7_space=simOMPL.createStateSpace('j7_space',simOMPL.StateSpaceType.joint_position,jh[7],{-170*math.pi/180},{170*math.pi/180},0)
--4.设置状态空间
simOMPL.setStateSpace(task,{j1_space,j2_space,j3_space,j4_space,j5_space,j6_space,j7_space})
--5.设置碰撞对
simOMPL.setCollisionPairs(task,collisionPairs)
--6.设置开始和目标状态
simOMPL.setStartState(task,startConfig)
simOMPL.setGoalState(task,goalConfigs[1])--
for i=2,#goalConfigs,1 do
--Add a goal state, without clearing previously set goal state(s), if any.
simOMPL.addGoalState(task,goalConfigs[i])
end
local path=nil
local l=999999999999
forbidThreadSwitches(true)
for i=1,cnt,1 do
--7.计算出合适的路径
local res,_path=simOMPL.compute(task,4,-1,300)
if res and _path then
local _l=getPathLength(_path)
if _l以上的8个步骤为常规方法，缺乏灵活性，为了提高灵活性可以使用以下三个回调函数;
- simOMPL.setStateValidationCallback
- simOMPL.setProjectionEvaluationCallback
- simOMPL.setGoalCallback
configuration space metric 也要讲一讲 看看怎么插进来
1.运动规划主要注意事项
5. 关于验证state
6.不足之处：规划出的路径并不完美，会有锯齿：
生成的路径曲线乍一看还不错，但是放大一些局部地区可以知道其实还不是很平滑，也许放慢机器人的动作速度，就可以知道
它运动的是否平滑。局部还有一些其它这样的突变。如何改善这个缺点我想是一个大课题。
参考：
1.Path and motion planning