Better way to get robot end effector pose through observation managers

[mikelasa]

Hello,

I'm trying to get some observations for my simulation and getting the robot end effector pose through the ObsTerm class. I managed to get the joint position and the robot base pose using the mdp module provided functions, mdp.joint_pos_rel and mdp.root_pos_w. This is what I have now in my code:

def robot_ee_pose(
    env: ManagerBasedEnv,
    robot_cfg: SceneEntityCfg = SceneEntityCfg("robot1"),
) -> torch.Tensor:
    """The pose of the robot."""
    robot: RigidObject = env.scene[robot_cfg.name]
    robot_ee_pose_act = robot.data.body_state_w[:, 8, 0:7]
    
    return robot_ee_pose_act

@configclass
class ObservationsCfg:

    @configclass
    class LowLevelActionsCfg(ObsGroup):
        # robot 1 joint position and velocity
        # mdp needs robot asset name to be robot1
        joint_pos_1 = ObsTerm(func=mdp.joint_pos_rel, params={"asset_cfg": SceneEntityCfg("robot1")})
        
        # robot root position and orientation
        robot1_root_pos = ObsTerm(func=mdp.root_pos_w, params={"asset_cfg": SceneEntityCfg("robot1")})
        robot2_root_pos = ObsTerm(func=mdp.root_pos_w, params={"asset_cfg": SceneEntityCfg("robot2")})
        robot1_root_ori = ObsTerm(func=mdp.root_quat_w, params={"asset_cfg": SceneEntityCfg("robot1")})
        robot2_root_ori = ObsTerm(func=mdp.root_quat_w, params={"asset_cfg": SceneEntityCfg("robot2")})
        
        # robot ee pose and orientation
        #robot1_ee_pos = ObsTerm(func=robot_ee_pose, params={"asset_cfg": SceneEntityCfg("robot1")})
        #robot2_ee_pos = ObsTerm(func=robot_ee_pose, params={"asset_cfg": SceneEntityCfg("robot2")})

        # robot ee pose and orientation
        robot1_ee_pos = ObsTerm(func=robot_ee_pose, params={"robot_cfg": SceneEntityCfg("robot1")})
        robot2_ee_pos = ObsTerm(func=robot_ee_pose, params={"robot_cfg": SceneEntityCfg("robot2")})

Using the interactive scene previous to the manager implementation I managed to get the pose like this:

def main():

extract robot entity

robot1_entity_cfg = SceneEntityCfg("robot1", joint_names=["panda_joint.*"], body_names=["panda_hand.*"])
robot2_entity_cfg = SceneEntityCfg("robot2", joint_names=["lbr_iiwa_joint.*"], body_names=["robotiq_coupler.*"])
# resolve the scene entities
robot1_entity_cfg.resolve(env.scene)
robot2_entity_cfg.resolve(env.scene)
# simulate physics
while simulation_app.is_running():
    with torch.inference_mode():
        # state of all bodies ([pos, quat, lin_vel, ang_vel])
        robot1_ee_pose_act = robot1.data.body_state_w[:, robot1_entity_cfg.body_ids[0], 0:7]
        robot2_ee_pose_act = robot2.data.body_state_w[:, robot2_entity_cfg.body_ids[0], 0:7]

My question is if the way I'm trying to get the robot pose is a good way to do so, or if it is a better way to do this kind of operations. Same happens with the jacobian.

Thanks in advance

[Mayankm96]

Still trying to figure out what the ask is above. That is currently the way to get the states from PhysX. You can also use the FrameTransformer class if you want to compute poses relative to each other: https://isaac-sim.github.io/IsaacLab/source/api/lab/omni.isaac.lab.sensors.html#omni.isaac.lab.sensors.FrameTransformer

[mikelasa]

Sory for the delay answering.

I asked that because I previously tried to get the transformations through FrameTransformer. The thing is that I need to calculate the transformations from an object in the scene to both of the end effectors. In the documentation says:

"Currently, this implementation only handles frames within an articulation. This is because the frame regex expressions are resolved based on their parent prim path. This can be extended to handle frames outside of articulation by using the frame prim path instead. However, this would require additional checks to ensure that the user-specified frames are valid which is not currently implemented."

Because of that "within an articulation" I didnt used this class, as I'm not sure if I could compute the pose of the end effectors relative to a given object in the scene. Sorry if it wasnt well explained, I'm quite new to the simulator and there are many things that I dont control yet.