[BUG] Multiple TSRs with same base and target breaks DART IK

ell0ry commented 4 years ago

When multiple TSRs are defined in a DART environment with the same base and target, the IK module from DART only solves for the most recently defined one.

Expected Behavior

It's expected that the IK module would report information about the TSR that it's being called with.

Current Behavior

I have an example with two TSRs defined from a table to a cube. The first TSR is considered the placement region for the cube and the second is considered the goal for the cube. Whenever I run anything that calls the IK module (i.e. getErrorWorld or solveWorld from TSR), it only reports the solution for the goal TSR for the cube.

However, if I remove the goal TSR, it provides the correct solution. Additionally, if I define these TSRs in inverse order (goal before the table TSR), I always get the solutions for the table TSR.

Steps to Reproduce

Define two TSRs with the same target and base.
Call TSR::solveWorld on the first defined TSR.

Context (Environment)

For Multi-Modal Planning, we rely on a foliation abstraction on top of TSRs. This means that there will likely be many TSRs defined in a given environment. Without this working, it's not possible to define goal regions as I'd like to atm.

ell0ry commented 4 years ago

@tonypan1995 pointed out the following comment from tsr.h that addresses this issue:

         * Note, the TSR creates an IK node in the underlying Dart skeleton for the provided world. If you
         * have multiple TSRs for the same target frame these will overwrite each other, unless you use a
         * clone of the World.

As I'm likely going to have many TSRs that share frame information, I'm going to try and address this in a branch. I'm thinking that modifying the TSR::solve() method so that it creates a copied world, gets the IK solution, and then applies it to the current world.

ell0ry commented 4 years ago

If we re-initialize the TSR before running solveWorld(), we ensure that the DART IK has the correct frame. I've fixed this problem locally by making this change. I'm unsure how it'll affect the stability of the rest of the robowflex_dart stack, however.

KavrakiLab / robowflex