Custom Frame ArduSub SITL with Gazebo

Hi,
I am trying to simulate a rover with a custom frame with 8 thrusters. @patrickelectric suggested I edit the following files

  1. Modify the Frame_Vectored.h.
  2. Add more two thrusters and channels in the BlueRov2 Gazebo model, like this one and more two channels here.
  3. and finallly some modification in SIM_Submarine.cpp.

I added 2 more thrusters to Frame_Vectored.h, BlueRov2.sdf, BlueRov2.urdf.xacro, BlueRov2_thrusters.urdf with the proper values for my rover and also changed the other 6 thruster values where ever needed (like pose of the object)

I am not sure about SIM_Submarine.cpp though. I added 2 thrusters to the Thruster vector and updated the ‘for’ loop in the calculate_forces function to iterate over all 8 thrusters by changing 6 to 8.

Is that all that I need to do to get it working? I am not able to test it at the moment as I still haven’t figured out the map values for the 8 thrusters in the freefloating_gazebo_control plugin in the BlueRov2.urdf.xacro file.

The BlueRov2.urdf.xacro and BlueRov2_thrusters.urdf are for ROS simulation, you don’t need do modify them, only BlueRov2.sdf is necessary for SITL simulation.

Again, this is for ROS simulation and not SITL simulation, this folder and this one are the only directories that deal with SITL simulation using freebuoyancy_gazebo, everything else is for ROS and freefloating-gazebo.

About the freefloating (ROS simulation):
The thruster map is the 6x1 vector that links a force of 1 N on the thruster with the corresponding wrench applied to the body frame.
For example, an horizontal thruster located at 10 cm on the right side of the body will push along the X direction (forward) and also induce a yaw. The map will be : <1 0 0 0 0 0.1>.

About the freebuoyancy (SITL simulation):
This plugin is only used to perform buoyancy simulation of the Subsea Buoyancy Foam, the only necessary parameter inside BlueRov2 model are buoyancy compensation and the foam position.

<buoyancy>
    <compensation>0.2525</compensation>
    <origin xyz= "0.12585 0.111 0.1789"/>
</buoyancy>

Got it. Since the iris quad-copter in gazebo has the drag plug-in to simulate the lift of the props, I kept assuming that we need a plug-in to simulate drag or in this case force from the thruster . I got rid of the urdf files, added the thruster links and joints along with 2 more channels in ardupilot plug-in in the sdf file.

When I start gazebo with the rov, it is floating properly but, when I start the sitl with the changes I mentioned in SIM_SUBMARINE.cpp and FRAME_VECTORED.h the rov starts spinning around even though I didn’t arm the rov in qgroundcontrol. And the rov stops moving if I remove the extra channels in ardupilot plugin. I was able to arm it and move it a bit, but in the FRAME_VECTORED.h file I had 0 for everything.

You will also need to modify the apmotors_6dof in libraries.

Just tried it, same thing is happening. The ROV is spinning around even though it’s not armed. I made sure the control matrix is the same in both apmotors_6dof and frame_vectored files.

I got it working, in AP_MOTORS6DOF.cpp I changed the SUB_FRAME_VECTORED’s frame configuration with my 8 thruster rov’s frame configuration. It stopped spinning around without being armed and is working properly. I left my frame_vectored file with 0 for all values but the SITL is just ignoring everything and just using the values in AP_MOTORS6DOF.cpp. I also tried to change the frame in QGroundControl but it’s still just using the values for SUB_FRAME_VECTORED. @jwalser @patrickelectric do one of you guys know if the sitl is hard coded somewhere to use the SUB_FRAME_VECTORED frame from AP_MOTORS6DOF.cpp somewhere?

And @patrickelectric in the sdf file for bluerov2 I see that the fixed thruster_joints have a axis tag defined. Is there any reason you did that? As far as what I know only revolute joints need an axis tag.

Maybe @jwalser can cofirm this, but probably you need to add you frame in SUB_FRAME_CUSTOM.

Yes, right now the thrusters have fixed joints, but one day we’ll be able to add a propeller simulation and the thruster_joints type will change from fixed to revolute.

The motor mixing and motor output logic is in the AP_Motors library. There is support for several pre-configured motor configurations; the motor configuration is selected at runtime (once at boot) and is controlled by the FRAME_CONFIG parameter. It doesn’t matter where you put your mapping matrix, as long as you have the correct FRAME_CONFIG setting selected.

The simulation physics of the motor outputs is in FRAME_VECTORED.h in the SITL directory. This is where the simulation is hard coded to use the vectored frame. If you are using gazebo, the simulation physicsis in the sdf file .

@patrickelectric when I start gazebo with bluerov2 I get a warning saying IMU sensor convention not supported and in QGroundControl the imu is inverted , after I start the sitl. Do you know a way around this issue?

Where this error message appear ?
This same error appear with the default Gazebo BlueRov2 model ?

@patrickelectric It’s not an error, just a warning

[Wrn] [msgs.cc:1808] Conversion of sensor type[imu] not suppported.

I am getting it even when when I run your model. And my compass in qgroundcontrol is showing that the rover is inverted even with your model. I think the 2 issues are related.

I agree with you, both problem seems to be related. Which gazebo version are you using ?

7.9.0

Ok, maybe the problem can be related with the ROV parameters.
Can you try this one ?
bluerov2_gazebo_sitl.params (23.1 KB)

Didn’t work.

@jakkala-kalvik the bluerov appears upside down in QGC with this parameters ?

Yes.

Check your AHRS_ORIENTATION parameter of your SITL environment in QGC, this parameters are working correctly here with gazebo 8.2 and ArduSub master. I’ll check asap in gazebo 7.9 to make sure that the SITL environment is working in this version.

Changed it to Roll 180 and its working properly now thanks.

1 Like