Hello, I have been designing an omniX boat, and I am sure that the control allocation is not problematic. I am preparing to simulate the Mission mode in SITL, and my custom model has successfully run in Gazebo. After the armed, I can see the propellers rotating, but the boat does not move. I suspect that the libgazebo_usv_dynamics_plugin I am using might not be suitable or the parameters might not be set correctly. Can you advise me on how to debug the simulation environment? @Jaeyoung-Lim
This is my sdf file:
<sdf version='1.6'>
<model name='omnix_boat'>
<link name='base_link'>
<pose frame=''>0 0 0 0 -0 0</pose>
<inertial>
<pose frame=''>0 0 0 0 -0 0</pose>
<mass>0.90444</mass>
<inertia>
<ixx>0.0061404</ixx>
<ixy>1.5992e-06</ixy>
<ixz>5.1716e-07</ixz>
<iyy>0.0061403</iyy>
<iyz>-1.4898e-08</iyz>
<izz>0.011554</izz>
</inertia>
</inertial>
<collision name='base_link_collision'>
<pose frame=''>0 0 0 0 -0 0</pose>
<geometry>
<mesh>
<scale>1 1 1</scale>
<uri>model://fish_detector/meshes/base_link.STL</uri>
</mesh>
</geometry>
</collision>
<visual name='base_link_visual'>
<pose frame=''>0 0 0 0 -0 0</pose>
<geometry>
<mesh>
<scale>1 1 1</scale>
<uri>model://fish_detector/meshes/base_link.STL</uri>
</mesh>
</geometry>
</visual>
</link>
<joint name='imu_joint' type='revolute'>
<pose frame='base_link'>0 0 0 0 0 0</pose>
<parent>base_link</parent>
<child>imu_link</child>
<axis>
<xyz>1 0 0</xyz>
<limit>
<lower>0</lower>
<upper>0</upper>
<effort>0</effort>
<velocity>0</velocity>
</limit>
<dynamics>
<spring_reference>0</spring_reference>
<spring_stiffness>0</spring_stiffness>
</dynamics>
</axis>
</joint>
<link name='imu_link'>
<pose>0 0 0 0 -0 0</pose>
<inertial>
<pose>0 0 0 0 0 0</pose>
<mass>0.05</mass>
<inertia>
<ixx>2.2917e-08</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>2.2917e-08</iyy>
<iyz>0</iyz>
<izz>-4.1667e-08</izz>
</inertia>
</inertial>
<visual name='imu_link_visual'>
<pose>0 0 0 0 -0 0</pose>
<geometry>
<mesh>
<scale>1 1 1</scale>
<uri>model://fish_detector/meshes/imu_link.STL</uri>
</mesh>
</geometry>
</visual>
</link>
<link name='motor1_link'>
<pose frame=''>0.1293 0.14604 -0.007 0 0 -0.7854</pose>
<inertial>
<pose frame=''>0 0 0 0 -0 0</pose>
<mass>0.04548</mass>
<inertia>
<ixx>7.1618e-06</ixx>
<ixy>3.2424e-08</ixy>
<ixz>-4.7703e-07</ixz>
<iyy>1.2249e-05</iyy>
<iyz>7.1728e-10</iyz>
<izz>1.1362e-05</izz>
</inertia>
</inertial>
<collision name='motor1_link_collision'>
<pose frame=''>0 0 0 0 -0 0</pose>
<geometry>
<mesh>
<scale>1 1 1</scale>
<uri>model://fish_detector/meshes/motor1_link.STL</uri>
</mesh>
</geometry>
</collision>
<visual name='motor1_link_visual'>
<pose frame=''>0 0 0 0 -0 0</pose>
<geometry>
<mesh>
<scale>1 1 1</scale>
<uri>model://fish_detector/meshes/motor1_link.STL</uri>
</mesh>
</geometry>
</visual>
</link>
<joint name='motor1_joint' type='revolute'>
<child>motor1_link</child>
<parent>base_link</parent>
<axis>
<xyz>1 0 0</xyz>
<limit>
<lower>-1e+16</lower>
<upper>1e+16</upper>
</limit>
<dynamics>
<spring_reference>0</spring_reference>
<spring_stiffness>0</spring_stiffness>
</dynamics>
<use_parent_model_frame>0</use_parent_model_frame>
</axis>
</joint>
<link name='motor2_link'>
<pose frame=''>0.1293 -0.14604 -0.007 0 -0 0.7854</pose>
<inertial>
<pose frame=''>0 0 0 0 -0 0</pose>
<mass>0.04548</mass>
<inertia>
<ixx>7.1618e-06</ixx>
<ixy>3.2424e-08</ixy>
<ixz>-4.7703e-07</ixz>
<iyy>1.2249e-05</iyy>
<iyz>7.1729e-10</iyz>
<izz>1.1362e-05</izz>
</inertia>
</inertial>
<collision name='motor2_link_collision'>
<pose frame=''>0 0 0 0 -0 0</pose>
<geometry>
<mesh>
<scale>1 1 1</scale>
<uri>model://fish_detector/meshes/motor2_link.STL</uri>
</mesh>
</geometry>
</collision>
<visual name='motor2_link_visual'>
<pose frame=''>0 0 0 0 -0 0</pose>
<geometry>
<mesh>
<scale>1 1 1</scale>
<uri>model://fish_detector/meshes/motor2_link.STL</uri>
</mesh>
</geometry>
</visual>
</link>
<joint name='motor2_joint' type='revolute'>
<child>motor2_link</child>
<parent>base_link</parent>
<axis>
<xyz>1 0 0</xyz>
<limit>
<lower>-1e+16</lower>
<upper>1e+16</upper>
</limit>
<dynamics>
<spring_reference>0</spring_reference>
<spring_stiffness>0</spring_stiffness>
</dynamics>
<use_parent_model_frame>0</use_parent_model_frame>
</axis>
</joint>
<link name='motor3_link'>
<pose frame=''>-0.14604 0.1293 -0.007 0 -0 0.7854</pose>
<inertial>
<pose frame=''>0 0 0 0 -0 0</pose>
<mass>0.04548</mass>
<inertia>
<ixx>7.1618e-06</ixx>
<ixy>3.2424e-08</ixy>
<ixz>-4.7703e-07</ixz>
<iyy>1.2249e-05</iyy>
<iyz>7.1731e-10</iyz>
<izz>1.1362e-05</izz>
</inertia>
</inertial>
<collision name='motor3_link_collision'>
<pose frame=''>0 0 0 0 -0 0</pose>
<geometry>
<mesh>
<scale>1 1 1</scale>
<uri>model://fish_detector/meshes/motor3_link.STL</uri>
</mesh>
</geometry>
</collision>
<visual name='motor3_link_visual'>
<pose frame=''>0 0 0 0 -0 0</pose>
<geometry>
<mesh>
<scale>1 1 1</scale>
<uri>model://fish_detector/meshes/motor3_link.STL</uri>
</mesh>
</geometry>
</visual>
</link>
<joint name='motor3_joint' type='revolute'>
<child>motor3_link</child>
<parent>base_link</parent>
<axis>
<xyz>1 0 0</xyz>
<limit>
<lower>-1e+16</lower>
<upper>1e+16</upper>
</limit>
<dynamics>
<spring_reference>0</spring_reference>
<spring_stiffness>0</spring_stiffness>
</dynamics>
<use_parent_model_frame>0</use_parent_model_frame>
</axis>
</joint>
<link name='motor4_link'>
<pose frame=''>-0.14604 -0.1293 -0.007 0 0 -0.7854</pose>
<inertial>
<pose frame=''>0 0 0 0 -0 0</pose>
<mass>0.04548</mass>
<inertia>
<ixx>7.1618e-06</ixx>
<ixy>3.2424e-08</ixy>
<ixz>-4.7703e-07</ixz>
<iyy>1.2249e-05</iyy>
<iyz>7.173e-10</iyz>
<izz>1.1362e-05</izz>
</inertia>
</inertial>
<collision name='motor4_link_collision'>
<pose frame=''>0 0 0 0 -0 0</pose>
<geometry>
<mesh>
<scale>1 1 1</scale>
<uri>model://fish_detector/meshes/motor4_link.STL</uri>
</mesh>
</geometry>
</collision>
<visual name='motor4_link_visual'>
<pose frame=''>0 0 0 0 -0 0</pose>
<geometry>
<mesh>
<scale>1 1 1</scale>
<uri>model://fish_detector/meshes/motor4_link.STL</uri>
</mesh>
</geometry>
</visual>
</link>
<joint name='motor4_joint' type='revolute'>
<child>motor4_link</child>
<parent>base_link</parent>
<axis>
<xyz>1 0 0</xyz>
<limit>
<lower>-1e+16</lower>
<upper>1e+16</upper>
</limit>
<dynamics>
<spring_reference>0</spring_reference>
<spring_stiffness>0</spring_stiffness>
</dynamics>
<use_parent_model_frame>0</use_parent_model_frame>
</axis>
</joint>
<include>
<uri>model://gps</uri>
<pose>0 0 0.06 0 0 0</pose>
<name>gps</name>
</include>
<joint name='gps_joint' type='fixed'>
<child>gps::link</child>
<parent>base_link</parent>
</joint>
<plugin name='gazebo_imu_plugin' filename='libgazebo_imu_plugin.so'>
<robotNamespace></robotNamespace>
<linkName>imu_link</linkName>
<imuTopic>/imu</imuTopic>
<gyroscopeNoiseDensity>0.0003394</gyroscopeNoiseDensity>
<gyroscopeRandomWalk>3.8785e-05</gyroscopeRandomWalk>
<gyroscopeBiasCorrelationTime>1000.0</gyroscopeBiasCorrelationTime>
<gyroscopeTurnOnBiasSigma>0.0087</gyroscopeTurnOnBiasSigma>
<accelerometerNoiseDensity>0.004</accelerometerNoiseDensity>
<accelerometerRandomWalk>0.006</accelerometerRandomWalk>
<accelerometerBiasCorrelationTime>300.0</accelerometerBiasCorrelationTime>
<accelerometerTurnOnBiasSigma>0.196</accelerometerTurnOnBiasSigma>
</plugin>
<plugin name='magnetometer_plugin' filename='libgazebo_magnetometer_plugin.so'>
<robotNamespace/>
<pubRate>100</pubRate>
<noiseDensity>0.0004</noiseDensity>
<randomWalk>6.4e-06</randomWalk>
<biasCorrelationTime>600</biasCorrelationTime>
<magTopic>/mag</magTopic>
</plugin>
<plugin name='barometer_plugin' filename='libgazebo_barometer_plugin.so'>
<robotNamespace/>
<pubRate>200</pubRate>
<baroTopic>/baro</baroTopic>
</plugin>
<static>0</static>
<plugin name='motor1_model' filename='libgazebo_motor_model.so'>
<robotNamespace></robotNamespace>
<reversible>true</reversible>
<jointName>motor1_joint</jointName>
<linkName>motor1_link</linkName>
<turningDirection>cw</turningDirection>
<timeConstantUp>0.0125</timeConstantUp>
<timeConstantDown>0.025</timeConstantDown>
<maxRotVelocity>2100</maxRotVelocity>
<motorConstant>8.54858e-03</motorConstant>
<momentConstant>0.01</momentConstant>
<commandSubTopic>/gazebo/command/motor_speed</commandSubTopic>
<motorNumber>0</motorNumber>
<rotorDragCoefficient>0.000806428</rotorDragCoefficient>
<rollingMomentCoefficient>1e-06</rollingMomentCoefficient>
<motorSpeedPubTopic>/motor_speed/0</motorSpeedPubTopic>
<rotorVelocitySlowdownSim>10</rotorVelocitySlowdownSim>
</plugin>
<plugin name='motor2_model' filename='libgazebo_motor_model.so'>
<robotNamespace></robotNamespace>
<reversible>true</reversible>
<jointName>motor2_joint</jointName>
<linkName>motor2_link</linkName>
<turningDirection>cw</turningDirection>
<timeConstantUp>0.0125</timeConstantUp>
<timeConstantDown>0.025</timeConstantDown>
<maxRotVelocity>2100</maxRotVelocity>
<motorConstant>8.54858e-03</motorConstant>
<momentConstant>0.01</momentConstant>
<commandSubTopic>/gazebo/command/motor_speed</commandSubTopic>
<motorNumber>1</motorNumber>
<rotorDragCoefficient>0.000806428</rotorDragCoefficient>
<rollingMomentCoefficient>1e-06</rollingMomentCoefficient>
<motorSpeedPubTopic>/motor_speed/1</motorSpeedPubTopic>
<rotorVelocitySlowdownSim>10</rotorVelocitySlowdownSim>
</plugin>
<plugin name='motor3_model' filename='libgazebo_motor_model.so'>
<robotNamespace></robotNamespace>
<reversible>true</reversible>
<jointName>motor3_joint</jointName>
<linkName>motor3_link</linkName>
<turningDirection>cw</turningDirection>
<timeConstantUp>0.0125</timeConstantUp>
<timeConstantDown>0.025</timeConstantDown>
<maxRotVelocity>2100</maxRotVelocity>
<motorConstant>8.54858e-03</motorConstant>
<momentConstant>0.01</momentConstant>
<commandSubTopic>/gazebo/command/motor_speed</commandSubTopic>
<motorNumber>2</motorNumber>
<rotorDragCoefficient>0.000806428</rotorDragCoefficient>
<rollingMomentCoefficient>1e-06</rollingMomentCoefficient>
<motorSpeedPubTopic>/motor_speed/2</motorSpeedPubTopic>
<rotorVelocitySlowdownSim>10</rotorVelocitySlowdownSim>
</plugin>
<plugin name='motor4_model' filename='libgazebo_motor_model.so'>
<robotNamespace></robotNamespace>
<reversible>true</reversible>
<jointName>motor4_joint</jointName>
<linkName>motor4_link</linkName>
<turningDirection>cw</turningDirection>
<timeConstantUp>0.0125</timeConstantUp>
<timeConstantDown>0.025</timeConstantDown>
<maxRotVelocity>2100</maxRotVelocity>
<motorConstant>8.54858e-03</motorConstant>
<momentConstant>0.01</momentConstant>
<commandSubTopic>/gazebo/command/motor_speed</commandSubTopic>
<motorNumber>3</motorNumber>
<rotorDragCoefficient>0.000806428</rotorDragCoefficient>
<rollingMomentCoefficient>1e-06</rollingMomentCoefficient>
<motorSpeedPubTopic>/motor_speed/3</motorSpeedPubTopic>
<rotorVelocitySlowdownSim>10</rotorVelocitySlowdownSim>
</plugin>
<plugin name='usv_dynamics_wamv_dynamics_plugin' filename='libgazebo_usv_dynamics_plugin.so'>
<bodyName>base_link</bodyName>
<waterLevel>0</waterLevel>
<waterDensity>997.8</waterDensity>
<xDotU>0.0</xDotU>
<yDotV>0.0</yDotV>
<nDotR>0.0</nDotR>
<xU>51.3</xU>
<xUU>0.0</xUU>
<yV>102.6</yV>
<yVV>0.0</yVV>
<zW>450.0</zW>
<kP>50.0</kP>
<mQ>50.0</mQ>
<nR>400.0</nR>
<nRR>0.0</nRR>
<hullRadius>0.213</hullRadius>
<boatWidth>0.4</boatWidth>
<boatLength>0.4</boatLength>
<length_n>2</length_n>
<wave_model>ocean_waves</wave_model>
</plugin>
<plugin name='mavlink_interface' filename='libgazebo_mavlink_interface.so'>
<robotNamespace/>
<imuSubTopic>/imu</imuSubTopic>
<magSubTopic>/mag</magSubTopic>
<baroSubTopic>/baro</baroSubTopic>
<mavlink_addr>INADDR_ANY</mavlink_addr>
<mavlink_tcp_port>4560</mavlink_tcp_port>
<mavlink_udp_port>14560</mavlink_udp_port>
<serialEnabled>0</serialEnabled>
<serialDevice>/dev/ttyACM0</serialDevice>
<baudRate>57600</baudRate>
<qgc_addr>INADDR_ANY</qgc_addr>
<qgc_udp_port>14550</qgc_udp_port>
<sdk_addr>INADDR_ANY</sdk_addr>
<sdk_udp_port>14540</sdk_udp_port>
<hil_mode>0</hil_mode>
<hil_state_level>0</hil_state_level>
<send_vision_estimation>0</send_vision_estimation>
<send_odometry>1</send_odometry>
<enable_lockstep>0</enable_lockstep>
<use_tcp>1</use_tcp>
<motorSpeedCommandPubTopic>/gazebo/command/motor_speed</motorSpeedCommandPubTopic>
<control_channels>
<channel name="rotor1">
<input_index>0</input_index>
<input_offset>0</input_offset>
<input_scaling>100</input_scaling>
<zero_position_disarmed>0</zero_position_disarmed>
<zero_position_armed>0</zero_position_armed>
<joint_control_type>velocity</joint_control_type>
<joint_name>motor1_joint</joint_name>
</channel>
<channel name="rotor2">
<input_index>1</input_index>
<input_offset>0</input_offset>
<input_scaling>100</input_scaling>
<zero_position_disarmed>0</zero_position_disarmed>
<zero_position_armed>0</zero_position_armed>
<joint_control_type>velocity</joint_control_type>
<joint_name>motor2_joint</joint_name>
</channel>
<channel name="rotor3">
<input_index>2</input_index>
<input_offset>0</input_offset>
<input_scaling>100</input_scaling>
<zero_position_disarmed>0</zero_position_disarmed>
<zero_position_armed>0</zero_position_armed>
<joint_control_type>velocity</joint_control_type>
<joint_name>motor3_joint</joint_name>
</channel>
<channel name="rotor4">
<input_index>3</input_index>
<input_offset>0</input_offset>
<input_scaling>100</input_scaling>
<zero_position_disarmed>0</zero_position_disarmed>
<zero_position_armed>0</zero_position_armed>
<joint_control_type>velocity</joint_control_type>
<joint_name>motor4_joint</joint_name>
</channel>
</control_channels>
</plugin>
</model>
</sdf>