Retrieve rigid body velocity in an Isaac Custom C++ extension

Isaac Sim Version

4.5.0
4.2.0
4.1.0
4.0.0
4.5.0
2023.1.1
2023.1.0-hotfix.1
Other (please specify):

Operating System

Ubuntu 22.04
Ubuntu 20.04
Windows 11
Windows 10
Other (please specify):

GPU Information

  • Model: NVIDIA GeForce RTX 3060
  • Driver Version: 535.230.02

Topic Description

Detailed Description

Hi, I want to implement a custom C++ extension to Isaac Sim to retrieve the velocity and accelerations of several rigid bodies that are simulated with Isaac Sim.
The simulation is started via a python script with a 0.005s physics dt and 0.05 render dt. With the python script I load several nodes to the omni graph which include my custom C++ node.
The custom node is connected to a OnPhysicsEvent so that is executed in every physics step. I was hoping to retrieve the rigid body properties that are in the scene. Following this code snippet, the result is that the velocity is the same in 10 consecutive steps, which means that the way I am acessing the velocity it seems to be updated only every render step:

  static bool compute(OgnRobotInterfaceNodeDatabase &db)
  {
    auto begin = std::chrono::high_resolution_clock::now();

    const std::vector<PXR_NS::UsdStageRefPtr> allStages = PXR_NS::UsdUtilsStageCache::Get().GetAllStages();
    if (allStages.size() != 1)
    {
      std::cerr << "There should be only one stage open" << std::endl;
      return false;
    }
    PXR_NS::UsdStageRefPtr activeStage = allStages[0];
    activeStage->Reload();

    static const PXR_NS::SdfPath kPhysicsScenePath("/World/Robot/base");
    pxr::UsdPrim prim = activeStage->GetPrimAtPath(kPhysicsScenePath);
    pxr::UsdPhysicsRigidBodyAPI rigidBodyAPI(prim);
    if (!rigidBodyAPI)
    {
      std::cerr << "The prim is not a rigid body" << std::endl;
      return false;
    }
    // Get the local velocity of the rigid body
    GfVec3f velocity;
    if (!rigidBodyAPI.GetVelocityAttr().Get(&velocity, UsdTimeCode::EarliestTime()))
    {
      std::cerr << "Failed to get the velocity of the rigid body" << std::endl;
      return false;
    }
    std::cout << "Velocity: " << velocity[0] << ", " << velocity[1] << ", " << velocity[2]
              << std::endl;

I get a result like this:

Velocity: 0.549024, 1.16357, -2.88835
Velocity: 0.549024, 1.16357, -2.88835
Velocity: 0.549024, 1.16357, -2.88835
Velocity: 0.549024, 1.16357, -2.88835
Velocity: 0.549024, 1.16357, -2.88835
Velocity: 0.549024, 1.16357, -2.88835
Velocity: 0.549024, 1.16357, -2.88835
Velocity: 0.549024, 1.16357, -2.88835
Velocity: 0.549024, 1.16357, -2.88835
Velocity: 0.549024, 1.16357, -2.88835
Velocity: -0.115129, 0.576302, -0.743352
Velocity: -0.115129, 0.576302, -0.743352
Velocity: -0.115129, 0.576302, -0.743352
Velocity: -0.115129, 0.576302, -0.743352
Velocity: -0.115129, 0.576302, -0.743352
Velocity: -0.115129, 0.576302, -0.743352
Velocity: -0.115129, 0.576302, -0.743352
Velocity: -0.115129, 0.576302, -0.743352
Velocity: -0.115129, 0.576302, -0.743352
Velocity: -0.115129, 0.576302, -0.743352

In the scene I am also loading an IMU sensor as well as a Joint State, I can see that the data coming from the IMU as well as the data coming from the joints is updated in every physics step.

Am I therefore just not following the correct pattern to obtain an updated rigid body properties in every Physics Step? Thank you very much. I am also asking because there is a lot of documentation with Python, but I am struggling to find documentation in C++.

Should I be using something similar to this?
omni::physx::tensors::GpuRigidBodyView::getVelocities
I don’t find documentation on how to use the physx C++ API to access the GPU Rigid body velocities.

Thank you very much in advance for your help.

Hello?

Hi @jeferrandiz98, thank you for posting your question and sorry for the delay. I am looking into this now.

Perfect, thank you very much.

Hi @michalin, were you able to get any info regarding this issue? Thank you.

Sorry for the delay. The problem that is happening here is that when you access information in the on_physics_step, that is running at the sub-stepping loop and the physics data is not surfaced to the USD Physics layer. The function you are calling is a USD Physics API call. So that makes it such that for a number of frames you will see the same value.

One work around is that you can use the get_velocities function from RigidPrim which will get the data directly from the physics rigid body view.

Hi @michalin, thank you very much for your help. You point to a Python API, which is the way I understand that could work and views a rigid body state in every physics state.
In this issue, I am also asking if it is possible to do the same using a C++ API and how (I can’t find documentation to do this). My C++ application will communicate with another library and it would be much cleaner to have a single C++ node to do this.

My question then is how can I get the physics state (position, velocity) in every physics step.
I also point to an API that I believe it is used to implement the IMU sensor:
omni::physx::tensors::GpuRigidBodyView::getVelocities

Hello @michalin?

@jeferrandiz98 So sorry for the slow reply. I think this is what you are looking for: PxRigidBody — PhysX SDK Documentation

Hello!

We noticed that this topic hasn’t received any recent responses, so we are closing it for now to help keep the forum organized.

If you’re still experiencing this issue or have additional questions, please feel free to create a new topic with updated details. When doing so, we recommend mentioning or linking to this original topic in your new post—this helps provide context and makes it easier for others to assist you.

Thank you for being part of the NVIDIA Isaac Sim community.

Best regards,
The NVIDIA Isaac Sim Forum Team