Rigid body implicit integration fails

[axla-io]

I'm trying to integrate a rigid body problem using the implicit midpoint method:

using RobotDynamics
using Rotations
using StaticArrays, LinearAlgebra

# Define the model struct to inherit from `RigidBody{R}`
struct Satellite{R,T} <: RobotDynamics.RigidBody{R}
    mass::T
    J::Diagonal{T,SVector{3,T}}
end
RobotDynamics.control_dim(::Satellite) = 6

# Define some simple "getter" methods that are required to evaluate the dynamics
RobotDynamics.mass(model::Satellite) = model.mass
RobotDynamics.inertia(model::Satellite) = model.J

# Define the 3D forces at the center of mass, in the world frame
function RobotDynamics.forces(model::Satellite, x::StaticVector, u::StaticVector)
    q = RobotDynamics.orientation(model, x)
    F = @SVector [u[1], u[2], u[3]]
    q*F  # world frame
end

# Define the 3D moments at the center of mass, in the body frame
function RobotDynamics.moments(model::Satellite, x::StaticVector, u::StaticVector)
    return @SVector [u[4], u[5], u[6]]  # body frame
end

# Build model
T = Float64
R = UnitQuaternion{T}
mass = 1.0
J = Diagonal(@SVector ones(3))
model = Satellite{R,T}(mass, J)

# Initialization
x,u = rand(model)
z = KnotPoint(x,u,0.1, 0.1)
∇f = RobotDynamics.DynamicsJacobian(model)

function RobotDynamics.wrench_jacobian!(F, model::Satellite, z)
    x = state(z)
    u = control(z)
    q = RobotDynamics.orientation(model, x)
    ir, iq, iv, iω, iu = RobotDynamics.gen_inds(model)
    iF = SA[1,2,3]
    iM = SA[4,5,6]
    F[iF, iq] .= Rotations.∇rotate(q, u[iF])
    F[iF, iu[iF]] .= RotMatrix(q)
    for i = 1:3
        F[iM[i], iu[i+3]] = 1
    end
    return F
end

function RobotDynamics.wrench_sparsity(::Satellite)
    SA[false true  false false true;
       false false false false true]
end

s_dim = RobotDynamics.state_dim(model)
c_dim = RobotDynamics.control_dim(model)

model_impmid = RobotDynamics.DiscretizedDynamics(model, RobotDynamics.ImplicitMidpoint(s_dim, c_dim));
RobotDynamics.discrete_dynamics(model_impmid, z)

I get errors on the last line relating to the jacobian! function
The line Jw = uview(get_data(F), 8:13, :) throws an error because UnsafeArrays is not a dependency.

The line wrench_jacobian!(Jw, model, z) throws an error because z is not defined. Assuming that z is a KnotPoint, we run into the issue that z is not passed to the function jacobian!

[axla-io]

@bjack205

[axla-io]

Might be good to mention that I'm on dev