Such as the Quadrotor. Ipynb example in TrajectoryOptimization.jl:
TrajectoryOptimization-Quadrotor.ipynb
When I build the trajectory optimization problem and provide a dynamically-feasible initialization:
prob = Problem(model, obj, x0, tf, xf=xf, constraints=conSet)
initial_controls!(prob, U_hover)
rollout!(prob);
I get the error:
UndefVarError: orientation not defined
Stacktrace:
[1] forces(model::Quadrotor{QuatRotation{Float64}}, x::SubArray{Float64, 1, Vector{Float64}, Tuple{UnitRange{Int64}}, true}, u::SubArray{Float64, 1, Vector{Float64}, Tuple{UnitRange{Int64}}, true})
@ Main d:\Document\Julia\Vscode\TrajectoryOptimization.jl-main\examples\jl_notebook_cell_df34fa98e69747e1a8f8a730347b8e2f_X11sZmlsZQ==.jl:2
[2] forces
@ C:\Users\xxx\.julia\packages\RobotDynamics\OMNjA\src\rigidbody.jl:250 [inlined]
[3] wrenches(model::Quadrotor{QuatRotation{Float64}}, x::SubArray{Float64, 1, Vector{Float64}, Tuple{UnitRange{Int64}}, true}, u::SubArray{Float64, 1, Vector{Float64}, Tuple{UnitRange{Int64}}, true}, t::Float64)
@ RobotDynamics C:\Users\xxx\.julia\packages\RobotDynamics\OMNjA\src\rigidbody.jl:245
[4] dynamics(model::Quadrotor{QuatRotation{Float64}}, x::SubArray{Float64, 1, Vector{Float64}, Tuple{UnitRange{Int64}}, true}, u::SubArray{Float64, 1, Vector{Float64}, Tuple{UnitRange{Int64}}, true}, t::Float64)
@ RobotDynamics C:\Users\xxx\.julia\packages\RobotDynamics\OMNjA\src\rigidbody.jl:217
[5] integrate(#unused#::RobotDynamics.RK4, model::Quadrotor{QuatRotation{Float64}}, x::SubArray{Float64, 1, Vector{Float64}, Tuple{UnitRange{Int64}}, true}, u::SubArray{Float64, 1, Vector{Float64}, Tuple{UnitRange{Int64}}, true}, t::Float64, h::Float64)
@ RobotDynamics C:\Users\xxx\.julia\packages\RobotDynamics\OMNjA\src\integration.jl:281
...
@ TrajectoryOptimization C:\Users\xxx\.julia\packages\TrajectoryOptimization\9vKEc\src\problem.jl:330
[12] top-level scope
@ d:\Document\Julia\Vscode\TrajectoryOptimization.jl-main\examples\jl_notebook_cell_df34fa98e69747e1a8f8a730347b8e2f_X26sZmlsZQ==.jl:6
Output is truncated. View as a scrollable element or open in a text editor. Adjust cell output settings...
Based on the error message, I looked at the force function.In this example, the force function is defined as follows:
function RobotDynamics.forces(model::Quadrotor, x, u)
q = orientation(model, x)
kf = model.kf
g = model.gravity
m = model.mass
# Extract motor speeds
w1 = u[1]
w2 = u[2]
w3 = u[3]
w4 = u[4]
# Calculate motor forces
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
m*g + q*F # forces in world frame
end
I guess the error may come from q = orientation(model, x). But how to fix it?
Here is the package I use:
using RobotDynamics, Rotations
using TrajectoryOptimization
using StaticArrays, LinearAlgebra