How to use getPathInterpolatedConfig

[Chiara]

Hi!

In my scene I use this command

Code: Select all

local path=simIK.generatePath(ikEnv,ikGroup,ikJoints,ikTip,300)

in order to have the joints configuration (angular position in this case) that I need to apply for reach the target.

I would like that the joints' velocity vary during the time..I saw the tutorial pathToTrajection and I would like to do something similar to func2.

The problem is that the function

Code: Select all

sim.getPathInterpolatedConfig

wants "a path containing two 3D poses (position+quaternion)" : how can I convert my angular joint positions obtained into a 3D poses? Or, there is another better way to solve my problem?

Many thanks!

[Chiara]

Hi, I tried again and I think I did some improvements but the results is not okay.

By looking the documentation I understood that the only way to handle path trajection is via coordinates (x,y,z + quaternion). For this reason I changed my code as follows:

Code: Select all

local path={}
for k, v in pairs(gripperPositionsRead) do
	for i=1,#v do
        	table.insert(path, tonumber(v[i]))                
         end
         if #path%7~=0 then error('incorrect number of points') end
end
pathHandle = sim.createPath(path)
local ctrlPts = sim.unpackDoubleTable(sim.readCustomDataBlock(pathHandle, 'PATHCTRLPTS'))
func2(simJoints,ctrlPts,maxVel,maxAccel)

I changed this line in func2

Code: Select all

local pl,totL=sim.getPathLengths(path,7)

The robot is moving but in a wrong way: it doesn't follow the points I previously created.

Can anybody help me, please? I'm not sure this is the proper way to set the velocity.

[coppelia]

Hello,

the easiest is to probably look at the demo scene scenes/movingAlongAPath-lua.ttt, and open the child script attached to the red cube:

The path object contains the path data tha is read with:

Code: Select all

    pathData=sim.unpackDoubleTable(sim.readCustomDataBlock(path,'PATH'))

Path data is basically just a successions of poses (x1, y1, z1, qx1, qy1, qz1, qz1, x2, y2, z2, ...). Those are split in 2 in order to separate positions and quaternions:

Code: Select all

    local m=Matrix(#pathData//7,7,pathData)
    pathPositions=m:slice(1,1,m:rows(),3):data()
    pathQuaternions=m:slice(1,4,m:rows(),7):data()

Next, it is important to have one additional information: the relative distance between individual position points:

Code: Select all

    pathLengths,totalLength=sim.getPathLengths(pathPositions,3)

And now you can extract an interpolated position or quaternion from the pathPositions and pathQuaternions with sim.getPathInterpolatedConfig:

Code: Select all

    local pos=sim.getPathInterpolatedConfig(pathPositions, pathLengths, posAlongPath)
    local quat=sim.getPathInterpolatedConfig(pathQuaternions ,pathLengths, posAlongPath, nil, {2, 2, 2, 2})

(in above, {2, 2, 2, 2} indicates the data is quaternion data)

Cheers