Sorry for bothering sincerely, I met an issue, that is when I doing path planning, I giving different goal position to a dummy to let pioneer robot following. But, the first time, second times, third times, fourth times all making sense, I mean the robot can flowing the path successfully, but when the five times goal change, the robot doesn't following the path, it will just heading to the goal dummy straightly without following the path we generated from OMPL function.
In my understand, the problem may occur at the many data comes out, because I got a large map (200*300meters) for path planning. Also in the Scene hierarchy, the path becomes more and more on the list, I think we should do clean up function each time to help robot get the clearly data.
Here is my code, thanks! Looking forward help from your guys expert!
Yours Sincerely
Jack
Code: Select all
function sysCall_init()
--Handle of the robot and target
--startHandle=sim.getObjectHandle('StartConfiguration')
goalHandle=sim.getObjectHandle('UGV_GOAL_1')
robotHandle=sim.getObjectHandle('Pioneer_p3dx')
--robotHandle=sim.getObjectHandle('UGV1_Basic')
--motor handle
rightMotor=sim.getObjectHandle('Pioneer_p3dx_rightMotor')
leftMotor=sim.getObjectHandle('Pioneer_p3dx_leftMotor')
--This is for recompute path
desiredTargetPos={-99,-99}
--Create a task object, used to represent the motion planning task
t=simOMPL.createTask('t')
--it return a task handle: t
--Create a component of the state space for the motion planning problem
--????????????????????????????????????????????????????????????????????????????????????????????????????????????
--simOMPL.setStateValidityCheckingResolution(t,0.002)
--Create a component of the state space for the motion planning problem
ss={simOMPL.createStateSpace('2d',simOMPL.StateSpaceType.pose2d,robotHandle,{-45,-150},{263,39},1)} --This is for full plant
--ss={simOMPL.createStateSpace('2d',simOMPL.StateSpaceType.pose2d,robotHandle,{218,-66},{235,-50},1)}
--[1]: a name for state space
--[2]: set 2d mode
--[3]: robot handle
--[4]: low bounds
--[5]: high bounds
--[6]: Lua 1 was true
--if 1 then
--print('jack')
--end
--Set the state space of this task object
simOMPL.setStateSpace(t,ss)
--[1]: a handle name created from simOMPL.createTask
--[2]: a table of handles to state space components created from simOMPL.createStateSpace
--Set the search algorithm for the specified task
simOMPL.setAlgorithm(t,simOMPL.Algorithm.TRRT)
--[1]: a handle name created from simOMPL.createTask
--[2]: selected algorithm
--Set the obstacles can be detected
simOMPL.setCollisionPairs(t,{robotHandle,sim.getCollectionHandle('Maze')})
--[1]: a handle name created from simOMPL.createTask
--[2]: a table containing 2 entity handles for each collision pair, e.g: {robot,obstacles}
--Start pose get and set
startpos=sim.getObjectPosition(robotHandle,-1)
startorient=sim.getObjectOrientation(robotHandle,-1)
startpose={startpos[1],startpos[2],startorient[3]} --x,y,c
simOMPL.setStartState(t,startpose)
--Goal pose get and set
goalpos=sim.getObjectPosition(goalHandle,-1)
goalorient=sim.getObjectOrientation(goalHandle,-1)
goalpose={goalpos[1],goalpos[2],goalorient[3]}
simOMPL.setGoalState(t,goalpose)
--Compute the path
r,path=simOMPL.compute(t,4,-1,800)
--[1]: a handle name created from simOMPL.createTask
--[2]: maximum time used for the path searching procedure, in seconds
--[3]: maximum time used for the path simplification procedure, in seconds. -1 for a default simplification procedure.
--[4]: minimum number of states to be returned
--return value: r is bool, true or false. True is path solution found
--path is a table, from start to goal. States are specified linearly
--Creat path
pathHandle=sim.createPath(-1, nil, nil, nil)
--[1]: a combination of path properties, -1 for default attributes
--[2]: input the line size{[1]}, the path length calculation method{[2]}, reserved{[3]}????
--[3]: input the control point size{[1]}, the angular to linear conversion coefficient{[2]}, the virtual distance scaling factor{[3]}
--[4]: the colors of the path
--Inserts one or several control points into a path object
pc=#path/3 --800 is pc, the total number of path point solution, #path is 2400 in this case
for i=1,pc-1,1 do
sim.insertPathCtrlPoints(pathHandle,0,i,1,{path[(i-1)*3+1],path[(i-1)*3+2],0,0,0,0,0,0,0,0,0})
--[1]: the handle of the path
--[2]: bit coded. 0 is for 11 bits, 1 is for 16 bits
--[3]: the first new control point be inserted
--[4]: the number of control points to insert each time
--[5]: a buffer of [4]*[2], 11bits from O bit code setting.
--{[1]-[6]}: the position of the control point (x,y,z,a,b,c), relative to the path object
end
--Get the path object lengths in meters
path_length=sim.getPathLength(pathHandle)
--[1]: handle of the path object
nominalVelocity=150*math.pi/180
rightV=0
leftV=0
currentPosOnPath=0
--[[ --get the handle of distance objects
--??????????????????
L=0.381
R=0.0975
Kp=0.8
--This is
pos_on_path=0
--Robot speed
vn=0.2--]]
end
function sysCall_actuation()
-- Read the distance between mona and different object
--res,distancepionner=sim.readDistance(distpionner)
--real_time=sim.getSimulationTime()
--Read goal position data
targetP=sim.getObjectPosition(goalHandle,-1)
xtarget=targetP[1]
ytarget=targetP[2]
--This is for recompute the path when target moving
xylength={targetP[1]-desiredTargetPos[1],targetP[2]-desiredTargetPos[2]} --[1]: x direction length of moving --[2]: y direction length of moving
if (math.sqrt(xylength[1]*xylength[1]+xylength[2]*xylength[2])>0.01) then --if the target change position, the sqrt of (a^2+b^2) has to big than 0
--No path now, need to recompute the path when it's 0
desiredTargetPos[1]=targetP[1] --To put currenlty target position data in to the desired target position for coving the data
desiredTargetPos[2]=targetP[2]
goalHandle=sim.getObjectHandle('UGV_GOAL_1')
robotHandle=sim.getObjectHandle('Pioneer_p3dx')
--robotHandle=sim.getObjectHandle('UGV1_Basic')
--motor handle
rightMotor=sim.getObjectHandle('Pioneer_p3dx_rightMotor')
leftMotor=sim.getObjectHandle('Pioneer_p3dx_leftMotor')
--This is
--pos_on_path=0
--Create a task object, used to represent the motion planning task
t1=simOMPL.createTask('t1')
--it return a task handle: t
--Create a component of the state space for the motion planning problem
--????????????????????????????????????????????????????????????????????????????????????????????????????????????
--simOMPL.setStateValidityCheckingResolution(t1,0.002)
--Create a component of the state space for the motion planning problem
ss={simOMPL.createStateSpace('2d',simOMPL.StateSpaceType.pose2d,robotHandle,{-45,-150},{263,39},1)}
--[1]: a name for state space
--[2]: set 2d mode
--[3]: robot handle
--[4]: low bounds
--[5]: high bounds
--[6]: Lua 1 was true
--if 1 then
--print('jack')
--end
--Set the state space of this task object
simOMPL.setStateSpace(t1,ss)
--[1]: a handle name created from simOMPL.createTask
--[2]: a table of handles to state space components created from simOMPL.createStateSpace
--Set the search algorithm for the specified task
simOMPL.setAlgorithm(t1,simOMPL.Algorithm.TRRT)
--[1]: a handle name created from simOMPL.createTask
--[2]: selected algorithm
--Set the obstacles can be detected
simOMPL.setCollisionPairs(t1,{robotHandle,sim.getCollectionHandle('Maze')})
--[1]: a handle name created from simOMPL.createTask
--[2]: a table containing 2 entity handles for each collision pair, e.g: {robot,obstacles}
--Start pose get and set
startpos=sim.getObjectPosition(robotHandle,-1)
startorient=sim.getObjectOrientation(robotHandle,-1)
startpose={startpos[1],startpos[2],startorient[3]} --x,y,c
simOMPL.setStartState(t1,startpose)
--Goal pose get and set
goalpos=sim.getObjectPosition(goalHandle,-1)
goalorient=sim.getObjectOrientation(goalHandle,-1)
goalpose={goalpos[1],goalpos[2],goalorient[3]}
simOMPL.setGoalState(t1,goalpose)
--Compute the path
r,path=simOMPL.compute(t1,20,-1,800)
--[1]: a handle name created from simOMPL.createTask
--[2]: maximum time used for the path searching procedure, in seconds
--[3]: maximum time used for the path simplification procedure, in seconds. -1 for a default simplification procedure.
--[4]: minimum number of states to be returned
--return value: r is bool, true or false. True is path solution found
--path is a table, from start to goal. States are specified linearly
--Creat path
pathHandle=sim.createPath(-1, nil, nil, nil)
--[1]: a combination of path properties, -1 for default attributes
--[2]: input the line size{[1]}, the path length calculation method{[2]}, reserved{[3]}????
--[3]: input the control point size{[1]}, the angular to linear conversion coefficient{[2]}, the virtual distance scaling factor{[3]}
--[4]: the colors of the path
--Inserts one or several control points into a path object
pc=#path/3 --800 is pc, the total number of path point solution, #path is 2400 in this case
for i=1,pc-1,1 do
sim.insertPathCtrlPoints(pathHandle,0,i,1,{path[(i-1)*3+1],path[(i-1)*3+2],0,0,0,0,0,0,0,0,0})
--[1]: the handle of the path
--[2]: bit coded. 0 is for 11 bits, 1 is for 16 bits
--[3]: the first new control point be inserted
--[4]: the number of control points to insert each time
--[5]: a buffer of [4]*[2], 11bits from O bit code setting.
--{[1]-[6]}: the position of the control point (x,y,z,a,b,c), relative to the path object
end
--Get the path object lengths in meters
path_length=sim.getPathLength(pathHandle)
end
--Read goal position data
l=sim.getPathLength(pathHandle)
r=sim.getObjectPosition(robotHandle,-1)
--position=sim.getPositionOnPath(pathHandle,pos_on_path)
p=sim.getPositionOnPath(pathHandle,currentPosOnPath/l)
--[1]: handle of the path object
--[2]: a value between 0 and 1, where 0 is the beginning of the path, and 1 the end of the path
d=math.sqrt((p[1]-r[1])*(p[1]-r[1])+(p[2]-r[2])*(p[2]-r[2]))
currentPosOnPath=currentPosOnPath+0.01
M=sim.getObjectMatrix(robotHandle,-1) --world frame
--print('world frame robot=',M)
sim.invertMatrix(M) --invert the rotation matrix
--print('invert results=',M)
path_pos=sim.multiplyVector(M,p) --robot frame
--print('robot frame=',path_pos)
--distancia=math.sqrt((path_pos[1])^2+(path_pos[2])^2)
angulo=math.atan2(path_pos[2],path_pos[1])
if (angulo>=0)and(angulo<math.pi*0.5) then
rightV=nominalVelocity
leftV=nominalVelocity*(1-2*angulo/(math.pi*0.5))
end
if (angulo>=math.pi*0.5) then
leftV=-nominalVelocity
rightV=nominalVelocity*(1-2*(angulo-math.pi*0.5)/(math.pi*0.5))
end
if (angulo<0)and(angulo>-math.pi*0.5) then
leftV=nominalVelocity
rightV=nominalVelocity*(1+2*angulo/(math.pi*0.5))
end
if (angulo<=-math.pi*0.5) then
rightV=-nominalVelocity
leftV=nominalVelocity*(1+2*(angulo+math.pi*0.5)/(math.pi*0.5))
end
sim.setJointTargetVelocity(leftMotor,leftV)
sim.setJointTargetVelocity(rightMotor,rightV)
--Get the currently robot Position and Orientation on the Path
--[[ position=sim.getPositionOnPath(pathHandle,pos_on_path)
--[1]: handle of the path object
--[2]: a value between 0 and 1, where 0 is the beginning of the path, and 1 the end of the path
orientation=sim.getOrientationOnPath(pathHandle,pos_on_path)
--[1]: handle of the path object
--[2]: a value between 0 and 1, where 0 is the beginning of the path, and 1 the end of the path
--if r then
--print('r=',r)
--print('#path=',#path)
--print('path=',path)
--print('path1x=',path[1],'path1y=',path[2],'path1c=',path[3])
--print('path2x=',path[4],'path2y=',path[5],'path2c=',path[6])
--print('pc=',pc)
--print('path_length=',path_length)
--print('pathHandle=',pathHandle)
--print('position=',position)
--print('orientation=',orientation)
--end
--
M=sim.getObjectMatrix(robotHandle,-1) --world frame
--print('world frame robot=',M)
sim.invertMatrix(M) --invert the rotation matrix
--print('invert results=',M)
path_pos=sim.multiplyVector(M,position) --robot frame
--print('robot frame=',path_pos)
distancia=math.sqrt((path_pos[1])^2+(path_pos[2])^2)
angulo=math.atan2(path_pos[2],path_pos[1])
if (pos_on_path < 1) then
v=vn
o=Kp*angulo
else -- Stop at the end of the path
v=0
o=0
end
wr=(v+L*o)/R
wl=(v-L*o)/R
sim.setJointTargetVelocity(leftMotor,wl)
sim.setJointTargetVelocity(rightMotor,wr)
if (distancia < 1/path_length) then
pos_on_path=pos_on_path+v*sim.getSimulationTimeStep()
end--]]
end
function sysCall_sensing()
-- put your sensing code here
end
function sysCall_cleanup()
end