bluerobotics / bluesim

BlueROV2 Simulator using godot engine.
GNU General Public License v2.0
36 stars 17 forks source link

Bluesim with SITL robot doing loops #55

Closed edwnerd closed 3 weeks ago

edwnerd commented 1 year ago

After setting up with Bluesim with SITL, running this step "Before opening Bluesim (it launches its own SITL instance), run: sim_vehicle.py -j6 -L RATBeach --frame JSON --out=udpout:0.0.0.0:14550" and opening up Bluesim, the robot does constant backflips and loops. Anyone have any fixes for this or know what's going on?

Vince-vd commented 1 year ago

@edwnerd I'm running into the same issue. Did you ever find a solution, or get an idea of what was going on?

It seems like one of the thrusters is just permanently on and spins the ROV around its axis. The problem does not happen when launching the sim without SITL

JSpencerPittman commented 11 months ago

@edwnerd This is happening to me as well.

I checked the power being delivered to each thruster in the BlueRov2Heavy.gd actuate_servo function and all of my six thrusters are being told to run at 50% simultaneously creating the seen back flips and looping. The command for my thrusters to do this came directly from ArduSub.

JSpencerPittman commented 11 months ago

I take back what I said about 50% this seems to be ideal for the rover as sort of neutral state. The problem for me is ArduSub is only sending data for the first six servos and not the last two (7,8). For some reason my program will randomly work and it's only when ArduSub is sending 50% on the 7 & 8 servos as well.

It's as if it's mistaking the BlueRov2Heavy Model for its 6-thruster sibling BlueRov2.

JSpencerPittman commented 11 months ago

The issue seems to stem from the FRAME_CONFIG parameter used by ArduPilot's sim_vehicle.py. When set to 1, it references the vectored frame which supports only six thrusters. However, the simulation expects the vectored6dof frame which supports eight thrusters.

I encountered a problem when trying to set the FRAME_CONFIG to 2 (which should be the correct value). For some reason, sim_vehicle.py didn't accept this change in my parameters.

To resolve this, I manually updated the ArduPilot/ArduSub/Parameters.cpp file. I found the line:

GSCALAR(frame_configuration, "FRAME_CONFIG", AP_Motors6DOF::SUB_FRAME_VECTORED)

And modified it to:

GSCALAR(frame_configuration, "FRAME_CONFIG", AP_Motors6DOF::SUB_FRAME_VECTORED_6DOF)

This change seemed to address the discrepancy.

Vince-vd commented 10 months ago

I did find the solution, but forgot to update this thread!

@JSpencerPittman You are right about the 6dof frame. But you can achieve the same fix by specifying it in the launch command instead of modifying the parameters file (-v is optional, but it makes it so you don't have to launch it from the ardusub folder):

sim_vehicle.py -j6 -L RATBeach -v ArduSub -f vectored_6dof --model=JSON --out=udp:0.0.0.0:14550

JSpencerPittman commented 10 months ago

@Vince-vd thank you so much. My solution fixed the movement of the bot but the camera tilt servo wouldn't work. Your command worked perfectly.