StateOrder(config_names, vel_names, torque_names = vel_names)

Struct containing the orders of the configuration, velocity and torque vectors. Can be used to help convert between different vector orderings

ConversionIndices

Holds index vectors mapping model vectors from one form to another, for config, velocity, state, error_state, and torques.

change_order!(model::PinnZooModel, input::AbstractVector, from::Symbol, to::Symbol)

Gets the relevant ConversionIndices from model.conversions[(from, to)] and converts the input vector order in-place, deducing which conversion to apply (config, velocity, state, error_state, or torque) based on the input vector size.

change_order!(model::PinnZooModel, input::AbstractVector, from::Symbol, to::Symbol; dims= (1, 2))

Gets the relevant ConversionIndices from model.conversions[(from, to)] and converts the input matrix order in-place, deducing which conversion to apply (config, velocity, state, error_state, or torque) based on the input matrix rows and columns. Default is to transform both input and output, but the dimensions can be specified using the dims option.

change_order!(model::PinnZooModel, input::Adjoint, from::Symbol, to::Symbol)

Gets the relevant ConversionIndices from model.conversions[(from, to)] and converts the input vector order in-place, deducing which conversion to apply (config, velocity, state, error_state, or torque) based on the input vector size.

change_order(model::PinnZooModel, input::AbstractVector, from, to)

Gets the relevant ConversionIndices from model.conversions[(from, to)] and converts the input vector order, deducing which conversion to apply (config, velocity, state, error_state, or torque) based on the input vector size.

change_order(model::PinnZooModel, input::AbstractMatrix, from::Symbol, to::Symbol)

Gets the relevant ConversionIndices from model.conversions[(from, to)] and converts the input matrix order in-place, deducing which conversion to apply (config, velocity, state, error_state, or torque) based on the input matrix rows and columns. Default is to transform both input and output, but the dimensions can be specified using the dims option.

change_order!(model::PinnZooModel, input::Adjoint, from::Symbol, to::Symbol)

Gets the relevant ConversionIndices from model.conversions[(from, to)] and converts the input vector order, deducing which conversion to apply (config, velocity, state, error_state, or torque) based on the input vector size.

change_orders!(model::PinnZooModel, inputs::Vector{<:AbstractArray}, from, to; dims = (1, 2))

Changes ordering in-place according to the appropriate convention for each object in inputs

change_orders!(model::PinnZooModel, inputs::Vector{<:AbstractArray}, from, to; dims = (1, 2))

Changes ordering according to the appropriate convention for each object in inputs

generate_conversions(orders::Dict{Symbol, StateOrder}, 
        conversions::Dict{Tuple{Symbol, Symbol}, ConversionIndices}=Dict{Tuple{Symbol, Symbol}, ConversionIndices}();
        from_scratch = false)

Given the vector orders specified, creates ConversionIndices objects that can be used with change_orders to convert between each order in orders (i.e. nominal, Pinocchio, MuJoCo)

M_func(model::PinnZooModel, x::AbstractVector{Float64})

Return the mass matrix of the model as a function of the configuration (x[1:model.nq])

C_func(model::PinnZooModel, x::AbstractVector{Float64})

Return the dynamics bias of the model (coriolos + centrifugal + gravitational forces)

dynamics(model::PinnZooModel, x::AbstractVector{Float64}, τ::AbstractVector{Float64})

Returns dynamics [v; v̇] where v̇ = M(x) \ (τ - C) where M is the mass matrix and C is the dynamics bias vector.

dynamics_deriv(model::PinnZooModel, x::AbstractVector{Float64}, τ::AbstractVector{Float64})

Return a tuple of derivatives of the dynamics (ẋ) with respect to x and τ

forward_dynamics(model::PinnZooModel, x::AbstractVector{Float64}, τ::AbstractVector{Float64})

Return the forward dynamics v̇ = M(x) \ (τ - C) where M is the mass matrix and C is the dynamics bias vector.

forward_dynamics_deriv(model::PinnZooModel, x::AbstractVector{Float64}, τ::AbstractVector{Float64})

Return a tuple of derivatives of the forward dynamics (v̇) with respect to x and τ

inverse_dynamics(model::PinnZooModel, x::AbstractVector{Float64}, v̇::AbstractVector{Float64})

Return the inverse dynamics τ = M⩒ + C where M is the mass matrix and C is the dynamics bias vector

inverse_dynamics_deriv(model::PinnZooModel, x::AbstractVector{Float64}, v̇::AbstractVector{Float64}

Return a tuple of derivatives of the inverse dynamics (τ) with respect to x and v̇

velocity_kinematics(model::PinnZooModel, x::AbstractVector{Float64})

Return the matrix E mapping v to q̇, i.e. q̇ = E(q)v, where q = x[1:model.nq]. This mapping is exact, i.e. ET*E = I where ET comes from velocitykinematicsT. For an explanation for why Eᵀ != E_T refer to TODO

velocity_kinematics_T(model::PinnZooModel, x::AbstractVector{Float64})

Return the matrix ET projecting q̇ onto v, i.e. v = ET(q)q̇, where q = x[1:model.nq]. This respects the tangent space structure. For a further explanation, as well as details on why E_T != Eᵀ refer to TODO

velocity_kinematics_jvp_deriv(model::PinnZooModel, x::AbstractVector{Float64}, v::AbstractVector{Float64})

Derivative of the jacobian vector product E(x)v with respect to x

velocity_kinematics_T_jvp_deriv(model::PinnZooModel, x::AbstractVector{Float64}, q::AbstractVector{Float64})

Derivative of the jacobian vector product E_T(x)q with respect to x

state_error(model::PinnZooFloatingBaseModel, x, x0)

Return the state_error between x and x0, using axis-angles for quaternion error, and representing body position error in the body frame (matches with body velocity convention).

Here we use q0'q for the quaternion error because we defined the attitude jacobian as f(q0Δq) (delta on the left), so q0Δq = q => Δq = q0'q

apply_Δx(model::PinnZooFloatingBaseModel, x_k, Δx)

Return Δx added to x while respecting the configuration space/tangent space relationship (i.e. do quaternion multiplication, rotate Δbody pos from body frame to world frame). Δx should be model.nv*2, x_k should be model.nx. Floating base rotation in Δx should be axis angle.

error_jacobian(model::PinnZooFloatingBaseModel, x)

Return the jacobian mapping Δx to x where Δx is in the tangent space (look at stateerror for our choice of tangent space). This matches the derivative of applyΔx(model, x, Δx) around Δx = 0

error_jacobian_T(model::PinnZooFloatingBaseModel, x)

Return the jacobian mapping x to Δx where Δx is in the tangent space (look at stateerror for our choice of tangent space). This matches the derivative of stateerror(model, _x, x) around _x = x (error = 0)

error_jacobian_jvp_deriv(model::PinnZooFloatingBaseModel, x, Δx)

Returns the jacobian of E(x)*Δx with respect to x

error_jacobian_T_jvp_deriv(model::PinnZooFloatingBaseModel, x, Δx)

Returns the jacobian of E_T(x)*x2 with respect to x

kinematics(model::PinnZooModel, x::AbstractVector{Float64})

Return a list of the locations of each body in model.kinematics_bodies in the world frame.

kinematics_jacobian(model::PinnZooModel, x::AbstractVector{Float64})

Return the jacobian of the kinematics function with respect to x (not projected into the tangent space).

kinematics_velocity(model::PinnZooModel, x::AbstractVector{Float64})

Return a list of the instantaneous linear velocities of each body in model.kinematics_bodies in the world frame.

kinematics_velocity_jacobian(model::PinnZooModel, x::AbstractVector{Float64})

Return the jacobian of the kinematics_velocity function with respect to x (not in the tangent space). If $J_q$ is the derivative of the kinematics with respect to $q$, this jacobian $J$ = [$\dot{J_q}$ $J_q$] where $\dot{J_q} = \frac{\partial}{\partial q} J_qv$ and $J_qv$ is equal to kinematics_velocity. This also means that $J\dot{x}$ expresses the constraint at the acceleration level, i.e. $\dot{J_q}\dot{q} + J_q\dot{v} = 0$

kinematics_force_jacobian(model::PinnZooModel, x::AbstractVector{Float64}, λ::AbstractVector{Float64})

Return the jacobian (nv x nx matrix) of (J(x))'*λ with respect to x, where J(x) maps joint velocities into kinematics velocities

is_floating(model::PinnZooModel)

Return whether the model includes a floating base joint

zero_state(model::PinnZooModel)

Return the neutral state of the model.

randn_state(model::PinnZooModel)

Return a state vector where every coordinate is drawn from a normal distribution

init_state(model::Go1)

Return state vector with the robot on the ground in the starting pose

init_state(model::Go2)

Return state vector with the robot on the ground in the starting pose

init_state(model::PinnZooModel)

Return a custom initial state for the model (like standing) if defined, otherwise returns all zeros

quat_to_axis_angle(q; tol = 1e-12)

Return the axis angle corresponding to the provided quaternion

axis_angle_to_quat(ω; tol = 1e-12)

Return the quaternion corresponding to the provided axis angle

quat_conjugate(q)

Return the conjugate of the given quaternion (negates the velocity part)

skew(v)

Return a matrix M such that $v \times x = Mx$ where $\times$ denotes the cross product

L_mult(q)

Return a matrix representation of left quaternion multiplication, i.e. $q1 \cdot q2 = L(q1)q2$ where $\cdot$ is quaternion multiplication.

R_mult(q)

Return a matrix representation of right quaternion multiplication, i.e. $q1 \cdot q2 = R(q2)q1$ where $\cdot$ is quaternion multiplication.

attitude_jacobian(q)

Return the attitude jacobian G define as $\dot{q} = 0.5G\omega$, mapping angular velocity into quaternion time derivative.

quat_to_rot(q)

Return a rotation matrix R that q represents, defined by $\hat{p}^+ = q\hat{p}q^\dagger$ where $\hat{p}$ turns $p$ into a quaternion with zero scalar part and $p$ as the vector part, and $^\dagger$ is the quaternion conjugate.

B_func(quad::Quadruped)

Return the input jacobian mapping motor torques into joint torques

fix_joint_limits(model::Quadruped, x; supress_error = false)

Return x with joint angles wrapped to 2pi to fit within joint limits if possible. If not and supress_error is false, this will error. Otherwise, this will return a clamped version.

nearest_ik(model::Quadruped, q, foot_locs; obey_limits = true)

Return the ik solution for the given foot_locs and body orientation in q that is closest to the current joint angles in q (defined by minimum norm per joint).

Pendulum() <: PinnZooModel

Return an inverted pendulum dynamics model. θ = 0 is up, m = 1, l = 1, point mass at tip. Rotation axis is the positive x-axis (positive is counter-clockwise, right hand rule).

DoublePendulum() <: PinnZooModel

Return an inverted double pendulum dynamics model. θ = 0 is up, m = 1, l = 1, point mass at tip for both poles. Rotation axis is the positive x-axis (positive is counter-clockwise, right hand rule).

Cartpole() <: PinnZooModel

Return a Cartpole dynamics model, cart moves along the y-axis, pole rotates around positive x-axis. cart m = 1, pole m = 1, l = 1 (mass concentrated at pole tip).

DoubleCartpole() <: PinnZooModel

Return a DoubleCartpole dynamics model, cart moves along the y-axis, poles rotates around positive x-axis. cart m = 1, pole m = 1, l = 1 (mass concentrated at pole tip for both poles).

RigidBody() <: PinnZooFloatingBaseModel

Return a RigidBody dynamics model, with m = 1, I = Diag([1.0; 1.0; 1.0]).

Quadrotor() <: PinnZooFloatingBaseModel

Return a Quadrotor dynamics model, with m = 1, I = Diag([0.0046; 0.0046; 0.008])

Go1(; μ = 0.3) <: Quadruped

Return the Unitree Go1 dynamics and kinematics model

init_state(model::Go1)

Return state vector with the robot on the ground in the starting pose

inverse_kinematics(model::Go1, x, foot_locs)

Return 12 by 2 matrix containing the 2 possible joint angle solutions that put the feet at foot_locs given the body pose in x. Will be populated with NaN or Inf if solutions don't exist.

Go2(; μ = 0.3) <: Quadruped

Return the Unitree Go2 dynamics and kinematics model

init_state(model::Go2)

Return state vector with the robot on the ground in the starting pose

inverse_kinematics(model::Go2, x, foot_locs)

Return 12 by 2 matrix containing the 2 possible joint angle solutions that put the feet at foot_locs given the body pose in x. Will be populated with NaN or Inf if solutions don't exist.

Nadia(; simple = true, nc_per_foot = 1, μ = 1.0, kinematics_ori = :None)

Return a Nadia dynamics and kinematics model. Currently supports 1 or 4 contact points per foot, and only the simple knee (does not support simple = false). For 1 contact point you can also include foot orientation (kinematics_ori = :Quaterion or :AxisAngle).