add broken test

Author: baggepinnen

src/fancy_joints.jl

@@ -447,7 +447,8 @@ end
             angle ~ compute_angle2(length_constraint, e_array, r_a_array, r_b_array, positive_branch)[1]
             # angle ~ Symbolics.term(compute_angle2, length_constraint, e_array, r_a_array, r_b_array, positive_branch, type=Real)
         else
-            angle ~ compute_angle2(length_constraint, e, r_a, r_b, positive_branch)[1]
+            # angle ~ Symbolics.term(compute_angle, length_constraint, e..., r_a..., r_b..., positive_branch, type=Real)
+            angle ~ compute_angle(length_constraint, e, r_a, r_b, positive_branch)
         end
     ]
 
@@ -721,7 +722,7 @@ end
     eltype = Bool
 end
 
-function compute_angle(L::Real, e::AbstractVector, r_a::AbstractVector, r_b::AbstractVector, positive_branch)
+function compute_angle(L::Real, e, r_a, r_b, positive_branch)
     e_r_a = e'r_a
     e_r_b = e'r_b
     A = -2*(r_b'r_a - e_r_b'e_r_a)
@@ -730,7 +731,7 @@ function compute_angle(L::Real, e::AbstractVector, r_a::AbstractVector, r_b::Abs
     k1 = A^2 + B^2
     k1a = k1 - C^2
     if k1a isa AbstractFloat
-        k1a > 1e-10 || error("Singular position of loop (either no or two analytic solutions; the mechanism has lost one-degree-of freedom in this position). Try to use another joint-assembly component. In most cases it is best to let joints outside of the JointXXX component be revolute and _not_ prismatic joints. If this also leads to singular positions, it could be that this kinematic loop cannot be solved analytically. In this case you have to build up the loop with basic joints (_no_ aggregation JointXXX components) and rely on dynamic state selection, i.e., during simulation the states will be dynamically selected in such a way that in no position a degree of freedom is lost.")
+        # k1a > 1e-10 || error("Singular position of loop (either no or two analytic solutions; the mechanism has lost one-degree-of freedom in this position). Try to use another joint-assembly component. In most cases it is best to let joints outside of the JointXXX component be revolute and _not_ prismatic joints. If this also leads to singular positions, it could be that this kinematic loop cannot be solved analytically. In this case you have to build up the loop with basic joints (_no_ aggregation JointXXX components) and rely on dynamic state selection, i.e., during simulation the states will be dynamically selected in such a way that in no position a degree of freedom is lost.")
     end
     k1b = max(k1a, 1.0e-12)
     k2 = sqrt(k1b)
@@ -747,7 +748,7 @@ function compute_angle2(L::Real, e::AbstractVector, r_a::AbstractVector, r_b::Ab
     C = r_a'r_a + r_b'r_b - L^2 - 2*e_r_b'e_r_a
     k1 = A^2 + B^2
     k1a = k1 - C^2
-    k1a > 1e-10 || error("Singular position of loop (either no or two analytic solutions; the mechanism has lost one-degree-of freedom in this position). Try to use another joint-assembly component. In most cases it is best to let joints outside of the JointXXX component be revolute and _not_ prismatic joints. If this also leads to singular positions, it could be that this kinematic loop cannot be solved analytically. In this case you have to build up the loop with basic joints (_no_ aggregation JointXXX components) and rely on dynamic state selection, i.e., during simulation the states will be dynamically selected in such a way that in no position a degree of freedom is lost.")
+    # k1a > 1e-10 || error("Singular position of loop (either no or two analytic solutions; the mechanism has lost one-degree-of freedom in this position). Try to use another joint-assembly component. In most cases it is best to let joints outside of the JointXXX component be revolute and _not_ prismatic joints. If this also leads to singular positions, it could be that this kinematic loop cannot be solved analytically. In this case you have to build up the loop with basic joints (_no_ aggregation JointXXX components) and rely on dynamic state selection, i.e., during simulation the states will be dynamically selected in such a way that in no position a degree of freedom is lost.")
     k1b = max(k1a, 1.0e-12)
     k2 = sqrt(k1b)
     kcos1 = -A*C + B*k2*ifelse(positive_branch == true, 1, -1)
@@ -764,9 +765,9 @@ end
 
 # @register_symbolic compute_angle(L::Num, e1::Num, e1::Num, e2::Num, r_a1::Num, r_a2::Num, r_a3::Num, r_b1::Num, r_b2::Num, r_b3::Num, positive_branch)
 
-# function compute_angle(L::Real, e1::Real, e2::Real, e3::Real, r_a1::Real, r_a2::Real, r_a3::Real, r_b1::Real, r_b2::Real, r_b3::Real, positive_branch)
-#     e = SA[e1, e2, e3]
-#     r_a = SA[r_a1, r_a2, r_a3]
-#     r_b = SA[r_b1, r_b2, r_b3]
-#     compute_angle(L, e, r_a, r_b, positive_branch)
-# end
+function compute_angle(L::Real, e1::Real, e2::Real, e3::Real, r_a1::Real, r_a2::Real, r_a3::Real, r_b1::Real, r_b2::Real, r_b3::Real, positive_branch)
+    e = SA[e1, e2, e3]
+    r_a = SA[r_a1, r_a2, r_a3]
+    r_b = SA[r_b1, r_b2, r_b3]
+    compute_angle(L, e, r_a, r_b, positive_branch)
+end

src/joints.jl

@@ -616,7 +616,7 @@ If a planar loop is present, e.g., consisting of 4 revolute joints where the joi
 
     Rrel0 = planar_rotation(n, 0, 0)
     varw = false
-    @named Rrel = NumRotationMatrix(; R = Rrel0.R, w = Rrel0.w, varw)
+    @named Rrel = NumRotationMatrix(; R = Rrel0.R, w = Rrel0.w, varw, state_priority = -1)
 
     n = collect(n)
     ey_a = collect(ey_a)

test/test_JointUSR_RRR.jl

@@ -32,9 +32,11 @@ end
 @named model = TestUSR()
 model = complete(model)
 ssys = structural_simplify(IRSystem(model))
-prob = ODEProblem(ssys, [], (0.0, 1.0))
-sol = solve(prob, FBDF(autodiff=true))
+##
 
+prob = ODEProblem(ssys, [model.b1.a_0[1]=>0.0, D(D(model.p1.s))=>0.0], (0.0, 1.0))
+sol = solve(prob, FBDF(autodiff=true))
+@test_broken !all(iszero, sol(1.0))
 
 # NOTE: I was working on trying to register the compute_angle function so that there are no symbolic arguments left in the generated code