Spawn robot in sim in stable sit position like in real life

[ayin21]

Also use same default kp/kd as on the real robot.

Checklist

• Run pixi run build
• Write documentation
• Test on your machine
• Test on the robot
• Create issues for future work
• Triage this PR and label it

[texhnolyze]

I'm very confused what is going on here, without experience with the mujoco API.
It would be very nice to have more comments or even extract parts of this into named methods for better understandability.

Why can the same values for a joint in home_qpos be applied to both:

                self.data.qpos[joint.joint_instance.qposadr[0]] = value
                # For position actuators ctrl is the target position — same as the initial qpos
                self.data.ctrl[joint.actuator_instance.id] = value

Is this numpy and joint.actuator_instance.id is actually a list or something?

Also, would it not make sense to directly access floating_base_joint (our currently only free joint`), to make it clearer that's what we want so its easier to understand that this joint is only used to set the "spawn height", if that's even correct?

[Flova]

The mujoco API is like a c dev who was forced to write python, full of pointer like index lists etc..

The ctrl sets the setpoint for the actuator to hold the position and the qpos directly sets the simulator state to that position initially.

Is this numpy and joint.actuator_instance.id is actually a list or something?

What do you mean? we iterate through all joints and access the "numpy array" which is actually the exposed state of the mujoco C api. joint.actuator_instance.id is a scalar.

[Flova]

What do you mean with the last one?

[texhnolyze]

I talked about this with @MegaIng. With the API I meant, that we have only a single free joint named floating_base_joint, which purpose as far as I understand it only is to be able to position the robot with it e.g. with a keyframe.

So I thought could we not just simplify the following code:

# Build a map from bare joint name → keyframe qpos value using the standalone robot model
home_qpos: dict[str, float] = {}
home_z: float | None = None
for i in range(robot_model.njnt):
    jnt = robot_model.joint(i)
    if jnt.type == mujoco.mjtJoint.mjJNT_FREE:
        # qpos layout for freejoint: [x, y, z, qw, qx, qy, qz] — grab z (offset 2)
        home_z = float(robot_model.key_qpos[key_id, jnt.qposadr[0] + 2])
        continue

    home_qpos[jnt.name] = robot_model.key_qpos[key_id, jnt.qposadr[0]]

for robot_sim in self.robots:
    if home_z is not None:
        freejoint_name = f"robot_floating_base_joint_{robot_sim.robot.index}"
        freejoint_id = mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_JOINT, freejoint_name)
        if freejoint_id >= 0:
            self.data.qpos[self.model.joint(freejoint_id).qposadr[0] + 2] = home_z

To something like:

home_qpos: dict[str, float] = {}

for i in range(robot_model.njnt):
    jnt = robot_model.joint(i)
    home_qpos[jnt.name] = robot_model.key_qpos[key_id, jnt.qposadr[0]]

apply_z_spawn_height_from_keyframe(robot_model)

def apply_z_spawn_height_from_keyframe(robot_model):
  keyframe_floating_base_joint_id = ... Some way to get this
  keyframe_floating_base_joint = robot_model.joint(keyframe_floating_base_joint_id)

  model_floating_base_joint_name = f"robot_floating_base_joint_{robot_sim.robot.index}"
  model_floating_base_joint_id = mujoco.mj_name2id(self.model, mujoco.mjtObj.mjOBJ_JOINT, floating_base_joint_name)
  model_floating_base_joint = self.model.joint(floating_base_joint_id)

  # qpos layout for freejoint: [x, y, z, qw, qx, qy, qz] — grab z (offset 2)
  robot_spawn_heigth = float(robot_model.key_qpos[key_id, keyframe_floating_base_joint.qposadr[0] + 2])
  # Set z spawn height from keyframe while preserving x/y placement and orientation
  self.data.qpos[.qposadr[0] + 2] = robot_spawn_heigth

To make it clearer that is what is actually going on.

[jaagut]

@texhnolyze ping