[Solved] Problem with Collision Checking

Typically: "How do I... ", "How can I... " questions
francofusco
Posts: 9
Joined: 22 Apr 2017, 09:20

[Solved] Problem with Collision Checking

Postby francofusco » 15 May 2017, 14:56

Hello,

I am currently working with a dual-arm robot. To validate motions, I need to check collision between some entities. I defined functions that should check the collision state of the robot, but they seems not to work properly.

All my code is contained in three threaded child scripts: one is used for motion validation, the other two for "low level" control of the arms. The idea is that from the first script I move a target (by actuating some joints), while the "controllers" solve IK and move the robot to the target. I am (quite) sure that arms scripts do not contain bugs of any kind. The problem should be in the first one.

Regarding collisions, I defined three collections of objects called "left_arm", "right_arm" and "body". When starting the simulation, all collection handles are grouped inside a table called "collision_pairs" as follow:

Code: Select all

collision_pairs = {
    {simGetCollectionHandle('left_arm'),simGetCollectionHandle('right_arm')},
    {simGetCollectionHandle('left_arm'),simGetCollectionHandle('body')},
    {simGetCollectionHandle('body'),simGetCollectionHandle('right_arm')}
}

(the reason of such choice is that I have other collision groups which I do not want to use; this structure allows me to "quickly" specify which pairs should be considered)

I then defined a function that checks if the robot is in collision:

Code: Select all

function isCurrentConfigCollisionFree()
    for i=1,#collision_pairs do
        local coll = simCheckCollision(collision_pairs[i][1], collision_pairs[i][2])
        if coll~=0 then return false end
    end
    return true
end

Here, the complete script (which moves the target to random configurations):

Code: Select all

require "my_ik" -- defines Inverse Kinematics of my robot

simSetThreadSwitchTiming(2) -- Default timing for automatic thread switching

target_joint_handles = {simGetObjectHandle("tj1"),simGetObjectHandle("tj2"),simGetObjectHandle("tj3")}
jhL = { ... } -- stores joint handles of the left arm
jhR = { ... } -- stores joint handles of the right arm

-- get joint positions (listed in a table)
-- jh: table with joint handles
function getConfig(jh)
   local q = {}
   for i=1,#jh do q[i]=simGetJointPosition(jh[i]) end
   return q
end

-- set joint positions
-- jh: table with joint handles
-- q: table with joint coordinates
function setConfig(jh, q)
   for i=1,#jh do simSetJointPosition(jh[i], q[i]) end
end

-- check if the configuration of the target is valid
function sampleIsValid(s)
        simSetThreadAutomaticSwitch(false) -- lock switching
        local conf = getConfig(target_joint_handles) -- configuration of the target
        local qL = getConfig(jhL) -- configuration of the left arm
        local qR = getConfig(jhR) -- configuration of the right arm
        local config_free = false

        setConfig(target_joint_handles, s) -- temporarily change configuration of the target

        -- check if there exist inverse kinematic solutions for the arms
        -- the functions ikL and ikR return the IK solutions or nil if the target cannot be reached
        local qinvL = ikL(s)
        local qinvR = ikR(s)

        if (qinvL~=nil) and (qinvR~=nil) then
            -- solution found for both arms: can then check for collision
            setConfig(jhL, qinvL) -- update position of left arm
            setConfig(jhR, qinvR) -- update position of right arm
            config_free = isCurrentConfigCollisionFree()
        end
       
        setConfig(target_joint_handles, conf) -- reset target's configuration
        if (qinvL~=nil) and (qinvR~=nil) then
            setConfig(jhL, qL) -- reset left arm's configuration
            setConfig(jhR, qR) -- reset right arm's configuration
        end
        simSetThreadAutomaticSwitch(true) -- unlock switching

        if (qinvL==nil) or (qinvR==nil) or (config_free==false) then
            return false -- configuration is not valid
        else
            return true -- configuration is valid
        end
end

-- generate random (valid) configurations of the target
function validTarget()
    local q = {}
    repeat
        for i=1,#target_joint_handles do
            q[i]=Qmax*(2*math.random()-1)
        end
    until (sampleIsValid(q)==true)
    return q
end

-- move target to random positions and check collision
function threadFunction()
    simSetThreadSwitchTiming(200)
    while simGetSimulationState()~=sim_simulation_advancig_abouttostop do
       local cfg = validTarget()
       
        simSetIntegerSignal("follow_target",1) -- tells the other scripts to move the arms
        moveToConfiguration(target_joint_handles, cfg) -- this function makes the target move slowly towards cfg
        simSetIntegerSignal("follow_target",0) -- arms still
       
        -- check if current configuration is collision free
        assert(sampleIsValid(getConfig(target_joint_handles))==true)
        assert(isCurrentConfigurationCollisionFree()==true)
        simWait(1,true)
    end
end

pcall(threadFunction) -- execute, ignore errors
-- in the following I do some cleanup

What I would expect is to see the robot moving to collision-free states (eventually colliding between two configuration, but I don't care). Instead, it happens (quite often) that the target configuration is in collision (the assertions do not raise any error, though).

Additional info that may help:
  • Objects of interest are all collidable
  • Joints are all in torque/force mode.
  • All robot shapes are static (and this is why I use simSetJointPosition rather than simSetJointTargetPosition)
  • I added collision detection modules for each collision pair: the collision are correctly evaluated, and the arms become red when in collision

Any idea of what is happening?

Thanks in advance!
Last edited by francofusco on 16 May 2017, 10:16, edited 1 time in total.

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

Re: Problem with Collision Checking

Postby coppelia » 15 May 2017, 15:57

Hello,

can you post a simplified scene that illustrates your problem?

Cheers

francofusco
Posts: 9
Joined: 22 Apr 2017, 09:20

Re: Problem with Collision Checking

Postby francofusco » 16 May 2017, 10:15

Shame on me!

Just found I was simply checking collision for the wrong objects. Indeed, I had a "body" collection and a "body_simpl" one. I was using the wrong one...

But thanks anyway!


Return to “General questions”

Who is online

Users browsing this forum: Baidu [Spider] and 24 guests