generate path from sim.generateIkPath

Typically: "How do I... ", "How can I... " questions
Post Reply
davide.barchi
Posts: 5
Joined: 28 May 2018, 08:52

generate path from sim.generateIkPath

Post by davide.barchi »

Hi, i am working on a simple application about a manipulator's path plannig. On the basis of the example "motionPlanningDemo1" i started doing my scene. In particular, the first step i would like to check is the generation of an inverse kinematic path computed by sim.generateIkPath. For this porpouse, the logic that the following code uses is:
- generate the destination pose i want to reach from a non threated child script and set the command to the robot trought an integer signal
- associate to a minipulator a non threated child script that is always listening to the previous signal in order to start its mission. Once the command is given, it has to move from its actual config to the table6_source position through a straight line path. For simplicity, no collisions are considered.

The child script associated to the manipulator is (avoiding initialization part):

Code: Select all

function sysCall_actuation()
    if stateMan == 0 then
        if sim.getIntegerSignal('Do Manipulator Mission') == 1 then
            local boxPosHandle = sim.getObjectHandle('TestTarget')
            local pos=sim.getObjectPosition(boxPosHandle,-1)
            local orientation=sim.getObjectOrientation(boxPosHandle,-1)
            table6_source={pos[1],pos[2],pos[3], orientation[1],orientation[2],orientation[3]}

            stateMan = 1
        end
    end

    if stateMan == 1 then
        --move orientation target to the dst position
        print('move orientation target to ',table6_source)
        sim.setObjectPosition(orientationTargetHandle,-1,{table6_source[1],table6_source[2],table6_source[3]})
        
        --shift along desired axis
        local m = sim.getObjectMatrix(orientationTargetHandle,-1)
        m = getMatrixShiftedAlongAxis(m,-0.8,2) -- shoft along the z axis, a personal modification about original function
        sim.setObjectPosition(orientationTargetHandle,-1,{m[4],m[8],m[12]}) -- this position is checked, it's ok!
        
        -- Compute a straight-line path from current config to pose m:
        path=generateIkPath(getConfig(),m,ikSteps)
        if path == nil then 
            print('null path')
        end

        -- Follow the path:
        followPath(path)
        stateMan = 2
    end

    if stateMan ==2 then
        print('end')
        sim.setIntegerSignal('Do Manipulator Mission',0)
        stateMan = 0 
    end

end
Where the function generateIkPath is :

Code: Select all

generateIkPath=function(startConfig,goalPose,steps)
    -- Generates (if possible) a linear, collision free path between a robot config and a target pose
    local currentConfig=getConfig()
    setConfig(startConfig)
    sim.setObjectMatrix(ikTarget,-1,goalPose)
    ikCalculationResult,jointValues=sim.checkIkGroup(ikGroup,jh,nil)
    print('results from ik check',ikCalculationResult,jointValues)
    local c=sim.generateIkPath(ikGroup,jh,steps,nil,nil)
    setConfig(currentConfig)
    return c
end
The problem is that the code is printing null path, meaning that the API sim.generateIkPath is not working. The pose is reachable, ikGroup is checked and the robot use is the one copied from the mentioned Demo.

What am i missing?
Cheers,
Davide

coppelia
Site Admin
Posts: 10336
Joined: 14 Dec 2012, 00:25

Re: generate path from sim.generateIkPath

Post by coppelia »

Hello,

please post your scene, it is impossible to say otherwise what is going wrong.

Cheers

Post Reply