Question about simGenerateIkPath() function

Typically: "How do I... ", "How can I... " questions
21207_iHome
Posts: 7
Joined: 25 Apr 2017, 03:13

Question about simGenerateIkPath() function

Postby 21207_iHome » 25 Aug 2017, 03:09

Dear Coppelia,
According to motionPlanningDemo1.ttt the simGenerateIkPath function will generates (if possible) a linear, collision free path between a robot config and a target pose. However, when I tried it ion my simple 3-bar mechanism it generated a straight line path which collided with the environment. I have set the collision pairs but it didn't work. Can you give me some suggestions?
Image
Image: https://d.pr/se1njq
Scene:https://d.pr/nIXF5R

Code: Select all

visualizePath = function(path)
    if not _lineContainer then
        _lineContainer = simAddDrawingObject(sim_drawing_lines, 3, 0, -1, 99999, {0.2,0.2,0.2})
    end

    simAddDrawingObjectItem(_lineContainer, nil) -- clear all items

    if path then
        local pc = #path / 3
        for i=1, pc - 1,1 do
            lineDat = {path[(i-1)*3+1], path[(i-1)*3+2], 0, path[i*3+1], path[i*3+2], 0}
            simAddDrawingObjectItem(_lineContainer, lineDat)
        end
    end
end


if (sim_call_type==sim_childscriptcall_initialization) then

    -- Put some initialization code here
    jh={-1,-1,-1}
    for i=1,3,1 do
        jh[i]=simGetObjectHandle('J'..i)
    end

    ikGroup=simGetIkGroupHandle('IK_Group')
    ikTarget=simGetObjectHandle('Target')
    collisionPairs={simGetCollectionHandle('manipulator'),simGetCollectionHandle('environment')}

    simSetObjectPosition(ikTarget, -1, {0.5,0.5,0})

    -- Generates a path that drives a robot from its current configuration to its target dummy in a straight line
    path = simGenerateIkPath(ikGroup, jh, 20, collisionPairs, nil)

    visualizePath(path)
end

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

Re: Question about simGenerateIkPath() function

Postby coppelia » 25 Aug 2017, 14:23

Hello,

it is important to understand that the API function you mention won't do any path planning (or won't search for a collision-free path). This is probably not what you want. You should look into path planning instead.

Also, in your example you are using the script in a funny way. You realize that for your case a threaded child script would be more appropriate. In your current scene your non-threaded child script is constantly being called. And if you print variable path would would realize that it would be nil.

Cheers

david07
Posts: 6
Joined: 22 Feb 2017, 14:43

Re: Question about simGenerateIkPath() function

Postby david07 » 04 Sep 2017, 16:03

Dear Coppelia,

I would have some question regarding the abovementioned function. My original plan is to make the teaching easier with using similar methods like in industrial application (teach points, and tell what kind of movement should the robot perform, p2p (=joint movement), or linear movement'), since we would like to use your software (with license) for industrial purposes.
MoveJ is fine, but i am having problem with implementing the linear movement. Ideal would be to define a dummy and from the current configuration of the robot i can generate a linear path to that dummy. For this purpose i've used some code from some demo scene in a threaded child script, but it does not work, my path variable is always a nil value:

Code: Select all

-- Put some initialization code here
rob1_target = simGetObjectHandle('UR10_1_target')
rob1_tip = simGetObjectHandle('UR10_1_tip')
rob1_ik = simGetIkGroupHandle('rob1_ik')

path1 = simGetObjectHandle('Path1')
path2 = simGetObjectHandle('Path2')

pos1 = {2.7548696994781, -3.0332839488983, -2.3495359420776, 2.2396476268768, 2.7547559738159, 1.5693582296371}
pos2 = {2.7548861503601, -3.5293252468109, -2.8574450016022, 3.2413556575775, 2.7548768520355, 1.5669993162155}

testpos1 = simGetObjectHandle('testpos1')
testpos2 = simGetObjectHandle('testpos2')

rob1_JH={-1,-1,-1,-1,-1,-1}
for i=1,6,1 do
    rob1_JH[i]=simGetObjectHandle('UR10_1_joint'..i)
end

vel=10
accel=40
jerk=80
currentVel={0,0,0,0,0,0,0}
currentAccel={0,0,0,0,0,0,0}
maxVel={vel*math.pi/180,vel*math.pi/180,vel*math.pi/180,vel*math.pi/180,vel*math.pi/180,vel*math.pi/180,vel*math.pi/180}
maxAccel={accel*math.pi/180,accel*math.pi/180,accel*math.pi/180,accel*math.pi/180,accel*math.pi/180,accel*math.pi/180,accel*math.pi/180}
maxJerk={jerk*math.pi/180,jerk*math.pi/180,jerk*math.pi/180,jerk*math.pi/180,jerk*math.pi/180,jerk*math.pi/180,jerk*math.pi/180}
targetVel={0,0,0,0,0,0,0}

forbidLevel=0


MoveJ = function(pos)
    simSetExplicitHandling(rob1_ik,1) -- IK is turned off, so i can control the robot with joint positions
    simRMLMoveToJointPositions(rob1_JH,-1,currentVel,currentAccel,maxVel,maxAccel,maxJerk, pos,targetVel)
    simSetObjectMatrix(rob1_target,-1, simGetObjectMatrix(rob1_tip,-1))   -- because IK is turned off, after the movement the target jumps on the place of the tip
    simSetExplicitHandling(rob1_ik,0)
end


getConfig=function()
    -- Returns the current robot configuration
    local config={}
    for i=1,#rob1_JH,1 do
        config[i]=simGetJointPosition(rob1_JH[i])
    end
    return config
end

setConfig=function(config)
    -- Applies the specified configuration to the robot
    if config then
        for i=1,#rob1_JH,1 do
            simSetJointPosition(rob1_JH[i],config[i])
        end
    end
end

generateIkPath=function(startConfig,goalPose,steps)
    -- Generates (if possible) a linear, collision free path between a robot config and a target pose
    forbidThreadSwitches(true)
    local currentConfig=getConfig()
    setConfig(startConfig)
    simSetObjectMatrix(rob1_target,-1,goalPose)
    local c=simGenerateIkPath(rob1_ik,rob1_JH,steps,NULL)
    setConfig(currentConfig)
    forbidThreadSwitches(false)
    return c
end

forbidThreadSwitches=function(forbid)
    -- Allows or forbids automatic thread switches.
    -- This can be important for threaded scripts. For instance,
    -- you do not want a switch to happen while you have temporarily
    -- modified the robot configuration, since you would then see
    -- that change in the scene display.
    if forbid then
        forbidLevel=forbidLevel+1
        if forbidLevel==1 then
            simSetThreadAutomaticSwitch(false)
        end
    else
        forbidLevel=forbidLevel-1
        if forbidLevel==0 then
            simSetThreadAutomaticSwitch(true)
        end
    end
end

followPath=function(path)
    -- Follows the specified path points. Each path point is a robot configuration. Here we don't do any interpolation
    if path then
        local l=#rob1_JH
        local pc=#path/l
        for i=1,pc,1 do
            local config={path[(i-1)*l+1],path[(i-1)*l+2],path[(i-1)*l+3],path[(i-1)*l+4],path[(i-1)*l+5],path[(i-1)*l+6]}
            setConfig(config)
            simSwitchThread()
        end
    end
end

-- Put your main loop here, e.g.:

-- simFollowPath(rob1_target, path1, 3, 0, 0.1, 0.01)
-- simFollowPath(rob1_target, path2, 3, 0, 0.1, 0.01)


-- while simGetSimulationState()~=sim_simulation_advancing_abouttostop do
   
    MoveJ(pos1)
    local m=simGetObjectMatrix(testpos2,-1)
    local path=generateIkPath(getConfig(),m, 2)
    print(path)
    followPath(path)

-- end


The dynamics engine is disabled in my simulation, and the joints are in inverse kinematics mode in order to use simRMLMoveToJointPositions command (it doesnt want to work when the joints in torque-force mode).
Could you give some advice what is the problem and how can i improve my simulation?
(the scene can be found under this link in my google drive: https://drive.google.com/open?id=0B7kF1 ... lBnM1hRN0E )

Thanking you in advance!
Best regards
David

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

Re: Question about simGenerateIkPath() function

Postby coppelia » 06 Sep 2017, 08:56

You have 2 main problems in your scene. The most important, is that you are using a damped resolution method for your IK (why?). The damped resolution method is much slower to converge and this is the reason why no path is found. Switch to the pseudo inverse method, or decrease the damping factor.

The second problem is that you switch back to the implicit IK handling method at some point. With the implicit handling, IK will always be computed in the main script and applied to the robot for the current position of the target dummy. Simply keep the IK group in explicit handling all the time.

Cheers

david07
Posts: 6
Joined: 22 Feb 2017, 14:43

Re: Question about simGenerateIkPath() function

Postby david07 » 07 Sep 2017, 06:48

Thank you for your answer, I will try that!

(Btw. I am using DLS method, because as I mentioned I would like to program the robot with move and teach. I drag the TCP, move it around and create dummies. For that the DLS is much better for obvious reason.) But i got stuck afterwards, how to use these dummies with the generateIkPath function, so i hope it will work now. Thanks again.

Best regards,
David


Return to “General questions”

Who is online

Users browsing this forum: Baidu [Spider], Bing [Bot], Google [Bot] and 26 guests