From 8a2bb862a64cbfa00c0ff5e391e38db09d89ab8d Mon Sep 17 00:00:00 2001 From: Brian Jackson Date: Wed, 20 Mar 2024 16:37:04 -0400 Subject: [PATCH] Upgrade rotations (#14) * Upgrade Rotations * Update formatting --------- Co-authored-by: Brian Jackson --- Project.toml | 4 +- src/cartpole.jl | 12 +- src/copymodel.jl | 124 ++++++++--------- src/double_integrator.jl | 8 +- src/freebody.jl | 14 +- src/linearmodel.jl | 150 +++++++++++---------- src/quadrotor.jl | 80 +++++------ src/satellite.jl | 8 +- src/testrigidbody.jl | 10 +- src/yak_plane.jl | 282 +++++++++++++++++++-------------------- test/models_test.jl | 58 ++++---- 11 files changed, 377 insertions(+), 373 deletions(-) diff --git a/Project.toml b/Project.toml index 0006c2c..76ab48b 100644 --- a/Project.toml +++ b/Project.toml @@ -1,7 +1,7 @@ name = "RobotZoo" uuid = "74be38bb-dcc2-4b9e-baf3-d6373cd95f10" authors = ["Brian Jackson "] -version = "0.3.2" +version = "0.3.3" [deps] FiniteDiff = "6a86dc24-6348-571c-b903-95158fe2bd41" @@ -14,7 +14,7 @@ StaticArrays = "90137ffa-7385-5640-81b9-e52037218182" [compat] FiniteDiff = "2.10" ForwardDiff = "0.10" -RobotDynamics = "0.4.1" +RobotDynamics = "0.4.8" Rotations = "1.0" StaticArrays = "0.12, 1.0" julia = "1" diff --git a/src/cartpole.jl b/src/cartpole.jl index d0d4f1f..d1d86b4 100644 --- a/src/cartpole.jl +++ b/src/cartpole.jl @@ -13,7 +13,7 @@ with keyword arguments * `l` - length of the pendulum, in m (default = 0.5) * `g` - gravity, in m/s² (default = 9.81) """ -@autodiff struct Cartpole{T} <: ContinuousDynamics +@autodiff struct Cartpole{T} <: ContinuousDynamics mc::T mp::T l::T @@ -32,18 +32,18 @@ function dynamics(model::Cartpole, x, u) l = model.l # length of the pole in m g = model.g # gravity m/s^2 - q = x[ @SVector [1,2] ] - qd = x[ @SVector [3,4] ] + q = x[@SVector [1, 2]] + qd = x[@SVector [3, 4]] s = sin(q[2]) c = cos(q[2]) H = @SMatrix [mc+mp mp*l*c; mp*l*c mp*l^2] C = @SMatrix [0 -mp*qd[2]*l*s; 0 0] - G = @SVector [0, mp*g*l*s] + G = @SVector [0, mp * g * l * s] B = @SVector [1, 0] - qdd = -H\(C*qd + G - B*u[1]) + qdd = -H \ (C * qd + G - B * u[1]) return [qd; qdd] end @@ -55,4 +55,4 @@ RobotDynamics.state_dim(::Cartpole) = 4 RobotDynamics.control_dim(::Cartpole) = 1 # Base.position(::Cartpole, x::StaticVector) = SA[0,x[1],0] -# RobotDynamics.orientation(::Cartpole, x::StaticVector) = UnitQuaternion(expm(SA[1,0,0]*x[2])) +# RobotDynamics.orientation(::Cartpole, x::StaticVector) = QuatRotation(expm(SA[1,0,0]*x[2])) diff --git a/src/copymodel.jl b/src/copymodel.jl index b5ddb79..05b1426 100644 --- a/src/copymodel.jl +++ b/src/copymodel.jl @@ -7,20 +7,20 @@ struct CopyModel{K,N,M,L} <: ContinuousDynamics uinds::Vector{SVector{M,Int}} end -function CopyModel(model::L, K::Int) where L <: ContinuousDynamics - n,m = size(model) +function CopyModel(model::L, K::Int) where {L<:ContinuousDynamics} + n, m = size(model) xind = SVector{n}(1:n) uind = SVector{m}(1:m) - xinds = [xind .+ (i-1)*n for i = 1:K] - uinds = [uind .+ (i-1)*m for i = 1:K] + xinds = [xind .+ (i - 1) * n for i = 1:K] + uinds = [uind .+ (i - 1) * m for i = 1:K] ix = SVector{n}(1:n) iu = SVector{m}(n .+ (1:m)) return CopyModel{K,n,m,L}(model, ix, iu, xinds, uinds) end -Base.size(::CopyModel{K,N,M}) where {K,N,M} = N*K,M*K +Base.size(::CopyModel{K,N,M}) where {K,N,M} = N * K, M * K -function build_state(model::CopyModel{K}, xs...) where K +function build_state(model::CopyModel{K}, xs...) where {K} @assert length(xs) == K x = xs[1] for i = 2:K @@ -30,23 +30,23 @@ function build_state(model::CopyModel{K}, xs...) where K end function Base.zeros(model::CopyModel{K,N,M}) where {K,N,M} - x,u = zeros(model.model) - return repeat(x,K), repeat(u,K) + x, u = zeros(model.model) + return repeat(x, K), repeat(u, K) end -function Base.rand(model::CopyModel{K}) where K - x,u = rand(model.model) +function Base.rand(model::CopyModel{K}) where {K} + x, u = rand(model.model) for i = 2:K - x_,u_ = rand(model.model) + x_, u_ = rand(model.model) x = [x; x_] u = [u; u_] end - return x,u + return x, u end function states(model::CopyModel{K,N,M}, x) where {K,N,M} - reshape(x,N,K) + reshape(x, N, K) return [x[i] for i in model.xinds] end @@ -61,7 +61,7 @@ function states(model::CopyModel, Z::Traj, k::Int) end function controls(model::CopyModel{K,N,M}, u) where {K,N,M} - reshape(u,M,K) + reshape(u, M, K) end controls(model::CopyModel{K,L}, z::KnotPoint, k::Int) where {L,K} = @@ -80,11 +80,11 @@ function TrajectoryOptimization.get_trajectory(model::CopyModel, Z::Traj, k::Int map(Z) do z x = state(z)[xinds[k]] u = control(z)[uinds[k]] - KnotPoint(x,u,z.dt,z.t) + KnotPoint(x, u, z.dt, z.t) end end -function TrajectoryOptimization.dynamics(model::CopyModel{K}, x, u, t=0.0) where K +function TrajectoryOptimization.dynamics(model::CopyModel{K}, x, u, t=0.0) where {K} xinds = model.xinds uinds = model.uinds xdot = dynamics(model.model, x[xinds[1]], u[uinds[1]], t) @@ -96,10 +96,10 @@ function TrajectoryOptimization.dynamics(model::CopyModel{K}, x, u, t=0.0) where end function TrajectoryOptimization.discrete_dynamics(::Type{Q}, model::CopyModel{K}, - z::KnotPoint) where {K,Q <: TrajectoryOptimization.Implicit} + z::KnotPoint) where {K,Q<:TrajectoryOptimization.Implicit} xinds = model.xinds uinds = model.uinds - x,u = state(z), control(z) + x, u = state(z), control(z) xdot = discrete_dynamics(Q, model.model, x[xinds[1]], u[uinds[1]], z.t, z.dt) for i = 2:K xdot_ = discrete_dynamics(Q, model.model, x[xinds[i]], u[uinds[i]], z.t, z.dt) @@ -109,25 +109,25 @@ function TrajectoryOptimization.discrete_dynamics(::Type{Q}, model::CopyModel{K} end function TrajectoryOptimization.jacobian(model::CopyModel{K,N0,M0}, - z::KnotPoint{T,N,M,NM}) where {K,N0,M0,T,N,M,NM} - A0 = @SMatrix zeros(N0,N0) - B0 = @SMatrix zeros(N0,M0) + z::KnotPoint{T,N,M,NM}) where {K,N0,M0,T,N,M,NM} + A0 = @SMatrix zeros(N0, N0) + B0 = @SMatrix zeros(N0, M0) ix = model.ix iu = model.iu xinds = model.xinds uinds = model.uinds - x,u = state(z), control(z) + x, u = state(z), control(z) # Process fist model z_ = [x[xinds[1]]; u[uinds[1]]] z_ = TrajectoryOptimization.StaticKnotPoint(z_, ix, iu, z.dt, z.t) ∇f = jacobian(model.model, z_) - A,B = ∇f[ix,ix], ∇f[ix,iu] + A, B = ∇f[ix, ix], ∇f[ix, iu] # append zeros after for i = 2:K - A = [A A0] - B = [B B0] + A = [A A0] + B = [B B0] end # loop over the rest of the models, appending below the first @@ -135,7 +135,7 @@ function TrajectoryOptimization.jacobian(model::CopyModel{K,N0,M0}, z_ = [x[xinds[i]]; u[uinds[i]]] z_ = TrajectoryOptimization.StaticKnotPoint(z_, ix, iu, z.dt, z.t) ∇f = jacobian(model.model, z_) - A_,B_ = ∇f[ix,ix], ∇f[ix,iu] + A_, B_ = ∇f[ix, ix], ∇f[ix, iu] # prepend zeros for j = 1:i-1 @@ -195,33 +195,33 @@ end # end function discrete_jacobian!(::Type{Q}, ∇f, model::Dynamics.CopyModel{K,N0,M0}, - z::KnotPoint{T,N,M,NM}) where {T,N,M,NM,K,N0,M0,Q<:TrajectoryOptimization.Implicit} + z::KnotPoint{T,N,M,NM}) where {T,N,M,NM,K,N0,M0,Q<:TrajectoryOptimization.Implicit} xinds = model.xinds uinds = model.uinds - ix = model.ix - iu = model.iu - iz = [ix; iu; @SVector [N0+M0+1]] - x,u = state(z), control(z) - for i = 1:K - z_ = [x[xinds[i]]; u[uinds[i]]] - z_ = StaticKnotPoint(z_, ix, iu, z.dt, z.t) - ∇f_ = discrete_jacobian(Q, model.model, z_) - A,B,C = ∇f_[ix,ix], ∇f_[ix,iu], ∇f_[ix,N0+M0+1] + ix = model.ix + iu = model.iu + iz = [ix; iu; @SVector [N0 + M0 + 1]] + x, u = state(z), control(z) + for i = 1:K + z_ = [x[xinds[i]]; u[uinds[i]]] + z_ = StaticKnotPoint(z_, ix, iu, z.dt, z.t) + ∇f_ = discrete_jacobian(Q, model.model, z_) + A, B, C = ∇f_[ix, ix], ∇f_[ix, iu], ∇f_[ix, N0+M0+1] - ix_ = ix .+ (i-1)*N0 - iu_ = iu .+ (i-1)*M0 .+ N .- N0 - it_ = NM+1 - ∇f[ix_,ix_] .= A - ∇f[ix_,iu_] .= B - ∇f[ix_,it_] .= C - end + ix_ = ix .+ (i - 1) * N0 + iu_ = iu .+ (i - 1) * M0 .+ N .- N0 + it_ = NM + 1 + ∇f[ix_, ix_] .= A + ∇f[ix_, iu_] .= B + ∇f[ix_, it_] .= C + end end -function state_diff_size(model::CopyModel{K}) where K - return state_diff_size(model.model)*K +function state_diff_size(model::CopyModel{K}) where {K} + return state_diff_size(model.model) * K end -state_diff(model::CopyModel, x::SVector, x0::SVector) = x-x0 +state_diff(model::CopyModel, x::SVector, x0::SVector) = x - x0 function state_diff(model::CopyModel{K,N,M,L}, x::SVector, x0::SVector) where {K,N,M,L<:RigidBody} xinds = model.xinds dx = state_diff(model.model, x[xinds[1]], x0[xinds[1]]) @@ -234,7 +234,7 @@ end TrajectoryOptimization.state_diff_jacobian!(G, model::CopyModel, Z::Traj) = nothing function TrajectoryOptimization.state_diff_jacobian!(G, - model::CopyModel{<:Any,<:Any,<:Any,L}, Z::Traj) where {L<:RigidBody{R}} where R + model::CopyModel{<:Any,<:Any,<:Any,L}, Z::Traj) where {L<:RigidBody{R}} where {R} for k in eachindex(Z) G[k] .= state_diff_jacobian(model, state(Z[k])) end @@ -243,15 +243,15 @@ end state_diff_jacobian(::CopyModel, x::SVector) = I @generated function state_diff_jacobian(model::CopyModel{K,N,M,L}, - x::SVector) where {K,N,M,L<:RigidBody{R}} where R - if R <: UnitQuaternion - if R <: UnitQuaternion{T,IdentityMap} where T + x::SVector) where {K,N,M,L<:RigidBody{R}} where {R} + if R <: QuatRotation + if R <: QuatRotation{T,IdentityMap} where {T} return :(I) else - G0 = @SMatrix zeros(N,N-1) + G0 = @SMatrix zeros(N, N - 1) end else - G0 = @SMatrix zeros(N,N) + G0 = @SMatrix zeros(N, N) end quote xinds = model.xinds @@ -274,19 +274,19 @@ state_diff_jacobian(::CopyModel, x::SVector) = I end @generated function TrajectoryOptimization.∇²differential(model::CopyModel{K,N,M,L}, - x::SVector, dx::SVector) where {K,N,M,L<:RigidBody{R}} where R + x::SVector, dx::SVector) where {K,N,M,L<:RigidBody{R}} where {R} - if R <: UnitQuaternion - if R <: UnitQuaternion{T,IdentityMap} where T - return :(I*0) + if R <: QuatRotation + if R <: QuatRotation{T,IdentityMap} where {T} + return :(I * 0) else - G0 = @SMatrix zeros(N-1,N-1) + G0 = @SMatrix zeros(N - 1, N - 1) end else - G0 = @SMatrix zeros(N,N) + G0 = @SMatrix zeros(N, N) end quote - dx_ = reshape(dx,:,K) + dx_ = reshape(dx, :, K) ix_ = SVector{12}(1:12) xinds = model.xinds G = TrajectoryOptimization.∇²differential(model.model, x[xinds[1]], dx[ix_]) @@ -294,7 +294,7 @@ end G = [G $G0] end for i = 2:K - dx_ = dx[ix_ .+ (i-1)*12] + dx_ = dx[ix_.+(i-1)*12] G_ = TrajectoryOptimization.∇²differential(model.model, x[xinds[i]], dx_) for j = 1:i-1 G_ = [$G0 G_] @@ -309,11 +309,11 @@ end end function ∇²differential!(G, model::Dynamics.CopyModel{K,N,M,L}, - x::SVector, dx::Vector) where {K,N,M,L<:RigidBody{R}} where R + x::SVector, dx::Vector) where {K,N,M,L<:RigidBody{R}} where {R} ix = SVector{12}(1:12) xinds = model.xinds for i = 1:K - ix_ = ix .+ (i-1)*12 + ix_ = ix .+ (i - 1) * 12 dx_ = dx[ix_] G_ = ∇²differential(model.model, x[xinds[i]], dx_) G[ix_, ix_] .= G_ diff --git a/src/double_integrator.jl b/src/double_integrator.jl index 1363a7b..150ccab 100644 --- a/src/double_integrator.jl +++ b/src/double_integrator.jl @@ -19,7 +19,7 @@ end function DoubleIntegrator(D=1; gravity=zeros(D)) pos = SVector{D,Int}(1:D) vel = SVector{D,Int}(D .+ (1:D)) - DoubleIntegrator{2D,D}(pos,vel, gravity) + DoubleIntegrator{2D,D}(pos, vel, gravity) end RobotDynamics.state_dim(::DoubleIntegrator{N,M}) where {N,M} = N @@ -29,11 +29,11 @@ RobotDynamics.control_dim(::DoubleIntegrator{N,M}) where {N,M} = M @generated function dynamics(di::DoubleIntegrator{N,M}, x, u) where {N,M} vel = [:(x[$i]) for i = M+1:N] us = [:(u[$i] + di.gravity[$i]) for i = 1:M] - :(SVector{$N}($(vel...),$(us...))) + :(SVector{$N}($(vel...), $(us...))) end @generated function dynamics!(di::DoubleIntegrator{N,M}, xdot, x, u) where {N,M} vel = [:(xdot[$i-M] = x[$i]) for i = M+1:N] - us = [:(xdot[$(i+M)] = u[$i] + di.gravity[$i]) for i = 1:M] + us = [:(xdot[$(i + M)] = u[$i] + di.gravity[$i]) for i = 1:M] quote $(vel...) $(us...) @@ -42,4 +42,4 @@ end end Base.position(::DoubleIntegrator{<:Any,2}, x) = @SVector [x[1], x[2], 0] -orientation(::DoubleIntegrator, x) = UnitQuaternion(I) +orientation(::DoubleIntegrator, x) = QuatRotation(I) diff --git a/src/freebody.jl b/src/freebody.jl index b1737b2..4bba46e 100644 --- a/src/freebody.jl +++ b/src/freebody.jl @@ -6,7 +6,7 @@ at the center of mass, for a total of 6 controls. # Constructors FreeBody{R,T}(mass, J) - FreeBody(R=UnitQuaternion{Float64}; kwargs...) + FreeBody(R=QuatRotation{Float64}; kwargs...) where `R <: Rotation{3}` with keyword arguments * `mass` - mass of the body @@ -25,13 +25,13 @@ struct FreeBody{R,T} <: RigidBody{R} end RobotDynamics.control_dim(::FreeBody) = 6 -FreeBody(R=UnitQuaternion{Float64}; mass=1.0, J=@SVector ones(3)) = +FreeBody(R=QuatRotation{Float64}; mass=1.0, J=@SVector ones(3)) = FreeBody{R,promote_type(typeof(mass), eltype(J))}(mass, J) function forces(model::FreeBody, x::StaticVector, u::StaticVector) q = orientation(model, x) F = @SVector [u[1], u[2], u[3]] - q*F # world frame + q * F # world frame end function moments(model::FreeBody, x::StaticVector, u::StaticVector) @@ -43,8 +43,8 @@ function RobotDynamics.wrench_jacobian!(F, model::FreeBody, z) u = control(z) q = orientation(model, x) ir, iq, iv, iω, iu = RobotDynamics.gen_inds(model) - iF = SA[1,2,3] - iM = SA[4,5,6] + 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 @@ -54,8 +54,8 @@ function RobotDynamics.wrench_jacobian!(F, model::FreeBody, z) end function RobotDynamics.wrench_sparsity(::FreeBody) - SA[false true false false true; - false false false false true] + SA[false true false false true; + false false false false true] end inertia(model::FreeBody, x, u) = model.J diff --git a/src/linearmodel.jl b/src/linearmodel.jl index ef44cb3..8f7f874 100644 --- a/src/linearmodel.jl +++ b/src/linearmodel.jl @@ -11,29 +11,29 @@ struct LinearModel{M,V} <: RD.ContinuousDynamics A::M B::M d::V - function LinearModel(A::M, B::M, d::V) where {M<:AbstractMatrix, V<:AbstractVector} - if !(size(A,1) == size(B,1) == size(d,1)) + function LinearModel(A::M, B::M, d::V) where {M<:AbstractMatrix,V<:AbstractVector} + if !(size(A, 1) == size(B, 1) == size(d, 1)) throw(DimensionMismatch("All inputs must have the same number of rows.")) - elseif size(d,2) != 1 + elseif size(d, 2) != 1 throw(DimensionMismatch("d must have a single column.")) end - new{M,V}(A,B,d) + new{M,V}(A, B, d) end end function LinearModel(A::AbstractMatrix, B::AbstractMatrix) - d = similar(A, size(A,1)) + d = similar(A, size(A, 1)) d .= 0 LinearModel(A, B, d) end -function LinearModel(sig::RD.FunctionSignature, diffmethod::RD.DiffMethod, - model::RD.ContinuousDynamics, x, u, t=zero(eltype(x)); affine::Bool=true) - n,m = RD.dims(model) - J = zeros(n,n+m) +function LinearModel(sig::RD.FunctionSignature, diffmethod::RD.DiffMethod, + model::RD.ContinuousDynamics, x, u, t=zero(eltype(x)); affine::Bool=true) + n, m = RD.dims(model) + J = zeros(n, n + m) d = zeros(n) z = RD.KnotPoint{n,m}(x, u, t, NaN) - RD.jacobian!(sig, diffmethod, model, J, d, z) + RD.jacobian!(sig, diffmethod, model, J, d, z) if affine if sig == RD.InPlace() RD.dynamics!(model, d, x, u, t) @@ -43,14 +43,14 @@ function LinearModel(sig::RD.FunctionSignature, diffmethod::RD.DiffMethod, else d .= 0 end - A = J[:,1:n] - B = J[:,n+1:n+m] + A = J[:, 1:n] + B = J[:, n+1:n+m] LinearModel(A, B, d) end # Generates a random controllable linear system function Base.rand(::Type{LinearModel}, n::Integer, m::Integer) - A,B = RandomLinearModels.gencontrollable(n, m, :continuous) + A, B = RandomLinearModels.gencontrollable(n, m, :continuous) d = randn(n) LinearModel(A, B, d) end @@ -85,9 +85,9 @@ function dynamics!(model::LinearModel, xn, x, u) end function jacobian!(model::LinearModel, J, xn, x, u) - n,m = RD.dims(model) - J[:,1:n] .= model.A - J[:,n+1:n+m] .= model.B + n, m = RD.dims(model) + J[:, 1:n] .= model.A + J[:, n+1:n+m] .= model.B nothing end @@ -109,12 +109,12 @@ struct DiscreteLinearModel{M,V} <: RD.DiscreteDynamics CA::M CB::M Cd::V - function DiscreteLinearModel(dt, A::M, B::M, C::M, d::V) where {M<:AbstractMatrix, V<:AbstractVector} - if !(size(A,1) == size(B,1) == size(C,1) == size(d,1)) + function DiscreteLinearModel(dt, A::M, B::M, C::M, d::V) where {M<:AbstractMatrix,V<:AbstractVector} + if !(size(A, 1) == size(B, 1) == size(C, 1) == size(d, 1)) throw(DimensionMismatch("All inputs must have the same number of rows.")) - elseif size(A,2) != size(C,2) + elseif size(A, 2) != size(C, 2) throw(DimensionMismatch("A and C must have the same number of columns.")) - elseif size(d,2) != 1 + elseif size(d, 2) != 1 throw(DimensionMismatch("d must have a single column.")) end if C isa SMatrix @@ -122,30 +122,30 @@ struct DiscreteLinearModel{M,V} <: RD.DiscreteDynamics else F = factorize(C) end - CA = -(F\A) - CB = -(F\B) - Cd = -(F\d) - new{M,V}(A,B,C,d,dt, CA,CB,Cd) + CA = -(F \ A) + CB = -(F \ B) + Cd = -(F \ d) + new{M,V}(A, B, C, d, dt, CA, CB, Cd) end end -function DiscreteLinearModel(dt, A::AbstractMatrix, B::AbstractMatrix, - d::AbstractVector=(similar(A, size(A,1)) .= zero(eltype(A)))) +function DiscreteLinearModel(dt, A::AbstractMatrix, B::AbstractMatrix, + d::AbstractVector=(similar(A, size(A, 1)) .= zero(eltype(A)))) C = similar(A) * zero(eltype(A)) - for i = 1:size(C,1) - C[i,i] = -one(eltype(A)) + for i = 1:size(C, 1) + C[i, i] = -one(eltype(A)) end DiscreteLinearModel(dt, A, B, C, d) end -function DiscreteLinearModel(sig::RD.FunctionSignature, diffmethod::RD.DiffMethod, - model::RD.DiscretizedDynamics, - x, u, t, h; affine::Bool=true) - n,m = RD.dims(model) - J = zeros(n,n+m) +function DiscreteLinearModel(sig::RD.FunctionSignature, diffmethod::RD.DiffMethod, + model::RD.DiscretizedDynamics, + x, u, t, h; affine::Bool=true) + n, m = RD.dims(model) + J = zeros(n, n + m) d = zeros(n) - z = RD.KnotPoint{n,m}([x;u], t, h) - RD.jacobian!(sig, diffmethod, model, J, d, z) + z = RD.KnotPoint{n,m}([x; u], t, h) + RD.jacobian!(sig, diffmethod, model, J, d, z) if affine if sig == RD.InPlace() RD.discrete_dynamics!(model, d, x, u, t, h) @@ -155,22 +155,22 @@ function DiscreteLinearModel(sig::RD.FunctionSignature, diffmethod::RD.DiffMetho else d .= 0 end - A = J[:,1:n] - B = J[:,n+1:n+m] + A = J[:, 1:n] + B = J[:, n+1:n+m] DiscreteLinearModel(h, A, B, d) end -function DiscreteLinearModel(sig::RD.FunctionSignature, diffmethod::RD.DiffMethod, - model::RD.ImplicitDynamicsModel, - x, u, t, h; affine::Bool=true) - n,m = RD.dims(model) - J1 = zeros(n,n+m) - J2 = zeros(n,n+m) +function DiscreteLinearModel(sig::RD.FunctionSignature, diffmethod::RD.DiffMethod, + model::RD.ImplicitDynamicsModel, + x, u, t, h; affine::Bool=true) + n, m = RD.dims(model) + J1 = zeros(n, n + m) + J2 = zeros(n, n + m) d = zeros(n) - y1 = similar(d) - z1 = RD.StaticKnotPoint{Any,Any}(n,m, [x;u], t, h) - z2 = RD.StaticKnotPoint{Any,Any}(n,m, [x;u], t, h) - RD.dynamics_error_jacobian!(sig, diffmethod, model, J2, J1, d, y1, z2, z1) + y1 = similar(d) + z1 = RD.StaticKnotPoint{Any,Any}(n, m, [x; u], t, h) + z2 = RD.StaticKnotPoint{Any,Any}(n, m, [x; u], t, h) + RD.dynamics_error_jacobian!(sig, diffmethod, model, J2, J1, d, y1, z2, z1) if affine if sig == RD.InPlace() RD.dynamics_error!(model, d, y1, z2, z1) @@ -180,10 +180,10 @@ function DiscreteLinearModel(sig::RD.FunctionSignature, diffmethod::RD.DiffMetho else d .= 0 end - A = J1[:,1:n] - B = J1[:,n+1:n+m] - C = J2[:,1:n] - if norm(J2[:,n+1:n+m]) > 0 + A = J1[:, 1:n] + B = J1[:, n+1:n+m] + C = J2[:, 1:n] + if norm(J2[:, n+1:n+m]) > 0 error("Cannot form a DiscreteLinearModel from an implicit dynamics function dependent on u2.") end DiscreteLinearModel(h, A, B, C, d) @@ -194,12 +194,12 @@ function DiscreteLinearModel(model::LinearModel, h) if norm(getaffinecoeffs(model)) > 0 throw(ArgumentError("Cannot use exponential integrator for an integrator with an affine term.")) end - n,m = RD.dims(model) + n, m = RD.dims(model) A = getstatecoeffs(model) B = getcontrolcoeffs(model) - expAB = exp([A*h B*h; zeros(m,n+m)]) - A = expAB[1:n,1:n] - B = expAB[1:n,n+1:n+m] + expAB = exp([A*h B*h; zeros(m, n + m)]) + A = expAB[1:n, 1:n] + B = expAB[1:n, n+1:n+m] DiscreteLinearModel(h, A, B) end @@ -213,6 +213,10 @@ getcontrolcoeffs(model::DiscreteLinearModel) = model.B getnextstatecoeffs(model::DiscreteLinearModel) = model.C getaffinecoeffs(model::DiscreteLinearModel) = model.d +function Base.copy(model::DiscreteLinearModel) + DiscreteLinearModel(model.dt, model.A, model.B, model.C, model.d) +end + isstable(model::DiscreteLinearModel) = RandomLinearModels.isstabled(getstatecoeffs(model)) iscontrollable(model::DiscreteLinearModel) = RandomLinearModels.iscontrollable(model.CA, model.CB) @@ -232,30 +236,30 @@ function RD.discrete_dynamics!(model::DiscreteLinearModel, xn, x, u, t, h) checktimestep(model, h) xn .= model.Cd mul!(xn, model.CA, x, true, true) - mul!(xn, model.CB, u, true, true) - nothing + mul!(xn, model.CB, u, true, true) + nothing end function RD.jacobian!(model::DiscreteLinearModel, J, xn, x, u, t, h) - checktimestep(model, h) - n,m = RD.dims(model) - J[:,1:n] .= model.CA - J[:,n+1:n+m] .= model.CB + checktimestep(model, h) + n, m = RD.dims(model) + J[:, 1:n] .= model.CA + J[:, n+1:n+m] .= model.CB nothing end -function RD.dynamics_error(model::DiscreteLinearModel, - z2::RD.AbstractKnotPoint, z1::RD.AbstractKnotPoint) +function RD.dynamics_error(model::DiscreteLinearModel, + z2::RD.AbstractKnotPoint, z1::RD.AbstractKnotPoint) checktimestep(model, RD.timestep(z1)) - x1,u1 = RD.state(z1), RD.control(z1) + x1, u1 = RD.state(z1), RD.control(z1) x2 = RD.state(z1) model.A * x1 + model.B * u1 + model.C * x2 + model.d end -function RD.dynamics_error!(model::DiscreteLinearModel, y2, y1, - z2::RD.AbstractKnotPoint, z1::RD.AbstractKnotPoint) +function RD.dynamics_error!(model::DiscreteLinearModel, y2, y1, + z2::RD.AbstractKnotPoint, z1::RD.AbstractKnotPoint) checktimestep(model, RD.timestep(z1)) - x1,u1 = RD.state(z1), RD.control(z1) + x1, u1 = RD.state(z1), RD.control(z1) x2 = RD.state(z1) y2 .= model.d mul!(y2, model.A, x1, true, true) @@ -264,12 +268,12 @@ function RD.dynamics_error!(model::DiscreteLinearModel, y2, y1, nothing end -function RD.dynamics_error_jacobian!(model::DiscreteLinearModel, J2, J1, y2, y1, - z2::RD.AbstractKnotPoint, z1::RD.AbstractKnotPoint) +function RD.dynamics_error_jacobian!(model::DiscreteLinearModel, J2, J1, y2, y1, + z2::RD.AbstractKnotPoint, z1::RD.AbstractKnotPoint) checktimestep(model, RD.timestep(z1)) - n,m = RD.dims(model) - J2[:,1:n] .= model.C - J1[:,1:n] .= model.A - J1[:,n+1:n+m] .= model.B + n, m = RD.dims(model) + J2[:, 1:n] .= model.C + J1[:, 1:n] .= model.A + J1[:, n+1:n+m] .= model.B nothing end diff --git a/src/quadrotor.jl b/src/quadrotor.jl index 9ca6ec8..a104ab2 100644 --- a/src/quadrotor.jl +++ b/src/quadrotor.jl @@ -10,7 +10,7 @@ in the positive z direction. Quadrotor(; kwargs...) Quadrotor{R}(; kwargs...) -where `R <: Rotation{3}` and defaults to `UnitQuaternion{Float64}` if omitted. The keyword arguments are +where `R <: Rotation{3}` and defaults to `QuatRotation{Float64}` if omitted. The keyword arguments are * `mass` - mass of the quadrotor, in kg (default = 0.5) * `J` - inertia of the quadrotor, in kg⋅m² (default = `Diagonal([0.0023, 0.0023, 0.004])`) * `gravity` - gravity vector, in kg/m² (default = [0,0,-9.81]) @@ -32,25 +32,25 @@ end control_dim(::Quadrotor) = 4 function Quadrotor{R}(; - mass=0.5, - J=Diagonal(@SVector [0.0023, 0.0023, 0.004]), - gravity=SVector(0,0,-9.81), - motor_dist=0.1750, - kf=1.0, - km=0.0245, - bodyframe=false, - ned=false, - ) where R + mass=0.5, + J=Diagonal(@SVector [0.0023, 0.0023, 0.004]), + gravity=SVector(0, 0, -9.81), + motor_dist=0.1750, + kf=1.0, + km=0.0245, + bodyframe=false, + ned=false, +) where {R} @assert issymmetric(J) - Quadrotor{R}(mass,J,inv(J),gravity,motor_dist,kf,km,bodyframe,ned) + Quadrotor{R}(mass, J, inv(J), gravity, motor_dist, kf, km, bodyframe, ned) end -(::Type{Quadrotor})(;kwargs...) = Quadrotor{UnitQuaternion{Float64}}(;kwargs...) +(::Type{Quadrotor})(; kwargs...) = Quadrotor{QuatRotation{Float64}}(; kwargs...) @inline RobotDynamics.velocity_frame(model::Quadrotor) = model.bodyframe ? :body : :world function trim_controls(model::Quadrotor) - @SVector fill(-model.gravity[3]*model.mass/4.0, size(model)[2]) + @SVector fill(-model.gravity[3] * model.mass / 4.0, size(model)[2]) end function forces(model::Quadrotor, x, u) @@ -64,17 +64,17 @@ function forces(model::Quadrotor, x, u) w3 = u[3] w4 = u[4] - F1 = max(0,kf*w1); - F2 = max(0,kf*w2); - F3 = max(0,kf*w3); - F4 = max(0,kf*w4); - F = @SVector [0., 0., F1+F2+F3+F4] #total rotor force in body frame + F1 = max(0, kf * w1) + F2 = max(0, kf * w2) + F3 = max(0, kf * w3) + F4 = max(0, kf * w4) + F = @SVector [0.0, 0.0, F1 + F2 + F3 + F4] #total rotor force in body frame if model.ned - F = SA[0,0,-F[3]] + F = SA[0, 0, -F[3]] g = -g end - f = m*g + q*F # forces in world frame + f = m * g + q * F # forces in world frame return f end @@ -88,16 +88,16 @@ function moments(model::Quadrotor, x, u) w3 = u[3] w4 = u[4] - F1 = max(0,kf*w1); - F2 = max(0,kf*w2); - F3 = max(0,kf*w3); - F4 = max(0,kf*w4); + F1 = max(0, kf * w1) + F2 = max(0, kf * w2) + F3 = max(0, kf * w3) + F4 = max(0, kf * w4) - M1 = km*w1; - M2 = km*w2; - M3 = km*w3; - M4 = km*w4; - tau = @SVector [L*(F2-F4), L*(F3-F1), (M1-M2+M3-M4)] #total rotor torque in body frame + M1 = km * w1 + M2 = km * w2 + M3 = km * w3 + M4 = km * w4 + tau = @SVector [L * (F2 - F4), L * (F3 - F1), (M1 - M2 + M3 - M4)] #total rotor torque in body frame if model.ned tau = SA[tau[1], -tau[2], -tau[3]] end @@ -115,22 +115,22 @@ function wrenches(model::Quadrotor, x, u) # Calculate force and moments w = max.(u, 0) # keep forces positive - fM = forceMatrix(model)*w + fM = forceMatrix(model) * w f = fM[1] M = @SVector [fM[2], fM[3], fM[4]] - e3 = @SVector [0,0,1] - F = mass*g - q*(f*e3) - return F,M + e3 = @SVector [0, 0, 1] + F = mass * g - q * (f * e3) + return F, M end function forceMatrix(model::Quadrotor) kf, km = model.kf, model.km L = model.motor_dist @SMatrix [ - kf kf kf kf; - 0 L*kf 0 -L*kf; - -L*kf 0 L*kf 0; - km -km km -km; + kf kf kf kf; + 0 L*kf 0 -L*kf; + -L*kf 0 L*kf 0; + km -km km -km ] end @@ -139,8 +139,8 @@ RobotDynamics.inertia(model::Quadrotor) = model.J RobotDynamics.inertia_inv(model::Quadrotor) = model.Jinv RobotDynamics.mass(model::Quadrotor) = model.mass -function Base.zeros(model::Quadrotor{R}) where R +function Base.zeros(model::Quadrotor{R}) where {R} x = RobotDynamics.build_state(model, zero(RBState)) - u = @SVector fill(-model.mass*model.gravity[end]/4, 4) - return x,u + u = @SVector fill(-model.mass * model.gravity[end] / 4, 4) + return x, u end diff --git a/src/satellite.jl b/src/satellite.jl index 49f4d93..ae5b5f2 100644 --- a/src/satellite.jl +++ b/src/satellite.jl @@ -20,16 +20,16 @@ Satellite() = Satellite(Diagonal(@SVector ones(3))) RobotDynamics.control_dim(::Satellite) = 3 Base.position(::Satellite, x) = @SVector zeros(3) -orientation(::Satellite, x) = UnitQuaternion(x[4], x[5], x[6], x[7]) +orientation(::Satellite, x) = QuatRotation(x[4], x[5], x[6], x[7]) -RobotDynamics.LieState(::Satellite) = RobotDynamics.LieState(UnitQuaternion{Float64}, (3,0)) +RobotDynamics.LieState(::Satellite) = RobotDynamics.LieState(QuatRotation{Float64}, (3, 0)) function dynamics(model::Satellite, x::SVector, u::SVector) ω = @SVector [x[1], x[2], x[3]] q = normalize(@SVector [x[4], x[5], x[6], x[7]]) J = model.J - ωdot = J\(u - ω × (J*ω)) - qdot = 0.5*lmult(q)*hmat()*ω + ωdot = J \ (u - ω × (J * ω)) + qdot = 0.5 * lmult(q) * hmat() * ω return [ωdot; qdot] end diff --git a/src/testrigidbody.jl b/src/testrigidbody.jl index 3cdbd25..a562278 100644 --- a/src/testrigidbody.jl +++ b/src/testrigidbody.jl @@ -4,7 +4,7 @@ abstract type TestRigidBody{B,R} <: RigidBody{R} end # OVERRIDE ALL ROTATION STUFF ############################################################################################ -Dynamics.state_diff_size(model::TestRigidBody{false,<:UnitQuaternion}) = 13 +Dynamics.state_diff_size(model::TestRigidBody{false,<:QuatRotation}) = 13 Dynamics.state_diff_size(model::TestRigidBody{false}) = 12 function Dynamics.state_diff(model::TestRigidBody{false}, x::SVector, x0::SVector) @@ -16,8 +16,8 @@ function Dynamics.state_diff_jacobian(model::TestRigidBody{false}, x::SVector) end function ∇²differential(model::TestRigidBody{false}, - x::SVector, dx::SVector) - return I*0 + x::SVector, dx::SVector) + return I * 0 end function inverse_map_jacobian(model::TestRigidBody{false}, x::SVector) @@ -25,6 +25,6 @@ function inverse_map_jacobian(model::TestRigidBody{false}, x::SVector) end function TrajectoryOptimization.inverse_map_∇jacobian(model::TestRigidBody{false}, - x::SVector, b::SVector) - return I*0 + x::SVector, b::SVector) + return I * 0 end diff --git a/src/yak_plane.jl b/src/yak_plane.jl index e78949a..777d95c 100644 --- a/src/yak_plane.jl +++ b/src/yak_plane.jl @@ -12,96 +12,96 @@ Has 4 control inputs: throttle, aileron, elevator, rudder. All inputs nominally where `R <: Rotation{3}`. See the source code for the keyword parameters and their default values. """ @autodiff Base.@kwdef mutable struct YakPlane{R,T} <: RigidBody{R} - g::T = 9.81; #Gravitational acceleration (m/s^2) - rho::T = 1.2; #Air density at 20C (kg/m^3) - m::T = .075; #Mass of plane (kg) + g::T = 9.81 #Gravitational acceleration (m/s^2) + rho::T = 1.2 #Air density at 20C (kg/m^3) + m::T = 0.075 #Mass of plane (kg) - Jx::T = 4.8944e-04; #roll axis inertia (kg*m^2) - Jy::T = 6.3778e-04; #pitch axis inertia (kg*m^2) - Jz::T = 7.9509e-04; #yaw axis inertia (kg*m^2) + Jx::T = 4.8944e-04 #roll axis inertia (kg*m^2) + Jy::T = 6.3778e-04 #pitch axis inertia (kg*m^2) + Jz::T = 7.9509e-04 #yaw axis inertia (kg*m^2) - J::Diagonal{T,SVector{3,T}} = Diagonal(@SVector [Jx,Jy,Jz]); - Jinv::Diagonal{T,SVector{3,T}} = Diagonal(@SVector [1/Jx,1/Jy,1/Jz]); #Assuming products of inertia are small + J::Diagonal{T,SVector{3,T}} = Diagonal(@SVector [Jx, Jy, Jz]) + Jinv::Diagonal{T,SVector{3,T}} = Diagonal(@SVector [1 / Jx, 1 / Jy, 1 / Jz]) #Assuming products of inertia are small - Jm::T = .007*(.0075)^2 + .002*(.14)^2/12; #motor + prop inertia (kg*m^2) + Jm::T = 0.007 * (0.0075)^2 + 0.002 * (0.14)^2 / 12 #motor + prop inertia (kg*m^2) # All lifting surfaces are modeled as unsweapt tapered wings - b::T = 45/100; #wing span (m) - l_in::T = 6/100; #inboard wing length covered by propwash (m) - cr::T = 13.5/100; #root chord (m) - ct::T = 8/100; #tip chord (m) - cm::T = (ct + cr)/2; #mean wing chord (m) - S::T = b*cm; #planform area of wing (m^2) - S_in::T = 2*l_in*cr; - S_out::T = S-S_in; + b::T = 45 / 100 #wing span (m) + l_in::T = 6 / 100 #inboard wing length covered by propwash (m) + cr::T = 13.5 / 100 #root chord (m) + ct::T = 8 / 100 #tip chord (m) + cm::T = (ct + cr) / 2 #mean wing chord (m) + S::T = b * cm #planform area of wing (m^2) + S_in::T = 2 * l_in * cr + S_out::T = S - S_in #Ra::T = b^2/S; %wing aspect ratio (dimensionless) - Rt::T = ct/cr; #wing taper ratio (dimensionless) - r_ail::T = (b/6)*(1+2*Rt)/(1+Rt); #aileron moment arm (m) - - ep_ail::T = 0.63; #flap effectiveness (Phillips P.41) - trim_ail::T = 106; #control input for zero deflection - g_ail::T = (15*pi/180)/100; #maps control input to deflection angle - - b_elev::T = 16/100; #elevator span (m) - cr_elev::T = 6/100; #elevator root chord (m) - ct_elev::T = 4/100; #elevator tip chord (m) - cm_elev::T = (ct_elev + cr_elev)/2; #mean elevator chord (m) - S_elev::T = b_elev*cm_elev; #planform area of elevator (m^2) - Ra_elev::T = b_elev^2/S_elev; #wing aspect ratio (dimensionless) - r_elev::T = 22/100; #elevator moment arm (m) - - ep_elev::T = 0.88; #flap effectiveness (Phillips P.41) - trim_elev::T = 106; #control input for zero deflection - g_elev::T = (20*pi/180)/100; #maps control input to deflection angle - - b_rud::T = 10.5/100; #rudder span (m) - cr_rud::T = 7/100; #rudder root chord (m) - ct_rud::T = 3.5/100; #rudder tip chord (m) - cm_rud::T = (ct_rud + cr_rud)/2; #mean rudder chord (m) - S_rud::T = b_rud*cm_rud; #planform area of rudder (m^2) - Ra_rud::T = b_rud^2/S_rud; #wing aspect ratio (dimensionless) - r_rud::T = 24/100; #rudder moment arm (m) - z_rud::T = 2/100; #height of rudder center of pressure (m) - - ep_rud::T = 0.76; #flap effectiveness (Phillips P.41) - trim_rud::T = 106; #control input for zero deflection - g_rud::T = (35*pi/180)/100; #maps from control input to deflection angle - - trim_thr::T = 24; #control input for zero thrust (deadband) - g_thr::T = 0.006763; #maps control input to Newtons of thrust - g_mot::T = 3000*2*pi/60*7/255; #maps control input to motor rad/sec + Rt::T = ct / cr #wing taper ratio (dimensionless) + r_ail::T = (b / 6) * (1 + 2 * Rt) / (1 + Rt) #aileron moment arm (m) + + ep_ail::T = 0.63 #flap effectiveness (Phillips P.41) + trim_ail::T = 106 #control input for zero deflection + g_ail::T = (15 * pi / 180) / 100 #maps control input to deflection angle + + b_elev::T = 16 / 100 #elevator span (m) + cr_elev::T = 6 / 100 #elevator root chord (m) + ct_elev::T = 4 / 100 #elevator tip chord (m) + cm_elev::T = (ct_elev + cr_elev) / 2 #mean elevator chord (m) + S_elev::T = b_elev * cm_elev #planform area of elevator (m^2) + Ra_elev::T = b_elev^2 / S_elev #wing aspect ratio (dimensionless) + r_elev::T = 22 / 100 #elevator moment arm (m) + + ep_elev::T = 0.88 #flap effectiveness (Phillips P.41) + trim_elev::T = 106 #control input for zero deflection + g_elev::T = (20 * pi / 180) / 100 #maps control input to deflection angle + + b_rud::T = 10.5 / 100 #rudder span (m) + cr_rud::T = 7 / 100 #rudder root chord (m) + ct_rud::T = 3.5 / 100 #rudder tip chord (m) + cm_rud::T = (ct_rud + cr_rud) / 2 #mean rudder chord (m) + S_rud::T = b_rud * cm_rud #planform area of rudder (m^2) + Ra_rud::T = b_rud^2 / S_rud #wing aspect ratio (dimensionless) + r_rud::T = 24 / 100 #rudder moment arm (m) + z_rud::T = 2 / 100 #height of rudder center of pressure (m) + + ep_rud::T = 0.76 #flap effectiveness (Phillips P.41) + trim_rud::T = 106 #control input for zero deflection + g_rud::T = (35 * pi / 180) / 100 #maps from control input to deflection angle + + trim_thr::T = 24 #control input for zero thrust (deadband) + g_thr::T = 0.006763 #maps control input to Newtons of thrust + g_mot::T = 3000 * 2 * pi / 60 * 7 / 255 #maps control input to motor rad/sec end control_dim(::YakPlane) = 4 -(::Type{<:YakPlane})(::Type{Rot}=MRP{Float64}; kwargs...) where Rot = +(::Type{<:YakPlane})(::Type{Rot}=MRP{Float64}; kwargs...) where {Rot} = YakPlane{Rot,Float64}(; kwargs...) trim_controls(model::YakPlane) = @SVector [41.6666, 106, 74.6519, 106] -function RobotDynamics.dynamics(p::YakPlane{<:UnitQuaternion}, x, u, t=0) +function RobotDynamics.dynamics(p::YakPlane{<:QuatRotation}, x, u, t=0) if isempty(u) u = @SVector zeros(4) end - yak_dynamics(p, SVector{13}(x), SVector{4}(u), t) + yak_dynamics(p, SVector{13}(x), SVector{4}(u), t) end function RobotDynamics.dynamics(p::YakPlane, x, u, t=0) if isempty(u) u = @SVector zeros(4) end - yak_dynamics(p, SVector{12}(x), SVector{4}(u), t) + yak_dynamics(p, SVector{12}(x), SVector{4}(u), t) end function yak_dynamics(p::YakPlane, x::StaticVector, u::StaticVector, t=0) - r,q,v,w = RobotDynamics.parse_state(p, x) + r, q, v, w = RobotDynamics.parse_state(p, x) Q = SMatrix(q) # control input - thr = u[1]; #Throttle command (0-255 as sent to RC controller) - ail = u[2]; #Aileron command (0-255 as sent to RC controller) - elev = u[3]; #Elevator command (0-255 as sent to RC controller) - rud = u[4]; #Rudder command (0-255 as sent to RC controller) + thr = u[1] #Throttle command (0-255 as sent to RC controller) + ail = u[2] #Aileron command (0-255 as sent to RC controller) + elev = u[3] #Elevator command (0-255 as sent to RC controller) + rud = u[4] #Rudder command (0-255 as sent to RC controller) #Note that body coordinate frame is: # x: points forward out nose @@ -109,98 +109,98 @@ function yak_dynamics(p::YakPlane, x::StaticVector, u::StaticVector, t=0) # z: points down # ------- Input Checks -------- # - thr = clamp(thr, 0, 255) - ail = clamp(ail, 0, 255) + thr = clamp(thr, 0, 255) + ail = clamp(ail, 0, 255) elev = clamp(elev, 0, 255) - rud = clamp(rud, 0, 255) + rud = clamp(rud, 0, 255) # ---------- Map Control Inputs to Angles ---------- # - delta_ail = (ail-p.trim_ail)*p.g_ail; - delta_elev = (elev-p.trim_elev)*p.g_elev; - delta_rud = (rud-p.trim_rud)*p.g_rud; + delta_ail = (ail - p.trim_ail) * p.g_ail + delta_elev = (elev - p.trim_elev) * p.g_elev + delta_rud = (rud - p.trim_rud) * p.g_rud # ---------- Aerodynamic Forces (body frame) ---------- # - v_body = Q'*v; #body-frame velocity - v_rout = v_body + cross(w, @SVector [0, p.r_ail, 0]); - v_lout = v_body + cross(w, @SVector [0, -p.r_ail, 0]); - v_rin = v_body + cross(w, @SVector [0, p.l_in, 0]) + propwash(thr); - v_lin = v_body + cross(w, @SVector [0, -p.l_in, 0]) + propwash(thr); - v_elev = v_body + cross(w, @SVector [-p.r_elev, 0, 0]) + propwash(thr); - v_rud = v_body + cross(w, @SVector [-p.r_rud, 0, -p.z_rud]) + propwash(thr); + v_body = Q' * v #body-frame velocity + v_rout = v_body + cross(w, @SVector [0, p.r_ail, 0]) + v_lout = v_body + cross(w, @SVector [0, -p.r_ail, 0]) + v_rin = v_body + cross(w, @SVector [0, p.l_in, 0]) + propwash(thr) + v_lin = v_body + cross(w, @SVector [0, -p.l_in, 0]) + propwash(thr) + v_elev = v_body + cross(w, @SVector [-p.r_elev, 0, 0]) + propwash(thr) + v_rud = v_body + cross(w, @SVector [-p.r_rud, 0, -p.z_rud]) + propwash(thr) # --- Outboard Wing Sections --- # - a_rout = alpha(v_rout); - a_lout = alpha(v_lout); - a_eff_rout = a_rout + p.ep_ail*delta_ail; #effective angle of attack - a_eff_lout = a_lout - p.ep_ail*delta_ail; #effective angle of attack + a_rout = alpha(v_rout) + a_lout = alpha(v_lout) + a_eff_rout = a_rout + p.ep_ail * delta_ail #effective angle of attack + a_eff_lout = a_lout - p.ep_ail * delta_ail #effective angle of attack - F_rout = -p_dyn(p, v_rout)*.5*p.S_out* @SVector [Cd_wing(a_eff_rout), 0, Cl_wing(a_eff_rout)]; - F_lout = -p_dyn(p, v_lout)*.5*p.S_out* @SVector [Cd_wing(a_eff_lout), 0, Cl_wing(a_eff_lout)]; + F_rout = -p_dyn(p, v_rout) * 0.5 * p.S_out * @SVector [Cd_wing(a_eff_rout), 0, Cl_wing(a_eff_rout)] + F_lout = -p_dyn(p, v_lout) * 0.5 * p.S_out * @SVector [Cd_wing(a_eff_lout), 0, Cl_wing(a_eff_lout)] - F_rout = arotate(a_rout,F_rout); #rotate to body frame - F_lout = arotate(a_lout,F_lout); #rotate to body frame + F_rout = arotate(a_rout, F_rout) #rotate to body frame + F_lout = arotate(a_lout, F_lout) #rotate to body frame # --- Inboard Wing Sections (Includes Propwash) --- # - a_rin = alpha(v_rin); - a_lin = alpha(v_lin); - a_eff_rin = a_rin + p.ep_ail*delta_ail; #effective angle of attack - a_eff_lin = a_lin - p.ep_ail*delta_ail; #effective angle of attack + a_rin = alpha(v_rin) + a_lin = alpha(v_lin) + a_eff_rin = a_rin + p.ep_ail * delta_ail #effective angle of attack + a_eff_lin = a_lin - p.ep_ail * delta_ail #effective angle of attack - F_rin = -p_dyn(p, v_rin)*.5*p.S_in* @SVector [Cd_wing(a_eff_rin), 0, Cl_wing(a_eff_rin)]; - F_lin = -p_dyn(p, v_lin)*.5*p.S_in* @SVector [Cd_wing(a_eff_lin), 0, Cl_wing(a_eff_lin)]; + F_rin = -p_dyn(p, v_rin) * 0.5 * p.S_in * @SVector [Cd_wing(a_eff_rin), 0, Cl_wing(a_eff_rin)] + F_lin = -p_dyn(p, v_lin) * 0.5 * p.S_in * @SVector [Cd_wing(a_eff_lin), 0, Cl_wing(a_eff_lin)] - F_rin = arotate(a_rin,F_rin); #rotate to body frame - F_lin = arotate(a_lin,F_lin); #rotate to body frame + F_rin = arotate(a_rin, F_rin) #rotate to body frame + F_lin = arotate(a_lin, F_lin) #rotate to body frame # println("AoA: right: $(rad2deg(a_rout)), left: $(rad2deg(a_lout))") # --- Elevator --- # - a_elev = alpha(v_elev); - a_eff_elev = a_elev + p.ep_elev*delta_elev; #effective angle of attack + a_elev = alpha(v_elev) + a_eff_elev = a_elev + p.ep_elev * delta_elev #effective angle of attack - F_elev = -p_dyn(p, v_elev)*p.S_elev*@SVector [Cd_elev(p, a_eff_elev), 0, Cl_plate(a_eff_elev)]; + F_elev = -p_dyn(p, v_elev) * p.S_elev * @SVector [Cd_elev(p, a_eff_elev), 0, Cl_plate(a_eff_elev)] - F_elev = arotate(a_elev,F_elev); #rotate to body frame + F_elev = arotate(a_elev, F_elev) #rotate to body frame # --- Rudder --- # - a_rud = beta(v_rud); - a_eff_rud = a_rud - p.ep_rud*delta_rud; #effective angle of attack + a_rud = beta(v_rud) + a_eff_rud = a_rud - p.ep_rud * delta_rud #effective angle of attack - F_rud = -p_dyn(p, v_rud)*p.S_rud*@SVector [Cd_rud(p,a_eff_rud), Cl_plate(a_eff_rud), 0]; + F_rud = -p_dyn(p, v_rud) * p.S_rud * @SVector [Cd_rud(p, a_eff_rud), Cl_plate(a_eff_rud), 0] - F_rud = brotate(a_rud,F_rud); #rotate to body frame + F_rud = brotate(a_rud, F_rud) #rotate to body frame # --- Thrust --- # if thr > p.trim_thr - F_thr = @SVector [(thr-p.trim_thr)*p.g_thr, 0, 0]; - w_mot = @SVector [p.g_mot*thr, 0, 0]; + F_thr = @SVector [(thr - p.trim_thr) * p.g_thr, 0, 0] + w_mot = @SVector [p.g_mot * thr, 0, 0] else #deadband - F_thr = @SVector zeros(3); - w_mot = @SVector zeros(3); + F_thr = @SVector zeros(3) + w_mot = @SVector zeros(3) end # ---------- Aerodynamic Torques (body frame) ---------- # - T_rout = cross((@SVector [0, p.r_ail, 0]),F_rout); - T_lout = cross((@SVector [0, -p.r_ail, 0]),F_lout); + T_rout = cross((@SVector [0, p.r_ail, 0]), F_rout) + T_lout = cross((@SVector [0, -p.r_ail, 0]), F_lout) - T_rin = cross((@SVector [0, p.l_in, 0]),F_rin); - T_lin = cross((@SVector [0, -p.l_in, 0]),F_lin); + T_rin = cross((@SVector [0, p.l_in, 0]), F_rin) + T_lin = cross((@SVector [0, -p.l_in, 0]), F_lin) - T_elev = cross((@SVector [-p.r_elev, 0, 0]),F_elev); + T_elev = cross((@SVector [-p.r_elev, 0, 0]), F_elev) - T_rud = cross((@SVector [-p.r_rud, 0, -p.z_rud]),F_rud); + T_rud = cross((@SVector [-p.r_rud, 0, -p.z_rud]), F_rud) # ---------- Add Everything Together ---------- # # problems: F_lout, F_rin - F_aero = F_rout + F_lout + F_rin + F_lin + F_elev + F_rud + F_thr; - F = Q*F_aero - @SVector [0, 0, p.m*p.g]; + F_aero = F_rout + F_lout + F_rin + F_lin + F_elev + F_rud + F_thr + F = Q * F_aero - @SVector [0, 0, p.m * p.g] - T = T_rout + T_lout + T_rin + T_lin + T_elev + T_rud + cross((p.J*w + p.Jm*w_mot),w); + T = T_rout + T_lout + T_rin + T_lin + T_elev + T_rud + cross((p.J * w + p.Jm * w_mot), w) - rdot = v; + rdot = v qdot = Rotations.kinematics(q, w) - vdot = F/p.m - wdot = p.Jinv*T + vdot = F / p.m + wdot = p.Jinv * T # xdot = [v; # .25*((1-r'*r)*w - 2*cross(w,r) + 2*(w'*r)*r); @@ -210,8 +210,8 @@ function yak_dynamics(p::YakPlane, x::StaticVector, u::StaticVector, t=0) RobotDynamics.build_state(p, rdot, qdot, vdot, wdot) end -@generated function dynamics!(model::YakPlane{R}, xdot, x, u) where R - if R <: UnitQuaternion +@generated function dynamics!(model::YakPlane{R}, xdot, x, u) where {R} + if R <: QuatRotation Nx = 14 else Nx = 12 @@ -225,29 +225,29 @@ end end "Angle of attack" -@inline alpha(v) = atan(v[3],v[1]) +@inline alpha(v) = atan(v[3], v[1]) "Sideslip angle" -@inline beta(v) = atan(v[2],v[1]) +@inline beta(v) = atan(v[2], v[1]) "Rotate by angle of attack" -function arotate(a,r) - sa,ca = sincos(a) +function arotate(a, r) + sa, ca = sincos(a) R = @SMatrix [ ca 0 -sa; - 0 1 0; - sa 0 ca] - R*r + 0 1 0; + sa 0 ca] + R * r end "Rotate by sideslip angle" -function brotate(b,r) - sb,cb = sincos(b) +function brotate(b, r) + sb, cb = sincos(b) R = @SMatrix [ cb -sb 0; - sb cb 0; - 0 0 1] - R*r + sb cb 0; + 0 0 1] + R * r end """ Propwash wind speed (body frame) @@ -255,9 +255,9 @@ Fit from anemometer data taken at tail No significant different between wing/tail measurements """ function propwash(thr)::SVector{3} - trim_thr = 24; # control input for zero thrust (deadband) + trim_thr = 24 # control input for zero thrust (deadband) if thr > trim_thr - v = @SVector [5.568*thr^0.199 - 8.859, 0, 0]; + v = @SVector [5.568 * thr^0.199 - 8.859, 0, 0] else #deadband v = @SVector zeros(3) end @@ -265,7 +265,7 @@ end "Dynamic pressure" function p_dyn(p::YakPlane, v) - pd = 0.5*p.rho*(v'v) + pd = 0.5 * p.rho * (v'v) end """ Lift coefficient (alpha in radians) @@ -273,16 +273,16 @@ end Good to about ±20° """ function Cl_wing(a) - a = clamp(a, -0.5*pi, 0.5*pi) - cl = -27.52*a^3 - .6353*a^2 + 6.089*a; + a = clamp(a, -0.5 * pi, 0.5 * pi) + cl = -27.52 * a^3 - 0.6353 * a^2 + 6.089 * a end """ Lift coefficient (alpha in radians) Ideal flat plate model used for wing and rudder """ function Cl_plate(a) - a = clamp(a, -0.5*pi, 0.5*pi) - cl = 2pi*a + a = clamp(a, -0.5 * pi, 0.5 * pi) + cl = 2pi * a end """ Drag coefficient (alpha in radians) @@ -290,8 +290,8 @@ end Good to about ±20° """ function Cd_wing(a) - a = clamp(a, -0.5*pi, 0.5*pi) - cd = 2.08*a^2 + .0612; + a = clamp(a, -0.5 * pi, 0.5 * pi) + cd = 2.08 * a^2 + 0.0612 end """ Drag coefficient (alpha in radians) @@ -299,8 +299,8 @@ Induced drag for a tapered finite wing From phillips P.55 """ function Cd_elev(p::YakPlane, a) - a = clamp(a, -0.5*pi, 0.5*pi) - cd = (4*pi*a^2)/p.Ra_elev + a = clamp(a, -0.5 * pi, 0.5 * pi) + cd = (4 * pi * a^2) / p.Ra_elev end """ Drag coefficient (alpha in radians) @@ -308,6 +308,6 @@ Induced drag for a tapered finite wing From Phillips P.55 """ function Cd_rud(p::YakPlane, a) - a = clamp(a, -0.5*pi, 0.5*pi) - cd = (4*pi*a^2)/p.Ra_rud; + a = clamp(a, -0.5 * pi, 0.5 * pi) + cd = (4 * pi * a^2) / p.Ra_rud end diff --git a/test/models_test.jl b/test/models_test.jl index 6efe287..1d029a1 100644 --- a/test/models_test.jl +++ b/test/models_test.jl @@ -2,29 +2,29 @@ const RD = RobotDynamics using RobotDynamics: KnotPoint, dynamics, dynamics!, jacobian! using RobotDynamics: StaticReturn, InPlace, ForwardAD, FiniteDifference using Random -function test_model(model; evals=1, samples=1, tol=1e-6, customjacobian=false) +function test_model(model; evals=1, samples=1, tol=1e-6, customjacobian=false) println(typeof(model)) dmodel = RD.DiscretizedDynamics{RD.RK4}(model) - t,dt = 1.1,0.1 + t, dt = 1.1, 0.1 Random.seed!(1) x, u = rand(model) - n,m = RD.dims(model) + n, m = RD.dims(model) z = KnotPoint(x, u, t, dt) - ∇c1 = zeros(n,n+m) - ∇c2 = zeros(n,n+m) + ∇c1 = zeros(n, n + m) + ∇c2 = zeros(n, n + m) xdot = zeros(n) allocs = 0 - allocs += @ballocated RobotDynamics.dynamics($model, $x, $u) evals=evals samples=samples - allocs += @ballocated RobotDynamics.dynamics!($model, $xdot, $x, $u) evals=evals samples=samples + allocs += @ballocated RobotDynamics.dynamics($model, $x, $u) evals = evals samples = samples + allocs += @ballocated RobotDynamics.dynamics!($model, $xdot, $x, $u) evals = evals samples = samples @test xdot == RobotDynamics.dynamics(model, x, u) - @test allocs == 0 + @test allocs == 0 RobotDynamics.jacobian!(StaticReturn(), ForwardAD(), model, ∇c1, xdot, z) RobotDynamics.jacobian!(StaticReturn(), FiniteDifference(), model, ∇c2, xdot, z) - @test ∇c1 ≈ ∇c2 atol=tol + @test ∇c1 ≈ ∇c2 atol = tol RobotDynamics.jacobian!(InPlace(), ForwardAD(), model, ∇c2, xdot, z) @test ∇c1 ≈ ∇c2 RobotDynamics.jacobian!(InPlace(), FiniteDifference(), model, ∇c1, xdot, z) - @test ∇c1 ≈ ∇c2 atol=tol + @test ∇c1 ≈ ∇c2 atol = tol if customjacobian RobotDynamics.jacobian!(InPlace(), RD.UserDefined(), model, ∇c1, xdot, z) @test ∇c1 ≈ ∇c2 @@ -33,71 +33,71 @@ end # Acrobot acrobot = RobotZoo.Acrobot() -@test RD.dims(acrobot) == (4,1,4) +@test RD.dims(acrobot) == (4, 1, 4) test_model(acrobot) # Car car = RobotZoo.DubinsCar() -@test RD.dims(car) == (3,2,3) +@test RD.dims(car) == (3, 2, 3) test_model(car, customjacobian=true) # Bicycle Car bicycle = RobotZoo.BicycleModel() -@test RD.dims(bicycle) == (4,2,4) +@test RD.dims(bicycle) == (4, 2, 4) test_model(bicycle) bicycle = RobotZoo.BicycleModel(ref=:rear) -@test RD.dims(bicycle) == (4,2,4) +@test RD.dims(bicycle) == (4, 2, 4) test_model(bicycle) # Rover rover = RobotZoo.Rover() -@test RD.dims(rover) == (6,2,6) +@test RD.dims(rover) == (5, 2, 5) test_model(rover) # Planar Rocket rocket = RobotZoo.PlanarRocket() -@test RD.dims(rocket) == (8,2,8) +@test RD.dims(rocket) == (8, 2, 8) test_model(rocket) # Planar Quad quad = RobotZoo.PlanarQuadrotor() -@test RD.dims(quad) == (6,2,6) +@test RD.dims(quad) == (6, 2, 6) test_model(quad) # Cartpole cartpole = RobotZoo.Cartpole() -@test RD.dims(cartpole) == (4,1,4) +@test RD.dims(cartpole) == (4, 1, 4) test_model(cartpole) # Double Integrator dim = 3 di = RobotZoo.DoubleIntegrator(dim) -n,m = RD.dims(di) -@test (n,m) == (6,3) +n, m = RD.dims(di) +@test (n, m) == (6, 3) test_model(di) # Pendulum pend = RobotZoo.Pendulum() -RobotZoo.Pendulum{Float64}(1,1,1, 1,1,1) -@test RD.dims(pend) == (2,1,2) +RobotZoo.Pendulum{Float64}(1, 1, 1, 1, 1, 1) +@test RD.dims(pend) == (2, 1, 2) test_model(pend) # Quadrotor quad = RobotZoo.Quadrotor() -@test RD.dims(quad) == (13,4,13) -test_model(quad,tol=1e-4) +@test RD.dims(quad) == (13, 4, 13) +test_model(quad, tol=1e-4) # Yak Plane yak = RobotZoo.YakPlane(MRP{Float64}) -@test RD.dims(yak) == (12,4,12) -test_model(yak,tol=1e-4) +@test RD.dims(yak) == (12, 4, 12) +test_model(yak, tol=1e-4) # Test other functions dt = 0.1 car = RobotZoo.DubinsCar() -n,m = RD.dims(car) +n, m = RD.dims(car) @test zeros(car) == (zeros(n), zeros(m)) -@test zeros(Int,car)[1] isa SVector{n,Int} -@test fill(car,0.1) == (fill(0.1,n), fill(0.1,m)) +@test zeros(Int, car)[1] isa SVector{n,Int} +@test fill(car, 0.1) == (fill(0.1, n), fill(0.1, m)) # @test ones(Float32,car)[2] isa SVector{m,Float32}