PinnZoo.jl
Conventions
- Quaternions use [$q_w$ $q_x$ $q_y$ $q_z$] order and represent body to world rotations, using Hamilton's convention $i^2 = j^2 = k^2 = -1$
- Floating base joints use [$x$ $y$ $z$ $q_w$ $q_x$ $q_y$ $q_z$]$ order where the position is in the world frame
- Linear and angular velocities corresponding to a floating base joint are in the body frame
Inputs/Variables
- nq $\rightarrow$ # of configuration variables (1 per revolute/prismatic joint, 7 for floating joint)
- nv $\rightarrow$ # of velocity variables. Also the number of degrees of freedom (1 per revolute/prismatic joint, 6 for floating joint)
- nx $\rightarrow$ # of states, nq + nv
- nc $\rightarrow$ # of points on the robot that kinematics were generated for
- q $\rightarrow$ configuration vector, size nq.
- x $\rightarrow$ state vector, size nx. The first nq elements are the configuration and the last nv elements are velocity.
- x_dot $\rightarrow$ state vector derivative, size nx. The first nq elements are the derivative of the configuration with respect to time, and the last nv are the acceleration (derivative of velocity).
- tau $\rightarrow$ generalized force vector, size nv. Represents forces/torques at each degree of freedom.
For the dynamics, we assume the following manipulator equation and velocity kinematics:
\[M(x)\dot{v} + C(x) = \tau\]
\[\dot{q} = E(q)v\]
Velocity Kinematics
E(q) is typically the identity except in the following cases (not an exhaustive list):
When the configuration includes a quaternion, E(q) includes the mapping from angular velocity into a quaternion time derivative, respecting $\dot{q}q = 0$, the unit norm constraint at the velocity level. Because angular velocities are related to axis-angles, there is a factor of 2 that shows up, so the mapping back from $v$ to $\dot{q}$ which we refer to as $E\_T$ is not equal to $E^T$.
When the translation velocity of a floating is in the body frame, but the position is in the world frame, so E(q) includes a body-to-world rotation matrix.
Index
PinnZoo.CartpolePinnZoo.ConversionIndicesPinnZoo.DoubleCartpolePinnZoo.DoublePendulumPinnZoo.Go1PinnZoo.Go2PinnZoo.NadiaPinnZoo.PendulumPinnZoo.PineapplePinnZoo.QuadrotorPinnZoo.RigidBodyPinnZoo.StateOrderPinnZoo.B_funcPinnZoo.C_funcPinnZoo.L_multPinnZoo.M_funcPinnZoo.R_multPinnZoo.apply_ΔxPinnZoo.attitude_jacobianPinnZoo.axis_angle_to_quatPinnZoo.change_orderPinnZoo.change_order!PinnZoo.change_ordersPinnZoo.change_orders!PinnZoo.dynamicsPinnZoo.dynamics_derivPinnZoo.error_jacobianPinnZoo.error_jacobian_TPinnZoo.error_jacobian_T_jvp_derivPinnZoo.error_jacobian_jvp_derivPinnZoo.fix_joint_limitsPinnZoo.forward_dynamicsPinnZoo.forward_dynamics_derivPinnZoo.generate_conversionsPinnZoo.init_statePinnZoo.init_statePinnZoo.init_statePinnZoo.inverse_dynamicsPinnZoo.inverse_dynamics_derivPinnZoo.inverse_dynamics_hTvpPinnZoo.inverse_dynamics_hvpPinnZoo.inverse_kinematicsPinnZoo.inverse_kinematicsPinnZoo.inverse_kinematicsPinnZoo.is_floatingPinnZoo.kinematicsPinnZoo.kinematics_force_hTvpPinnZoo.kinematics_force_hvpPinnZoo.kinematics_force_jacobianPinnZoo.kinematics_hvpPinnZoo.kinematics_jacobianPinnZoo.kinematics_jacobianTvpPinnZoo.kinematics_rotationPinnZoo.kinematics_velocityPinnZoo.kinematics_velocity_jacobianPinnZoo.nearest_ikPinnZoo.quat_conjugatePinnZoo.quat_to_axis_anglePinnZoo.quat_to_rotPinnZoo.randn_statePinnZoo.rot_to_quatPinnZoo.skewPinnZoo.state_errorPinnZoo.velocity_kinematicsPinnZoo.velocity_kinematics_TPinnZoo.velocity_kinematics_T_jvp_derivPinnZoo.velocity_kinematics_jvp_derivPinnZoo.zero_state
Order Conversion Functions
The functions below can be used as helpers to convert between different vector orders to help interfacing with different dynamics packages. For example, to use a configuration vector from this package in mujoco, you can do
q_mujoco = change_order(model, q_pinnzo, :nominal, :mujoco)PinnZoo.StateOrder — TypeStateOrder(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
PinnZoo.ConversionIndices — TypeConversionIndicesHolds index vectors mapping model vectors from one form to another, for config, velocity, state, error_state, and torques.
PinnZoo.change_order! — Functionchange_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.
PinnZoo.change_order — Functionchange_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.
PinnZoo.change_orders! — Functionchange_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
PinnZoo.change_orders — Functionchange_orders!(model::PinnZooModel, inputs::Vector{<:AbstractArray}, from, to; dims = (1, 2))Changes ordering according to the appropriate convention for each object in inputs
PinnZoo.generate_conversions — Functiongenerate_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)
Dynamics Functions
PinnZoo.M_func — FunctionM_func(model::PinnZooModel, x::AbstractVector{Float64})Return the mass matrix of the model as a function of the configuration (x[1:model.nq])
PinnZoo.C_func — FunctionC_func(model::PinnZooModel, x::AbstractVector{Float64})Return the dynamics bias of the model (coriolos + centrifugal + gravitational forces)
PinnZoo.dynamics — Functiondynamics(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.
PinnZoo.dynamics_deriv — Functiondynamics_deriv(model::PinnZooModel, x::AbstractVector{Float64}, τ::AbstractVector{Float64})Return a tuple of derivatives of the dynamics (ẋ) with respect to x and τ
PinnZoo.forward_dynamics — Functionforward_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.
PinnZoo.forward_dynamics_deriv — Functionforward_dynamics_deriv(model::PinnZooModel, x::AbstractVector{Float64}, τ::AbstractVector{Float64})Return a tuple of derivatives of the forward dynamics (v̇) with respect to x and τ
PinnZoo.inverse_dynamics — Functioninverse_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
PinnZoo.inverse_dynamics_deriv — Functioninverse_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̇
PinnZoo.inverse_dynamics_hvp — Functioninverse_dynamics_hvp(model::PinnZooModel, x::AbstractVector{Float64}, v̇::AbstractVector{Float64}, lam::AbstractVector{Float64})Return a tuple of derivatives of (dτdx)*lam wrt x and (dτdx)lam wrt v̇. (dτ_dv̇)lam with respect to v̇ is 0 since dτ_dv̇ = M(q)
PinnZoo.inverse_dynamics_hTvp — Functioninverse_dynamics_hTvp(model::PinnZooModel, x::AbstractVector{Float64}, v̇::AbstractVector{Float64}, lam::AbstractVector{Float64})Return a tuple of derivatives of (dτdx)'*lam wrt x and (dτdv̇)'lam wrt x. (dτ_dv̇)'lam (τ) with respect to v̇ is 0 since dτ_dv̇ = M(q)
PinnZoo.velocity_kinematics — Functionvelocity_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
PinnZoo.velocity_kinematics_T — Functionvelocity_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
PinnZoo.velocity_kinematics_jvp_deriv — Functionvelocity_kinematics_jvp_deriv(model::PinnZooModel, x::AbstractVector{Float64}, v::AbstractVector{Float64})Derivative of the jacobian vector product E(x)v with respect to x
PinnZoo.velocity_kinematics_T_jvp_deriv — Functionvelocity_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
PinnZoo.state_error — Functionstate_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
PinnZoo.apply_Δx — Functionapply_Δ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.
PinnZoo.error_jacobian — Functionerror_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
PinnZoo.error_jacobian_T — Functionerror_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)
PinnZoo.error_jacobian_jvp_deriv — Functionerror_jacobian_jvp_deriv(model::PinnZooFloatingBaseModel, x, Δx)Returns the jacobian of E(x)*Δx with respect to x
PinnZoo.error_jacobian_T_jvp_deriv — Functionerror_jacobian_T_jvp_deriv(model::PinnZooFloatingBaseModel, x, Δx)Returns the jacobian of E_T(x)*x2 with respect to x
Kinematics Functions
PinnZoo.kinematics — Functionkinematics(model::PinnZooModel, x::AbstractVector{Float64})Return a list of the locations of each body in model.kinematics_bodies in the world frame.
PinnZoo.kinematics_rotation — Functionkinematics_rotation(model::PinnZooModel, x::AbstractVector{Float64})Return a list of rotation matrices in the world frame.
PinnZoo.kinematics_jacobian — Functionkinematics_jacobian(model::PinnZooModel, x::AbstractVector{Float64})Return the jacobian of the kinematics function with respect to x (not projected into the tangent space).
PinnZoo.kinematics_hvp — Functionkinematics_hvp(model::PinnZooModel, x::AbstractVector{Float64}, dx::AbstractVector{Float64})Return the hessian-vector product of the kinematics function or d/dx kinematics_jacobian * dx
PinnZoo.kinematics_velocity — Functionkinematics_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. This is J(q)E(q)v
PinnZoo.kinematics_velocity_jacobian — Functionkinematics_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$
PinnZoo.kinematics_force_jacobian — Functionkinematics_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
PinnZoo.kinematics_jacobianTvp — Functionkinematics_jacobianTvp(model::PinnZooModel, x::AbstractVector{Float64}, λ::AbstractVector{Float64})Returns the nv-dim jacobian-transpose vector product J(x)'*λ, where J(x) maps joint velocities into kinematics velocities
PinnZoo.kinematics_force_hvp — Functionkinematics_force_hvp_ptr(model::PinnZooModel, x::AbstractVector{Float64}, λ::AbstractVector{Float64})Return the jacobian of (d/dx J(x, λ))*mult w.r.t x and λ where J(x, λ) is kinematicsforcejacobian(model, x, λ)
PinnZoo.kinematics_force_hTvp — Functionkinematics_force_hTvp_ptr(model::PinnZooModel, x::AbstractVector{Float64}, λ::AbstractVector{Float64})Return the jacobian of J(x, λ)'*mult w.r.t x and λ where J(x, λ) is kinematicsforcejacobian(model, x, λ)
Utility Functions
PinnZoo.is_floating — Functionis_floating(model::PinnZooModel)Return whether the model includes a floating base joint
PinnZoo.zero_state — Functionzero_state(model::PinnZooModel)Return the neutral state of the model.
PinnZoo.randn_state — Functionrandn_state(model::PinnZooModel)Return a state vector where every coordinate is drawn from a normal distribution
PinnZoo.init_state — Functioninit_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
Quaternion functions
PinnZoo.quat_to_axis_angle — Functionquat_to_axis_angle(q; tol = 1e-12)Return the axis angle corresponding to the provided quaternion, using a regularized norm to avoid the singularity
PinnZoo.axis_angle_to_quat — Functionaxis_angle_to_quat(ω; tol = 1e-12)Return the quaternion corresponding to the provided axis angle
PinnZoo.quat_conjugate — Functionquat_conjugate(q)Return the conjugate of the given quaternion (negates the velocity part)
PinnZoo.skew — Functionskew(v)Return a matrix M such that $v \times x = Mx$ where $\times$ denotes the cross product
PinnZoo.L_mult — FunctionL_mult(q)Return a matrix representation of left quaternion multiplication, i.e. $q1 \cdot q2 = L(q1)q2$ where $\cdot$ is quaternion multiplication.
PinnZoo.R_mult — FunctionR_mult(q)Return a matrix representation of right quaternion multiplication, i.e. $q1 \cdot q2 = R(q2)q1$ where $\cdot$ is quaternion multiplication.
PinnZoo.attitude_jacobian — Functionattitude_jacobian(q)Return the attitude jacobian G define as $\dot{q} = 0.5G\omega$, mapping angular velocity into quaternion time derivative.
PinnZoo.quat_to_rot — Functionquat_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.
PinnZoo.rot_to_quat — Functionrot_to_quat(R)This method comes from Rotations.jl, and solves the system of equations in Section 3.1 of https://arxiv.org/pdf/math/0701759.pdf. This cheap method only works for matrices that are already orthonormal (orthogonal and unit length columns), but avoids some of the issues of other methods (division by zero, sqrt of negative number) when evaluated on non-orthonormal matrices.
Model specific functions
TODO: Generalize state error and related functions to all models
PinnZoo.B_func — FunctionB_func(quad::Quadruped)Return the input jacobian mapping motor torques into joint torques
B_func(quad::Pineapple)Return the input jacobian mapping motor torques into joint torques
PinnZoo.fix_joint_limits — Functionfix_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.
fix_joint_limits(model::Pineapple, 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.
PinnZoo.inverse_kinematics — Functioninverse_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.
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.
inverse_kinematics(model::Pineapple, x, wheel_locs)Return 8 by 2 matrix containing the 2 possible joint angle solutions that put the wheels at wheel_locs given the body pose in x. For Pineapple v1 (8-DOF), this includes 2 legs (right and left) with 3 joints each plus 2 wheel joints. Will be populated with NaN or Inf if solutions don't exist.
PinnZoo.nearest_ik — Functionnearest_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).
nearest_ik(model::Pineapple, q, wheel_locs; obey_limits = true)Return the ik solution for the given wheel_locs and body orientation in q that is closest to the current joint angles in q (defined by minimum norm per joint). For Pineapple robot with wheels.
Models
Pendulum
PinnZoo.Pendulum — TypePendulum() <: PinnZooModelReturn 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).
Double Pendulum
PinnZoo.DoublePendulum — TypeDoublePendulum() <: PinnZooModelReturn 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
PinnZoo.Cartpole — TypeCartpole() <: PinnZooModelReturn 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).
Double Cartpole
PinnZoo.DoubleCartpole — TypeDoubleCartpole() <: PinnZooModelReturn 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
PinnZoo.RigidBody — TypeRigidBody() <: PinnZooFloatingBaseModelReturn a RigidBody dynamics model, with m = 1, I = Diag([1.0; 1.0; 1.0]).
Quadrotor
PinnZoo.Quadrotor — TypeQuadrotor() <: PinnZooFloatingBaseModelReturn a Quadrotor dynamics model, with m = 1, I = Diag([0.0046; 0.0046; 0.008])
Unitree Go1
PinnZoo.Go1 — TypeGo1(; μ = 0.3) <: QuadrupedReturn the Unitree Go1 dynamics and kinematics model
PinnZoo.init_state — Methodinit_state(model::Go1)Return state vector with the robot on the ground in the starting pose
PinnZoo.inverse_kinematics — Methodinverse_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.
Unitree Go2
PinnZoo.Go2 — TypeGo2(; μ = 0.3) <: QuadrupedReturn the Unitree Go2 dynamics and kinematics model
PinnZoo.init_state — Methodinit_state(model::Go2)Return state vector with the robot on the ground in the starting pose
PinnZoo.inverse_kinematics — Methodinverse_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.
IHMC Nadia
PinnZoo.Nadia — TypeNadia(; 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).
Pineapple
PinnZoo.Pineapple — TypePineapple(; μ = 0.3, kinematics_ori::Symbol = :Quaternion) <: PinnZooFloatingBaseModelReturn the Pineapple wheeled biped dynamics and kinematics model
ForwardDiff.jl Compatability
We have made many of the PinnZoo functions compatible with ForwardDiff.jl to enable building more complex constraints and objectives in Julia on top of Pinocchio/PinnZoo without having to derive custom derivatives or using slow finite differences. To support this, sometimes certain calls need to be used to trigger the appropriate derivatives.
Functions that support ForwardDiff.jacobian
kinematicskinematics_rotationkinematics_velocitykinematics_jacobianTvp- use this for J(x)'λ (mapping kinematics forces into generalized forces)forward_dynamicsdynamicsinverse_dynamics
Functions that support ForwardDiff.hessian
You can use ForwardDiff.hessian on scalar functions (for example, Lagrangians in optimization) that use any of the following internally.
kinematicskinematics_rotationkinematics_jacobianTvp- use this for J(x)'λ (mapping kinematics forces into generalized forces)inverse_dynamics