"""Operational space control — task-space dynamics compensation for a robot arm.""" 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, "end_effector") target_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_SITE, "target") nv = model.nv # Gains kp = np.diag([600.0, 600.0, 600.0]) kd = np.diag([50.0, 50.0, 50.0]) jacp = np.zeros((3, nv)) jacr = np.zeros((3, nv)) with mujoco.viewer.launch_passive(model, data) as viewer: while viewer.is_running() and data.time < 15.0: # Forward kinematics mujoco.mj_forward(model, data) # Positions x = data.site_xpos[ee_id].copy() x_des = data.site_xpos[target_id].copy() # Jacobian mujoco.mj_jacSite(model, data, jacp, jacr, ee_id) J = jacp # position Jacobian only # End-effector velocity dx = J @ data.qvel # Joint-space inertia matrix M = np.zeros((nv, nv)) mujoco.mj_fullM(model, M, data.qM) # Task-space inertia: Lambda = (J M^-1 J^T)^-1 M_inv = np.linalg.inv(M) Lambda = np.linalg.inv(J @ M_inv @ J.T + 1e-6 * np.eye(3)) # Coriolis + gravity compensation h = data.qfrc_bias.copy() # C(q,dq)*dq + g(q) # Task-space PD force F = kp @ (x_des - x) - kd @ dx # Operational space control law: # tau = J^T * Lambda * F + h tau = J.T @ (Lambda @ F) + h data.ctrl[:] = np.clip(tau, -100, 100) mujoco.mj_step(model, data) viewer.sync() if __name__ == "__main__": main()