Rebase: Mysterious negative sign required sometimes #105 (#200)

* Bugfix: Transpose rotation matrices to fix the direction of the rotation

* Get rid of neg_w

* Update unit tests

---------

Co-authored-by: ElMapacheDelOeste <[email protected]>

Author: baggepinnen

src/components.jl

@@ -376,7 +376,6 @@ This component has a single frame, `frame_a`. To represent bodies with more than
               isroot = false,
               state = false,
               sequence = [1,2,3],
-              neg_w = true,
               phi0 = zeros(3),
               phid0 = zeros(3),
               r_0 = state || isroot ? 0 : nothing,
@@ -456,7 +455,7 @@ This component has a single frame, `frame_a`. To represent bodies with more than
         if quat
             @named frame_a = Frame(varw=false)
             Ra = ori(frame_a, false)
-            qeeqs = nonunit_quaternion_equations(Ra, w_a; neg_w)
+            qeeqs = nonunit_quaternion_equations(Ra, w_a)
         else
             @named frame_a = Frame(varw=true)
             Ra = ori(frame_a, true)
@@ -469,13 +468,8 @@ This component has a single frame, `frame_a`. To represent bodies with more than
                     phid .~ D.(phi)
                     phidd .~ D.(phid)
                     Ra.w .~ ar.w
-                    if neg_w
-                        # w_a .~ -ar.w # This is required for FreeBody and ThreeSprings 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
-                        collect(w_a .~ -angular_velocity2(ar))
-                    else
-                        collect(w_a .~ (angular_velocity2(ar)))
+                    collect(w_a .~ (angular_velocity2(ar)))
                         # w_a .~ ar.w # This one for most systems
-                    end
                     Ra ~ ar
                     ]
         end
@@ -788,7 +782,7 @@ Rigid body with cylinder shape. The mass properties of the body (mass, center of
 - `a_0`: Absolute acceleration of `frame_a` resolved in world frame (= D(v_0))
 """
 @component function BodyCylinder(; name, r = [1, 0, 0], r_shape = [0, 0, 0], isroot = false,
-                                 state = false, quat = false, sequence = [1, 2, 3], neg_w = true,
+                                 state = false, quat = false, sequence = [1, 2, 3],
                                  diameter = 1, inner_diameter = 0, density = 7700, color = purple)
     pars = @parameters begin
         dir[1:3] = r - r_shape, [description = "Vector in length direction of cylinder, resolved in frame_a"]
@@ -815,7 +809,7 @@ Rigid body with cylinder shape. The mass properties of the body (mass, center of
         frame_a = Frame()
         frame_b = Frame()
         translation = FixedTranslation(r = r)
-        body = Body(; m, r_cm, I_11 = I[1,1], I_22 = I[2,2], I_33 = I[3,3], I_21 = I[2,1], I_31 = I[3,1], I_32 = I[3,2], state, quat, isroot, sequence, neg_w, sparse_I=true)
+        body = Body(; m, r_cm, I_11 = I[1,1], I_22 = I[2,2], I_33 = I[3,3], I_21 = I[2,1], I_31 = I[3,1], I_32 = I[3,2], state, quat, isroot, sequence, sparse_I=true)
     end
 
     vars = @variables begin

src/joints.jl

@@ -181,7 +181,6 @@ Joint with 3 constraints that define that the origin of `frame_a` and the origin
                    z_rel_a_fixed = false, sequence = [1, 2, 3], phi = 0,
                    phid = 0,
                    d = 0,
-                   neg_w = true,
                    phidd = 0,
                    color = [1, 1, 0, 1],
                    radius = 0.1,
@@ -224,7 +223,7 @@ Joint with 3 constraints that define that the origin of `frame_a` and the origin
 
     if state
         if quat
-            append!(eqs, nonunit_quaternion_equations(Rrel, w_rel; neg_w))
+            append!(eqs, nonunit_quaternion_equations(Rrel, w_rel))
             # append!(eqs, collect(w_rel) .~ angularVelocity2(Rrel))
         else
             @variables begin
@@ -485,7 +484,6 @@ The relative position vector `r_rel_a` from the origin of `frame_a` to the origi
                     state_priority = 4,
                     phid = 0,
                     phidd = nothing,
-                    neg_w = true,
                     w_rel_b = 0,
                     r_rel_a = 0,
                     v_rel_a = 0,
@@ -546,14 +544,14 @@ The relative position vector `r_rel_a` from the origin of `frame_a` to the origi
         end
 
         if quat
-            append!(eqs, nonunit_quaternion_equations(Rrel, w_rel_b; neg_w))
+            append!(eqs, nonunit_quaternion_equations(Rrel, w_rel_b))
 
         else
             append!(eqs,
                     [phid .~ D.(phi)
                     phidd .~ D.(phid)
                     Rrel ~ axes_rotations(sequence, phi, phid)
-                    w_rel_b .~ (neg_w ? -1 : 1) * angular_velocity2(Rrel)])
+                    w_rel_b .~ angular_velocity2(Rrel)])
         end
 
     else

src/orientation.jl

@@ -262,7 +262,7 @@ function from_Q(Q2, w; normalize=false)
         Q2 = Q2 / _norm(Q2)
     end
     q = Rotations.QuatRotation(Q2, false)
-    R = RotMatrix(q)
+    R = RotMatrix(q)'
     RotationMatrix(R, w)
 end
 
@@ -313,11 +313,11 @@ Returns a `RotationMatrix` object.
 """
 function axis_rotation(sequence, angle; name = :R)
     if sequence == 1
-        return RotationMatrix(Rotations.RotX(angle), zeros(3))
+        return RotationMatrix(Rotations.RotX(angle)', zeros(3))
     elseif sequence == 2
-        return RotationMatrix(Rotations.RotY(angle), zeros(3))
+        return RotationMatrix(Rotations.RotY(angle)', zeros(3))
     elseif sequence == 3
-        return RotationMatrix(Rotations.RotZ(angle), zeros(3))
+        return RotationMatrix(Rotations.RotZ(angle)', zeros(3))
     else
         error("Invalid sequence $sequence")
     end
@@ -436,7 +436,7 @@ function get_frame(sol, frame, t)
     [R tr; 0 0 0 1]
 end
 
-function nonunit_quaternion_equations(R, w; neg_w = true)
+function nonunit_quaternion_equations(R, w)
     @variables Q(t)[1:4]=[1,0,0,0], [state_priority=-1, description="Unit quaternion with [w,i,j,k]"] # normalized
     @variables Q̂(t)[1:4]=[1,0,0,0], [state_priority=1000, description="Non-unit quaternion with [w,i,j,k]"] # Non-normalized
     @variables Q̂d(t)[1:4]=[0,0,0,0], [state_priority=1000]
@@ -452,9 +452,6 @@ function nonunit_quaternion_equations(R, w; neg_w = true)
     # where angularVelocity2(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)
     # They also have w_a = angularVelocity2(frame_a.R) even for quaternions, so w_a = angularVelocity2(Q, der(Q)), this is their link between w_a and D(Q), while ours is D(Q̂) .~ (Ω * 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]
-    if neg_w
-        Ω = Ω'
-    end
 
     # QR = from_Q(Q, angular_velocity2(Q, D.(Q)))
     QR = from_Q(Q̂ ./ sqrt(n), w)

src/wheels.jl

@@ -117,7 +117,7 @@ this frame.
     @named frame_a = Frame(varw=true)
     Ra = ori(frame_a, true)
 
-    Rarot = axes_rotations(sequence, angles, -der_angles) # The - is the neg_w change
+    Rarot = axes_rotations(sequence, angles, der_angles)
 
     equations = if surface === nothing
         [ # Road description
@@ -400,7 +400,7 @@ plot!(
     @named frame_a = Frame(varw=state)
     Ra = ori(frame_a, state)
 
-    Rarot = axes_rotations(sequence, angles, -der_angles) # The - is the neg_w change
+    Rarot = axes_rotations(sequence, angles, der_angles)
 
     equations = if surface === nothing
         [ # Road description

test/runtests.jl

@@ -68,7 +68,7 @@ t = Multibody.t
 D = Differential(t)
 @testset "spring - harmonic oscillator" begin
 
-    @named body = Body(; m = 1, isroot = true, r_cm = [0, -1, 0], quat=true, neg_w=true) # This time the body isroot since there is no joint containing state
+    @named body = Body(; m = 1, isroot = true, r_cm = [0, -1, 0], quat=true) # This time the body isroot since there is no joint containing state
     @named spring = Multibody.Spring(c = 1)
 
     connections = [connect(world.frame_b, spring.frame_a)
@@ -989,7 +989,16 @@ prob = ODEProblem(ssys, [
     collect(body.w_a) .=> [1,1,1];
     collect(body.v_0) .=> [10,10,10]
 ], (0, 10))
-@time "Flexible rope pendulum" sol = solve(prob, Rodas4(autodiff=false); u0 = prob.u0 .+ 0.5);
+@time "Flexible rope pendulum" sol = solve(prob, Rodas4(autodiff=false); u0 = prob.u0 .+ [-0.5, -0.5, -0.5,
+                                                                                        -0.5, -0.5, -0.5,
+                                                                                        -0.5, -0.5, -0.5,
+                                                                                        0.5, 0.5, 0.5,
+                                                                                        -0.5, -0.5, -0.5,
+                                                                                        -0.5, -0.5, -0.5,
+                                                                                        -0.5, -0.5, -0.5,
+                                                                                        -0.5, -0.5, -0.5,
+                                                                                        0.5, 0.5, 0.5,
+                                                                                        -0.5, -0.5, -0.5]); 
 @test SciMLBase.successful_retcode(sol)
 if false
     import GLMakie
@@ -1008,7 +1017,7 @@ end
 
 systems = @named begin
     joint = Spherical(state=true, isroot=true, phi = [π/2, 0, 0], d = 0.3)
-    bar = FixedTranslation(r = [0, -1, 0])
+    bar = FixedTranslation(r = [0, 1, 0])
     body = Body(; m = 1, isroot = false)
 
 
@@ -1200,7 +1209,7 @@ world = Multibody.world
 
 @named joint = Multibody.Spherical(isroot=false, state=false, quat=false)
 @named rod = FixedTranslation(; r = [1, 0, 0])
-@named body = Body(; m = 1, isroot=true, quat=true, neg_w=true)
+@named body = Body(; m = 1, isroot=true, quat=true)
 
 connections = [connect(world.frame_b, joint.frame_a)
                connect(joint.frame_b, rod.frame_a)
@@ -1219,7 +1228,7 @@ sol1 = solve(prob, FBDF(), abstol=1e-8, reltol=1e-8)
 @test SciMLBase.successful_retcode(sol1)
 
 ## quat in joint
-@named joint = Multibody.Spherical(isroot=true, state=true, quat=true, neg_w=true)
+@named joint = Multibody.Spherical(isroot=true, state=true, quat=true)
 @named rod = FixedTranslation(; r = [1, 0, 0])
 @named body = Body(; m = 1, isroot=false, quat=false)
 
@@ -1240,7 +1249,7 @@ sol2 = solve(prob, FBDF(), abstol=1e-8, reltol=1e-8)
 @test SciMLBase.successful_retcode(sol2)
 
 ## euler
-@named joint = Multibody.Spherical(isroot=true, state=true, quat=false, neg_w=true)
+@named joint = Multibody.Spherical(isroot=true, state=true, quat=false)
 @named rod = FixedTranslation(; r = [1, 0, 0])
 @named body = Body(; m = 1, isroot=false, quat=false)
 

test/test_quaternions.jl

@@ -78,7 +78,7 @@ using OrdinaryDiffEq, Test
 
 t = Multibody.t
 @named joint = Multibody.FreeMotion(isroot = true, state=false)
-@named body = Body(; m = 1, r_cm = [0.0, 0, 0], isroot=true, quat=true, w_a=[1,0.5,0.2], neg_w=true)
+@named body = Body(; m = 1, r_cm = [0.0, 0, 0], isroot=true, quat=true, w_a=[1,0.5,0.2])
 
 # @named joint = Multibody.FreeMotion(isroot = true, state=true, quat=true)
 # @named body = Body(; m = 1, r_cm = [0.0, 0, 0], isroot=false, w_a=[1,1,1])
@@ -152,7 +152,7 @@ end
     t = Multibody.t
     world = Multibody.world
 
-    @named joint = Multibody.FreeMotion(isroot = true, state=true, quat=true, neg_w=true)
+    @named joint = Multibody.FreeMotion(isroot = true, state=true, quat=true)
     @named body = Body(; m = 1, r_cm = [0.0, 0, 0])
 
     connections = [connect(world.frame_b, joint.frame_a)
@@ -206,7 +206,7 @@ end
 
     @named joint = Multibody.Spherical(isroot=false, state=false, quat=false)
     @named rod = FixedTranslation(; r = [1, 0, 0])
-    @named body = Body(; m = 1, isroot=true, quat=true, air_resistance=0.0, neg_w=true)
+    @named body = Body(; m = 1, isroot=true, quat=true, air_resistance=0.0)
 
     # @named joint = Multibody.Spherical(isroot=true, state=true, quat=true)
     # @named body = Body(; m = 1, r_cm = [1.0, 0, 0], isroot=false)
@@ -275,7 +275,7 @@ using Multibody.Rotations: params
 
     @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], r_cm = [0.2, 0, 0], isroot = true, quat=true, neg_w=true)
+                        r_0 = [0.2, -0.5, 0.1], r_cm = [0.2, 0, 0], 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)

test/test_wheels.jl

@@ -13,7 +13,7 @@ using LinearAlgebra
                             I_long = 0.12,
                             x0 = 0.2,
                             z0 = 0.2,
-                            der_angles = [0, 5, 1])
+                            der_angles = [0, -5, -1])
     end
 
     pars = @parameters begin
@@ -95,7 +95,7 @@ defs = Dict([
     worldwheel.wheel.body.r_0[1] => 0.2;
     worldwheel.wheel.body.r_0[2] => 0.3;
     worldwheel.wheel.body.r_0[3] => 0.2;
-    collect(worldwheel.wheel.wheeljoint.der_angles) .=> [0, 5, 1];
+    collect(worldwheel.wheel.wheeljoint.der_angles) .=> [0, -5, -1];
     # collect(D.(cwheel.wheel.angles)) .=> [0, 5, 1]
 ])
 
@@ -152,7 +152,7 @@ import ModelingToolkitStandardLibrary.Blocks
                             x0 = 1,
                             z0 = 1,
                             iscut=true,
-                            der_angles = [0, 5, 0])
+                            der_angles = [0, -5, 0])
     end
 
     pars = @parameters begin