Problems in Space Relocatable Manipulator Path Planning Using OMPL

[zorro]

Hello! I am conducting a simulation experiment on a spatial crawling robotic arm using Coppeliasim. The crawling mechanism of this robotic arm involves alternating between the head and tail, similar to the Canadarm2 robotic arm on the International Space Station.

I've previously raised some issues regarding simIK and simOMPL, and I appreciate the excellent solutions provided. However, I've encountered some challenging problems in the subsequent process. Firstly, I designed a 7-DOF symmetric robotic arm, imported it into Coppeliasim using a URDF model, and controlled the arm using the ZMQ Remote API in Python. Implementing the alternating head and tail movement in the simulation proved to be quite difficult. I resorted to using `sim.setObjectParent()` to swap the order of joints, links, and dummies of the robotic arm to achieve this functionality.

The basic flow of the scenario is as follows: The robotic arm starts at the zero position of the joints. The end-effector A moves to target position 1, then my custom alternating function is called. Subsequently, end-effector B moves to target position 2, and this process repeats until all target positions are reached.

During this process, I use simIK to find the solution for the target pose and simOMPL for path planning. When the robotic arm is in the initial position, I can usually find a suitable path to reach the target position each time. However, after the alternating head and tail process, although OMPL can still find a path, the robotic arm often seems to collide with itself, preventing it from reaching the target position. I verified the OMPL path and found that the final point of the path is indeed at the target position, but the arm cannot execute to that location.

I'm unsure of the specific cause of this issue. Is it because my alternating method is too aggressive? Does the OMPL process need to be reset? Or could it be due to joint constraints on the robotic arm? I use `valid = simOMPL.isStateValid(task, config)` to check the target pose, but is it possible that the generated path is still infeasible?

I've come across papers using Coppeliasim and simOMPL to achieve a similar process, so the implementation of the entire scenario should be feasible. I would like to upload my scene file and code for reference, but this content is related to my thesis. Is there a way to send the project separately to you without others being able to view it?

[zorro]

I think it's the collisionpairs problem, my collisionpairs was set like this:

Code: Select all

simOMPL.setCollisionPairs(task, [roboCollection, sim.handle_all])

In which

Code: Select all

roboCollection

was the collection of my manipulator chain. So I checked the demo pathPlanningAndGrasping.ttt, the collisionpairs was set like this:

Code: Select all

simOMPL.setCollisionPairs(task, [roboCollection, roboCollection, roboCollection, sim.handle_all])

So I changed my code just like the demo, but there was an warning and error:

Code: Select all

[simOMPL:warning] OMPL: D:\coppeliaRobotics\programming\simOMPL\build\ompl-1.5.0-prefix\src\ompl-1.5.0\src\ompl\base\src\Planner.cpp:258: RRTConnect: Skipping invalid start state (invalid state)
[simOMPL:error] OMPL: D:\coppeliaRobotics\programming\simOMPL\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!

which indicates that my links are colliding, so I cheked this post:https://forum.coppeliarobotics.com/viewtopic.php?t=9170. So, I attempted to adjust the values in the self-collision indicator, but the text and images in the official user manual are a bit inconsistent. I'm unsure how to configure it correctly to avoid collisions between adjacent links. Could you please guide me on how to set this up?

[coppelia]

Hello,

those can be tricky bugs to work out. The best is to always first start by ignoring all collisions and check if appropriate nodes are found.
Then, make sure that your collection is correctly defined, especially if your collection's content is changing. Then only use one collision pair at first: the self-collision pair collection-collection.

Only if all of above works fine, should you add additional collision-pairs. Ideally, each collision-pair should be tested on its own to make sure there is no subtle bug.

As to your specific situation, it would be helpful if you could post a minimalistic, self-contained scene that illustrates your problem: it is difficult to imagine what kind of situation you have just from text ;)

About the collection self-collision indicator: when using e.g.:

Code: Select all

sim.checkCollision(collectionHandle, collectionHandle)
-- or
sim.checkDistance(collectionHandle, collectionHandle)

Then only object pairs in that collection, where:

Code: Select all

abs(object1SelfCollIndicator - object2SelfCollIndicator) == 1

are ignored.

Cheers

[zorro]

Thank you for the response! This time, I've uploaded two scene files and several pieces of code to illustrate the issues I'm facing. I've addressed the self-collision problem by modifying the model, leaving some gaps between each joint to avoid collisions. I thought that resolving the self-collision issue would allow OMPL to plan a suitable path. However, even though OMPL plans a path, the robotic arm stops running after a while. The scene file corresponding to this issue is located at: `ttt/switch_base_self_collision_more1129.ttt`, and the corresponding code is at: `code/main_test`. This is the scene file and corresponding code for my crawling robotic arm. Running the code several times will reproduce the problem I mentioned.

Afterward, I tried to find inspiration from the official examples. I referred to the official example file `pathPlanningAndGrasping.ttt`. I removed the script for Jaco in the scene because I wanted to replicate this demo using the ZMQ RemoteApi in Python. I translated almost all the Lua code in the Jaco script to Python. I believed that replicating the code entirely through the remote API could achieve the same effect. However, contrary to my expectations, after running the program, it took a long time to generate the planned path (I understand there's a delay with the remote API, but it shouldn't be this extreme). Moreover, the displayed path didn't reach the target positions as in the original demo. Additionally, the execution of the robotic arm's path was not smooth, often stopping after a short distance. I can't comprehend why this is happening. Is it destined that the remote API cannot achieve the same functionality? The scene file corresponding to this issue is located at: `ttt/pathPlanningAndGrasping_RemoteapiTest.ttt`, and the code file is at: `code/pathPlanningAndGrasping_remote_api.py`.

I'm unable to solve these issues on my own, and I would appreciate it if you could download my files and help me resolve them. (Apologies for my poor coding skills; I thought passing parameters would be more convenient, but in the end, it seems the more I pass, the more issues arise.)


https://1drv.ms/f/s!Ar1_NNuNx1urmzIQRK8 ... A?e=oI0cYK

[coppelia]

Hello again,

I had a look at the scene. First of all, I couldn't dive much into your code, since that would be quite time consuming, since the problem is not narrowed-down to a precise aspect.

But following things I noticed / could be the cause of a problem:

• When you create a collection as a tree or as a chain, then the collection content will dynamically grow/shrink, depending on what new objects are attached to (or detached from) the tree or chain. E.g. if you attach another collidable object to the end-effector, then it becomes part of the robot and might immediately collide with the environment. This is often tricky to detect. One way to avoid this is to create a collection that contains objects added as sim.handle_single, i.e. the collection might shrink if one of those objects is deleted, but will not grow. But then there might be another problem: the object that the end-effector is grasping/attaching, will not be part of the collection and might collide itself constantly with the collection objects.
• The ZeroMQ remote API has a bug and is currently not able to handle anonymous functions or class methods correctly. We are going to release rev. 10 in a few hours hopefully, that should among others, also address that problem. You will have to update the coppeliasim_zmqremoteapi_client package, or use the local source (from zmqRemoteApi import RemoteAPIClient)
• if you have callbacks that are called several thousand of times, then an external Python client is not really efficient, and you'll see a large slowdown due to communication overhead: CoppeliaSim will call-back Python x times (for the x callbacks), and the Python callback function itself will then call CoppeliaSim at least 3*6 + 1 times for each of the x times (get the config (6 calls), apply the config (6 calls), check collision (1 call), restore config (6 calls). So that makes about 20*x back-and-forth calls between CoppeliaSim and the client. If each back-and-forth communication takes e.g. 0.2 ms, then it would already take 1 second for x=250. Much better would be to handle/service those callback from within an embedded Lua script. If you were to have your Python code as a Child script inside CoppeliaSim, then you could add a #luaExec directive that handles the callback function in Lua. But in your case, since you operate from an external client, you'd have to use e.g. a Lua embedded script with a function that reads.e.g. findCollisionFreeConfig(args...) and have your Python client call once sim.callScriptFunction('findCollisionFreeConfig', scriptHandle, args...): that would be much much faster.

Cheers

[zorro]

Happy New Year!

I was almost ready to give up on my previous approach, but I finally resolved the issue. As I mentioned before, to facilitate the path planning of a spatial crawling robotic arm, I swapped the order of all joints and links each time the end effector of the robotic arm reached the target pose. However, the problem was that while the robotic arm could always reach the target pose the first time, after swapping the order once, the arm often couldn't reach the next target pose.

One issue that puzzled me was that despite having a solution in the OMPL planning, the robotic arm still couldn't reach the target position. Yesterday, I discovered using the simOMPL.hasExactSolution() function that OMPL does not always provide an exact solution but often gives an approximate one, leading to the robotic arm being unable to reach the target pose.

The next question was why the robotic arm couldn't provide an exact solution. After swapping the base of the robotic arm once, I set the position of each joint to 0 in the joint dialog to observe the joint zero position status after the base swapping. I found that after swapping the base, the robotic arm's zero position was in a very strange posture, causing the working space of the arm at this zero position to be very narrow. This often resulted in OMPL being unable to provide an accurate solution.

Then I finally understood the problem. My initial solution was simply swapping the parent-child relationships and order of joints and links that needed to be reordered in the hierarchical tree. This caused an issue where, for one of the joints, originally fixed on link A and driving link B to rotate, after swapping the hierarchy order, this joint became fixed on link B and drove link A to rotate. Essentially, The rigid body to which the coordinate system of the joint is attached underwent a change, leading to a change in the robotic arm joint zero position.

The final solution was to rotate each joint around its own x-axis by 180° each time the hierarchy order was swapped. Only this way could the zero position states of the joints before and after swapping the base of the robotic arm be unified.

Thank you very much for your previous answers and suggestions. These suggestions have been crucial for the subsequent optimization of my code!