Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Mysterious negative sign required sometimes #105 #188

Open
wants to merge 4 commits into
base: main
Choose a base branch
from
Open

Mysterious negative sign required sometimes #105 #188

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
7f155c4
Resolve JuliaSimCompiler version
ElMapacheDelOeste Feb 27, 2025
566e33d
Bugfix: Transpose rotation matrices to fix the direction of the rotation
ElMapacheDelOeste Feb 27, 2025
fb9791b
Get rid of neg_w
ElMapacheDelOeste Feb 27, 2025
c24d9a5
Update unit tests
ElMapacheDelOeste Feb 27, 2025
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 1 addition & 1 deletion Project.toml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -34,7 +34,7 @@ DataInterpolations = "5, 6"
FileIO = "1"
Graphs = "1.0"
JuliaFormatter = "1.0"
JuliaSimCompiler = "0.1.12"
JuliaSimCompiler = "0.1.26"
LightXML = "0.9"
LinearAlgebra = "1.6"
MeshIO = "0.4"
Expand Down
13 changes: 3 additions & 10 deletions src/components.jl
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -374,7 +374,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 = 0,
Expand Down Expand Up @@ -454,7 +453,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)
Expand All @@ -467,13 +466,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
Expand Down Expand Up @@ -793,7 +787,6 @@ Rigid body with cylinder shape. The mass properties of the body (mass, center of
state = false
quat = false
sequence = [1,2,3]
neg_w = true
end

@parameters begin
Expand Down Expand Up @@ -856,7 +849,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

@equations begin
Expand Down
8 changes: 3 additions & 5 deletions src/joints.jl
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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,
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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 = 0,
neg_w = true,
w_rel_b = 0,
r_rel_a = 0,
v_rel_a = 0,
Expand Down Expand Up @@ -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
Expand Down
13 changes: 5 additions & 8 deletions src/orientation.jl
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Copy link
Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Should the transpose be done on the RotationMatrix(R,w) and therefore have -w as well?

end

Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -419,7 +419,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]
Expand All @@ -435,9 +435,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)
Expand Down
4 changes: 2 additions & 2 deletions src/wheels.jl
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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
Expand Down
21 changes: 15 additions & 6 deletions test/runtests.jl
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -67,7 +67,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)
Expand Down Expand Up @@ -960,7 +960,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
Expand All @@ -979,7 +988,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)


Expand Down Expand Up @@ -1153,7 +1162,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)
Expand All @@ -1172,7 +1181,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)

Expand All @@ -1193,7 +1202,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)

Expand Down
8 changes: 4 additions & 4 deletions test/test_quaternions.jl
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -75,7 +75,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])
Expand Down Expand Up @@ -149,7 +149,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)
Expand Down Expand Up @@ -203,7 +203,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)
Expand Down Expand Up @@ -272,7 +272,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)
Expand Down
6 changes: 3 additions & 3 deletions test/test_wheels.jl
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
end

Expand Down Expand Up @@ -78,7 +78,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]
])

Expand Down Expand Up @@ -135,7 +135,7 @@ import ModelingToolkitStandardLibrary.Blocks
x0 = 1,
z0 = 1,
iscut=true,
der_angles = [0, 5, 0])
der_angles = [0, -5, 0])
end
@equations begin
connect(world.frame_b, prismatic.frame_a)
Expand Down
Loading
Loading