How to get non-random results from simIK.findConfig ?

[RalfR]

I have a 7Dof manipulator and want to follow some defined target points (as given by various path planners). However, if I calculate the (cartesian) target points and try to solve the inverse kinematics by calling simIK.findConfig, I get a random joint configuration taken from the infinite solutions (because of the additional DoF). The solution is not similar to the current configuration of the robot, so it would generate a lot of unnecessary robot motion. Can I force simIk.findConfig to provide a solution similar to the current state of the robot ? Do I have to use a different function to solve the IK ?
When looking at the results of simIK.generatePath, there seems to be a possibility to generate a string of similar configurations. How is this done internally ?
A probably related problem: I see some suggestions to use the ruckig generator function sim.moveToConfig to calculate positions along a line, treating it as a 1DoF problem. While I understand the idea, I do not understand how this is done with the syntax of the function. sim.moveToConfig takes as input a start and target pose, but not a complete path. Is theree a sample implemenation for this ?

Thanks in advance !

[coppelia]

simIK.findConfig is doing a randomized search, without directly using the Jacobian. This is very different from using simIK.handleGroup, which computes IK by linearizing around the current configuration (i.e. with the Jacobian). The Jacobian is useful when moving from one pose to another pose that is relatively close to the first pose. If you try to move in one step from pose A to pose B that is far away, then IK will probably fail. So you should do intermediate steps, and this is exactly what simIK.generatePath is doing. Have a look at the source code of simIK.generatePath found in the simIK repository, in simIK/lua/simIK.lua:

Code: Select all

function simIK.generatePath(...)
    local ikEnv, ikGroup, ikJoints, tip, ptCnt, callback, auxData = checkargs({
        {type = 'int'},
        {type = 'int'},
        {type = 'table', size = '1..*', item_type = 'int'},
        {type = 'int'},
        {type = 'int'},
        {type = 'any', default = NIL, nullable = true},
        {type = 'any', default = NIL},
    }, ...)

    local lb = sim.setStepping(true)

    local env = simIK.duplicateEnvironment(ikEnv)
    local targetHandle = simIK.getTargetDummy(env, tip)
    local startMatrix = simIK.getObjectMatrix(env, tip)
    local goalMatrix = simIK.getObjectMatrix(env, targetHandle)
    local retPath = {{}}
    for i = 1, #ikJoints, 1 do retPath[1][i] = simIK.getJointPosition(env, ikJoints[i]) end
    local success = true
    if callback then
        if type(callback) == 'string' then
            success = _G[callback](retPath[1])
        else
            success = callback(retPath[1])
        end
    end
    if success then
        for j = 1, ptCnt - 1, 1 do
            local t = j / (ptCnt - 1)
            local m = sim.interpolateMatrices(startMatrix, goalMatrix, t)
            simIK.setObjectMatrix(env, targetHandle, m)
            success = simIK.handleGroups(env, {ikGroup}) == simIK.result_success
            if not success then break end
            retPath[j + 1] = {}
            for i = 1, #ikJoints, 1 do
                retPath[j + 1][i] = simIK.getJointPosition(env, ikJoints[i])
            end
            if callback then
                if type(callback) == 'string' then
                    success = _G[callback](retPath[j + 1])
                else
                    success = callback(retPath[j + 1])
                end
            end
            if not success then break end
        end
    end
    simIK.eraseEnvironment(env)
    sim.setStepping(lb)
    if not success then
        retPath = {}
    else
        retPath = Matrix:fromtable(retPath)
        retPath = retPath:data()
    end
    return retPath
end

You can see that simIK.handleGroup (or simIK.handleGroups) is successively called while movine the target gradually on a linear path (by calling sim.interpolateMatrices).

Cheers