OMPL Path Structure

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

OMPL Path Structure

Post by sc18ldh »

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[3]}
            orient={initOrient[1],initOrient[2],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: 1193
Joined: 09 Sep 2013, 19:28

Re: OMPL Path Structure

Post by fferri »

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

Post by sc18ldh »

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

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

Re: OMPL Path Structure

Post by TrimItOut »

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: 1193
Joined: 09 Sep 2013, 19:28

Re: OMPL Path Structure

Post by fferri »

Code: Select all

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

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

Re: OMPL Path Structure

Post by TrimItOut »

Perfect!

Cheers

Post Reply