### Install MuJoCo Visualiser Dependencies

Source: https://github.com/jamiemair/mujoco.jl/blob/main/docs/src/intro/getting_started.md

Installs additional dependencies required for the MuJoCo.jl visualiser. This function should be called after the main package is installed.

```julia
using MuJoCo
install_visualiser()
```

--------------------------------

### Initialize and Run Visualiser with Controller

Source: https://github.com/jamiemair/mujoco.jl/blob/main/docs/src/intro/getting_started.md

Initializes the MuJoCo visualiser and runs a simulation, applying a specified controller function. The visualise! function allows interactive exploration of the model's behavior.

```julia
init_visualiser()
visualise!(model, data, controller=random_controller!)
```

--------------------------------

### Install MuJoCo.jl Package

Source: https://github.com/jamiemair/mujoco.jl/blob/main/README.md

Installs the MuJoCo.jl package and its underlying C library using Julia's package manager. This is a one-time setup step required before using the package for simulations.

```julia
import Pkg
Pkg.add("MuJoCo.jl")
```

--------------------------------

### Install MuJoCo.jl and Visualizer Dependencies

Source: https://context7.com/jamiemair/mujoco.jl/llms.txt

This snippet shows how to install the MuJoCo.jl package using Julia's Pkg manager and then install the necessary dependencies for the visualizer. This is typically a one-time setup step.

```julia
import Pkg
Pkg.add("MuJoCo")

# Install visualizer dependencies (only needed once)
using MuJoCo
install_visualiser()
```

--------------------------------

### Load Model and Data, Inspect Properties

Source: https://github.com/jamiemair/mujoco.jl/blob/main/docs/src/intro/getting_started.md

Loads a sample MuJoCo model and its associated data, then displays their types and specific properties like simulation timestep and joint positions. This demonstrates basic interaction with the Model and Data structs.

```julia
using MuJoCo

model, data = MuJoCo.sample_model_and_data()
@show typeof(model), typeof(data)

println("Simulation timestep: ", model.opt.timestep)
println("Positions of joints: ", data.qpos)
```

--------------------------------

### Simulate Model with Controller and Reset Data

Source: https://github.com/jamiemair/mujoco.jl/blob/main/docs/src/intro/getting_started.md

Simulates the MuJoCo model for 100 timesteps using a random controller and the step! function. After simulation, it resets the model's data to its initial state using mj_resetData.

```julia
for t in 1:100
    random_controller!(model, data)
    step!(model, data)
end

mj_resetData(model, data)
println("Reset joint positions: ", data.qpos)
```

--------------------------------

### Initialize and Run MuJoCo Visualizer

Source: https://github.com/jamiemair/mujoco.jl/blob/main/README.md

Sets up and runs the MuJoCo physics simulator with visualization. It includes installing the visualiser dependencies, initializing the session, loading a sample humanoid model, defining a simple random controller, and launching the interactive visualizer.

```julia
using MuJoCo
install_visualiser() # Run this to install dependencies only once
init_visualiser()    # Load required dependencies into session

# Load a model of a humanoid
model, data = MuJoCo.sample_model_and_data()

# Define a controller
function ctrl!(model, data)
    data.ctrl .= 2*rand(model.nu) .- 1
end

# Run the visualiser (press F1 for available options)
visualise!(model, data, controller=ctrl!)
```

--------------------------------

### Visualize Humanoid Keyframes

Source: https://github.com/jamiemair/mujoco.jl/blob/main/docs/src/examples/humanoid_lqr.md

Initializes the visualizer and iterates through the first three keyframes of the humanoid model. For each keyframe, it resets the model to that pose and then visualizes it. This helps in understanding the different starting configurations of the humanoid.

```julia
init_visualiser()

for i in 1:3
    resetkey!(model, data, i)
    visualise!(model, data)
end
```

--------------------------------

### Visualizing Multiple Trajectories (Julia)

Source: https://github.com/jamiemair/mujoco.jl/blob/main/docs/src/intro/getting_started.md

Visualizes the simulation trajectories of a MuJoCo model, allowing playback of passive dynamics and controlled motion. It takes a list of state arrays as input and enables interactive features within the visualiser for exploring the simulation.

```julia
visualise!(model, data, trajectories = [states, ctrl_states])
```

--------------------------------

### Check MuJoCo_jll Version

Source: https://github.com/jamiemair/mujoco.jl/blob/main/docs/src/index.md

This example shows how to check the installed version of the MuJoCo_jll package using Julia's Pkg API. This is useful for verifying compatibility between the MuJoCo library and the Julia wrapper.

```julia
import Pkg
Pkg.status("MuJoCo_jll"; mode=Pkg.PKGMODE_MANIFEST)
```

--------------------------------

### Load Humanoid Model and Initialize Data

Source: https://github.com/jamiemair/mujoco.jl/blob/main/docs/src/examples/humanoid_lqr.md

Loads the 'humanoid.xml' model and initializes the simulation data. This sets up the physical environment for subsequent simulations. It requires the MuJoCo package and assumes the model file is accessible.

```julia
using MuJoCo

model = load_model(joinpath(example_model_files_directory(), "humanoid", "humanoid.xml"))
data = init_data(model)
nothing # hide
```

--------------------------------

### Simulating and Recording Physics States (Julia)

Source: https://github.com/jamiemair/mujoco.jl/blob/main/docs/src/intro/getting_started.md

Simulates a MuJoCo model for a specified number of timesteps (tmax) and records the physics state at each step. It calculates the state vector dimension (nx) based on model properties and stores the states in a 2D array. This allows for later analysis or playback of the simulation.

```julia
tmax = 400
x = model.nq + model.nv + model.na # State vector dimension
states = zeros(nx, tmax)
for t in 1:tmax
    states[:,t] = get_physics_state(model, data)
    step!(model, data)
end
```

--------------------------------

### Recording States Under Random Controller (Julia)

Source: https://github.com/jamiemair/mujoco.jl/blob/main/docs/src/intro/getting_started.md

Resets the model and simulates for tmax timesteps, recording the physics state at each step while applying a random controller. This captures the model's behavior under a dynamic, non-fixed control input, useful for comparing against passive dynamics.

```julia
reset!(model, data)
tmax = 400
x = model.nq + model.nv + model.na # State vector dimension
ctrl_states = zeros(nx, tmax)
for t in 1:tmax
    ctrl_states[:,t] = get_physics_state(model, data)
    random_controller!(model, data)
    step!(model, data)
end
```

--------------------------------

### MuJoCo.jl Utility Functions

Source: https://context7.com/jamiemair/mujoco.jl/llms.txt

Highlights several utility functions provided by MuJoCo.jl, including displaying documentation for model/data fields, getting the simulation timestep, creating MuJoCo arrays, and accessing example model file paths.

```julia
using MuJoCo

model, data = MuJoCo.sample_model_and_data()

# Show documentation for a field
show_docs(model, :nv)
# Output: Model.nv: number of degrees of freedom = dim(qvel)

show_docs(data, :qvel)
# Output: Data.qvel: velocity (nv x 1)

# Get timestep
dt = timestep(model)
println("Timestep: ", dt)

# Create MuJoCo arrays (properly sized)
arr = mj_zeros(model.nq)  # Zero array of size nq
arr2 = mj_array([1.0, 2.0, 3.0])  # Convert to MuJoCo array

# Get example model directory
model_dir = example_model_files_directory()
println("Example models: ", example_model_files())
```

--------------------------------

### Resetting and Setting Initial Model State (Julia)

Source: https://github.com/jamiemair/mujoco.jl/blob/main/docs/src/intro/getting_started.md

Resets the MuJoCo model and data, then sets the initial height of the humanoid model to 2 meters above the ground before propagating the physics forward. This is useful for setting up specific initial conditions for a simulation.

```julia
reset!(model, data)
data.qpos[3] = 2
forward!(model, data) # Propagate the physics forward
```

--------------------------------

### Simulate Humanoid at Set-point

Source: https://github.com/jamiemair/mujoco.jl/blob/main/docs/src/examples/humanoid_lqr.md

Resets the humanoid model to a pre-defined set-point position and control input, then visualizes its initial behavior. This helps observe the stability of the equilibrium point.

```julia
reset!(model, data)
data.qpos .= qpos0
data.ctrl .= ctrl0
visualise!(model, data)
```

--------------------------------

### Initialize LQR Weight Matrices

Source: https://github.com/jamiemair/mujoco.jl/blob/main/docs/src/examples/humanoid_lqr.md

Initializes the LQR weight matrices R (control cost) and sets up the structure for the Q matrix (state cost) for the humanoid controller. It determines the number of actuators (nu) and degrees of freedom (nv) from the model.

```julia
using LinearAlgebra

nu = model.nu # number of actuators/controls
nv = model.nv # number of degrees of freedom
R = Matrix{Float64}(I, nu, nu)
# Q matrix will be constructed in parts
```

--------------------------------

### Record and Visualize Trajectories

Source: https://context7.com/jamiemair/mujoco.jl/llms.txt

This example demonstrates how to record both passive and controlled dynamics trajectories by capturing the physics state at each simulation step. It then visualizes these recorded trajectories, allowing for playback and comparison.

```julia
using MuJoCo

init_visualiser()
model, data = MuJoCo.sample_model_and_data()

# Record passive dynamics trajectory
tmax = 400
nx = model.nq + model.nv + model.na
passive_traj = zeros(nx, tmax)

reset!(model, data)
data.qpos[3] = 2.0  # Set initial height
for t in 1:tmax
    passive_traj[:, t] = get_physics_state(model, data)
    step!(model, data)
end

# Record controlled trajectory
controlled_traj = zeros(nx, tmax)
reset!(model, data)
for t in 1:tmax
    controlled_traj[:, t] = get_physics_state(model, data)
    data.ctrl .= 0.3*randn(model.nu)
    step!(model, data)
end

# Visualize both trajectories
visualise!(model, data, trajectories=[passive_traj, controlled_traj])

# Use UP/DOWN arrows to cycle between trajectories
# Use CTRL+R for reverse playback
# Use CTRL+B for burst mode (faster playback)
```

--------------------------------

### LQR Control Example using MuJoCo.jl Linearization

Source: https://context7.com/jamiemair/mujoco.jl/llms.txt

Illustrates designing and applying a Linear Quadratic Regulator (LQR) controller. It linearizes the cart-pole model around equilibrium using finite differences, solves the Riccati equation, and simulates the system with the computed LQR controller.

```julia
using MuJoCo
using LinearAlgebra
using MatrixEquations

# Load cart-pole model
model = load_model("cartpole.xml")
data = init_data(model)

# Get dimensions
nx = 2 * model.nv  # State: [qpos; qvel]
nu = model.nu      # Control dimension

# Linearize around equilibrium
A = mj_zeros(nx, nx)
B = mj_zeros(nx, nu)
ϵ = 1e-6
mjd_transitionFD(model, data, ϵ, true, A, B, nothing, nothing)

# Design LQR controller
Q = diagm([1.0, 10.0, 1.0, 5.0])  # State cost
R = diagm([1.0])                   # Control cost
S = zeros(nx, nu)                  # Cross term

_, _, K, _ = ared(A, B, R, Q, S)   # Solve Riccati equation

# Apply LQR controller
function lqr_controller!(m::Model, d::Data)
    state = vcat(d.qpos, d.qvel)
    d.ctrl .= -K * state
    return nothing
end

# Simulate with controller
reset!(model, data)
for t in 1:1000
    lqr_controller!(model, data)
    step!(model, data)
end

println("Final state: ", vcat(data.qpos, data.qvel))
```

--------------------------------

### Benchmark Basic Humanoid Controller

Source: https://github.com/jamiemair/mujoco.jl/blob/main/docs/src/examples/humanoid_lqr.md

This Julia code snippet uses the `BenchmarkTools.jl` package to benchmark the performance of the basic `humanoid_ctrl!` function. It measures the execution time and memory allocation of the controller without functor optimization, providing a baseline for comparison.

```julia
using BenchmarkTools
@benchmark humanoid_ctrl!(model, data)
```

--------------------------------

### Define Random Controller Function

Source: https://github.com/jamiemair/mujoco.jl/blob/main/docs/src/intro/getting_started.md

Defines a Julia function that sets random control torques for a given MuJoCo model and data. This function takes Model and Data objects as input and modifies the data.ctrl field.

```julia
function random_controller!(m::Model, d::Data)
    nu = m.nu
    d.ctrl .= 2*rand(nu) .- 1
    return nothing
end
```

--------------------------------

### Set Humanoid Position and Compute Control Set-point

Source: https://github.com/jamiemair/mujoco.jl/blob/main/docs/src/examples/humanoid_lqr.md

Resets the humanoid model, sets its position including a vertical offset, and computes the required control inputs (set-point) to achieve zero acceleration and a specific force configuration. It uses inverse dynamics and matrix pseudoinverse.

```julia
resetkey!(model, data, keyframe)
mj_forward(model, data)
data.qacc .= 0
data.qpos[3] += height
qpos0 = copy(data.qpos)
println(qpos0)

mj_inverse(model, data)
qfrc0 = copy(data.qfrc_inverse)

M_act = data.actuator_moment
ctrl0 = pinv(M_act)' * qfrc0
println(ctrl0)

# Sanity check
data.ctrl .= ctrl0
mj_forward(model, data)
qfrc_test = data.qfrc_actuator
println("Desired forces: ", qfrc0)
println("Actual forces:  ", qfrc_test)
println("Joint forces equal? ", all((qfrc_test .≈ qfrc0)[7:end]))
```

--------------------------------

### Visualize Humanoid Simulation with Custom Controller

Source: https://github.com/jamiemair/mujoco.jl/blob/main/docs/src/examples/humanoid_lqr.md

This Julia code snippet resets the simulation state and then visualizes the humanoid model. It allows for manual perturbation of the humanoid by interacting with the visualization window (double-click to select, CTRL+RightClick and drag to apply force). The `visualise!` function accepts a custom controller function, `humanoid_ctrl!`, to manage the simulation dynamics during visualization.

```julia
reset!(model, data)
data.qpos .= qpos0
visualise!(model, data, controller=humanoid_ctrl!)
```

--------------------------------

### Compute Control Set-Point using Inverse Dynamics

Source: https://github.com/jamiemair/mujoco.jl/blob/main/docs/src/examples/humanoid_lqr.md

Resets the humanoid model to a specific keyframe (keyframe 2), sets joint accelerations to zero, and then uses inverse dynamics (`mj_inverse`) to calculate the control forces required to hold this pose. It prints the resulting forces, highlighting potential unphysical values.

```julia
# Reset to desired keyframe
keyframe = 2
resetkey!(model, data, keyframe)

# Propagate derived quantities
mj_forward(model, data)
# Set joint accelerations to 0
data.qacc .= 0

# Inspect forces from inverse dynamics
mj_inverse(model, data)
println("Required control: ", data.qfrc_inverse)
```

--------------------------------

### Compare Humanoid Force to Weight

Source: https://github.com/jamiemair/mujoco.jl/blob/main/docs/src/examples/humanoid_lqr.md

Calculates and visualizes the vertical force acting on the humanoid compared to its weight. It plots the forces against vertical offset and saves the figure. Dependencies include MuJoCo.jl for model operations and plotting libraries.

```julia
weight = sum(model.body_mass) * norm(model.opt.gravity)

fig = Figure(size=(500,300))
ax = Axis(fig[1,1], xlabel="Vertical offset (mm)", ylabel="Vertical force (N)")
lines!(ax, heights_mm, u_vert, label="Vertical force")
lines!(ax, heights_mm, weight*ones(length(heights)), linestyle=:dash, label="Weight")
lines!(ax, [height_mm, height_mm], [minimum(u_vert), maximum(u_vert)], linestyle=:dash)
axislegend(ax,position=:rb)
save("humanoid_force_offset.svg",fig) # hide
nothing #hide
```

--------------------------------

### Benchmark Optimized Humanoid Controller with Functor

Source: https://github.com/jamiemair/mujoco.jl/blob/main/docs/src/examples/humanoid_lqr.md

This Julia code snippet uses `BenchmarkTools.jl` to benchmark the performance of the humanoid controller implemented using the `LQRHumanoidBalance` functor. By comparing the results of this benchmark with the benchmark for the basic controller, one can quantify the performance improvements gained from using functors and cached memory.

```julia
@benchmark controller(model, data)
```

--------------------------------

### Compute Jacobian for CoM and Foot Balancing

Source: https://github.com/jamiemair/mujoco.jl/blob/main/docs/src/examples/humanoid_lqr.md

Extracts Jacobians for the humanoid's torso center of mass (CoM) and its left foot. These Jacobians are crucial for designing the CoM balancing cost component of the LQR Q matrix. It utilizes MuJoCo.jl's utility functions for Jacobian computation.

```julia
import MuJoCo as MJ

torso = MJ.body(model, "torso")
left_foot = MJ.body(model, "foot_left")

# Get Jacobian for torso CoM
reset!(model, data)
data.qpos .= qpos0
forward!(model, data)
jac_com = mj_zeros(3, nv)
mj_jacSubtreeCom(model, data, jac_com, torso.id)

# Get (left) foot Jacobian for balancing
jac_foot = mj_zeros(3, nv)
mj_jacBodyCom(model, data, jac_foot, nothing, left_foot.id)

# Design Q-matrix to balance CoM over foot
jac_diff = jac_com .- jac_foot
Qbalance = jac_diff' * jac_diff
println(Qbalance)
```

--------------------------------

### Analyze Vertical Force vs. Height Offset

Source: https://github.com/jamiemair/mujoco.jl/blob/main/docs/src/examples/humanoid_lqr.md

Analyzes how the vertical force output from inverse dynamics changes as the humanoid's initial height is slightly adjusted. It maps a range of heights (±1mm) to their corresponding vertical forces and identifies the height offset that minimizes the fictitious vertical force.

```julia
using CairoMakie
using LinearAlgebra

heights = LinRange(-0.001, 0.001, 2001) # -1mm to +1mm
# Map each height to the corresponding high force output
u_vert = map(heights) do h
    # Set model in position with qacc == 0
    resetkey!(model, data, keyframe)
    mj_forward(model, data)
    data.qacc .= 0

    # Offset the height and check required vertical forces
    data.qpos[3] += h
    mj_inverse(model, data)
    return data.qfrc_inverse[3] # 3 -> z-force
end

# Find height corresponding to minimum fictitious force (best offset)
height = heights[argmin(abs.(u_vert))]
height_mm = height*1000
heights_mm = heights .* 1000
```

--------------------------------

### Accessing Contact Information in MuJoCo.jl

Source: https://context7.com/jamiemair/mujoco.jl/llms.txt

Provides an example of how to read collision contact information after a simulation step. It shows how to check the number of contacts, access individual contact details like position, normal, force, and the involved geometry IDs.

```julia
using MuJoCo

model, data = MuJoCo.sample_model_and_data()

# Run simulation until contact occurs
for t in 1:100
    step!(model, data)
end

# Access contact data
ncon = data.ncon
println("Number of contacts: ", ncon)

if ncon > 0
    # Access first contact
    contact = data.contact[1]
    println("Contact position: ", contact.pos)
    println("Contact normal: ", contact.frame[1:3])
    println("Contact force: ", contact.force)
    println("Contact geom IDs: ", contact.geom1, ", ", contact.geom2)
end
```

--------------------------------

### Construct Joint Cost Matrix Qjoint in Julia

Source: https://github.com/jamiemair/mujoco.jl/blob/main/docs/src/examples/humanoid_lqr.md

This code constructs the 'Qjoint' matrix, which defines the cost coefficients for different joint groups. It initializes a cost matrix based on identity and then scales specific joint cost coefficients (balance_joint_cost, other_joint_cost) to the corresponding joint dofs. Free dofs are assigned a cost of 0.

```julia
# Cost coefficients
balance_joint_cost = 3          # Joints can move a bit and still balance
other_joint_cost   = 0.3        # Other joints can do whatever

# Construct joint Q matrix
Qjoint = Matrix{Float64}(I, nv, nv)
Qjoint[free_dofs, free_dofs] *= 0
Qjoint[balance_dofs, balance_dofs] *= balance_joint_cost
Qjoint[other_dofs, other_dofs] *= other_joint_cost
nothing #hide
```

--------------------------------

### Combine Costs and Define Final Q Matrix in Julia

Source: https://github.com/jamiemair/mujoco.jl/blob/main/docs/src/examples/humanoid_lqr.md

This snippet combines the Qbalance matrix with the newly computed Qjoint matrix to form the final Qpos matrix, representing the position cost. A large balance_cost is applied to Qbalance to account for different unit scales. Finally, it constructs the complete LQR cost matrix Q by incorporating Qpos and zero matrices, adding a small identity matrix for positive definiteness.

```julia
balance_cost = 1000

Qpos = balance_cost*Qbalance + Qjoint
Q = [Qpos zeros(nv,nv); zeros(nv, 2nv)]  + (1e-10) * I # Add ϵI for positive definite Q
println(Q) # hide
```

--------------------------------

### Compute Linear System Jacobians A and B in Julia

Source: https://github.com/jamiemair/mujoco.jl/blob/main/docs/src/examples/humanoid_lqr.md

This code computes the Jacobians A (state transition) and B (control input) for the linearized system dynamics around a set-point. It initializes the model and data, sets the control input, and then uses the `mjd_transitionFD` function with finite differences to calculate these Jacobians. The `centred` parameter is set to true for centered finite differences.

```julia
# Initialise the model at our set point
reset!(model, data)
data.ctrl .= ctrl0
data.qpos .= qpos0

# Finite-difference parameters
ϵ = 1e-6
centred = true

# Compute the Jacobians
A = mj_zeros(2nv, 2nv)
B = mj_zeros(2nv, nu)
mjd_transitionFD(model, data, ϵ, centred, A, B, nothing, nothing)
@show A, B
```

--------------------------------

### Creating Row-Major Arrays for MuJoCo Functions (Julia)

Source: https://github.com/jamiemair/mujoco.jl/blob/main/docs/src/intro/overview.md

Illustrates how to create arrays with row-major memory layout using `MJ.mj_zeros` for compatibility with MuJoCo's C API. This is crucial for functions that mutate their arguments, ensuring correct data interpretation. The example computes a Jacobian for the torso. Dependencies: MuJoCo.jl, a model, and data.

```julia
jac_torso = MJ.mj_zeros(3, model.nv)
MJ.mj_jacSubtreeCom(model, data, jac_torso, torso.id)
@show jac_torso
```

--------------------------------

### Instantiate Functor for Humanoid Controller with Cached Memory

Source: https://github.com/jamiemair/mujoco.jl/blob/main/docs/src/examples/humanoid_lqr.md

This Julia code creates an instance of the `LQRHumanoidBalance` functor. It initializes the functor with the required state variables (`qpos0`, `ctrl0`, `K`) and pre-allocates memory for internal calculations (`Δq`, `Δx`, `KΔx`). This pre-allocation ensures that memory is not repeatedly created during controller updates, contributing to performance gains. The types of the cached memory are inferred from the types of `qpos0` and `ctrl0`.

```julia
controller = LQRHumanoidBalance(
    qpos0,
    ctrl0,
    K,
    zeros(eltype(qpos0), model.nv), # Δq
    zeros(eltype(qpos0), 2 * model.nv), # Δx
    similar(ctrl0) # KΔx
);
nothing # hide
```

--------------------------------

### Identify Joint Indices for Cost Calculation in Julia

Source: https://github.com/jamiemair/mujoco.jl/blob/main/docs/src/examples/humanoid_lqr.md

This code snippet identifies the array indices for different sets of joints (free, abdomen, left leg) using named access tools in Mujoco.jl. It leverages list comprehensions and set difference operations to categorize joints for cost assignment. The output includes the indices for balance-dependent and other joints.

```julia
# Get indices into relevant sets of joints.
free_dofs = 1:6
body_dofs = 7:nv

# Get all the joints using a list comprehension. 
# We add one to the raw ID to get the Julia 1-based index of the joint.
abdomen_dofs = [jnt.id+1 for jnt in MJ.joints(model) if occursin("abdomen", jnt.name)]
left_leg_dofs = [jnt.id+1 for jnt in MJ.joints(model) if occursin("left", jnt.name) && any(occursin(part, jnt.name) for part in ("hip", "knee", "ankle"))]

balance_dofs = vcat(abdomen_dofs, left_leg_dofs)
other_dofs = setdiff(body_dofs, balance_dofs)

println("Balance dofs: ", balance_dofs)
println("Other dofs:   ", other_dofs)
```

--------------------------------

### Define Functor for Optimized Humanoid Controller

Source: https://github.com/jamiemair/mujoco.jl/blob/main/docs/src/examples/humanoid_lqr.md

This Julia code defines a struct `LQRHumanoidBalance` which acts as a functor. It encapsulates state variables (`qpos0`, `ctrl0`, `K`) and pre-allocated memory (`Δq`, `Δx`, `KΔx`) required by the controller. This approach is used to improve performance by avoiding the re-allocation of memory and the capture of non-const global variables within the controller function.

```julia
struct LQRHumanoidBalance{TQ,TC,TK,TDQ,TDX,TKDX}
    qpos0::TQ
    ctrl0::TC
    K::TK
    Δq::TDQ
    Δx::TDX
    KΔx::TKDX
end
nothing # hide
```

--------------------------------

### Calculate LQR Gain Matrix K using MatrixEquations.jl in Julia

Source: https://github.com/jamiemair/mujoco.jl/blob/main/docs/src/examples/humanoid_lqr.md

This code snippet calculates the optimal LQR gain matrix K. It utilizes the `ared` function from the `MatrixEquations.jl` library, which solves the Algebraic Riccati Equation. This function takes the system Jacobians (A, B), cost matrices (R, Q), and a cross-covariance matrix (S) as input and returns the solution, including the gain matrix K.

```julia
using MatrixEquations

S = zeros(2nv, nu)
_, _, K, _ = ared(A,B,R,Q,S)
```

--------------------------------

### Implement Functor Call for Optimized Humanoid Controller

Source: https://github.com/jamiemair/mujoco.jl/blob/main/docs/src/examples/humanoid_lqr.md

This Julia code defines the function call operator for the `LQRHumanoidBalance` functor. When called with `model` and `data`, it reuses the cached memory (`Δq`, `Δx`, `KΔx`) stored within the functor instance. It performs the same control calculations as the non-functor version (`mj_differentiatePos`, matrix multiplication, and control update) but with improved performance due to memory reuse and encapsulation of necessary variables.

```julia
function (functor::LQRHumanoidBalance)(model::Model, data::Data)
    Δq = functor.Δq
    mj_differentiatePos(model, Δq, 1, functor.qpos0, data.qpos)

    Δx = functor.Δx
    Δx[1:length(Δq)] .= Δq
    Δx[(length(Δq)+1):end] .= data.qvel

    KΔx = functor.KΔx
    mul!(KΔx, functor.K, Δx)

    data.ctrl .= functor.ctrl0 .- KΔx
    nothing
end
nothing # hide
```

--------------------------------

### Implement and Test Humanoid Controller with mj_differentiatePos

Source: https://github.com/jamiemair/mujoco.jl/blob/main/docs/src/examples/humanoid_lqr.md

This Julia function sets control gains for a humanoid simulation at each time step. It utilizes `mj_differentiatePos` to compute position errors using finite differences, which is crucial for quaternion-based orientation representations in MuJoCo. The function takes the model and data structures as input and modifies the `data.ctrl` field. It's noted that this implementation can suffer performance issues due to non-const global variables.

```julia
function humanoid_ctrl!(m::Model, d::Data)
    Δq = zeros(m.nv)
    mj_differentiatePos(m, Δq, 1, qpos0, d.qpos)

    Δx = vcat(Δq, data.qvel)
    data.ctrl .= ctrl0 .- K*Δx
    nothing
end
nothing # hide
```

--------------------------------

### Launch and Interact with MuJoCo Visualizer

Source: https://context7.com/jamiemair/mujoco.jl/llms.txt

Initializes the MuJoCo visualizer and launches an interactive session with a specified controller. It also lists common keyboard controls for interacting with the visualization.

```julia
using MuJoCo

# Initialize visualizer (load dependencies into session)
init_visualiser()

model, data = MuJoCo.sample_model_and_data()

# Define a controller
function ctrl!(m::Model, d::Data)
    d.ctrl .= 0.5*randn(m.nu)
end

# Launch visualizer with controller
visualise!(model, data, controller=ctrl!)

# Keyboard controls:
# - SPACE: pause/unpause
# - Backspace: reset
# - CTRL+RightArrow: cycle between passive/controlled dynamics
# - ESC: exit
# - F1: help menu
```

--------------------------------

### Simulate and Visualize Cart-Pole System with LQR Controller

Source: https://github.com/jamiemair/mujoco.jl/blob/main/docs/src/examples/cartpole_balance.md

Initializes the Mujoco visualizer and runs the simulation of the cart-pole system using the defined LQR controller. This code snippet requires the `init_visualiser`, `visualise!`, `model`, and `data` to be defined and available in the environment. It visualizes the system's response to the controller.

```julia
init_visualiser()
visualise!(model, data; controller=lqr_balance!)
```

--------------------------------

### Load MuJoCo Cart-Pole Model and Initial State

Source: https://github.com/jamiemair/mujoco.jl/blob/main/docs/src/examples/cartpole_balance.md

Loads the 'cartpole.xml' MuJoCo model and initializes its data. It then prints the initial generalized positions and velocities of the system, which are expected to be zero for the upright equilibrium.

```julia
using MuJoCo

model = load_model("cartpole.xml")
data = init_data(model)

println("Initial position: ", data.qpos)
println("Initial velocity: ", data.qvel)
```

--------------------------------

### Load MuJoCo Models from XML

Source: https://context7.com/jamiemair/mujoco.jl/llms.txt

Demonstrates loading MuJoCo models, either from a built-in sample or a custom MJCF XML file. It also shows how to access basic model properties after loading.

```julia
using MuJoCo

# Load a built-in humanoid model
model, data = MuJoCo.sample_model_and_data()

# Or load a custom model
model = load_model("path/to/model.xml")
data = init_data(model)

# Access model properties
println("Number of bodies: ", model.nbody)
println("Number of joints: ", model.njnt)
println("Timestep: ", model.opt.timestep)
```

--------------------------------

### Comparing Wrapped vs. Original C API Function Signatures (Julia)

Source: https://github.com/jamiemair/mujoco.jl/blob/main/docs/src/intro/overview.md

Compares the method signature of a wrapped MuJoCo function (`MJ.mju_add`) with its original C API counterpart (`MJ.LibMuJoCo.mju_add`) accessible via the Julia wrapper. This highlights how MuJoCo.jl simplifies function calls by removing superfluous arguments. Requires importing MuJoCo.

```julia
println(methods(MJ.mju_add).ms[1])
```

```julia
println(methods(MJ.LibMuJoCo.mju_add).ms[1])
```

--------------------------------

### Run MuJoCo Simulations with Custom Controllers

Source: https://context7.com/jamiemair/mujoco.jl/llms.txt

This code illustrates how to run a physics simulation by stepping through timesteps. It includes defining a simple random controller and accessing simulation data like time and joint positions at specified intervals.

```julia
using MuJoCo

model, data = MuJoCo.sample_model_and_data()

# Define a controller function
function random_controller!(m::Model, d::Data)
    nu = m.nu  # Number of actuators
    d.ctrl .= 2*rand(nu) .- 1  # Random controls between -1 and 1
    return nothing
end

# Simulate for 1000 timesteps
for t in 1:1000
    random_controller!(model, data)
    step!(model, data)

    # Access simulation data
    if t % 100 == 0
        println("Time: ", data.time)
        println("Joint positions: ", data.qpos[1:3])
    end
end

# Reset to initial state
reset!(model, data)
```

--------------------------------

### Accessing Model Body Parts by Name (Julia)

Source: https://github.com/jamiemair/mujoco.jl/blob/main/docs/src/intro/overview.md

Demonstrates how to access specific body parts (like the 'torso') of a MuJoCo model using named wrappers provided by MuJoCo.jl. It shows how to retrieve the center of mass coordinates for a body part. Requires importing MuJoCo and having a sample model and data.

```julia
import MuJoCo as MJ
model, data = MJ.sample_model_and_data();
MJ.step!(model, data);
torso = MJ.body(data, "torso")
```

```julia
torso.com
```

--------------------------------

### Incorrect Row-Major Array Usage Example (Julia)

Source: https://github.com/jamiemair/mujoco.jl/blob/main/docs/src/intro/overview.md

Shows the potential issue of using standard Julia (column-major) arrays with MuJoCo functions that expect row-major order. This snippet demonstrates how `MJ.mj_jacSubtreeCom` might produce incorrect results if a standard `zeros` array is passed instead of a row-major one like `mj_zeros`. Dependencies: MuJoCo.jl, a model, and data.

```julia
jac_torso_wrong = zeros(3, model.nv)
MJ.mj_jacSubtreeCom(model, data, jac_torso_wrong, torso.id)
@show jac_torso_wrong
```

--------------------------------

### Linearize Cart-Pole Dynamics using Finite Differences

Source: https://github.com/jamiemair/mujoco.jl/blob/main/docs/src/examples/cartpole_balance.md

Computes the state transition (A) and control input (B) matrices for the cart-pole system using MuJoCo's finite difference method. It initializes the matrices with zeros and uses `mjd_transitionFD` to populate them based on the model and current data.

```julia
# Number of states and controlled inputs
nx = 2*model.nv
nu = model.nu

# Finite-difference parameters
ϵ = 1e-6
centred = true

# Compute the Jacobians
A = mj_zeros(nx, nx)
B = mj_zeros(nx, nu)
mjd_transitionFD(model, data, ϵ, centred, A, B, nothing, nothing)
@show A, B
nothing #hide
```

--------------------------------

### Manage MuJoCo Physics State

Source: https://context7.com/jamiemair/mujoco.jl/llms.txt

Shows how to retrieve the current physics state (positions, velocities, controls) using `get_physics_state`, modify it, and then apply the changes back to the simulation using `set_physics_state!` and `forward!`.

```julia
using MuJoCo

model, data = MuJoCo.sample_model_and_data()

# Get the current physics state (positions, velocities, actuator states)
state = get_physics_state(model, data)
println("State dimension: ", length(state))

# Modify the state
new_state = copy(state)
new_state[1:3] .+= 0.1  # Perturb first 3 positions

# Set the new state
set_physics_state!(model, data, new_state)
forward!(model, data)  # Recompute derived quantities

# Access individual components
println("Positions (qpos): ", data.qpos)
println("Velocities (qvel): ", data.qvel)
println("Controls (ctrl): ", data.ctrl)
```

--------------------------------

### Computing Jacobians and Linearization with MuJoCo.jl

Source: https://context7.com/jamiemair/mujoco.jl/llms.txt

Shows how to compute Jacobians (position and rotation) for a body's center of mass and perform finite-difference linearization to obtain the system's A and B matrices. This is crucial for control design.

```julia
using MuJoCo
using LinearAlgebra

model, data = MuJoCo.sample_model_and_data()

# Get dimensions
v = model.nv  # Degrees of freedom
nu = model.nu  # Number of actuators

# Compute body Jacobian for center of mass
torso = MuJoCo.body(model, "torso")
jac_pos = mj_zeros(3, nv)
jac_rot = mj_zeros(3, nv)
mj_jacBodyCom(model, data, jac_pos, jac_rot, torso.id)

println("Position Jacobian shape: ", size(jac_pos))

# Compute finite-difference linearization (A and B matrices)
nx = 2 * nv  # State dimension (qpos + qvel)
A = mj_zeros(nx, nx)
B = mj_zeros(nx, nu)
ϵ = 1e-6
centred = true

mjd_transitionFD(model, data, ϵ, centred, A, B, nothing, nothing)

println("A matrix shape: ", size(A))
println("B matrix shape: ", size(B))
```

--------------------------------

### Working with Keyframes in MuJoCo.jl

Source: https://context7.com/jamiemair/mujoco.jl/llms.txt

Explains how to load and reset a MuJoCo model to specific keyframes. It shows how to reset to a keyframe by its index and how to directly access keyframe data like time and joint positions.

```julia
using MuJoCo

model, data = MuJoCo.sample_model_and_data()

# Reset to first keyframe (1-indexed)
resetkey!(model, data, 1)
println("Keyframe 1 position: ", data.qpos[1:3])

# Reset to second keyframe
resetkey!(model, data, 2)
println("Keyframe 2 position: ", data.qpos[1:3])

# Access keyframe data directly
key = MuJoCo.key(model, 0)  # 0-indexed
println("Keyframe time: ", key.time)
println("Keyframe qpos: ", key.qpos)
```

--------------------------------

### Compute LQR Weights and Gain Matrix

Source: https://github.com/jamiemair/mujoco.jl/blob/main/docs/src/examples/cartpole_balance.md

Defines the state and control weights (Q and R) for the LQR controller and computes the LQR gain matrix K. It demonstrates using `MatrixEquations.jl` for the computation, which is a dependency for this functionality. The output is the gain matrix K.

```julia
using LinearAlgebra

Q = diagm([1, 10, 1, 5]) # Weights for the state vector
R = diagm([1])           # Weights for the controls

using MatrixEquations

S = zeros(nx, nu)
_, _, K, _ = ared(A,B,R,Q,S)
@show K
```

--------------------------------

### Access MuJoCo Model Elements by Name

Source: https://context7.com/jamiemair/mujoco.jl/llms.txt

Illustrates how to access various components of a MuJoCo model (bodies, joints, actuators) using their names, providing a more readable alternative to using integer IDs. It also shows how to filter elements based on name patterns.

```julia
using MuJoCo

model, data = MuJoCo.sample_model_and_data()

# Access body by name
torso = MuJoCo.body(model, "torso")
println("Torso ID: ", torso.id)

# Access joint by name
left_knee = MuJoCo.jnt(model, "left_knee")
println("Joint position: ", left_knee.qposadr)

# Get all joints with "left" in the name
all_joints = [jnt for jnt in MuJoCo.joints(model)
              if occursin("left", jnt.name)]

# Access actuator
actuator_0 = MuJoCo.actuator(data, 0)
println("Actuator control: ", actuator_0.ctrl)
```

--------------------------------

### Accessing MuJoCo Geom Information

Source: https://context7.com/jamiemair/mujoco.jl/llms.txt

Demonstrates how to access and print information about a specific geometric object (geom) within a MuJoCo model. It retrieves the 'floor' geom and prints its position.

```julia
using MuJoCo

# Access geom
floor_geom = MuJoCo.geom(model, "floor")
println("Floor position: ", floor_geom.pos)
```