Articulation Joint Sensors do not read correct values

Omniverse Isaac Sim
, ,
1

Implementation of end effector force sensor

To implement an end effector force sensor i followed this documentation and installed a cylinder at the tool0 of the robot with a fixed joint.

To test the sensor the robot is set up in a up right position and placed a cube with the mass of 1kg on top.

image
image374Ă—615 55.5 KB

The detected force should be g * 1 = 8.91

But i read:
joint forces: x=-0.001930191763676703, y=0.005002821795642376, z=1.4727673530578613

and this for all joints:

[[ 0.0000000e+00 0.0000000e+00 0.0000000e+00 0.0000000e+00
0.0000000e+00 0.0000000e+00]
[ 0.0000000e+00 0.0000000e+00 9.8100002e-04 0.0000000e+00
0.0000000e+00 0.0000000e+00]
[ 2.4458431e-03 7.5626111e-04 2.0742494e+02 -1.5149940e+01
1.5930146e+00 2.5189444e-04]
[ 1.6818484e+02 7.4255792e-04 -2.3956299e-03 2.5367737e-04
1.5921823e+00 1.5149865e+01]
[-6.9589913e-04 1.4429598e-01 1.3188777e+02 -1.5921844e+00
-1.5149855e+01 1.6547110e-02]
[-7.3272735e-04 8.5403465e-02 4.9552433e+01 -1.5496687e+00
-3.7872777e+00 6.5937703e-03]
[-7.4718520e-04 5.0591342e-02 2.7234718e+01 -1.5234911e+00
-3.7417954e-04 -6.8649999e-05]
[-2.9437888e-02 1.5276368e+01 1.2859625e-02 3.7435238e-04
6.4752367e-06 4.3600606e-04]
[ 3.3180218e+00 8.2689524e-03 3.3936501e-03 6.4761989e-06
-3.5903722e-05 -3.8815313e-04]
[ 1.4747292e+00 -1.9317770e-03 5.0063627e-03 6.5159602e-06
3.1345614e-04 -6.9297297e-05]
[-1.9309691e-03 5.0045582e-03 1.4737483e+00 3.1345626e-04
-6.9297057e-05 6.5160757e-06]
[-1.9301918e-03 5.0028218e-03 1.4727674e+00 3.1345640e-04
-6.9296882e-05 6.5162008e-06]]

Is the articulation joint sensor not working?
Is something wrong with my implementation down below?

Thank you for any help!

Code to Reproduce

import asyncio
import numpy as np

from omni.isaac.core import World
from omni.isaac.core.robots import Robot
from omni.isaac.robot_assembler import RobotAssembler
from omni.isaac.core.articulations import Articulation
from omni.isaac.core.objects import DynamicCuboid, DynamicCylinder
from omni.isaac.core import SimulationContext
from omni.isaac.core.utils.stage import add_reference_to_stage
from omni.isaac.core.utils.nucleus import get_assets_root_path
import omni.kit.commands
import omni.usd
import carb



async def setup_world():

    if World.instance():
        World.instance().clear_instance()
    world = World()
    world.scene.add_default_ground_plane()
    simulation_context = SimulationContext()
    await simulation_context.initialize_simulation_context_async()

async def transform_prim_pos(prim_path, position, rotation):
    stage = omni.usd.get_context().get_stage()
    if stage.GetPrimAtPath(prim_path).IsValid():
        prim = stage.GetPrimAtPath(prim_path)
        prim.GetAttribute("xformOp:translate").Set(position, 0)
        prim.GetAttribute("xformOp:orient").Set(rotation, 0)

def spawn_cylinder(x, y, z):
    cylinder = DynamicCylinder(prim_path="/World/ur5e/dummy_sensor",
                           name="dummy_sensor",
                           position=np.array([0, 0, 0]),
                           color=np.array([0, 0, 0]),
                           height=0.01,
                           radius=0.04 ,
                           mass=0.0001)
    return cylinder
    
        
async def setup_robot():
    world = World()
    assets_root_path = get_assets_root_path()
    if assets_root_path is None:
        carb.log_error("Could not find nucleus server with /Isaac folder")
    asset_path = assets_root_path + "/Isaac/Robots/UniversalRobots/ur5e/ur5e.usd" 
    add_reference_to_stage(usd_path=asset_path, prim_path="/World/ur5e")
    robot = world.scene.add(Robot(prim_path="/World/ur5e", name="ur5e"))
    await asyncio.sleep(1)
    spawn_cylinder(0,0,0)
    RobotAssembler().create_fixed_joint("/World/ur5e/tool0", "/World/ur5e/tool0", "/World/ur5e/dummy_sensor")
    base_robot_path = "/World/ur5e"
    robot = Articulation(base_robot_path)
    return robot

def spawn_dynamic_cuboid(position, mass):
    cuboid = DynamicCuboid(prim_path="/World/Xform/Cube",
                           name="test_forces_cube",
                           position=position,
                           color=np.array([1.0, 0.0, 0.0]),
                           size=0.07,
                           mass=mass)
    return cuboid



async def joint_force(robot: Articulation):
    world = World()
    await world.play_async()
    world.scene.add(robot)
    robot.initialize()
    joint_state = np.array([0, -1.57, 0, 0, 1.57, 0])
    robot.set_joint_positions(joint_state)

    cube_mass = 1
    spawn_dynamic_cuboid(position=np.array([0.1001, 0.13338, 1.085]), mass=cube_mass)
    await asyncio.sleep(2)

    #joint_names = robot._articulation_view._metadata.joint_names
    joint_indices = robot._articulation_view._metadata.joint_indices
    forces = robot.get_measured_joint_forces()

    flange_joint_forces = forces[joint_indices["AssemblerFixedJoint"] + 1]
    print(forces)
    print(f"Mass of cube: {cube_mass}kg")
    print(f"Dummy sensor joint forces: x={flange_joint_forces[0]}, y={flange_joint_forces[1]}, z={flange_joint_forces[2]}")
    
async def main():
    await setup_world()
    robot = await setup_robot()
    await joint_force(robot)


asyncio.ensure_future(main())

Screenshots or Videos

Additional Information

What I’ve Tried

I also tried to remove the tool0 and directly mount a cylinder to the lange and read the joint forces form a fixed joint between the flange and the cylinder but the results were the same.

Related Issues

The forces read with the joint force sensor are unreasonably high - Omniverse / Isaac Sim - NVIDIA Developer Forums

How to add a FT sensor using for peg in hole tasks? - Omniverse / Isaac Sim - NVIDIA Developer Forums

Isaac Sim Version

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

Operating System

Ubuntu 22.04
Ubuntu 20.04
Windows 11
Windows 10
Provided Container

GPU Information

  • Model: GeForce RTX 3080 Ti
  • Driver Version: 535.104.12
2

Hi, I’m currently able to read the correct six dimensional forces, but I’m not very good at modifying the Script Editor’s code since I’m using a standalone code, but I’m generally able to see that there are two places where your setup isn’t quite the same as mine, so maybe you can modify it to try it out.

  1. When adding fixed joints, you may need to create a PhysicsFixedJoint between two Xforms. Yours is directly connected to the rigid body, you can create an Xform and then put the clinder under the Xform hierarchy, then create the PhysicsFixedJoint.
    image

  2. The mode of the PhysicsScene at runtime is different.I have encountered some problems before, I found that my application can be executed correctly in “Enable GPU Dynamics” mode, you can try to change it.

    image
    image540Ă—330 56.4 KB

I hope this will help you.

Best wishes.

2 Likes
3

Thank you very much!

The first point definitely helped me :)

4

You’re welcome and good luck with everything!

5

Here is the solution that worked for me to implement an end effector force torque sensor:

import asyncio
import numpy as np
import torch
import carb

from omni.isaac.core import World
from omni.isaac.core.scenes.scene import Scene
from omni.isaac.core.objects import GroundPlane
from omni.isaac.core.articulations import Articulation
from omni.isaac.core.objects import DynamicCuboid, FixedCylinder
from omni.isaac.core.utils.stage import add_reference_to_stage, get_current_stage
from omni.isaac.core.utils.nucleus import get_assets_root_path
from omni.isaac.core.utils.rotations import euler_angles_to_quat
import omni.kit.commands
import omni.usd
from pxr import Sdf, UsdPhysics



async def run():
    stage = omni.usd.get_context().get_stage()
    if World.instance():
        World.instance().clear_instance()
    world = World(stage_units_in_meters=1.0, backend="torch", device="cpu")
    scene:Scene = world.scene
    await world.initialize_simulation_context_async()
    await omni.kit.app.get_app().next_update_async()

    # Add GroundPlane
    ground_plane = GroundPlane("/World/GroundPlane")
    scene.add(ground_plane)
    # Add Physics Scene
    physics_scene = UsdPhysics.Scene.Define(stage, Sdf.Path("/World/PhysicsScene"))

    # Add Robot
    assets_root_path = get_assets_root_path()
    if assets_root_path is None:
        carb.log_error("Could not find nucleus server with /Isaac folder")
    asset_path = assets_root_path + "/Isaac/Robots/UniversalRobots/ur5e/ur5e.usd" 
    add_reference_to_stage(usd_path=asset_path, prim_path="/UR5e")
    await omni.kit.app.get_app().next_update_async()

    # Add Cylinder to end effector
    cylinder = FixedCylinder(prim_path="/UR5e/tool0/dummy_sensor",
                           name="dummy_sensor",
                           translation=np.array([0, 0, 0]),
                           orientation=euler_angles_to_quat(np.array([0,0,0]), degrees=True),
                           color=np.array([0, 0, 0]),
                           height=0.001,
                           radius=0.04)
    await omni.kit.app.get_app().next_update_async()

    robot = Articulation("/UR5e")
    scene.add(robot)

    await world.reset_async(soft=False)
    stage = get_current_stage()

    await omni.kit.app.get_app().next_update_async()
    await world.play_async()

    joint_state = np.array([0, -1.57, 0, 0, 1.57, 0], dtype=np.float32)
    joint_state = torch.tensor(joint_state, dtype=torch.float32)
    robot.set_joint_positions(positions=joint_state)

    cube_mass = 1
    cube = spawn_dynamic_cuboid(position=np.array([0.1001, 0.13338, 1.085]), mass=cube_mass)
    world.scene.add(cube)
    await omni.kit.app.get_app().next_update_async()

    joint_indices = robot._articulation_view._metadata.joint_indices
    forces = robot.get_measured_joint_forces()

    flange_joint_forces = forces[joint_indices["flange_tool0"] + 1]
    print(forces)
    print(f"Mass of cube: {cube_mass}kg")
    print(f"Dummy sensor joint forces: x={flange_joint_forces[0]}, y={flange_joint_forces[1]}, z={flange_joint_forces[2]}")
    


def spawn_dynamic_cuboid(position, mass):
    cuboid = DynamicCuboid(prim_path="/World/Cube",
                           name="test_forces_cube",
                           position=position,
                           color=np.array([1.0, 0.0, 0.0]),
                           size=0.07,
                           mass=mass)
    return cuboid


asyncio.ensure_future(run())
6

This topic was automatically closed 14 days after the last reply. New replies are no longer allowed.