"""Inverse Kinematics using MuJoCo's Jacobian — modern example.""" import numpy as np import mujoco import mujoco.viewer XML = """ """ def ik_step(model, data, target_pos, site_name="end_effector", step_size=0.5, damping=1e-4): """One step of damped least-squares IK.""" site_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_SITE, site_name) current_pos = data.site_xpos[site_id] # Compute error error = target_pos - current_pos # Compute Jacobian jacp = np.zeros((3, model.nv)) mujoco.mj_jacSite(model, data, jacp, None, site_id) # Damped least-squares: dq = J^T (J J^T + λI)^-1 * error jjt = jacp @ jacp.T + damping * np.eye(3) dq = jacp.T @ np.linalg.solve(jjt, error) return step_size * dq def main(): model = mujoco.MjModel.from_xml_string(XML) data = mujoco.MjData(model) # Get target position target_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_SITE, "target") with mujoco.viewer.launch_passive(model, data) as viewer: while viewer.is_running() and data.time < 10.0: mujoco.mj_forward(model, data) target_pos = data.site_xpos[target_id] # IK control dq = ik_step(model, data, target_pos) data.ctrl[:] = 50.0 * dq - 10.0 * data.qvel[:] mujoco.mj_step(model, data) viewer.sync() if __name__ == "__main__": main()