How can I compute and get the length of a path planned by OMPL plugin

[Francis]

Hello,

Now I have planned a path by OMPL plugin in V-REP. After I looked up the CoppeliaSim helpFiles, I can't find any methods to get the length of a planned path returned by API function simOMPL.compute() not the length of a path object.

Please help me.
Thank you!

[fferri]

There is no such function because the state space is customly defined.
In general it does not make sense to compute a length between poses, or between angles. You should be able to figure out a "good" length metric on your customly defined state space.

What state space are you using?

Perhaps the length of the path projected onto the default euclidean projection would work?

[fferri]

Perhaps the length of the path projected onto the default euclidean projection would work?

Code: Select all

function simOMPL.getProjectedPathLength(taskHandle,path)
    local m=simOMPL.projectionSize(taskHandle)
    local pathProjection=simOMPL.projectStates(taskHandle,path)
    local length=0
    for i=2,#pathProjection/m do
        local s=0
        for j=1,m do
            s=s+math.pow(path[i*m+j]-path[(i-1)*m+j],2)
        end
        length=length+math.sqrt(s)
    end
    return length
end