Skip to content

Commit 847cf2b

Browse files
committed
Experiment with a scalar formulation
```julia using Test using Multibody using ModelingToolkit import ModelingToolkitStandardLibrary.Mechanical.Rotational using OrdinaryDiffEq using LinearAlgebra using JuliaSimCompiler t = Multibody.t D = Differential(t) world = Multibody.world W(args...; kwargs...) = Multibody.world @mtkmodel TestUSR begin @components begin world = W() j1 = JointUSR(positive_branch=true, use_arrays=false) fixed = FixedTranslation(r=[1,0,0]) b1 = Body(isroot=false, neg_w=true) p1 = Prismatic(state_priority=100) end @equations begin connect(world.frame_b, j1.frame_a, fixed.frame_a) connect(fixed.frame_b, p1.frame_a) connect(p1.frame_b, j1.frame_b) connect(j1.frame_im, b1.frame_a) end end @nAmed model = TestUSR() model = complete(model) ss = structural_simplify(IRSystem(model)) prob = ODEProblem(ss, [model.b1.a_0[1]=>0.0, D(D(model.p1.s))=>0.0], (0.0, 1.0)) sol = solve(prob, FBDF(autodiff=true)) ``` works
1 parent d9ec5bf commit 847cf2b

File tree

1 file changed

+14
-13
lines changed

1 file changed

+14
-13
lines changed

src/fancy_joints.jl

Lines changed: 14 additions & 13 deletions
Original file line numberDiff line numberDiff line change
@@ -443,12 +443,7 @@ end
443443
0 .~ collect(frame_a.f .+ resolve1(Rrel, frame_b.f))
444444
0 .~ collect(frame_a.tau .+ resolve1(Rrel, frame_b.tau))
445445

446-
if use_arrays
447-
angle ~ compute_angle2(length_constraint, e_array, r_a_array, r_b_array, positive_branch)[1]
448-
# angle ~ Symbolics.term(compute_angle2, length_constraint, e_array, r_a_array, r_b_array, positive_branch, type=Real)
449-
else
450-
angle ~ compute_angle2(length_constraint, e, r_a, r_b, positive_branch)[1]
451-
end
446+
angle ~ _compute_angle2(length_constraint, e, r_a, r_b, positive_branch)
452447
]
453448

454449
sys = ODESystem(eqs, t; name=:nothing, systems)#, parameter_dependencies = [positive_branch => select_branch(length_constraint, e, phi_offset + phi_guess, r_a, r_b)]) # JuliaSimCompiler ignores parameter dependencies, the user has to provide it instead
@@ -739,27 +734,33 @@ function compute_angle(L::Real, e::AbstractVector, r_a::AbstractVector, r_b::Abs
739734
atan(ksin1, kcos1)
740735
end
741736

742-
function compute_angle2(L::Real, e::AbstractVector, r_a::AbstractVector, r_b::AbstractVector, positive_branch)
737+
function _compute_angle2(L::Real, e::AbstractVector, r_a::AbstractVector, r_b::AbstractVector, positive_branch)
738+
@show e,r_a
743739
e_r_a = e'r_a
744740
e_r_b = e'r_b
745741
A = -2*(r_b'r_a - e_r_b'e_r_a)
746742
B = 2*r_b'cross(e, r_a)
747743
C = r_a'r_a + r_b'r_b - L^2 - 2*e_r_b'e_r_a
748744
k1 = A^2 + B^2
749745
k1a = k1 - C^2
750-
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.")
746+
#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.")
751747
k1b = max(k1a, 1.0e-12)
752748
k2 = sqrt(k1b)
753749
kcos1 = -A*C + B*k2*ifelse(positive_branch == true, 1, -1)
754750
ksin1 = -B*C + A*k2*ifelse(positive_branch == true, -1, 1)
755-
[atan(ksin1, kcos1)]
751+
atan(ksin1, kcos1)
756752
end
757753

758-
@register_array_symbolic compute_angle2(L::Real, e::AbstractVector, r_a::AbstractVector, r_b::AbstractVector, positive_branch::Bool) begin
759-
size = (1, )
760-
eltype = Float64
754+
function compute_angle2(L::Real, e::AbstractVector, r_a::AbstractVector, r_b::AbstractVector, positive_branch::Bool)
755+
_compute_angle2(L, e, r_a, r_b, positive_branch)
761756
end
762757

758+
@register_symbolic compute_angle2(L::Real, e::AbstractVector, r_a::AbstractVector, r_b::AbstractVector, positive_branch::Bool)
759+
#@register_array_symbolic compute_angle2(L::Real, e::AbstractVector, r_a::AbstractVector, r_b::AbstractVector, positive_branch::Bool) begin
760+
# size = (1, )
761+
# eltype = Float64
762+
#end
763+
763764
# @register_symbolic compute_angle(L::Real, e::AbstractVector, r_a::AbstractVector, r_b::AbstractVector, positive_branch::Bool)::Real
764765

765766
# @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)
@@ -769,4 +770,4 @@ end
769770
# r_a = SA[r_a1, r_a2, r_a3]
770771
# r_b = SA[r_b1, r_b2, r_b3]
771772
# compute_angle(L, e, r_a, r_b, positive_branch)
772-
# end
773+
# end

0 commit comments

Comments
 (0)