## OMPL Path Structure

Typically: "How do I... ", "How can I... " questions
sc18ldh
Posts: 3
Joined: 29 Jan 2021, 17:41

### OMPL Path Structure

Hi,

I am looking at the 3DoFHolonomicPathPlanning scene and I am just trying to work out the structure of the path returned by OMPL.

Here is a snippet of the relevent code from the demo

Code: Select all

    r,path=simOMPL.compute(t,4,-1,800)
print(path)
while path do
visualizePath(path)
-- Simply jump through the path points, no interpolation here:
for i=1,#path-3,3 do
pos={path[i],path[i+1],initPos}
orient={initOrient,initOrient,path[i+2]}
sim.setObjectPosition(robotHandle,-1,pos)
sim.setObjectOrientation(robotHandle,-1,orient)
sim.switchThread()
end
My question is that as for pos x and y are set to path [ i] and path[i + 1] is the path returned by OMPL structured to be an alternating series of x and then y values?

And if so does this path representation change when selecting an alternate stateSpaceType such as dubins?

I apologise if I have missed some obvious documentation explaining this, thanks!

fferri
Posts: 722
Joined: 09 Sep 2013, 19:28

### Re: OMPL Path Structure

The returned path is a sequence of states.

The length and content of a state depends on how the state space is defined:
• if the state space is a single simOMPL.StateSpaceType.position2d, then the state is $$\left<p_x, p_y\right>$$.
• if the state space is a single simOMPL.StateSpaceType.pose2d (SE(2)), then the state is $$\left<p_x, p_y, \theta\right>$$.
• if the state space is a single simOMPL.StateSpaceType.position3d, then the state is $$\left<p_x, p_y, p_z\right>$$.
• if the state space is a single simOMPL.StateSpaceType.pose3d (SE(3)), then the state is $$\left<p_x, p_y, p_z, q_x, q_y, q_z, q_w\right>$$.
• if the state space is a single simOMPL.StateSpaceType.joint_position, then the state is $$\left<p\right>$$.
• if the state space is a single simOMPL.StateSpaceType.dubins (SE(2)), then the state is $$\left<p_x, p_y, \theta\right>$$.
Where $$p_x$$, $$p_y$$, $$p_z$$ are the x, y, z position, $$\theta$$ is orientation on the (XY) plane, $$q_x$$, $$q_y$$, $$q_z$$, $$q_w$$ is the 3D orientation expressed as a unit quaternion, $$p$$ is the value of a joint position.

If the state space is a composition of the above, simple concatenation applies, e.g.: if the state space consists of two components: 1) simOMPL.StateSpaceType.position2d and 2) simOMPL.StateSpaceType.joint_position, then the state is $$\left<p_x, p_y, p\right>$$.

The path is simply the repetition of the n states, i.e. $$\left<{p_x}^{(1)}, {p_y}^{(1)}, {p}^{(1)}, {p_x}^{(2)}, {p_y}^{(2)}, p^{(2)}, \ldots, {p_x}^{(n)}, {p_y}^{(n)}, {p}^{(n)}, \right>$$.

You can use simOMPL.getPathStateCount(taskHandle,path) to get the number of states from the returned path, and simOMPL.getPathState(taskHandle,path,index) to extract the state at position index.

sc18ldh
Posts: 3
Joined: 29 Jan 2021, 17:41

### Re: OMPL Path Structure

That makes perfect sense, thank you for explaining it in such clear detail!

TrimItOut
Posts: 10
Joined: 01 Dec 2020, 18:39

### Re: OMPL Path Structure

Hi,

I have a question regarding the θ returned by the dubin’s state space.

As this is an orientation in the (xy) plane how would I get it into a format so that it can be used to set an object orientation using sim.setObjectOrientation?

As sim.setObjectOrientation is expecting a table of 3 Euler angles

Thanks in advance!

fferri
Posts: 722
Joined: 09 Sep 2013, 19:28

### Re: OMPL Path Structure

Code: Select all

sim.setObjectOrientation(handle,-1,{0,0,theta})

TrimItOut
Posts: 10
Joined: 01 Dec 2020, 18:39

Perfect!

Cheers