motionPlanningDemo1 cannot be ran in v4.2.0

[pwyq]

Hi CoppeliaSim,

I would like to achieve the exact same obstacle avoidance as shown in the `motionPlanningDemo1`. When I try to run the example scene `motionPlanningDemo1` from V4.1.0 in V4.2.0, it gets stuck.
I am wondering what modifications that need to be done so that it can run in V4.2.0.

Thank you.

[coppelia]

Hello,

you will have to add

Code: Select all

simOMPL.setup(task)

around line 195. Additionally that scene is not such a good example, and it should rather use a multiquery approach. See scene scenes/pathPlanning/roadmap_multiquery.ttt in V4.2.0+ for such an example.

Cheers

[pwyq]

Hi, I added the provided line, but the scene still gets stuck in "info: searching for a maximum of 60 valid goal configurations..."

Do I need to rebuild the scene (like changing API format to the latest) in V4.2.0?

[coppelia]

Normally there's nothing else to change. But as I previously mentioned, the approach is not very good in that scene. Try to adjust some parameters. e.g. instead of searching for 60 different valid goal configurations, search for 10 (around line 289). Same around line 303, try replacing the number of path searches with 1 instead of 6.

Cheers

[pwyq]

Hello Coppelia,

I am trying to move a robot arm between two configurations with an obstacle in between. I tried to follow roadmap_multiquery.ttt and motionPlanningDemo1.ttt, but failed to get the obstacle avoidance working.

My code:
https://drive.google.com/drive/folders/ ... sp=sharing

I have some questions
1. In the shared link, new_ompl_test.lua (which mimics roadmap_multiquery.ttt) the arm sometimes passes through the pillar obstacle. Is it because my collision pairs is wrong?
2. In the shared link, new_ompl_test.lua, the arm moves very fast. How can I makes the robot moves more natural?
3. In the shared link, new_ompl_test2.lua (which mimics motionPlanningDemo1.ttt), the code always stucks at line 77 for the sim.getConfigForTipPose() function. I found that if the collision pair is nil, the arm actually moves to the desired configuration; however, if the collision pair is set, the code stucks. I tried to set collision pairs in two ways (1) the collision pairs are two collections: a collection of robot joints vs. a collection of obstacles; (2) the collision pairs are the robot vs. individual obstacles. I am pretty sure the problem comes from the setup of collision pairs. I am wondering if it's possible to fix this?

Much thanks in advance.

[coppelia]

you have several things that are not right with your scene.

The first is that you do not correctly specify collision pairs. They should be, e.g.: entity1A,entity1B,entity2A,entity2B, etc.
Entity1A will be tested against entity1B, and entity2A will be tested against entity2B, etc.

The simplest to check if your robot (in its entirety) is colliding against the environment would be to do something like:

Code: Select all

    local robotBase=sim.getObjectHandle('IRB2600'),
    robotCollection=sim.createCollection(0)
    sim.addItemToCollection(robotCollection,sim.handle_tree,robotBase,0)
    
    environmentCollection=sim.createCollection(0)
    sim.addItemToCollection(environmentCollection,sim.handle_all,-1,0)
    sim.addItemToCollection(robotCollection,sim.handle_tree,robotBase,1)
    
    ...
    
    local collision_pairs = {robotCollection,environmentCollection}
    simOMPL.setCollisionPairs(OMPL_task, collision_pairs)

If additionally you need to check if the robot is self-colliding, then you'd need to specify one additional collision pair (i.e. 2 pairs in total):

Code: Select all

    local collision_pairs = {robotCollection,environmentCollection,robotCollection,robotCollection}
    simOMPL.setCollisionPairs(OMPL_task, collision_pairs)

Note that self-collision check works by checking every collidable object in collection robotCollection, against every other collidable object in the same collection. In your current robot model, there are many many objects colliding with each other. To avoid this, first disable the collidable flag for all shapes that are not visible. Then, make sure that consecutive shapes (i.e. adjacent shapes) do not generate a collision by appropriately adjusting their collection self-collision indicators. To test if your robot self-collides and which shapes are involved in that self-collision, you can simply type: sim.checkCollision(robotCollection,robotCollection): the 3rd return argument indicates the handles of 2 colliding objects.

Cheers

[pwyq]

Hello Coppelia,

Thank you for the suggestions. I made following changes (code: https://drive.google.com/drive/u/0/fold ... gHCK4xdOkI):
- only enabled visible shapes to be collidable
- changed all visible shape's self-collision indicators to 0, left others' indictors unfilled
- set up collision paris properly
- used checkCollision from this post, and no self collision was found

and I still have some issues...

1. Setting the same configurations directly in scene child script vs. setting in functions gives different robot movement. When I directly set the config in child script, the robot moves to the target pose (picture), but when I tried to set it in functions or external script, the robot moves to a random pose (picture). I included a minimal reproducible code in the shared link (setConfig_issue.lua). What is the possible cause for this?
2. By using ompl_multiquery.lua, the robot moves too fast and the obstacle avoidance doesn't seem to work...
3. By using ompl_motion_planning.lua, the robot takes too long to find a valid path or it simply stucks (I also tried other OMPL algorithms)

It would be much appreciated if you may shed some lights on these issues... Thank you very much

[coppelia]

Again, you have several problems in your scene:

• you still have an IK task trying to move the end-effector to a target pose. Delete object IRB2600_IkTarget
• your environment collection is not correct. It is not:

Code: Select all

    environment_collection = sim.createCollection(0)
    sim.addItemToCollection(environment_collection, sim.handle_all, -1, 0)
    sim.addItemToCollection(robot_collection, sim.handle_tree, robot_base, 1)

but

Code: Select all

    environment_collection = sim.createCollection(0)
    sim.addItemToCollection(environment_collection, sim.handle_all, -1, 0)
    sim.addItemToCollection(environment_collection, sim.handle_tree, robot_base, 1)

Then you'll see that the robot is massively self-colliding. That's because you set all collection self-collision indicators to 0. That's not how it is supposed to work. If consecutive joints should not be detected in self-collision, then they need a consecutive collection self-collision indicator, e.g. 1,2,3,4, etc.

Cheers