Map_base::at error when parsing SRDF

[Mattolo4]

Im trying to setup roboplan working for robots with complex models (>40 joints). With these models the SRDF file becomes very large due to the extensive <disable_collisions> entries required (+1k for full coverage).

When trying to load the complete SRDF via ros topic (/robot_description_semantic), in order to initialize the Scene object; the ** Map_base::at error arise. I guess it was correlated to the lenght of the SRDF since to overcome this i created a minimal srdf with only few <disable_collisions> entries, finally leading me to init that object.
I'm having trouble replicating the exact srdf-too-long error, but im getting the Map_base::at error shown below for the long srdf but not the minimal version.

In my case the SRDF is quite dynamic, and changes everytime the robot configuration changes, so refactor to a minimal srdf everytime is not ideal (i cannot have a single srdf for every configuration). Ideally i would subscribe to that topic and feed the Scene with the msg data.

Could roboplan generate these automatically based on the kinematic chain (adjacent links)? Or is there a way, unknown to me, that let me config bigger srdf files to init the Scene?

To reproduce the error i did a simple script in a sourced roboplan environment (reproduce_error.py). This loads both the minimal and the whole SRDF and try to init the Scene firstly with the whole file, if it fails it retries with the minimal. I attach the files used by the script (the .srdf is not supported, i attach the file with the extension changed to .txt), and the script itself.

srdf_minimal.txt
urdf.txt
whole_srdf.txt
reproduce_error.py

The output log i retrieve is something like:
`~/exchange/roboplan_ws$ ros2 run roboplan_grasp_demo reproduce_error
URDF loaded, length: 102375 characters
SRDF loaded, length: 225821 characters
Minimal SRDF loaded, length: 8398 characters

Creating RoboPlan Scene using LONG SRDF...
Failed to create scene: _Map_base::at

Retrying with minimal SRDF...
Scene created successfully with minimal SRDF!`

Thanks a lot in advance!

[sea-bass]

Thank you for all this information! I will take a look soon as I get a chance.

My immediate guess is this hasn't anything to do with the length of the file, but rather with RoboPlan improperly handling collision pairs that may not exist in the model.

Anyways, I will debug this with your reproduction case.

[Mattolo4]

in my case the urdf model and the srdf are related. The urdf model gets built based on the robot configs (which kind of sensors,arms, End-effectors etc..) and then srdf published based on the same configs and the urdf model, so in first place I excluded that the srdf may contain something not included in the model.

Ofc i can be wrong, just sharing my toughts.
Thanks again!

[sea-bass]

Hi @Mattolo4 -- I removed all references to meshes and was able to debug to find the issue.

It turns out your SRDF first defines a group named arm_left_torso, which itself refers to a group named torso... however torso appears further down in the SRDF so it hasn't been parsed yet. I added an error message in this PR (#157) which embarrassingly fixes a TODO I had left for myself :)

Failed to create scene: 
Group 'arm_left_torso' specifies a subgroup 'torso' which has not yet been parsed in the SRDF.

Indeed, it works with the minimal SRDF simply because arm_left_torso and arm_right_torso groups are not in that file!

---

Are you able to change your generated SRDF to first define the torso group before other groups that use it as a subgroup? This would be the easiest fix. I could resolve it by moving this up top:

    <group name="torso">
        <joint name="torso_lift_joint" />
    </group>

Otherwise, we may need to fix our SRDF parser to look more recursively at the file, assuming that you could be referring to a subgroup that is defined further down in the file.

[Mattolo4]

Hi @sea-bass, thanks a lot for the suggestions/debugging, i did as you said and now it's initializing the Scene without me requiring to do manual changes.
Question: In the case of wanting the robot to reach a target cartesian pose, what would it be the approach that exploits the most roboplan?

As far as i understood from the package, SimpleIK will solve for one-shot inverse kinematics, then having these joint values we can run RRT to get a path (to be smoothed/optimized with toppra). I saw also OINK which i guess is more optimized for real-time tracking since it solves for joint velocities and not for joint displacements, so given a single cartesian goal for the End-effector OINK would not be ideal.

I see that using simpleIK --> rrt --> smooth --> toppra i get some satisfying results, but if i spawn some collision objects in the scene it s more rare that simpleIK finds joint space solutions that satisfies the obstacles constrains, thus not able to plan a collision-free trajectory. What do you suggest in this case?

Thanks a lot anyway!

[sea-bass]

I think you're exactly right in your comments above!

As you said, SimpleIk is only collision-aware, not collision-avoiding. The "right" answer would to be have an IK solver that does solve around collisions, which neither of the existing solvers do currently, so that would be a completely new feature. With what is currently there, you could try to increase the number of random restarts (and maybe the max time too) in the SimpleIk solvers option to increase the likelihood of finding a collision-free solution.

(EDIT: The issue was closed by merging #157, but happy to keep discussing here or in a new issue)

[Mattolo4]

Thanks for the overview. I'd like to ask few things regarding this matter.
The ideal case scenario in our manipulation stack would be:

1. cartesian target pose for the End effector passed to an IK solver;
2. the IK outputs a valid collision-free IK solution (in joint space) that ideally has all the joint values are far as possible from joint limits;
3. the IK solution is fed to the RRT + smooth + toppra to generate a valid nice trajectory.

Now, the problem relies in the IK solver, the questions are:

• what could be main challenges in developing such feature?
• is this by chance on roboplan roadmap?
• how much time would it take?
• what could be the process of working in this togheter?

[sea-bass]

This is very much in the roadmap!

The main challenge here would easily be developing tasks for either SimpleIK or OInK solver to avoid collisions. Pinocchio has all the capabilities to check this, but the math is a little tricky with the contact Jacobian. Being far away from joint limits is much easier, and OInK already has tasks for joint centering and joint limit avoidance.

Here is a reference implementation of self-collision avoidance barrier in another IK solver: https://github.com/stephane-caron/pink/blob/main/pink/barriers/self_collision_barrier.py

I couldn't estimate how much time this would take, but I'd be willing to help as I find the time.

By the way! Another viable path in this effort if you prefer is to combine roboplan with one of these off the shelf IK solvers like Pink, PlaCo, etc. that already handle collision avoidance. Then you just use roboplan for the path planning and trajectory generation. This is the beauty of the Python bindings!