How to attach a peg to the end effector?

[Josias-C]

Hi,
I would like to attach a peg object with collision properties to the end effector of a UR10 robot for a peg-in-hole task.
Is there a way to do this in the SceneCfg? Since I want to try different sizes of the peg, I would like to create the peg in the code and not modify the USD.

I tried:

@configclass
class SceneCfg(InteractiveSceneCfg):
  ...
  peg = RigidObjectCfg(
          prim_path="{ENV_REGEX_NS}/Robot/ee_link/RoundPeg",
          spawn=sim_utils.CylinderCfg(
              radius=0.005,
              height=0.05,
              rigid_props=sim_utils.RigidBodyPropertiesCfg(
                  rigid_body_enabled=True  # tried False and True
              ),
              mass_props=sim_utils.MassPropertiesCfg(mass=0.044),
              collision_props=sim_utils.CollisionPropertiesCfg(
                  collision_enabled=True
              ),
          ),
          init_state=RigidObjectCfg.InitialStateCfg(pos=[0.0, 0, 0.0], rot=[1.0, 0.0, 0.0, 0.0]),
      )

But I always get the following error:

[Error] [omni.physicsschema.plugin] Rigid Body of (/World/envs/env_0/Robot/ee_link/RoundPeg) missing xformstack reset when child of rigid body (/World/envs/env_0/Robot/ee_link) in hierarchy. Simulation of multiple RigidBodyAPI's in a hierarchy will cause unpredicted results. Please fix the hierarchy or use XformStack reset.

[Dhoeller19]

Hi @Josias-C,
This is unfortunately not possible out of the box and is more involved. Here is how I would do it.
The ArticulationCfg has a spawn config that is usually a UsdFileCfg (for example https://github.com/isaac-sim/IsaacLab/blob/main/source/extensions/omni.isaac.lab_assets/omni/isaac/lab_assets/anymal.py#L60).
I would change the func attribute in this config (https://github.com/isaac-sim/IsaacLab/blob/main/source/extensions/omni.isaac.lab/omni/isaac/lab/sim/spawners/from_files/from_files_cfg.py#L90) to a custom method that reproduces the behavior of from_files.spawn_from_usd but also creates the rigid body of your choice and attaches it to the end effector with a fixed joint. Of course some plumbing on the config side has to be done so that you can easily change the rigid body attributes you want.

[Jackkert]

Hi @Dhoeller19 ,

Thank you for your answer, I have a similar question to the one asked above. In my case, I would like to have a number of drones in the env with some strings attached to them, each of these strings are then attached to a box that the drones have to carry collectively. It kind of looks like this:

(from the OmniDrones paper)

My own version of this setup is a bit different, and I implemented it as 1 big URDF. So I only spawn 1 Articulation which is this big drone setup. I would like to create a scene that spawns multiple robots, and then connects these with strings to a box, so I can use full functionality of Isaac lab (and use the DirectMARLEnv) to implement some (decentralized) MARL algorithms later. The approach you explained to @Josias-C would work to connect the strings, but I want all of my drones to interact with the same box, so spawning the box with the Articulation would not work for me.

Do you have any suggestions on this? Or are you working on an InteractiveScene feature where it would be able to connect multiple prims like this?

Thank you!

[Josias-C]

Thank you for your ideas.

I did it a little bit different and created the joint after scene creation in the startup event.

def create_ee_joint(
    env: ManagerBasedEnv,
    env_ids: torch.Tensor,
):
    import omni.usd
    stage = omni.usd.get_context().get_stage()

    for i in range(env.num_envs):
        joint = UsdPhysics.FixedJoint.Define(stage, f"/World/envs/env_{i}/PegJoint")
        joint.CreateBody0Rel().SetTargets([f"/World/envs/env_{i}/Robot/ee_link"])
        joint.CreateBody1Rel().SetTargets([f"/World/envs/env_{i}/Peg"])

        joint.GetLocalPos0Attr().Set(Gf.Vec3f(0.025, 0.0, 0.0))
        joint.GetLocalRot0Attr().Set(Gf.Quatf(0.5, 0.5, 0.5, 0.5))

@configclass
class EventCfg:
    add_ee_joint = EventTerm(
        func=mdp.create_ee_joint,
        mode="startup"
    )