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 lineJw = uview(get_data(F), 8:13, :) throws an error because UnsafeArrays is not a dependency.
The linewrench_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!
I'm trying to integrate a rigid body problem using the implicit midpoint method:
I get errors on the last line relating to the
jacobian!
function The lineJw = uview(get_data(F), 8:13, :)
throws an error becauseUnsafeArrays
is not a dependency.The line
wrench_jacobian!(Jw, model, z)
throws an error becausez
is not defined. Assuming thatz
is aKnotPoint
, we run into the issue thatz
is not passed to the functionjacobian!