Merge pull request #59 from JuliaComputing/quatup

Add Quaternion option to FreeMotion

docs/src/examples/free_motion.md

@@ -49,12 +49,12 @@ nothing # hide
 ```
 
 
-If we instead model a body suspended in springs without the presence of any joint, we need to give the body state variables. We do this by saying `isroot = true` when we create the body.
+If we instead model a body suspended in springs without the presence of any joint, we need to give the body state variables. We do this by saying `isroot = true` when we create the body, we also use quaternions to represent angles using `quat = true`.
 
 ```@example FREE_MOTION
 @named begin
  body = BodyShape(m = 1, I_11 = 1, I_22 = 1, I_33 = 1, r = [0.4, 0, 0],
- r_0 = [0.2, -0.5, 0.1], isroot = true)
+ r_0 = [0.2, -0.5, 0.1], isroot = true, quat=true)
  bar2 = FixedTranslation(r = [0.8, 0, 0])
  spring1 = Multibody.Spring(c = 20, s_unstretched = 0)
  spring2 = Multibody.Spring(c = 20, s_unstretched = 0)
@@ -78,7 +78,9 @@ ssys = structural_simplify(IRSystem(model))
 prob = ODEProblem(ssys, [
  collect(body.body.v_0 .=> 0);
  collect(body.body.w_a .=> 0);
-], (0, 3))
+ collect(body.body.Q .=> Multibody.to_q([0.0940609, 0.0789265, 0.0940609, 0.987965]));
+ collect(body.body.Q̂ .=> Multibody.to_q([0.0940609, 0.0789265, 0.0940609, 0.987965]));
+], (0, 4))
 
 sol = solve(prob, Rodas5P())
 @assert SciMLBase.successful_retcode(sol)

docs/src/examples/pendulum.md

@@ -155,7 +155,7 @@ defs = Dict(collect(multibody_spring.r_rel_0 .=> [0, 1, 0])...,
  collect(root_body.v_0 .=> [0, 0, 0])...,
 )
 
-prob = ODEProblem(ssys, defs, (0, 15))
+prob = ODEProblem(ssys, defs, (0, 10))
 
 sol = solve(prob, Rodas4(), u0 = prob.u0 .+ 1e-5 .* randn.())
 plot(sol, idxs = multibody_spring.r_rel_0[2], title="Mass-spring system without joint")
@@ -169,7 +169,7 @@ push!(connections, connect(multibody_spring.spring2d.flange_b, damper.flange_b))
 
 @named model = ODESystem(connections, t, systems = [world, multibody_spring, root_body, damper])
 ssys = structural_simplify(IRSystem(model))
-prob = ODEProblem(ssys, defs, (0, 15))
+prob = ODEProblem(ssys, defs, (0, 10))
 
 sol = solve(prob, Rodas4(), u0 = prob.u0 .+ 1e-5 .* randn.())
 plot(sol, idxs = multibody_spring.r_rel_0[2], title="Mass-spring-damper without joint")

docs/src/examples/three_springs.md

@@ -20,14 +20,13 @@ world = Multibody.world
 
 systems = @named begin
  body1 = Body(m = 0.8, I_11 = 0.1, I_22 = 0.1, I_33 = 0.1, r_0 = [0.5, -0.3, 0],
- r_cm = [0, -0.2, 0], isroot = false)
+ r_cm = [0, -0.2, 0], isroot=true, useQuaternions=true)
  bar1 = FixedTranslation(r = [0.3, 0, 0])
  bar2 = FixedTranslation(r = [0, 0, 0.3])
  spring1 = Multibody.Spring(c = 20, m = 0, s_unstretched = 0.1,
  r_rel_0 = [-0.2, -0.2, 0.2])
- spring2 = Multibody.Spring(c = 40, m = 0, s_unstretched = 0.1,
- fixed_rotation_at_frame_a = true, fixed_rotation_at_frame_b = true)
- spring3 = Multibody.Spring(c = 20, m = 0, s_unstretched = 0.1)
+ spring2 = Multibody.Spring(c = 40, m = 0, s_unstretched = 0.1)
+ spring3 = Multibody.Spring(c = 20, m = 0, s_unstretched = 0.1, fixedRotationAtFrame_b=true)
 end
 eqs = [connect(world.frame_b, bar1.frame_a)
  connect(world.frame_b, bar2.frame_a)
@@ -39,15 +38,13 @@ eqs = [connect(world.frame_b, bar1.frame_a)
 
 @named model = ODESystem(eqs, t, systems = [world; systems])
 ssys = structural_simplify(IRSystem(model))
-prob = ODEProblem(ssys, [
- D.(spring3.lineforce.r_rel_0) .=> 0;
- collect(body1.v_0) .=> 0;
-], (0, 10))
+prob = ODEProblem(ssys, [], (0, 10))
 
-sol = solve(prob, Rodas4(), u0=prob.u0 .+ 1e-1*randn(length(prob.u0)))
+sol = solve(prob, FBDF(), u0=prob.u0 .+ 1e-9*randn(length(prob.u0)))
 @assert SciMLBase.successful_retcode(sol)
 
 Plots.plot(sol, idxs = [body1.r_0...])
+Plots.plot(sol, idxs = ori(body1.frame_a).R[1])
 ```
 
 ## 3D animation

src/components.jl

@@ -222,14 +222,16 @@ Representing a body with 3 translational and 3 rotational degrees-of-freedom.
  phid0 = zeros(3),
  r_0 = 0,
  radius = 0.05,
+ v_0 = 0,
+ w_a = 0,
  air_resistance = 0.0,
  color = [1,0,0,1],
  quat=false,)
  @variables r_0(t)[1:3]=r_0 [
  state_priority = 2,
  description = "Position vector from origin of world frame to origin of frame_a",
  ]
- @variables v_0(t)[1:3] [guess = 0, 
+ @variables v_0(t)[1:3]=v_0 [guess = 0, 
  state_priority = 2,
  description = "Absolute velocity of frame_a, resolved in world frame (= D(r_0))",
  ]
@@ -238,7 +240,7 @@ Representing a body with 3 translational and 3 rotational degrees-of-freedom.
  description = "Absolute acceleration of frame_a resolved in world frame (= D(v_0))",
  ]
  @variables g_0(t)[1:3] [guess = 0, description = "gravity acceleration"]
- @variables w_a(t)[1:3] [guess = 0, 
+ @variables w_a(t)[1:3]=w_a [guess = 0, 
  state_priority = 2,
  description = "Absolute angular velocity of frame_a resolved in frame_a",
  ]
@@ -273,39 +275,25 @@ Representing a body with 3 translational and 3 rotational degrees-of-freedom.
  eqs = if isroot # isRoot
 
  if quat
- @named frame_a = Frame(varw = true)
- Ra = ori(frame_a, true)
- # @variables q(t)[1:4] = [0.0,0,0,1.0]
- # @variables qw(t)[1:3] = [0.0,0,0]
- # q = collect(q)
- # qw = collect(qw)
- # Q = Quaternion(q, qw)
- @named Q = NumQuaternion(varw=true) 
- ar = from_Q(Q, angular_velocity2(Q, D.(Q.Q)))
- Equation[
- 0 ~ orientation_constraint(Q)
- Ra ~ ar
- Ra.w .~ ar.w
- Q.w .~ ar.w
- collect(w_a .~ Ra.w)
- ]
+ @named frame_a = Frame(varw = false)
+ Ra = ori(frame_a, false)
+ nonunit_quaternion_equations(Ra, w_a);
  else
  @named frame_a = Frame(varw = true)
+ Ra = ori(frame_a, true)
  @variables phi(t)[1:3]=phi0 [state_priority = 10, description = "Euler angles"]
  @variables phid(t)[1:3]=phid0 [state_priority = 10]
  @variables phidd(t)[1:3]=zeros(3) [state_priority = 10]
  phi, phid, phidd = collect.((phi, phid, phidd))
  ar = axes_rotations([1, 2, 3], phi, phid)
-
- Ra = ori(frame_a, true)
-
  Equation[
- # 0 .~ orientation_constraint(Ra); 
  phid .~ D.(phi)
  phidd .~ D.(phid)
  Ra ~ ar
- Ra.w .~ ar.w
- collect(w_a .~ Ra.w)]
+ Ra.w .~ w_a
+ # w_a .~ angular_velocity2(ori(frame_a, false)) # This is required for FreeBody tests to pass, but the other one required for harmonic osciallator without joint to pass. FreeBody passes with quat=true so we use that instead
+ w_a .~ ar.w # This one for most systems
+ ]
  end
  else
  @named frame_a = Frame()
@@ -318,7 +306,6 @@ Representing a body with 3 translational and 3 rotational degrees-of-freedom.
  end
 
  eqs = [eqs;
- # collect(w_a .~ get_w(Ra));
  collect(r_0 .~ frame_a.r_0)
  collect(g_0 .~ gravity_acceleration(frame_a.r_0 .+ resolve1(Ra, r_cm)))
  collect(v_0 .~ D.(r_0))

src/forces.jl

@@ -140,9 +140,9 @@ function Force(; name, resolve_frame = :frame_b)
 end
 
 function LineForceBase(; name, length = 0, s_small = 1e-10, fixed_rotation_at_frame_a = false,
-  fixed_rotation_at_frame_b = false, r_rel_0 = 0, s0 = 0)
- @named frame_a = Frame()
- @named frame_b = Frame()
+ fixed_rotation_at_frame_b = false, r_rel_0 = 0, s0 = 0)
+ @named frame_a = Frame(varw = fixed_rotation_at_frame_a)
+ @named frame_b = Frame(varw = fixed_rotation_at_frame_b)
 
  @variables length(t) [
  description = "Distance between the origin of frame_a and the origin of frame_b",

src/joints.jl

@@ -177,7 +177,9 @@ Joint with 3 constraints that define that the origin of `frame_a` and the origin
  d = 0,
  phi_dd = 0,
  color = [1, 1, 0, 1],
- radius = 0.1)
+ radius = 0.1,
+ quat = false,
+ )
  @named begin
  ptf = PartialTwoFrames()
  R_rel = NumRotationMatrix()
@@ -209,19 +211,24 @@ Joint with 3 constraints that define that the origin of `frame_a` and the origin
  end
 
  if state
- @variables begin
- (phi(t)[1:3] = phi),
- [state_priority = 10, description = "3 angles to rotate frame_a into frame_b"]
- (phi_d(t)[1:3] = phi_d),
- [state_priority = 10, description = "3 angle derivatives"]
- (phi_dd(t)[1:3] = phi_dd),
- [state_priority = 10, description = "3 angle second derivatives"]
+ if quat
+ append!(eqs, nonunit_quaternion_equations(R_rel, w_rel))
+ # append!(eqs, collect(w_rel) .~ angularVelocity2(R_rel))
+ else
+ @variables begin
+ (phi(t)[1:3] = phi),
+ [state_priority = 10, description = "3 angles to rotate frame_a into frame_b"]
+ (phi_d(t)[1:3] = phi_d),
+ [state_priority = 10, description = "3 angle derivatives"]
+ (phi_dd(t)[1:3] = phi_dd),
+ [state_priority = 10, description = "3 angle second derivatives"]
+ end
+ append!(eqs,
+ [R_rel ~ axes_rotations(sequence, phi, phi_d)
+ collect(w_rel) .~ angular_velocity2(R_rel)
+ collect(phi_d .~ D.(phi))
+ collect(phi_dd .~ D.(phi_d))])
  end
- append!(eqs,
- [R_rel ~ axes_rotations(sequence, phi, phi_d)
- collect(w_rel) .~ angular_velocity2(R_rel)
- collect(phi_d .~ D.(phi))
- collect(phi_dd .~ D.(phi_d))])
  if isroot
  append!(eqs,
  [connect_orientation(ori(frame_b), absolute_rotation(frame_a, R_rel); iscut)
@@ -682,7 +689,9 @@ The relative position vector `r_rel_a` from the origin of `frame_a` to the origi
 - `a_rel_a`
 """
 @component function FreeMotion(; name, state = true, sequence = [1, 2, 3], isroot = true,
+ quat = false,
  w_rel_a_fixed = false, z_rel_a_fixed = false, phi = 0,
+ iscut = false,
  phi_d = 0,
  phi_dd = 0,
  w_rel_b = 0,
@@ -695,13 +704,13 @@ The relative position vector `r_rel_a` from the origin of `frame_a` to the origi
  end
  @variables begin
  (phi(t)[1:3] = phi),
- [state_priority = 10, description = "3 angles to rotate frame_a into frame_b"]
- (phi_d(t)[1:3] = phi_d), [state_priority = 10, description = "Derivatives of phi"]
+ [state_priority = 4, description = "3 angles to rotate frame_a into frame_b"]
+ (phi_d(t)[1:3] = phi_d), [state_priority = 4, description = "Derivatives of phi"]
  (phi_dd(t)[1:3] = phi_dd),
- [state_priority = 10, description = "Second derivatives of phi"]
+ [state_priority = 4, description = "Second derivatives of phi"]
  (w_rel_b(t)[1:3] = w_rel_b),
  [
- state_priority = 10,
+ state_priority = quat ? 4.0 : 1.0,
  description = "relative angular velocity of frame_b with respect to frame_a, resolved in frame_b",
  ]
  (r_rel_a(t)[1:3] = r_rel_a),
@@ -733,18 +742,24 @@ The relative position vector `r_rel_a` from the origin of `frame_a` to the origi
  if state
  if isroot
  append!(eqs,
- ori(frame_b) ~ absolute_rotation(frame_a, R_rel))
+ connect_orientation(ori(frame_b), absolute_rotation(frame_a, R_rel); iscut))
  else
  append!(eqs,
  [R_rel_inv ~ inverse_rotation(R_rel)
- ori(frame_a) ~ absolute_rotation(frame_b, R_rel_inv)])
+ connect_orientation(ori(frame_a), absolute_rotation(frame_b, R_rel_inv); iscut)])
  end
 
- append!(eqs,
- [phi_d .~ D.(phi)
- phi_dd .~ D.(phi_d)
- R_rel ~ axes_rotations(sequence, phi, phi_d)
- w_rel_b .~ angular_velocity2(R_rel)])
+ if quat
+ # @named Q = NumQuaternion() 
+ append!(eqs, nonunit_quaternion_equations(R_rel, w_rel_b))
+
+ else
+ append!(eqs,
+ [phi_d .~ D.(phi)
+ phi_dd .~ D.(phi_d)
+ R_rel ~ axes_rotations(sequence, phi, phi_d)
+ w_rel_b .~ angular_velocity2(R_rel)])
+ end
 
  else
  # Free motion joint does not have state

src/orientation.jl

@@ -273,13 +273,17 @@ orientation_constraint(q::Quaternion) = orientation_constraint(q.Q)
 # 2 * Q * q̇
 # end
 
+Base.:/(q::Rotations.Quaternions.Quaternion, x::Num) = Rotations.Quaternions.Quaternion(q.s / x, q.v1 / x, q.v2 / x, q.v3 / x)
 function from_Q(Q, w)
- R = 2*[(Q[1]*Q[1] + Q[4]*Q[4])-1 (Q[1]*Q[2] + Q[3]*Q[4]) (Q[1]*Q[3] - Q[2]*Q[4]);
- (Q[2]*Q[1] - Q[3]*Q[4]) (Q[2]*Q[2] + Q[4]*Q[4])-1 (Q[2]*Q[3] + Q[1]*Q[4]);
- (Q[3]*Q[1] + Q[2]*Q[4]) (Q[3]*Q[2] - Q[1]*Q[4]) (Q[3]*Q[3] + Q[4]*Q[4])-1]
+ Q2 = to_q(Q) # Due to different conventions
+ q = Rotations.QuatRotation(Q2)
+ R = RotMatrix(q)
  RotationMatrix(R, w)
 end
 
+to_q(Q) = SA[Q[4], Q[1], Q[2], Q[3]]
+to_mb(Q) = SA[Q[2], Q[3], Q[4], Q[1]]
+
 function angular_velocity1(Q, der_Q)
  2*([Q[4] -Q[3] Q[2] -Q[1]; Q[3] Q[4] -Q[1] -Q[2]; -Q[2] Q[1] Q[4] -Q[3]]*der_Q)
 end
@@ -475,4 +479,29 @@ function get_frame(sol, frame, t)
  R = get_rot(sol, frame, t)
  tr = get_trans(sol, frame, t)
  [R tr; 0 0 0 1]
+end
+
+function nonunit_quaternion_equations(R, w)
+ @variables Q(t)[1:4]=[0,0,0,1], [description="Unit quaternion with [i,j,k,w]"] # normalized
+ @variables Q̂(t)[1:4]=[0,0,0,1], [description="Non-unit quaternion with [i,j,k,w]"] # Non-normalized
+ @variables n(t)=1 c(t)=0
+ @parameters k = 0.1
+ Q̂ = collect(Q̂)
+ Ω = [0 -w[1] -w[2] -w[3]; w[1] 0 w[3] -w[2]; w[2] -w[3] 0 w[1]; w[3] w[2] -w[1] 0]
+ P = [
+ 0 0 0 1
+ 1 0 0 0
+ 0 1 0 0
+ 0 0 1 0
+ ]
+ Ω = P'Ω*P
+ [
+ n ~ Q̂'Q̂
+ # n ~ _norm(Q̂)^2
+ c ~ k * (1 - n)
+ D.(Q̂) .~ (Ω * Q̂) ./ 2 + c * Q̂
+ Q .~ Q̂ ./ sqrt(n)
+ R ~ from_Q(Q, w)
+ # R.w ~ w
+ ]
 end