gz-sim: lrauv_control_demo.sdf

[azeey]

Setup

• Os: Ubuntu Jammy
• BuildType: Binary

Links

• Source

Checks

• Instructions work
• Images (if there are any) match the result

---

You can find the code used to generate this test case here

[matosinho]

Hello!

That seems to be working really well! I've tried to launch the sim, change the angle of the horizontal fins, vertical fins and then turning on the thruster. I did this by launching these in another terminal:

gz topic -t /model/tethys/joint/horizontal_fins_joint/0/cmd_pos -m gz.msgs.Double -p 'data: -0.4'
gz topic -t /model/tethys/joint/vertical_fins_joint/0/cmd_pos -m gz.msgs.Double -p 'data: -0.5'
gz topic -t /model/tethys/joint/propeller_joint/cmd_thrust -m gz.msgs.Double -p 'data: -20'

I believe that it had the correct behaviour. Here is a gif:

One thing though, right after the recording the code broke. I think that might be due to a big value for the thruster rotation, but in case it wasn't, here is the error:

ODE INTERNAL ERROR 1: assertion "aabbBound >= dMinIntExact && aabbBound < dMaxIntExact" failed in collide() [collision_space.cpp:460]
Stack trace (most recent call last):
#31   Object "/lib/x86_64-linux-gnu/libruby-3.0.so.3.0", at 0x7f721b434c96, in 
#30   Object "/lib/x86_64-linux-gnu/libruby-3.0.so.3.0", at 0x7f721b431fc5, in 
#29   Object "/lib/x86_64-linux-gnu/libruby-3.0.so.3.0", at 0x7f721b42fc34, in 
#28   Object "/lib/x86_64-linux-gnu/libruby-3.0.so.3.0", at 0x7f721b37ba1e, in 
#27   Object "/lib/x86_64-linux-gnu/libruby-3.0.so.3.0", at 0x7f721b2a69ac, in rb_protect
#26   Object "/lib/x86_64-linux-gnu/libruby-3.0.so.3.0", at 0x7f721b43ec61, in rb_yield
#25   Object "/lib/x86_64-linux-gnu/libruby-3.0.so.3.0", at 0x7f721b43a30c, in rb_vm_exec
#24   Object "/lib/x86_64-linux-gnu/libruby-3.0.so.3.0", at 0x7f721b434c96, in 
#23   Object "/lib/x86_64-linux-gnu/libruby-3.0.so.3.0", at 0x7f721b431fc5, in 
#22   Object "/lib/x86_64-linux-gnu/libruby-3.0.so.3.0", at 0x7f721b42fc34, in 
#21   Object "/usr/lib/x86_64-linux-gnu/ruby/3.0.0/fiddle.so", at 0x7f721b0af44b, in 
#20   Object "/lib/x86_64-linux-gnu/libruby-3.0.so.3.0", at 0x7f721b3fd088, in rb_nogvl
#19   Object "/usr/lib/x86_64-linux-gnu/ruby/3.0.0/fiddle.so", at 0x7f721b0aed6b, in 
#18   Object "/lib/x86_64-linux-gnu/libffi.so.8", at 0x7f721b0a0492, in 
#17   Object "/lib/x86_64-linux-gnu/libffi.so.8", at 0x7f721b0a3e2d, in 
#16   Object "/usr/lib/x86_64-linux-gnu/libgz-sim8-gz.so.8.0.0~pre1", at 0x7f721adb47b2, in runServer
#15   Object "/lib/x86_64-linux-gnu/libgz-sim8.so.8", at 0x7f7216132b65, in 
#14   Object "/lib/x86_64-linux-gnu/libgz-sim8.so.8", at 0x7f7216140b52, in gz::sim::v8::SimulationRunner::Run(unsigned long)
#13   Object "/lib/x86_64-linux-gnu/libgz-sim8.so.8", at 0x7f721614d060, in gz::sim::v8::SimulationRunner::Step(gz::sim::v8::UpdateInfo const&)
#12   Object "/lib/x86_64-linux-gnu/libgz-sim8.so.8", at 0x7f721613ffb2, in gz::sim::v8::SimulationRunner::UpdateSystems()
#11   Object "/usr/lib/x86_64-linux-gnu/gz-sim-8/plugins/libgz-sim-physics-system.so", at 0x7f7204719f56, in gz::sim::v8::systems::Physics::Update(gz::sim::v8::UpdateInfo const&, gz::sim::v8::EntityComponentManager&)
#10   Object "/usr/lib/x86_64-linux-gnu/gz-sim-8/plugins/libgz-sim-physics-system.so", at 0x7f720471a5d1, in gz::sim::v8::systems::PhysicsPrivate::Step(std::chrono::duration<long, std::ratio<1l, 1000000000l> > const&)
#9    Object "/usr/lib/x86_64-linux-gnu/gz-physics-7/engine-plugins/libgz-physics-dartsim-plugin.so", at 0x7f71e3f95d6b, in gz::physics::dartsim::SimulationFeatures::WorldForwardStep(gz::physics::Identity const&, gz::physics::SpecifyData<gz::physics::RequireData<gz::physics::WorldPoses>, gz::physics::ExpectData<gz::physics::ChangedWorldPoses, gz::physics::Contacts, gz::physics::JointPositions> >&, gz::physics::CompositeData&, gz::physics::ExpectData<gz::physics::ApplyExternalForceTorques, gz::physics::ApplyGeneralizedForces, gz::physics::VelocityControlCommands, gz::physics::ServoControlCommands, std::chrono::duration<long, std::ratio<1l, 1000000000l> > > const&)
#8    Object "/lib/x86_64-linux-gnu/libdart.so.6.12", at 0x7f71e3b66e73, in dart::simulation::World::step(bool)
#7    Object "/lib/x86_64-linux-gnu/libdart.so.6.12", at 0x7f71e3b4c705, in dart::constraint::ConstraintSolver::solve()
#6    Object "/lib/x86_64-linux-gnu/libdart.so.6.12", at 0x7f71e3b4afe1, in dart::constraint::ConstraintSolver::updateConstraints()
#5    Object "/lib/x86_64-linux-gnu/libdart-collision-ode.so.6.12", at 0x7f71e7f161cb, in dart::collision::OdeCollisionDetector::collide(dart::collision::CollisionGroup*, dart::collision::CollisionOption const&, dart::collision::CollisionResult*)
#4    Object "/lib/x86_64-linux-gnu/libode.so.8", at 0x7f71e451e266, in dxHashSpace::collide(void*, void (*)(void*, dxGeom*, dxGeom*))
#3    Object "/lib/x86_64-linux-gnu/libode.so.8", at 0x7f71e45267b7, in dDebug
#2    Object "/lib/x86_64-linux-gnu/libc.so.6", at 0x7f721ae287f2, in abort
#1    Object "/lib/x86_64-linux-gnu/libc.so.6", at 0x7f721ae42475, in raise
#0    Object "/lib/x86_64-linux-gnu/libc.so.6", at 0x7f721ae96a7c, in pthread_kill
Aborted (Signal sent by tkill() 113251 1000)

Let me know if any additional tests are required.

[Voldivh]

Thanks @matosinho, It looks like it worked well during the simulation. It does sound weird to me that after you finished recording the process just aborted. Would you mind trying it out again with other values?

[matosinho]

I tried as well with -1 for the thrust and the same issue happened:

Looks like a very similar error message:

ODE INTERNAL ERROR 1: assertion "aabbBound >= dMinIntExact && aabbBound < dMaxIntExact" failed in collide() [collision_space.cpp:460]
Stack trace (most recent call last):
#31   Object "/lib/x86_64-linux-gnu/libruby-3.0.so.3.0", at 0x7fcb53c34c96, in 
#30   Object "/lib/x86_64-linux-gnu/libruby-3.0.so.3.0", at 0x7fcb53c31fc5, in 
#29   Object "/lib/x86_64-linux-gnu/libruby-3.0.so.3.0", at 0x7fcb53c2fc34, in 
#28   Object "/lib/x86_64-linux-gnu/libruby-3.0.so.3.0", at 0x7fcb53b7ba1e, in 
#27   Object "/lib/x86_64-linux-gnu/libruby-3.0.so.3.0", at 0x7fcb53aa69ac, in rb_protect
#26   Object "/lib/x86_64-linux-gnu/libruby-3.0.so.3.0", at 0x7fcb53c3ec61, in rb_yield
#25   Object "/lib/x86_64-linux-gnu/libruby-3.0.so.3.0", at 0x7fcb53c3a30c, in rb_vm_exec
#24   Object "/lib/x86_64-linux-gnu/libruby-3.0.so.3.0", at 0x7fcb53c34c96, in 
#23   Object "/lib/x86_64-linux-gnu/libruby-3.0.so.3.0", at 0x7fcb53c31fc5, in 
#22   Object "/lib/x86_64-linux-gnu/libruby-3.0.so.3.0", at 0x7fcb53c2fc34, in 
#21   Object "/usr/lib/x86_64-linux-gnu/ruby/3.0.0/fiddle.so", at 0x7fcb53d7f44b, in 
#20   Object "/lib/x86_64-linux-gnu/libruby-3.0.so.3.0", at 0x7fcb53bfd088, in rb_nogvl
#19   Object "/usr/lib/x86_64-linux-gnu/ruby/3.0.0/fiddle.so", at 0x7fcb53d7ed6b, in 
#18   Object "/lib/x86_64-linux-gnu/libffi.so.8", at 0x7fcb5382e492, in 
#17   Object "/lib/x86_64-linux-gnu/libffi.so.8", at 0x7fcb53831e2d, in 
#16   Object "/usr/lib/x86_64-linux-gnu/libgz-sim8-gz.so.8.0.0~pre1", at 0x7fcb4fa8a7b2, in runServer
#15   Object "/lib/x86_64-linux-gnu/libgz-sim8.so.8", at 0x7fcb4e932b65, in 
#14   Object "/lib/x86_64-linux-gnu/libgz-sim8.so.8", at 0x7fcb4e940b52, in gz::sim::v8::SimulationRunner::Run(unsigned long)
#13   Object "/lib/x86_64-linux-gnu/libgz-sim8.so.8", at 0x7fcb4e94d060, in gz::sim::v8::SimulationRunner::Step(gz::sim::v8::UpdateInfo const&)
#12   Object "/lib/x86_64-linux-gnu/libgz-sim8.so.8", at 0x7fcb4e93ffb2, in gz::sim::v8::SimulationRunner::UpdateSystems()
#11   Object "/usr/lib/x86_64-linux-gnu/gz-sim-8/plugins/libgz-sim-physics-system.so", at 0x7fcb3c619f56, in gz::sim::v8::systems::Physics::Update(gz::sim::v8::UpdateInfo const&, gz::sim::v8::EntityComponentManager&)
#10   Object "/usr/lib/x86_64-linux-gnu/gz-sim-8/plugins/libgz-sim-physics-system.so", at 0x7fcb3c61a5d1, in gz::sim::v8::systems::PhysicsPrivate::Step(std::chrono::duration<long, std::ratio<1l, 1000000000l> > const&)
#9    Object "/usr/lib/x86_64-linux-gnu/gz-physics-7/engine-plugins/libgz-physics-dartsim-plugin.so", at 0x7fcb1c795d6b, in gz::physics::dartsim::SimulationFeatures::WorldForwardStep(gz::physics::Identity const&, gz::physics::SpecifyData<gz::physics::RequireData<gz::physics::WorldPoses>, gz::physics::ExpectData<gz::physics::ChangedWorldPoses, gz::physics::Contacts, gz::physics::JointPositions> >&, gz::physics::CompositeData&, gz::physics::ExpectData<gz::physics::ApplyExternalForceTorques, gz::physics::ApplyGeneralizedForces, gz::physics::VelocityControlCommands, gz::physics::ServoControlCommands, std::chrono::duration<long, std::ratio<1l, 1000000000l> > > const&)
#8    Object "/lib/x86_64-linux-gnu/libdart.so.6.12", at 0x7fcb1c366e73, in dart::simulation::World::step(bool)
#7    Object "/lib/x86_64-linux-gnu/libdart.so.6.12", at 0x7fcb1c34c705, in dart::constraint::ConstraintSolver::solve()
#6    Object "/lib/x86_64-linux-gnu/libdart.so.6.12", at 0x7fcb1c34afe1, in dart::constraint::ConstraintSolver::updateConstraints()
#5    Object "/lib/x86_64-linux-gnu/libdart-collision-ode.so.6.12", at 0x7fcb1fff71cb, in dart::collision::OdeCollisionDetector::collide(dart::collision::CollisionGroup*, dart::collision::CollisionOption const&, dart::collision::CollisionResult*)
#4    Object "/lib/x86_64-linux-gnu/libode.so.8", at 0x7fcb1cd1e266, in dxHashSpace::collide(void*, void (*)(void*, dxGeom*, dxGeom*))
#3    Object "/lib/x86_64-linux-gnu/libode.so.8", at 0x7fcb1cd267b7, in dDebug
#2    Object "/lib/x86_64-linux-gnu/libc.so.6", at 0x7fcb536287f2, in abort
#1    Object "/lib/x86_64-linux-gnu/libc.so.6", at 0x7fcb53642475, in raise
#0    Object "/lib/x86_64-linux-gnu/libc.so.6", at 0x7fcb53696a7c, in pthread_kill
Aborted (Signal sent by tkill() 116093 1000)

[Voldivh]

Ok, I managed to replicate the issue. The value used for the topic /model/tethys/joint/horizontal_fins_joint/0/cmd_pos exceeds a value, I'm not exactly sure what that limit values is atm. However, I tried using -0.1 and worked fine.

[matosinho]

For me even with this value it crashed. It just took longer. It seems that the vehicle goes faster and faster until the sim breaks. It should reach a maximum speed instead of always speedup, no? For me with -0.1 and 1 for thrust took like 3 minutes but eventually went to a very high velocity and broke the simulation.

[matosinho]

I think I found out where the problem is. During the test of THIS EXAMPLE, I saw that the sim didn't brake. Comparing LRAUV sdf from both simulations, I saw the difference that made the code crash.

This is how the crashing hydrodynamis are being declared in this example:

      <plugin
        filename="gz-sim-hydrodynamics-system"
        name="gz::sim::systems::Hydrodynamics">
        <link_name>base_link</link_name>
        <xDotU>-4.876161</xDotU>
        <yDotV>-126.324739</yDotV>
        <zDotW>-126.324739</zDotW>
        <kDotP>0</kDotP>
        <mDotQ>-33.46</mDotQ>
        <nDotR>-33.46</nDotR>
        <xUU>-6.2282</xUU>
        <xU>0</xU>
        <yVV>-601.27</yVV>
        <yV>0</yV>
        <zWW>-601.27</zWW>
        <zW>0</zW>
        <kPP>-0.1916</kPP>
        <kP>0</kP>
        <mQQ>-632.698957</mQQ>
        <mQ>0</mQ>
        <nRR>-632.698957</nRR>
        <nR>0</nR>
      </plugin>

And here is how the non crash version is being declared on the auv_controls.sdf:

    <plugin
        filename="gz-sim-hydrodynamics-system"
        name="gz::sim::systems::Hydrodynamics">
        <link_name>base_link</link_name>
        <xDotU>-4.876161</xDotU>
        <yDotV>-126.324739</yDotV>
        <zDotW>-126.324739</zDotW>
        <kDotP>0</kDotP>
        <mDotQ>-33.46</mDotQ>
        <nDotR>-33.46</nDotR>
        <xUabsU>-6.2282</xUabsU>
        <xU>0</xU>
        <yVabsV>-601.27</yVabsV>
        <yV>0</yV>
        <zWabsW>-601.27</zWabsW>
        <zW>0</zW>
        <kPabsP>-0.1916</kPabsP>
        <kP>0</kP>
        <mQabsQ>-632.698957</mQabsQ>
        <mQ>0</mQ>
        <nRabsR>-632.698957</nRabsR>
        <nR>0</nR>
      </plugin>
      ```
      
I still need to check the updates on the hydrodynamics plugin, but probably just this need to be updated on this LRAUV model.

[Voldivh]

Thanks for the investigation @matosinho I'll try it out and get back to you.

[Voldivh]

I tried it out for a while now and it seems to be working just fine with the modification, I'll create the related PR.