Manipulators IK and Path Following

Typically: "How do I... ", "How can I... " questions
Post Reply
mttcest
Posts: 9
Joined: 15 Sep 2021, 08:09

Manipulators IK and Path Following

Post by mttcest »

Hello CoppeliaSim comunity 😊,

I have my open-chain manipulator (UR5 for this test) and I want to make the end-effector follow a path in a dynamic environment in order to study the velocity and torque profiles of the joints.

I have set up the IK in the "simple way" following the tutorial and I made the end-effector chaising the path using the code that I found in the user manual on the voice "path".

Here is my code:

Code: Select all

function sysCall_init()
    -- IK (simple way)
    local simBase=sim.getObjectHandle('UR5')
    local simTip=sim.getObjectHandle('UR5tip')
    local simTarget=sim.getObjectHandle('UR5target')
    -- create an IK environment:
    ikEnv=simIK.createEnvironment()
    -- create an IK group: 
    ikGroup_undamped=simIK.createIkGroup(ikEnv)
    -- set its resolution method to undamped: 
    simIK.setIkGroupCalculation(ikEnv,ikGroup_undamped,simIK.method_pseudo_inverse,0,6)
    -- create an IK element based on the scene content: 
    simIK.addIkElementFromScene(ikEnv,ikGroup_undamped,simBase,simTip,simTarget,simIK.constraint_pose)
    -- create another IK group: 
    ikGroup_damped=simIK.createIkGroup(ikEnv)
    -- set its resolution method to damped: 
    simIK.setIkGroupCalculation(ikEnv,ikGroup_damped,simIK.method_damped_least_squares,1,99)
    -- create an IK element based on the scene content: 
    simIK.addIkElementFromScene(ikEnv,ikGroup_damped,simBase,simTip,simTarget,simIK.constraint_pose) 
    -- IK (simple way)
    
    --Path Follow
    objectToFollowPath=sim.getObjectHandle('UR5target')
    path=sim.getObjectHandle('Path')
    pathData=sim.unpackDoubleTable(sim.readCustomDataBlock(path,'PATH'))
    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()
    pathLengths,totalLength=sim.getPathLengths(pathPositions,3)
    velocity=0.04 -- m/s
    posAlongPath=0
    previousSimulationTime=0
    corout=coroutine.create(coroutineMain)
    --Path Follow
    
    -- Joint Handle
    JH = {}
    JH[1] = sim.getObjectHandle('UR5_joint1')
    JH[2] = sim.getObjectHandle('UR5_joint2')
    JH[3] = sim.getObjectHandle('UR5_joint3')
    JH[4] = sim.getObjectHandle('UR5_joint4')
    JH[5] = sim.getObjectHandle('UR5_joint5')
    JH[6] = sim.getObjectHandle('UR5_joint6')
    -- Joint Handle
end

function sysCall_actuation()
    -- IK (simple way)
    -- try to solve with the undamped method:
    if simIK.applyIkEnvironmentToScene(ikEnv,ikGroup_undamped,true)==simIK.result_fail then 
        -- the position/orientation could not be reached.
        -- try to solve with the damped method:
        simIK.applyIkEnvironmentToScene(ikEnv,ikGroup_damped)
        -- We display a IK failure report message: 
        if not ikFailedReportHandle then 
            ikFailedReportHandle=sim.displayDialog("IK failure report","IK solver failed.",
                sim.dlgstyle_message,false,"",nil,{1,0.7,0,0,0,0})
        end
    else
        if ikFailedReportHandle then
        	-- We close any report message about IK failure:
            sim.endDialog(ikFailedReportHandle) 
            ikFailedReportHandle=nil
        end
    end
    -- IK (simple way)
    
    --Path Follow
    if coroutine.status(corout)~='dead' then
        local ok,errorMsg=coroutine.resume(corout)
        if errorMsg then
            error(debug.traceback(corout,errorMsg),2)
        end
    end
    --Path Follow
end

function sysCall_sensing()
    -- put your sensing code here
end

function sysCall_cleanup()
    -- erase the IK environment: 
    simIK.eraseEnvironment(ikEnv) 
end

-- See the user manual or the available code snippets for additional callback functions and details
function coroutineMain()
    sim.setThreadAutomaticSwitch(false)
    while true do
        local t=sim.getSimulationTime()
        posAlongPath=posAlongPath+velocity*(t-previousSimulationTime)
        posAlongPath=posAlongPath % totalLength
        local pos=sim.getPathInterpolatedConfig(pathPositions,pathLengths,posAlongPath)
        local quat=sim.getPathInterpolatedConfig(pathQuaternions,pathLengths,
                                                 posAlongPath,nil,{2,2,2,2})
        sim.setObjectPosition(objectToFollowPath,path,pos)
        sim.setObjectQuaternion(objectToFollowPath,path,quat)
        previousSimulationTime=t
        sim.switchThread()
    end
end
The problem is that the robot is flicking at some points and in those istances the IK is not solved.

So my questions are:
  • Is there a better way to set up these kind of tasks?
  • If not, is there a way to make my robot more stable while chaising a path?
Thank you in advance for your help 😁

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

Re: Manipulators IK and Path Following

Post by coppelia »

Hello,

probably you are passing through singularities, touching joint limits, etc. Difficult to say without having a look at your scene. Try posing a minimalistic, self-contained scene that illustrates your problem.

Cheers

mttcest
Posts: 9
Joined: 15 Sep 2021, 08:09

Re: Manipulators IK and Path Following

Post by mttcest »

Hello Admin,

First of all, thank you for answering.

Modifying the path control points I've been able to achieve a more stable behaviour.

But there are things that I still don't understand.
I've been looking at some IK example scenes and in some of them the IK application code was inside the coroutineMain, just before to move the end effector with a "blocking-function".

So my question is ... what is the point of having the coroutine for the path following? I think I am missing the logic of this kind of operation.

I've been studying the manual trying to understand it for a while but I am still lost 😅

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

Re: Manipulators IK and Path Following

Post by coppelia »

Following a path via IK with the end-effector is a complex task. The reason is that your path, even if a simple line, might bring the manipulator to one of its joint limits, or close to a singular configuration, if you do not carefully chose the start configuration: most robots have 2, sometimes more (e.g. an infinity, in case of redundant robots) possible configurations for a single end-effector pose.
Try to make all your joints cyclic and see if the result improves. If yes, this means that your path is brining your robot close to joint limits.
So complex planning is often required to avoid those situations and simply using IK alone won't make it.

Cheers

mttcest
Posts: 9
Joined: 15 Sep 2021, 08:09

Re: Manipulators IK and Path Following

Post by mttcest »

Ok setting the joints cyclic seems to work, I think that the issues were all about my path definition.

But since the robot links in real world are "respondable" each other I think I should start experimenting with path planning. 😁

Thank you for the quick answer 👍!

Post Reply