NaN Values in Simulation with QuaternionSpherical Joint

[jfkunz]

Hi,
I tried to simulate a simple pendulum with a link attached by a QuaternionSpherical joint (ball-socket joint). The joint is displaced with a translation. When I run the simulation, the output configurations contain NaN values. Here is the code to recreate the behavior:

using RigidBodyDynamics
using Rotations
using LinearAlgebra
using StaticArrays
 
# # working with zero offset 
# translation = [0,0,0]
 
# NaN with non-zero offset
translation = [0.3,0,0]
 
origin = RigidBody{Float64}("origin")
pendulum = Mechanism(origin; gravity=[0., 0., -9.81])
 
center_of_mass = [0,0,0.2]
q0 = [cos(pi/8);sin(pi/8);0.0;0.0]
 
joint1 = Joint("joint1", QuaternionSpherical{Float64}())
inertia1 = SpatialInertia(frame_after(joint1), com=center_of_mass, moment_about_com=diagm([1.,1.,1.]), mass=1.)
link1 = RigidBody("link1", inertia1)
before_joint1_to_origin = Transform3D(frame_before(joint1), default_frame(origin), one(RotMatrix{3}), SVector{3}(translation))
attach!(pendulum, origin, link1, joint1, joint_pose=before_joint1_to_origin)
 
state = MechanismState(pendulum)
 
set_configuration!(state, joint1, q0)
ts, qs, vs = simulate(state, 10., Δt=0.001)
display(qs)

Interestingly, the simulation works just fine if I set the translation to zero.
Thank you!

[tkoolen]

Thanks for the report and the reproducer. Will be fixed by #618.

[jfkunz]

That's great! Thanks. Am Fr., 19. März 2021 um 22:58 Uhr schrieb Twan Koolen < ***@***.***>:

…

Thanks for the report and the reproducer. Will be fixed by #618 <#618>. — You are receiving this because you authored the thread. Reply to this email directly, view it on GitHub <#617 (comment)>, or unsubscribe <https://github.com/notifications/unsubscribe-auth/AJTGLYISNVF6H5KUNGAQQPDTEPCKFANCNFSM4ZO66XXA> .