1 visualizePath = function(path)
 2     if not _lineContainer then
 3 -- simAddDrawingObject: Adds a drawing object that will be displayed in the scene
 4 -- Lua parameters:
 5     --sim_drawing_lines:items are pixel-sized lines(6 values per item (x0;y0;z0;x1;y1;z1) + auxiliary values)
 6     --size: size of the item (width of lines or size of points are in pixels, other sizes are in meters
 

 7     --parentObjectHandle: handle of the scene object where the drawing items should keep attached to. if the scene 
 8             --object moves, the drawing items will also move,-1 if the drawing items are relative to the world (fixed)
 9     --maxItemCount: maximum number of items this object can hold.
 

10     --ambient_diffuse: default ambient/diffuse color (pointer to 3 rgb values). Can be NULL
11         _lineContainer = simAddDrawingObject(sim_drawing_lines, 3, 0, -1, 99999, {0.2,0.2,0.2})
12     end
13 
14 -- Adds an item (or clears all items) to a previously inserted drawing object.
15 -- itemData: data relative to an item. If the item is a point item, 3 values are required (x;y;z). 
 

16 -- If the item is a line item, 6 values are required, If nil the drawing object is emptied of all its items
17     simAddDrawingObjectItem(_lineContainer, nil) -- clear all items
18 
19     if path then
20         local pc = #path / 3
21         for i=1, pc - 1,1 do
22             lineDat = {path[(i-1)*3+1], path[(i-1)*3+2], initPos[3], path[i*3+1], path[i*3+2], initPos[3]}
23             simAddDrawingObjectItem(_lineContainer, lineDat)
24         end
25     end
26 end
27 ---------------------------------------------------------------------------------------------------------------------
28 
29 robotHandle = simGetObjectHandle('StartConfiguration')
30 targetHandle = simGetObjectHandle('GoalConfiguration')
31 
32 initPos = simGetObjectPosition(robotHandle, -1)  --Specify -1 to retrieve the absolute position
33 initOrient = simGetObjectOrientation(robotHandle, -1)
34 
35 -- Create a task object, used to represent the motion planning task
36 -- Lua synopsis: number taskHandle = simExtOMPL_createTask(string name)
37 t = simExtOMPL_createTask('t')
38 
39 -- Create a component of the state space for the motion planning problem
40 -- stateSpaceHandle=simExtOMPL_createStateSpace(name,type,objectHandle,boundsLow,boundsHigh,useForProjection)
41 ss = {simExtOMPL_createStateSpace('2d',sim_ompl_statespacetype_pose2d,robotHandle,{-0.5,-0.5},{0.5,0.5},1)}
42 
43 -- Set the state space of this task object
44 -- Lua synopsis: number result=simExtOMPL_setStateSpace(number taskHandle, table stateSpaceHandles)
45 simExtOMPL_setStateSpace(t, ss)
46 
47 -- Set the search algorithm for the specified task. Default algorithm used is KPIECE1
48 -- Lua synopsis: number result=simExtOMPL_setAlgorithm(number taskHandle, number algorithm)
49 simExtOMPL_setAlgorithm(t,sim_ompl_algorithm_RRTConnect)
50 
51 
52 -- Set the collision pairs for the specified task object.
53 -- Lua synopsis: number result=simExtOMPL_setCollisionPairs(number taskHandle, table collisionPairHandles)
54 --[[
55 Lua parameters:
56 collisionPairHandles: a table containing 2 entity handles for each collision pair. A collision pair is represented
57 by a collider and a collidee, that will be tested against each other. The first pair could be used for robot 
58 self-collision testing, and a second pair could be used for robot-environment collision testing. The collider can 
59 be an object or a collection handle. The collidee can be an object or collection handle, or sim_handle_all, in which 
60 case the collider will be checked agains all other collidable objects in the scene.
61 --]]
62 simExtOMPL_setCollisionPairs(t,{simGetObjectHandle('L_start'),sim_handle_all})
63 
64 startpos = simGetObjectPosition(robotHandle, -1)  
65 startorient = simGetObjectOrientation(robotHandle, -1)
66 startpose={startpos[1], startpos[2], startorient[3]}
67 
68 -- Set the start state for the specified task object
69 -- Lua synopsis: number result=simExtOMPL_setStartState(number taskHandle, table state)
70 -- state: a table of numbers, whose size must be consistent with the robot's state space specified in this task object
71 simExtOMPL_setStartState(t, startpose)
72 
73 goalpos = simGetObjectPosition(targetHandle,-1)
74 goalorient = simGetObjectOrientation(targetHandle,-1)
75 goalpose = {goalpos[1], goalpos[2], goalorient[3]}
76 
77 -- Set the goal state for the specified task object
78 simExtOMPL_setGoalState(t, goalpose)
79 
80 -- Use OMPL to find a solution for this motion planning task.
81 -- number result,table states=simExtOMPL_compute(number taskHandle,number maxTime,number maxSimplificationTime=-1,number stateCnt=0)    
82 -- return value: states--a table of states, representing the solution, from start to goal. States are specified linearly.
83 r, path = simExtOMPL_compute(t, 4, -1, 800)
84 
85 visualizePath(path)
86 ----------------------------------------------------------------------------------------------------------------------------------
87 
88 -- Simply jump through the path points, no interpolation here:
89 for i=1, #path-3, 3 do   -- count from 1 to len(path)-3 with increments of 3
90     pos={path[i], path[i+1], initPos[3]}
91     orient={initOrient[1], initOrient[2], path[i+2]}
92     simSetObjectPosition(robotHandle, -1, pos)
93     simSetObjectOrientation(robotHandle, -1, orient)
94     simSwitchThread()
95 end