Reset robot position with correct localization in navsim application

[saralopez]

Hi,

I have already posted a few questions on the Isaac SDK forum category - due to the lack of activity I am trying my luck here hoping that someone from NVIDIA or with experience integrating Isaac SDK applications with Isaac Sim would help.

Following this tutorial I am running the navsim Isaac SDK application with the Carter navigation stack using Omniverse and the Robot Engine bridge, as specified in the mentioned tutorial.

I am trying to translate the robot in Isaac Sim in the following way:

        if self._ar == _dynamic_control.INVALID_HANDLE:
            self._ar = self._dci.get_articulation("/robot")
        if self._root_body == _dynamic_control.INVALID_HANDLE:
            self._root_body = self._dci.get_articulation_root_body(self._ar)

        self._dci.wake_up_articulation(self._ar)

        tf = _dynamic_control.Transform()
        tf.p = self.translate_position
        
        self._dci.set_rigid_body_pose(self._root_body, tf)

When I translate the robot in Isaac Sim, the localization in Isaac SDK is incorrect.

Is it possible to translate the robot and keep the localization working? Maybe a way to integrate this translation in the navsim app in Isaac SDK? Or how can I keep the robot localized in place when the translation happens in Isaac Sim?

[ltorabi]

Hi Sara lopez,

I am sorry to hear that you didn’t get any help.
I will make sure that someone either from SDK or Sim team will reply to you here asap.

Kindly,
Liila

[Hammad_M]

@saralopez
From the sim side you can use a REB teleport component to receive rigid body pose messages from Isaac SDK.
Two ideas I can think of to try:
It should be possible to set the new pose of the robot in the SDK pose tree manually once teleported which might help it stay localized.
You might also be able to force it to re-localize after sending the teleport message

[saralopez]

Thank you for your reply @Hammad_M .

I have added a REB teleport component to my robot in Isaac Sim:

However, although I am able to teleport the robot in isaac Sim, the Isaac SDK localization still does not work. These are the steps that I am following:

I have added a PoseMessageInjector node to navsim_navigate.subgraph.json and an edge:

{
  "source": "interface/output/pose",
  "target": "pose_injector/pose_injector/pose"
}

I have added this component thinking that every time the robot is teleported in Isaac Sim, the new pose (where the robot was teleported) would be injected in the pose tree.

Then, in navsim_navigate.app.json I have added a Teleportation component:

 {
        "name": "robot_teleportation",
        "components": [
          {
            "name": "message_ledger",
            "type": "isaac::alice::MessageLedger"
          },
          {
            "name": "random",
            "type": "isaac::alice::Random"
          },
          {
            "name": "Spline",
            "type": "isaac::map::Spline"
          },
          {
            "name": "Teleportation",
            "type": "isaac::ml::Teleportation"
          }
        ]
      }

a new edge:

{
        "source": "robot_teleportation/Teleportation/rigid_command",
        "target": "simulation.interface/input/teleport"
}

and the following configuration:

 "robot_teleportation": {
      "random": {
        "use_random_seed": true
      },
      "Teleportation": {
        "enable_on_relative_frame": true,
        "name": "robot",
        "tick_period": "30Hz"
      }
    }

However this does not work. The robot is correctly teleported in Isaac Sim (as it was initially) but the localization in Isaac SDK is still wrong (according to the Map View in Isaac Sight) and not updated when the robot is teleported.

Can it be that the robot localization works but Isaac Sight is not updated?

You might also be able to force it to re-localize after sending the teleport message

Could you elaborate on this? How can I force it to re-localize?

[saralopez]

Hi again,

After struggling with this problem for almost two weeks now - the way I fixed it was by:

First teleporting the robot in Isaac Sim using the dynamic control interface, unbinding the robot engine bridge and binding again.

This way, with a bit of delay the robot is correctly localized in Isaac SDK and Sight.

However, this is just a temporary solution that I came up with to fix the localization problem - Hopefully someone from NVIDIA could help me properly fix this problem.

I came across similar questions:

How to override the current position of the robot?

I am trying to find how to override the current position in SDK documentation but not found. Does anyone have any idea?

GridSearchLocalizer localization problem

Occasionally i’m seeing wrong localization pattern at the start as shown below with GridSearchLozalizer predicting wrong position. Localization at startup is blue, waypoint to go is green and arrow with “robot here” indicates actual position of the robot. Red dots readings from the lidar. [AAAimage]

How to re-run GridSearchLocalizer if needed? [ISAAC 2020.1]

Has this been fixed? I am also struggling with restarting the global localization using version 2020.2. I am teleporting the robot in Isaac Sim using dynamic control and it seems like the localization does not update as in the Isaac Sight Map View the robot is always in the same position and does not teleport when the robot teleports in Isaac Sim. I have tried to reseed the particle in the ParticleFilterLocalization but it still does not work. I have posted a similar question that might be re…

After reading these questions I do not believe that there is a problem with the way I am teleporting the robot but my guess is that this is a problem with the GridSearchLocalizer.

@Hammad_M hopefully these insights can help you better understand the problem and help me find a solution as this is critical for my project.

[ltorabi]

Hey Sara,

We will make sure you are helped.

Kindly,
Liila

[hemals]

If I understood correctly, when running through the NavSim Navigate app, you’ve been able to “teleport” the robot from its original initial world_T_robot pose, (Pose A which should be (25, 20, 0=on the ground, -180=facing north), to some new Pose B, but the localization in the SDK app still yields Pose A from the original starting pose? Setting a hint for setting the robot_init_global_localizer frame relative to the world might help resolve this on startup.

In //packages/flatscan_localization/apps/localization.subgraph.json, there is a section for “global_localization_from_config” which specifies the pose to initialize “world_T_robot_init_global_localizer”. You should try editing this to Pose B rather than Pose A and see if this at least unblocks you.

If you could post a screenshot of Sight localization and the map, that might help to diagnose.

[saralopez]

@hemals Thank you for answer.

You understood correctly. However, I am not trying to teleport it to (25, 20, 0=on the ground, -180=facing north), I am instead using the global localization with lidar sensors, not the file or the config localization. Thus, PoseA, initial world_T_robot pose is the initial position where the robot is spawned in isaac Sim as seen below:

Note that the goal does not correspond to the calculated global plan since the robot has been stationary for a while and the goal as been re-calculated. Besides that, at the start, the robot is correctly localized using the global_localization.

However, after the robot is teleported in Isaac Sim using the code uploaded in the first question of this thread. The localization is not updated, as seen below:

Besides, the global planner suddenly stops and the waypoints generated are [0,0,0]. When this happens the robot starts swinging out of control and the localization is still incorrect:

I do not need to modify the global_localization_from_config since I am using the lidar scans for localization.

[hemals]

So the robot thinks it is at Pose A and it is at Pose A at t=0. You then teleport to Pose B at t=1 without restarting the SDK app of course and global localization is not updated.

As @Hammad_M mentioned, we need to force the robot to relocalize. Global localization (GridSearchLocalizer) will solve for an initial estimate for global world_T_robot at t=0 and then stop, relying on scan matching localization in ParticleFilterLocalization to take over with the correct seed pose. If the robot is teleported after being activated, the robot will not automatically run an expensive global localization search to reassess where it is globally.

We need to set world_T_robot_init_global_localizer correctly after the teleport and then trigger ParticleFilterLocalization to reseed. This should help the robot reestablish its pose after teleportation.

We can either programmatically set the pose directly, inject the pose into the frame graph as you were, manually move the marker in Sight for robot_init_global_localizer which should be attached to the frame, or start the GridSearchLocalizer component to run again which could take a few seconds.

After world_T_robot_init_global_localizer has been correctly set at t=2, we then need to set the reseed_particles parameter to true on ParticleFilterLocalization at t=3 which will trigger the component to restart its search from the updated seed pose.

[saralopez]

@hemals Thank you for your answer. I have managed to correctly set the world_T_robot_init_global_localizer . However, how can I programmatically trigger ParticleFilterLocalization to reseed?

I saw config values can easily be changed in python, in my case I would need to do something like this:
app.nodes["navigation.localization.scan_localization"].components["ParticleFilterLocalization"].config["reseed_particles"] = True

But since my codelet is inC++, I was trying to use something like this: node()->app()->findComponentByName<ParticleFilterLocalization>(" navigation.localization.scan_localization/ParticleFilterLocalization") However, the ParticleFilterLocalization is not open-source and I cannot reference it. Is it possible to programmatically set the reseed_particles to True with C++ in an easy way? Or should I just switch to python?

[hemals]

My pleasure! You can use findComponentByName to get a pointer to the component and then invoke ->async_set_reseed_particles() to true which is a generated config hook to let you see the parameter. That should let you trigger reseeding programmatically.

[nithish_ks]

@hemals in order to run the statement node()->app()->findComponentByName<isaac::flatscan_localization::ParticleFilterLocalization>("navigation.localization.scan_localization/ParticleFilterLocalization");
how are the dependencies added in the BUILD and which header should be included. In this case ParticleFilterLocalization is a binary. Is there a way to include closed source libraries in a source file?

[saralopez]

Hi nks,

I solved that issue by doing the following:

auto *component = node()->app()->findComponentByName("navigation.localization.scan_localization/ParticleFilterLocalization");

node()->app()->findNodeByName("navigation.localization.scan_localization")->config().async_set(component, "reseed_particles", true);

This way, you are not required to add any additional dependencies - which would not be possible since as you mention, ParticleFilterLocalization is not open-source.