Skip to content

Commit

Permalink
Implement proper tracked motion model (#958)
Browse files Browse the repository at this point in the history 
* Absolem: fix bad merge.

* Implement more plausible model of tracks.

* Merge upstream thermal cam settings for MARV SC3

* Update with latest changes.

* Removed empty wheel slip plugin.

* Use power_draining_topic to start battery drain for tracked vehicles.
  • Loading branch information
@peci1
peci1 authored Jan 25, 2022
1 parent 8c1e339 commit b626b6d
Show file tree
Hide file tree
Showing 17 changed files with 10,656 additions and 29,434 deletions.
3 changes: 3 additions & 0 deletions submitted_models/ctu_cras_norlab_absolem_sensor_config_1/config/common.yaml
View file
Original file line number Diff line number Diff line change
@@ -1,5 +1,8 @@
revision: 2014 # 2014 for TRADR robot, 2021 for upgraded robot

wheels_instead_of_tracks: False
track_mu: 10

big_collision_box_on_top: False
big_collision_box_height: 0.5
big_collision_box_width: 0.5
Expand Down
149 changes: 109 additions & 40 deletions submitted_models/ctu_cras_norlab_absolem_sensor_config_1/launch/common.rb
View file
Original file line number Diff line number Diff line change
@@ -1,4 +1,16 @@
def _spawner(_name, _modelURI, _worldName, _x, _y, _z, _roll, _pitch, _yaw, _additionalSDF, _max_velocity=0.4, _max_acceleration=3)
# read robot configuration from the config/ dir to access robot geometry information
require 'yaml'
confdir = File.join(__dir__, '..', 'config')
config_yamls = [ File.join(confdir, 'common.yaml'), File.join(confdir, 'sdf.yaml') ]
config = Hash.new
for config_yaml in config_yamls
conf = YAML.load_file(config_yaml)
config.merge!(conf)
end

useWheels = config["wheels_instead_of_tracks"]

numTrackWheels = 8
numFlipperWheels = 5

Expand All @@ -22,53 +34,109 @@ def _spawner(_name, _modelURI, _worldName, _x, _y, _z, _roll, _pitch, _yaw, _add
diffdriveJoints[i] = ""
end
wheelSlip = ""
trackControllers = ""
trackLinks = ""
power_draining_topics = ""

for track in tracks
for wheel in 1..numTrackWheels
diffdriveJoints[0] += "<#{track}_joint>#{track}_track_wheel#{wheel}_j</#{track}_joint>\n"
wheelSlip += <<-HEREDOC
<wheel link_name="#{track}_track_wheel#{wheel}">
<slip_compliance_lateral>#{slipCompliance}</slip_compliance_lateral>
<slip_compliance_longitudinal>0</slip_compliance_longitudinal>
<wheel_normal_force>#{wheelNormalForce}</wheel_normal_force>
<wheel_radius>#{wheelRadiuses[0]}</wheel_radius>
</wheel>
HEREDOC
if useWheels
for wheel in 1..numTrackWheels
diffdriveJoints[0] += "<#{track}_joint>#{track}_track_wheel#{wheel}_j</#{track}_joint>\n"
wheelSlip += <<-HEREDOC
<wheel link_name="#{track}_track_wheel#{wheel}">
<slip_compliance_lateral>#{slipCompliance}</slip_compliance_lateral>
<slip_compliance_longitudinal>0</slip_compliance_longitudinal>
<wheel_normal_force>#{wheelNormalForce}</wheel_normal_force>
<wheel_radius>#{wheelRadiuses[0]}</wheel_radius>
</wheel>
HEREDOC
end
else
trackControllers += <<-HEREDOC
<plugin name='ignition::gazebo::systems::TrackController' filename='libignition-gazebo-track-controller-system.so'>
<link>#{track}_track</link>
<min_velocity>-#{_max_velocity}</min_velocity>
<max_velocity>#{_max_velocity}</max_velocity>
<min_acceleration>-#{_max_acceleration}</min_acceleration>
<max_acceleration>#{_max_acceleration}</max_acceleration>
</plugin>
HEREDOC
trackLinks += "<#{track}_track><link>#{track}_track</link></#{track}_track>"
power_draining_topics += "<power_draining_topic>/model/#{_name}/link/#{track}_track/track_cmd_vel</power_draining_topic>"
end
for flipper in flippersOfTrack[track]
for wheel in 1..numFlipperWheels
diffdriveJoints[wheel-1] += "<#{track}_joint>#{flipper}_flipper_wheel#{wheel}_j</#{track}_joint>\n"
wheelSlip += <<-HEREDOC
<wheel link_name="#{flipper}_flipper_wheel#{wheel}">
<slip_compliance_lateral>#{slipCompliance}</slip_compliance_lateral>
<slip_compliance_longitudinal>0</slip_compliance_longitudinal>
<wheel_normal_force>#{wheelNormalForce}</wheel_normal_force>
<wheel_radius>#{wheelRadiuses[wheel-1]}</wheel_radius>
</wheel>
if useWheels
for wheel in 1..numFlipperWheels
diffdriveJoints[wheel-1] += "<#{track}_joint>#{flipper}_flipper_wheel#{wheel}_j</#{track}_joint>\n"
wheelSlip += <<-HEREDOC
<wheel link_name="#{flipper}_flipper_wheel#{wheel}">
<slip_compliance_lateral>#{slipCompliance}</slip_compliance_lateral>
<slip_compliance_longitudinal>0</slip_compliance_longitudinal>
<wheel_normal_force>#{wheelNormalForce}</wheel_normal_force>
<wheel_radius>#{wheelRadiuses[wheel-1]}</wheel_radius>
</wheel>
HEREDOC
end
else
trackControllers += <<-HEREDOC
<plugin name='ignition::gazebo::systems::TrackController' filename='libignition-gazebo-track-controller-system.so'>
<link>#{flipper}_flipper</link>
<min_velocity>-#{_max_velocity}</min_velocity>
<max_velocity>#{_max_velocity}</max_velocity>
<min_acceleration>-#{_max_acceleration}</min_acceleration>
<max_acceleration>#{_max_acceleration}</max_acceleration>
</plugin>
HEREDOC
trackLinks += "<#{track}_track><link>#{flipper}_flipper</link></#{track}_track>"
power_draining_topics += "<power_draining_topic>/model/#{_name}/link/#{flipper}_flipper/track_cmd_vel</power_draining_topic>"
end
end
end

diffDrive = ""
for wheel in 0...numFlipperWheels
# we only want odometry from the first diffdrive
no_odom = ""
if wheel > 0
no_odom = "<odom_topic>unused_odom</odom_topic>\n"
end
diffDrive += <<-HEREDOC
<plugin filename="ignition-gazebo-diff-drive-system" name="ignition::gazebo::systems::DiffDrive">
#{diffdriveJoints[wheel]}
<wheel_separation>#{wheelSeparation}</wheel_separation>
<wheel_radius>#{wheelRadiuses[wheel]}</wheel_radius>
<topic>/model/#{_name}/cmd_vel_relay</topic>
<min_velocity>-#{_max_velocity}</min_velocity>
<max_velocity>#{_max_velocity}</max_velocity>
<min_acceleration>-#{_max_acceleration}</min_acceleration>
<max_acceleration>#{_max_acceleration}</max_acceleration>
#{no_odom}
</plugin>
trackedVehicle = ""
wheelSlipPlugin = ""
if useWheels
for wheel in 0...numFlipperWheels
# we only want odometry from the first diffdrive
no_odom = ""
if wheel > 0
no_odom = "<odom_topic>unused_odom</odom_topic>\n"
end
diffDrive += <<-HEREDOC
<plugin filename="ignition-gazebo-diff-drive-system" name="ignition::gazebo::systems::DiffDrive">
#{diffdriveJoints[wheel]}
<wheel_separation>#{wheelSeparation}</wheel_separation>
<wheel_radius>#{wheelRadiuses[wheel]}</wheel_radius>
<topic>/model/#{_name}/cmd_vel_relay</topic>
<min_velocity>-#{_max_velocity}</min_velocity>
<max_velocity>#{_max_velocity}</max_velocity>
<min_acceleration>-#{_max_acceleration}</min_acceleration>
<max_acceleration>#{_max_acceleration}</max_acceleration>
#{no_odom}
</plugin>
HEREDOC
end
wheelSlipPlugin = <<-HEREDOC
<plugin filename="ignition-gazebo-wheel-slip-system" name="ignition::gazebo::systems::WheelSlip">
#{wheelSlip}
</plugin>
HEREDOC
else
trackedVehicle += <<-HEREDOC
<plugin name="ignition::gazebo::systems::TrackedVehicle" filename="ignition-gazebo-tracked-vehicle-system">
#{trackLinks}
<tracks_separation>#{wheelSeparation}</tracks_separation>
<tracks_height>0.18094</tracks_height>
<steering_efficiency>0.5</steering_efficiency>
<topic>/model/#{_name}/cmd_vel_relay</topic>
<linear_velocity>
<min_velocity>-#{_max_velocity}</min_velocity>
<max_velocity>#{_max_velocity}</max_velocity>
<min_acceleration>-#{_max_acceleration}</min_acceleration>
<max_acceleration>#{_max_acceleration}</max_acceleration>
</linear_velocity>
</plugin>
HEREDOC
end

Expand All @@ -85,6 +153,8 @@ def _spawner(_name, _modelURI, _worldName, _x, _y, _z, _roll, _pitch, _yaw, _add
<uri>#{_modelURI}</uri>
<!-- Diff drive -->
#{diffDrive}
#{trackedVehicle}
#{trackControllers}
<!-- Publish robot state information -->
<plugin filename="libignition-gazebo-pose-publisher-system.so"
name="ignition::gazebo::systems::PosePublisher">
Expand All @@ -110,6 +180,7 @@ def _spawner(_name, _modelURI, _worldName, _x, _y, _z, _roll, _pitch, _yaw, _add
<smooth_current_tau>1.9499</smooth_current_tau>
<power_load>4.95</power_load>
<start_on_motion>true</start_on_motion>
#{power_draining_topics}
</plugin>
<!-- Gas Sensor plugin -->
<plugin filename="libGasEmitterDetectorPlugin.so"
Expand All @@ -119,9 +190,7 @@ def _spawner(_name, _modelURI, _worldName, _x, _y, _z, _roll, _pitch, _yaw, _add
<type>gas</type>
</plugin>
<!-- Wheel slip -->
<plugin filename="ignition-gazebo-wheel-slip-system" name="ignition::gazebo::systems::WheelSlip">
#{wheelSlip}
</plugin>
#{wheelSlipPlugin}
#{_additionalSDF}
</include>
</sdf>
Expand Down
Loading

0 comments on commit b626b6d

Please sign in to comment.