How to define the target of .generatePath correctly ?

[RalfR]

I have a manipulator robot and want to follow a straight line. The demo scene ikPathGeneration.ttt seems to provide an example for this, but I am struggling with some implementation issues when the code gets more complex.

What I do:
In sysCall_init(), I create the IK Environment, Group, and do simIK.addElementFromScene(ikEnv,ikGroup,ikBase,ikTip,ikTarget,simIK.constraint_pose) with the resprective values.

Later, the user should be able to change the target and initiate the path motion. I change the ikTarget object, call simIK.applySceneToIkEnvironment(ikEnv,ikGroup)
pathData=simIK.generatePath(ikEnv,ikGroup,ikJoints,ikTip,300)

and get the error: Invalid dummy handle (for simIK.getTargetDummy)

Now there are several aspects which I do not really understand:
- How does .generatePath() knows its target point ? There might be multiple elements in the ikGroup, or none, or they are changed...
- Does .applySceneToIkEnvironment() automatically update the values of all elements ? In my example, is ikTarget updated ? If not, how would I update the elements of an existing ikElement ?
- Does my approach make sense of having a global ikEnvironment for various tasks ? Or is it better to use temporal ikEnvironments and destroy them quickly (like in the demo scene) ?
- More generally: There seems to be .generatePath(), .generateIkPath(), and the implementions of ompl and ruckig trajectory planners. What is the preferred method ? Are the differences explained somewhere ?

[coppelia]

Hello,

try to modify the IK script in scene scenes/kinematics/ikPathGeneration.ttt with following:

Code: Select all

...

    -- Retrieve some handles of objects created in the IK environment:
    local ikTip=simToIkMap[simTip]
    local ikJoints={}
    for i=1,#simJoints,1 do
        ikJoints[i]=simToIkMap[simJoints[i]]
    end
    while true do
        -- Update the IK world from the simulation world (typically the position of the IK target):
        simIK.syncFromSim(ikEnv, {ikGroup})
        
        -- Generate a path:
        local path=simIK.generatePath(ikEnv,ikGroup,ikJoints,ikTip,300)
        
        -- Hop through the path configurations:
        hopThroughConfigs(path,simJoints,false,dynModel) -- forward
        hopThroughConfigs(path,simJoints,true,dynModel) -- reverse
    end
    simIK.eraseEnvironment(ikEnv)
end

You also seem to be using the older IK functions (i.e. simIK.applySceneToIkEnvironment was replaced with simIK.syncFromSim)

Then: simIK.generatePath knows its target since you provided it via simIK.addElementFromScene. If you move the target dummy while the robot is moving back and forth between the initial and target dummy, you must call simIK.syncFromSim, otherwise the IK world doesn't know the target was moved (it would still be at its initial pose while its sim-world counterpart would have moved).

You should try to use an individual IK environment for each IK task that is independent from others. And while the task is active, do not destroy it.

simIK.generatePath simply computes evenly spaced configurations on a line in 3D space: how you want to move along that line is entirely up to you. e.g. if you want to use a smooth velocity profile, in order to accelerate/decelerate on that straight line, you'd use sim.moveToConfig typically, but with a 1 DoF movement on that line: Ruckig doesn't work along several points/trajectory. If you want to follow a random path (also non-linear) with a specific velocity profile, then have a look at the demo scene scenes/trajectoryAndMotion/pathToTrajectory.ttt and click Time-optimal trajectory. You'll have to make sure you run the required docker container for that functionality:

In order to generate a time-optimal trajectory, given velocity and acceleration constraints for each individual DoF, you should use the TOPPRA library. Make sure you are running the 'docker-image-zmq-toppra' container. The Docker file and instructions are available here: https://github.com/CoppeliaRobotics/doc ... zmq-toppra

Cheers

[RalfR]

Okay, so I think I got it running now. For anybody else reading this, here are some of my learned lessons:

- The problem with the simIK.getTargetDummy error message was actually caused by using the wrong tip ! I used the same ikTip variable in simIK.addElementFromScene and simIK.generatePath, but these are actually different references ! You have to use something like ikTip=simToIkMap[normalTip], or otherwise the path generator will be confused.
- I exchanged simIK.applySceneToIkEnvironment with simIK.syncFromSim, but have not noticed a difference
- Using a global persistent ikEnvironment and group is working, as long as sync is done when needed
- The code changes to ikPathGeneration.ttt were helpful and running, although they were missing an actual re-definition of the target point - as it is, the same path is just re-calculated constantly.
- So simIK.generatePath uses the target from simIK.addElementFromScene. I still do not know what happens if there are multiple elements added. Or is every ikGroup supposed to have one and only one added element ?

Well, a big thanks anyway for the support !

[coppelia]

simIK.generatePath will internally call simIK.handleGroup. And simIK.handleGroup will try to solve IK for the n IK elements at once. If you only added one IK element to your group, then the simIK.generatePath will try to match only 1 tip-target pair. Otherwise it will try to match n tip-target pairs, like a tree where each branch is assigned to a different target, and the whole tree moves mowards individual targets at the same time.

More details about Jacobian-based IK calculations here.

Cheers