第三篇，终于来到了激动人心的机械臂路径规划环节
motionPlanningDemo1.ttt
这个scene相比于前面两个就稍微复杂一点了，除了机械臂，障碍物，还多了approachDirectionObstacle，而且涉及到机械臂逆解和碰撞检测的相关内容，我在文章里会稍微介绍一下，但是最好还是先看一下User Manual里面Calculation modules这一节
首先看一下Scene Hierarchy
有一个approachDirectionObstacle障碍物，四个obstacle，四个目标，以及机械臂LBR4p_base:
approachDirectionObstacle是那个蓝色的八边形，至于他的作用，会在下文代码中解释，三个obstacle是三个方块，第四个是地板
再看一下机械臂的结构：
除了常规的逆解结构，包括target和tip，还在tip的相同位置和姿态下多了一个ReferenceFrame，其作用同样放在代码中解释:
逆运动学参数：
关节参数：
可以发现关节是Passive mode(随动模式)
再看一下碰撞的配置：
有机械臂和环境两个
配置基本上是这样了，下面正餐来了，开始看Lua代码吧：
displayInfo=function(txt)
if dlgHandle then
sim.endDialog(dlgHandle)
end
dlgHandle=nil
if txt and #txt>0 then
dlgHandle=sim.displayDialog('info',txt,sim.dlgstyle_message,false)
sim.switchThread()
end
end
getMatrixShiftedAlongZ=function(matrix,localZShift)
--Returns a pose or matrix shifted by localZShift along the matrix z-axis
local m={}
for i=1,12,1 do
m[i]=matrix[i]
end
--m[4] m[8] m[12] 是位置，m[3] m[7] m[11] 是z轴单位向量，所以就是沿本身的z轴平移localZShift之后的矩阵
m[4]=m[4]+m[3]*localZShift
m[8]=m[8]+m[7]*localZShift
m[12]=m[12]+m[11]*localZShift
return m
end
forbidThreadSwitches=function(forbid)
-- Allows or forbids automatic thread switches.
-- This can be important for threaded scripts. For instance,
-- you do not want a switch to happen while you have temporarily
-- modified the robot configuration, since you would then see
-- that change in the scene display.
if forbid then
forbidLevel=forbidLevel+1
if forbidLevel==1 then
sim.setThreadAutomaticSwitch(false)
end
else
forbidLevel=forbidLevel-1
if forbidLevel==0 then
sim.setThreadAutomaticSwitch(true)
end
end
end
findCollisionFreeConfigAndCheckApproach=function(matrix)
--用于寻找满足预目标位姿的机械臂构型，generateIkPath函数用于判断预目标位姿和目标位姿之间能否直接通过
-- Here we search for a robot configuration..
-- 1. ..that matches the desired pose (matrix)
-- 2. ..that does not collide in that configuration
-- 3. ..that does not collide and that can perform the IK linear approach
sim.setObjectMatrix(ikTarget,-1,matrix)
-- Here we check point 1 & 2:
--寻找满足target位姿的机械臂构型，参数：
--ikHandle
--机械臂关节 Handle
--thresholdDist，与精确解的距离阈值，小于这个算满足的逆解
--maxTimeInMs:最大计算时间
--距离计算方式
--collisionPairs，默认二范数
local c=sim.getConfigForTipPose(ikGroup,jh,0.65,10,nil,collisionPairs)
if c then
-- Here we check point 3:
--这里又把m平移了回去，是为了把target放在偏置后的位置
local m=getMatrixShiftedAlongZ(matrix,ikShift)
local path=generateIkPath(c,m,ikSteps)
--如果找不到路径，那么这个c（初始臂型）没有意义
if path==nil then
c=nil
end
end
return c
end
findSeveralCollisionFreeConfigsAndCheckApproach=function(matrix,trialCnt,maxConfigs)
-- Here we search for several robot configurations...
-- 1. ..that matches the desired pose (matrix)
-- 2. ..that does not collide in that configuration
-- 3. ..that does not collide and that can perform the IK linear approach
forbidThreadSwitches(true)
sim.setObjectMatrix(ikTarget,-1,matrix)
--获取当前机械臂关节角
local cc=getConfig()
local cs={}
local l={}
--找trialCnt次，将获得的所有不同臂型返回
for i=1,trialCnt,1 do
local c=findCollisionFreeConfigAndCheckApproach(matrix)
if c then
local dist=getConfigConfigDistance(cc,c)
local p=0
local same=false
for j=1,#l,1 do
if math.abs(l[j]-dist)<0.001 then
-- we might have the exact same config. Avoid that
same=true
for k=1,#jh,1 do
if math.abs(cs[j][k]-c[k])>0.01 then
same=false
break
end
end
end
if same then
break
end
end
if not same then
cs[#cs+1]=c
l[#l+1]=dist
end
end
if #l>=maxConfigs then
break
end
end
forbidThreadSwitches(false)
if #cs==0 then
cs=nil
end
return cs
end
getConfig=function()
-- Returns the current robot configuration
local config={}
for i=1,#jh,1 do
config[i]=sim.getJointPosition(jh[i])
end
return config
end
setConfig=function(config)
-- Applies the specified configuration to the robot
if config then
for i=1,#jh,1 do
sim.setJointPosition(jh[i],config[i])
end
end
end
getConfigConfigDistance=function(config1,config2)
-- Returns the distance (in configuration space) between two configurations
local d=0
for i=1,#jh,1 do
local dx=(config1[i]-config2[i])*metric[i]
d=d+dx*dx
end
return math.sqrt(d)
end
getPathLength=function(path)
-- Returns the length of the path in configuration space
local d=0
local l=#jh
local pc=#path/l
for i=1,pc-1,1 do
local config1={path[(i-1)*l+1],path[(i-1)*l+2],path[(i-1)*l+3],path[(i-1)*l+4],path[(i-1)*l+5],path[(i-1)*l+6],path[(i-1)*l+7]}
local config2={path[i*l+1],path[i*l+2],path[i*l+3],path[i*l+4],path[i*l+5],path[i*l+6],path[i*l+7]}
d=d+getConfigConfigDistance(config1,config2)
end
return d
end
followPath=function(path)
-- Follows the specified path points. Each path point is a robot configuration. Here we do not do any interpolation
if path then
local l=#jh
local pc=#path/l
for i=1,pc,1 do
local config={path[(i-1)*l+1],path[(i-1)*l+2],path[(i-1)*l+3],path[(i-1)*l+4],path[(i-1)*l+5],path[(i-1)*l+6],path[(i-1)*l+7]}
setConfig(config)
sim.switchThread()
end
end
end
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
--熟悉的配方在这里
local task=simOMPL.createTask('task')
simOMPL.setAlgorithm(task,simOMPL.Algorithm.RRTConnect)
--注意这里机械臂State Space的设置方法
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)
simOMPL.setStateSpace(task,{j1_space,j2_space,j3_space,j4_space,j5_space,j6_space,j7_space})
simOMPL.setCollisionPairs(task,collisionPairs)
simOMPL.setStartState(task,startConfig)
simOMPL.setGoalState(task,goalConfigs[1])
--这里又进行了add，因此得到的goalConfigs是一个二维列表
for i=2,#goalConfigs,1 do
simOMPL.addGoalState(task,goalConfigs[i])
end
local path=nil
local l=999999999999
forbidThreadSwitches(true)
--返回距离最小的path
for i=1,cnt,1 do
local res,_path=simOMPL.compute(task,4,-1,300)
if res and _path then
local _l=getPathLength(_path)
if _l4 then
targetIndex=1
end
--类似于sim.getObjectPosition
--matrix描述了物体的位姿，与转换矩阵T一致，不过最后一行的 0 0 0 1 没有返回来
local m=sim.getObjectMatrix(theTarget,-1)
--沿物体本身的z轴平移-ikShift之后的矩阵
--这个平移的0.1m是为了让机械臂先到目标前面一点，再沿z轴伸过去，更符合抓取时的路径
m=getMatrixShiftedAlongZ(m,-ikShift)
-- Find several configs for pose m, and order them according to the
-- distance to current configuration (smaller distance is better).
--为位姿m搜索一些臂型，并按照这些臂型与当前臂型的距离排序（距离小更好）
-- In following function we also check for collisions and whether the
-- final IK approach is feasable:
--下面的函数也做了碰撞检测，以及判断逆解是否可行
--displayInfo这个函数在调试时非常有用。。直接弹出一个窗口，打印
displayInfo('searching for a maximum of 60 valid goal configurations...')
local c=findSeveralCollisionFreeConfigsAndCheckApproach(m,300,60)
-- Search a path from current config to a goal config. For each goal
-- config, search 6 times a path and keep the shortest.
--搜索从当前臂型到目标臂型的路径，对于每一个目标臂型，搜索六次，选择最短的
-- Do this for the first 3 configs returned by findCollisionFreeConfigs.
--对findCollisionFreeConfigs函数返回的前三个臂型执行上述操作
-- Since we do not want an approach along the negative Z axis, we place
-- an artificial obstacle into the scene (the blue orthogon):
--由于不想沿着-Z轴靠近的路径，把一个人工设置的障碍物（蓝色长方形）放置在场景中
local initialApproachDirectionObstaclePose=sim.getObjectMatrix(approachDirectionObstacle,-1)
--把我们的小蓝片放在目标位置沿Z轴向后0.1m，再向前0.01m
--真正的目标点是向后0.1m，再稍微向前一点点，其实这个0.01可有可无，不影响效果，0.02也可以
sim.setObjectPosition(approachDirectionObstacle,theTarget,{0,0,-ikShift+0.01})
sim.setObjectOrientation(approachDirectionObstacle,theTarget,{0,0,0})
sim.switchThread() -- in order see the change before next operation locks
local txt='Found '..#c..' different goal configurations for the desired goal pose.'
txt=txt..'&&nNow searching the shortest path of 6 searches...'
displayInfo(txt)
local path=findShortestPath(getConfig(),c,6)
displayInfo(nil)
--路径已经找到，再把我们的小蓝片放回去
sim.setObjectMatrix(approachDirectionObstacle,-1,initialApproachDirectionObstaclePose)
--运动到目标的偏移
-- Follow the path:
followPath(path)
-- For the final approach, the target is the original target pose:
m=sim.getObjectMatrix(theTarget,-1)
--计算直线路径，这段可以直接直线运动过去
-- Compute a straight-line path from current config to pose m:
path=generateIkPath(getConfig(),m,ikSteps)
-- Follow the path:
followPath(path)
--怎么来的还要怎么回去
-- Generate a reversed path in order to move back:
path=getReversedPath(path)
-- Follow the path:
followPath(path)
end
end
看似很长其实逻辑很简单，但是里面有个ikShift有点绕，为了达到让机械臂先到目标位姿z轴前面一点，再沿z轴直线运动过去的效果，添加了一个approachDirectionObstacle。以及，利用ikShift这样一个0.1m的偏移，生成了一个相对目标位姿z轴偏移了0.1m的新目标位姿，这里称之为预目标位姿。
下面整理一下程序逻辑：
首先用一个函数getMatrixShiftedAlongZ得到预目标位姿，然后用函数findSeveralCollisionFreeConfigsAndCheckApproach得到满足预目标位姿的机械臂构型（这里通过调用另一个同名的重载函数（在里面调用了generateIkPath（里面又调用了sim.generateIkPath）），保证了得到的满足预目标位姿的机械臂构型与目标位姿之间存在无碰的直线路径），然后将小蓝片放置在预目标位姿z轴后面一点点，保证机械臂不会从反方向运动到预目标位姿，这样有了机械臂的初始构型和预目标构型，调用findShortestPath（在里面调用findPath（task在这））得到机械臂路径，然后控制机械臂运动就好了。
值得一提的时，这次的程序中涉及到线程，使用了forbidThreadSwitches()、sim.switchThread()、sim.setThreadAutomaticSwitch()等函数，forbidThreadSwitches(true)和forbidThreadSwitches(false)成对使用，类似线程加锁解锁，很方便。
还涉及了简单的UI，displayInfo()，这个函数调试的时候很好用。
又到了惯例的上效果时间：
可以看到，通过添加多个目标位姿，手动设置障碍物等，可以灵活的实现机械臂避障的功能。