Self-Collision in UR5 with OMPL

[zhangkefang45]

Hi,
When Coppeliasim update to 4.2.0, I want to use UR5 do path planning with OMPL in this version, but I found that the motionPlanningDemo1.ttt is removed. So I referred to path_planning_and_grasping.ttt.
In path_planning_and_grasping.ttt, collisionPair is:

Code: Select all

local collection=sim.createCollection(0)
sim.addItemToCollection(collection,sim.handle_tree,simBase,0)
collisionPairs={collection,collection,collection,sim.handle_all}

When I use this in UR5, no path generate. When I was debugging, I found that UR5 can self-collision without moving!!

Code: Select all

sim.checkCollision(collection, collection)  -- is 1, {pair}

When I remove the self-collision detection(remove pre 2 Items), UR5 can generate a path, but it can Collision it self when moving.
I modified the code, want to check self-collision and ignore the collision of adjacent joints:

Code: Select all

namespace = {'UR5_link1_visible', 'UR5_link2_visible', 'UR5_link3_visible', 'UR5_link4_visible', 'UR5_link5_visible', 'UR5_link6_visible', 'UR5_link7_visible'}
function validationCb(config,auxData)
    local retVal=true
    local prev={}
    for i=1,#jh,1 do
        prev[i]=sim.getJointPosition(jh[i])
        sim.setJointPosition(jh[i],config[i])
    end
    
    --vis, pair = sim.checkCollision(collisionPairs[1],collisionPairs[2])
    -- enviroment
    vis, pair = sim.checkCollision(collisionPairs[1],collisionPairs[2])
    if vis>0 then
        retVal=false
    end
    
    -- self-clossision
    if retVal then
        vis, pair = sim.checkCollision(selfcollisionPair[1],selfcollisionPair[2])
        --vis, pair = sim.checkCollision(collisionPairs[3],collisionPairs[4])
        if vis>0 then
            local name1 = sim.getObjectName(pair[1])
            local name2 = sim.getObjectName(pair[2])
            print(name1..' '..name2)
            ......
            -- Ignore the collision of adjacent joints.
        end
    end

but still had above error...
I suspect the problem is here, because collisionPairs just UR5 and environment:

Code: Select all

simOMPL.setCollisionPairs(task,collisionPairs)

but, when I include 4 pair in collisionPair and check self-collision in validationCb, there are some other error reported:

[simExtOMPL:error] OMPL: /home/marc/Documents/coppeliaRobotics/programming/simExtOMPL/build/ompl-1.5.0-prefix/src/ompl-1.5.0/src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:207: RRTConnect: Motion planning start tree could not be initialized!

I don't know how to fix this... Some one can help me?
Thanks.

[coppelia]

Hello,

if the collision pairs are not correctly specified, then your robot might be colliding already in the initial configuration and no calculation will be possible.
So you really need to make sure that those collision pairs detect exactly what you want. e.g. if you have a collision collection of the collection collectionRobot, and check if the collection self-collides, then collision detection will check every shape (that is marked as collidable) against all other collidable shapes in that collection. This means that adjacent shapes will be checked for collision as well. In a robot, adjacent shapes often collide very slightly, so make sure that this is not the case. Have also a look at the collection self-collision indicator.

Cheers