ROS Published Joint States Incorrect when Using Non-Fixed Base

Omniverse Isaac Sim
,
1

While working on importing the Fetch robot into Isaac Sim, I noticed an issue with the omni.isaac.ros_bridge.ROS1PublishJointState node, specifically when working with a robot with a non-fixed base.

When importing the URDF with the base is fixed, the joint states are consistent with what is expected based on the URDF and the readings of the joint positions in Isaac Sim. However, when the base is not fixed, which is necessary as this is a mobile manipulation platform, the behavior is incorrect. The joint readings in the joint prims within the Isaac Sim UI are the same as before, which are correct, but the published joint states for some of these joints (but not all) are inverted from what they should be. This means that the readings for some of the joints are being inverted when publishing to the joint states ROS topic.

I have been able to replicate this on the Panda arm as well in which the first four joints are inverted only when the base is not fixed. I replicated this by importing the Panda URDF and adding a simple ActionGraph that published the joint states using the omni.isaac.ros_bridge.ROS1PublishJointState node. The results can be seen below for the first joint of the Panda arm:

Non-fixed base:
panda_joint1 UI (1st joint)
image

Echoing ROS joint state topic (notice that panda_joint1 is negative unlike above)

header: 
  seq: 4765
  stamp: 
    secs: 1538
    nsecs: 169965619
  frame_id: ''
name: [panda_joint4, panda_joint5, panda_joint3, panda_joint6, panda_joint2, panda_joint7,
  panda_joint1]
position: [0.5236, 0.5235, -0.5236, 0.0, -0.5236, 0.0, -0.5236]
velocity: [0.0008, 0.0019, -0.0003, 0.0, -0.0004, 0.0, 0.0005]
effort: [-0.4321, 0.0787, -0.1563, 0.2315, 0.4689, -0.0735, -0.0421]

Fixed base:
image

Echoing ROS joint state topic

header: 
  seq: 55
  stamp: 
    secs: 2382
    nsecs: 949946737
  frame_id: ''
name: [panda_joint1, panda_joint2, panda_joint3, panda_joint4, panda_joint5, panda_joint6,
  panda_joint7]
position: [0.5236, 0.0, 0.0, -0.0698, 0.0, 0.0, 0.0]
velocity: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
effort: [0.0, -4.7777, 0.0, -1.8668, 0.0, 1.137, 0.0]
2

After further inspecting this behavior, it seems to be caused by or at least related to the way that URDF files are imported in a flat style rather than the tree-like style that the URDF file specifies. This means that the link prims’ positions seem to be defined relative to the containing Xform prim which does not move, even if the base link is not fixed. This seems to cause the behavior of some of the joints to be incorrect. I have not been able to find a full solution to this issue as I have been unable to implement anything without running into errors. Two potential solutions that I thought of were to import the URDF file in the same tree-like structure as in the file so that the link prim’s are nested within their parent link, however this would require a significant amount of work to convert each imported URDF. The other potential approach would be to have the robot’s root Xform be allowed to move with the base link, however this seems to have issues with the physics simulation and the articulations below. Any insight or advice into this issue would be greatly appreciated.

3

Hi, could you please enable the Robotics Logging channel and check if the logs report a warning in one of the two cases a message similar to:
Physics USD: Joint %s body rel does not follow articulation hierarchy?

where %s is the name of the joint.

To enable the Robotics Logging Channel click Window → Simulation → Debug and then scroll down in the Physics Debug window that will happear until you find Loggin Channel and enable the checkbox on Robotics

IsaacSimRoboticsChannel
IsaacSimRoboticsChannel2161×1396 367 KB

4

Hi @scristiano,

I was able to see that this message is printed out for three of the joints on the robot.

2023-04-18 17:22:51  [Warning] [omni.physx.logging.robotics] Physics USD: Joint /fetch/shoulder_pan_link/shoulder_lift_joint body rel does not follow articulation hierarchy; consider swapping body0/body1 rels to match.
2023-04-18 17:22:51  [Warning] [omni.physx.logging.robotics] Physics USD: Joint /fetch/torso_lift_link/shoulder_pan_joint body rel does not follow articulation hierarchy; consider swapping body0/body1 rels to match.
2023-04-18 17:22:51  [Warning] [omni.physx.logging.robotics] Physics USD: Joint /fetch/base_link/torso_lift_joint body rel does not follow articulation hierarchy; consider swapping body0/body1 rels to match.

These joints seem to be defined in a similar fashion to the others on the robot which are not having this issue.

5

I was able to get these to go away by swapping the link order for each of these joints and the position/rotations for the links, however this does not seem like it should be necessary for a non-fixed base robot. Setting the initial position of the joints still seems to create undesired behavior where the entire robot’s body’s position will be set rather than the specific link.

6

Hi @comeauc
we are internally investigating the issue with how the ROS node grabs and publishes joint state, as it should match what you see in the UI. I will keep you posted if that will get fixed into some future release.

In order to bypass the issue until we’ll be able to provide a fix, it makes sense to try swapping the body0/1 of the joints you’re interested in such that you get coherent sign.
After swapping the body order, does the sign of joint0 in the ROS joint state topic became coherent with what you see in the User Interface Property panel?

Your last question is unclear to me, and I guess it’s unrelated to the initial question of this topic.
Would it be possible to share a video or describe more in detail what you’re trying to achieve?

I suggest also when working with articulations (especially if fixed)l to use the Physics Inspector as it allows to manipulate the articulation during authoring and persist its joint state values in the scene.

https://docs.omniverse.nvidia.com/prod_extensions/prod_extensions/ext_physics/support-ui.html#physics-inspector

7

If this issue is unrelated to the initial topic, we could move this discussion to a separate thread. Here is the code I am using to set the initial pose of the joints of the Fetch robot:

joints = ["shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint",
            "elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint"]
poses = [1.32, 1.40, -0.2, 1.72, 0.0, 1.66, 0.0]
initial_joint_positions = {joint: degrees(pos) for joint, pos in zip(joints,poses)}

...

for joint in self.robot_urdf.joints:
    if joint.limit is None:
        continue
    if joint.type == 'revolute' or joint.type == 'continuous':
        joint_type = 'angular'
    else:
        joint_type = 'linear'

    # Set initial joint positions
    # TODO potentially replace with articulation api: self.set_joint_positions
    if initial_joint_positions is not None and joint.name in initial_joint_positions:
        joint_position = initial_joint_positions[joint.name]
        omni.kit.commands.execute('ChangeProperty',
                    prop_path=f'{self._prim_path}/{joint.parent}/{joint.name}.state:{joint_type}:physics:position',
                    value=joint_position,
                    prev=0)
        omni.kit.commands.execute('ChangeProperty',
                    prop_path=f'{self._prim_path}/{joint.parent}/{joint.name}.drive:{joint_type}:physics:targetPosition',
                    value=joint_position,
                    prev=0)

This is effective at setting the joint poses to the desired values, however, this seems to cause the robot to behave violently at the start of the simulation as it seems as though instead of setting the new position of the child link relative to the parent link, it is doing something different which causes the entire chassis’ pose to be affected. This can be seen in the gif below where the robot is embedded into the ground when the simulation is started, with the arm links being in correct positions relative to each other. The second gif shows the transformation that would be required to the robot’s root Xform in order to align the robot post-initialization with the ground. Do you know why this behavior may be happening?

fetch_initial

fitch_reposed

Here is an image with the Physics Inspector visible. This seems to agree with what happens in the first gif as it seems to be misaligned from the visible model, where it should be.

image
image794×419 71.3 KB

8

Hi @comeauc I suggest to create a separate topic describing this issue.