Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Implement proper tracked motion model #958

Merged
merged 10 commits into from
Jan 25, 2022
Merged

Implement proper tracked motion model #958

Show file tree
Hide file tree
Changes from 6 commits
Commits
Show all changes
10 commits
Select commit Hold shift + click to select a range
2c6dc8c
Absolem: fix bad merge.
peci1 Jun 20, 2021
b6803aa
Implement more plausible model of tracks.
peci1 Jun 20, 2021
c0ee4ff
Merge upstream thermal cam settings for MARV SC3
peci1 Aug 16, 2021
c547f68
Merge branch 'final_event' into proper-tracks
peci1 Aug 16, 2021
c2f67ab
Merge branch 'master' into proper-tracks
peci1 Nov 15, 2021
0a298b2
Update with latest changes.
peci1 Nov 15, 2021
bbbbe39
Removed empty wheel slip plugin.
peci1 Nov 16, 2021
3aa0dea
Merge branch 'master' into proper-tracks
peci1 Nov 19, 2021
23bec12
Use power_draining_topic to start battery drain for tracked vehicles.
peci1 Jan 5, 2022
e1792d1
Merge pull request #2 from peci1/proper-tracks-battery-hotfix
peci1 Jan 5, 2022
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
3 changes: 3 additions & 0 deletions submitted_models/ctu_cras_norlab_absolem_sensor_config_1/config/common.yaml
View file
Open in desktop
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
145 changes: 105 additions & 40 deletions submitted_models/ctu_cras_norlab_absolem_sensor_config_1/launch/common.rb
View file
Open in desktop
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,106 @@ def _spawner(_name, _modelURI, _worldName, _x, _y, _z, _roll, _pitch, _yaw, _add
diffdriveJoints[i] = ""
end
wheelSlip = ""
trackControllers = ""
trackLinks = ""

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>"
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>"
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 +150,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 Down Expand Up @@ -119,9 +186,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