Make backstepping position controller compatible with the new definit…

…ion of the multicopter state (R to q) (#28)

* wip

* Bump a new version

Project.toml

@@ -1,7 +1,7 @@
 name = "FSimZoo"
 uuid = "e2d5807d-1594-43f7-91a6-54c6eef5a9d2"
 authors = ["JinraeKim <kjl950403@gmail.com> and contributors"]
-version = "0.11.0"
+version = "0.11.1"
 
 [deps]
 ComponentArrays = "b0b7db55-cfe3-40fc-9ded-d10e2dbeff66"

src/environments/controllers/BacksteppingPositionController.jl

@@ -106,31 +106,31 @@ function Command(controller::BacksteppingPositionController)
  # u1
  g_vec = [0, 0, g]
  u1 = m*(ad - g_vec) + Kp*ep
- zB = R' * [0, 0, 1]
+ zB = R * [0, 0, 1]
  td = -Td * zB
  et = u1 - td
  ėp = Ap*ep + Bp*et
  u̇1 = m*ȧd + Kp*ėp
  T = Td # TODO: no lag
  # u2
- u2 = T_u_inv(T) * R * (2*Bp' * P * ep + u̇1 + Kt*et)
+ u2 = T_u_inv(T) * R' * (2*Bp' * P * ep + u̇1 + Kt*et)
  Ṫd = u2[end] # third element
  Ṫ = Ṫd # TODO: no lag
- żB = -R' * T_ω(1.0) * ω
+ żB = -R * T_ω(1.0) * ω
  ėt = u̇1 + u2[end]*zB + Td*żB
  ëp = Ap*ėp + Bp*ėt
  ü1 = m*äd + Kp*ëp
- Ṙ = -skew(ω) * R
+ Ṙ = -skew(ω) * R'
  u̇2 = (
- (T_u_inv_dot(T, Ṫ)*R + T_u_inv(T)*Ṙ) * (2*Bp'*P*ep + u̇1 + Kt*et)
- + T_u_inv(T) * R * (2*Bp'*P*ėp + ü1 + Kt*ėt)
+ (T_u_inv_dot(T, Ṫ)*R' + T_u_inv(T)*Ṙ) * (2*Bp'*P*ep + u̇1 + Kt*et)
+ + T_u_inv(T) * R' * (2*Bp'*P*ėp + ü1 + Kt*ėt)
  )
  ω̇d = [1 0 0;
  0 1 0;
  0 0 0] * u̇2
  ωd = [u2[1:2]..., 0]
  eω = ωd - ω
- Md = cross(ω, J*ω) + J*(T_ω(T)'*R*et + ω̇d + Kω*eω)
+ Md = cross(ω, J*ω) + J*(T_ω(T)'*R'*et + ω̇d + Kω*eω)
  νd = vcat(Td, Md)
  e = [ep;
  et;

src/environments/integrated_environments/backstepping_position_controller_static_allocator_multicopter.jl

@@ -28,9 +28,10 @@ function Dynamics!(env::BacksteppingPositionController_StaticAllocator_Multicopt
  (; controller, allocator, multicopter) = env
  (; m, J, g) = multicopter
  @Loggable function dynamics!(dx, x, params, t; pos_cmd=nothing)
- (; p, v, R, ω) = x.multicopter
+ (; p, v, q, ω) = x.multicopter
  (; ref_model, Td) = x.controller
  xd, vd, ad, ȧd, äd = ref_model.x_0, ref_model.x_1, ref_model.x_2, ref_model.x_3, ref_model.x_4
+ R = quat2dcm(q)
  νd, Ṫd, _... = Command(controller)(
  p, v, R, ω,
  xd, vd, ad, ȧd, äd, Td,