"""Impedance control — compliant end-effector that yields to external forces.""" import numpy as np import mujoco import mujoco.viewer XML = """ """ def main(): model = mujoco.MjModel.from_xml_string(XML) data = mujoco.MjData(model) ee_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_SITE, "ee") nv = model.nv # Desired equilibrium position (into the obstacle) x_des = np.array([0.55, 0.0, 0.5]) # Impedance parameters (spring-damper behavior) # Low stiffness = compliant; high stiffness = stiff K_imp = np.diag([100.0, 100.0, 100.0]) # stiffness (N/m) D_imp = np.diag([20.0, 20.0, 20.0]) # damping (Ns/m) jacp = np.zeros((3, nv)) jacr = np.zeros((3, nv)) print("Impedance control demo:") print(f" Stiffness: {np.diag(K_imp)} N/m") print(f" Damping: {np.diag(D_imp)} Ns/m") print(" The arm pushes toward the obstacle but yields on contact.") with mujoco.viewer.launch_passive(model, data) as viewer: while viewer.is_running() and data.time < 20.0: mujoco.mj_forward(model, data) # Current EE state x = data.site_xpos[ee_id].copy() mujoco.mj_jacSite(model, data, jacp, jacr, ee_id) dx = jacp @ data.qvel # Impedance law: F = K*(x_des - x) - D*dx # This makes the EE behave like a mass-spring-damper F = K_imp @ (x_des - x) - D_imp @ dx # Gravity compensation + impedance torque tau = jacp.T @ F + data.qfrc_bias data.ctrl[:] = np.clip(tau, -100, 100) mujoco.mj_step(model, data) viewer.sync() if __name__ == "__main__": main()