import mujoco

xml_path = "./bi_diana_transfer_ee.xml"

robot_model = mujoco.MjModel.from_xml_path(filename=xml_path, assets=None)
robot_data = mujoco.MjData(robot_model)
mujoco.mj_forward(robot_model, robot_data)
for i in range(robot_model.njnt):
    joint_name = mujoco.mj_id2name(robot_model, mujoco.mjtObj.mjOBJ_JOINT, i)
    print(f"编号: {i}, 名称: {joint_name}")
# for i in range(robot_model.nbody):
#     body_name = mujoco.mj_id2name(robot_model, mujoco.mjtObj.mjOBJ_BODY, i)
#     print(f"编号: {i}, 名称: {body_name}")
print(robot_data.body("eef_left").xpos)
print(robot_data.body("eef_right").xpos)
print(robot_data.body("link6_right").xpos)