Added support for tracked vehicles

[peci1]

🎉 New feature

Closes osrf/subt#365

Depends on gazebo-forks/dart#22 and gazebosim/gz-physics#267 .

Is needed for osrf/subt#958 .

Summary

This PR adds support for tracked vehicles along the lines of https://osrf-migration.github.io/gazebo-gh-pages/#!/osrf/gazebo/pull-requests/2652/page/1 .

It is based on M. Pecka, K. Zimmermann and T. Svoboda, "Fast simulation of vehicles with non-deformable tracks," 2017 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), 2017, pp. 6414-6419, doi: 10.1109/IROS.2017.8206546.

This feature might be highly useful for DARPA SubT virtual challenge to allow implementation of a proper track drivetrain.

Discussion

I had to invent a way to allow passing callbacks to ign-physics. I chose implementation via events. So ign-gazebo registers a callback to ign-physics. This callback is called whenever a contact joint is created. Once the callback is called, it emits a Gazebo event which has a non-const reference to the contact properties object. Any subscriber of the event can modify this object, and once the Emit() call ends, ign-gazebo reads the object and passes it to ign-physics, which, in turn, modifies the contact surface properties. I'm not sure if this is the best approach, but it works and is surprisingly efficient (the example world with a tracked vehicle is not noticeably slowed down when the track controllers are enabled).

Test it

Run one of the added example worlds - tracked_vehicle_simple.sdf or conveyor.sdf and use WSADX keys to control the vehicle/belt.

Checklist

• Signed all commits for DCO
• Added tests
• Added example and/or tutorial
• Updated documentation (as needed)
• Updated migration guide (as needed)
• codecheck passed (See contributing)
• All tests passed (See test coverage)
• While waiting for a review on your PR, please help review another open pull request to support the maintainers

Note to maintainers: Remember to use Squash-Merge

[peci1]

Videos of the implementation (they only work in Chrome, I don't know why).

ign-tracked1.mp4.mp4

ign-tracked2.mp4.mp4

ign-tracked3.mp4.mp4

ign-tracked4.mp4.mp4

[ahcorde]

Review linters

[jennuine]

Great work! Did a first pass, left some minor comments

[jennuine]

For the TrackedVehicle plugin, individually sent velocity commands (to a particular link) aren't being processed. I suspect it has something to do with the TrackedVehiclePrivate::UpdateVelocity overwriting the recently sent command.

For example, in tracked_vehicle_simple.sdf I set the model to be static and place an object on the right_track link. Then when publishing

ign topic -t /model/simple_tracked/link/right_track/track_cmd_vel -m ignition.msgs.Double -p "data: 10.0"

I'm expecting the belt to move the object (like in conveyor.sdf) but nothing happens.

[peci1]

Thanks for the review, I've addressed all of the remarks.

[azeey]

I've done a first pass. This is a fantastic contribution, but there are some issues that we need to address. (1) There's a significant performance hit due to the use of physics::CompositeData for the ContactSurfaceParams. (2) Leaking of ign-physics data types into the core ign-gazebo library. Suggestions below.

[peci1]

Thanks very much for the review. Just a first quick reply from a tent in the middle of a rainy nowhere - the idea of the PhysicsEvents.hh file was that it is only a header file so it brings no compile-time dependencies unless explicitly used. And the idea was that it should only be used by systems that interact with ign-physics. I just didn't find a better place for this file than besides the standard Events.hh, which might suggest that PhysicsEvents.hh is somehow equal to it - but it is not.

Generally, it seems to me that the current structure of ign-gazebo doesn't very well support code sharing between systems (remember the complications with VelocityLimiter).

[peci1]

@azeey I addressed the simple issues from your review. We still need to make agreement on PhysicsEvents and the related comments.

[chapulina]

Waiting until the end of September to merge this series of PR.

[peci1]

End of September has passed and I see the required DART release has already been done. Is there anything I can do to help merging this?

[azeey]

End of September has passed and I see the required DART release has already been done. Is there anything I can do to help merging this?

@peci1 I'm working on making gazebosim/gz-physics#267 work Focal since we use upstream DART there. It'd also be nice to add tests there since the tests are only in ign-gazebo right now. A simple test that shows the callbacks are being triggered would be great.

[peci1]

Ok, I'll have a look at ign-physics#267 and try to add some tests. Does using upstream DART mean that all of the customizations need to be upstreamed and released upstream first?

[azeey]

Ok, I'll have a look at ign-physics#267 and try to add some tests.

Thanks!

Does using upstream DART mean that all of the customizations need to be upstreamed and released upstream first?

No, we've been using #if macros to check which version is being used (eg. https://github.com/ignitionrobotics/ign-physics/blob/main/dartsim/src/ShapeFeatures.cc#L510). That being said, we do plan to migrate to using upstream in the (hopefully) near future, so these customizations will have to be upstreamed at some point. Any help with that would be appreciated 🙂 .

[peci1]

So until the upstreaming happens, these changes will only be available in Dome on Melodic, but not in Dome on Noetic? That's kind of weird...

[peci1]

Thanks for the review, I fixed everything that was requested.

[azeey]

Thanks again for this great contribution and for iterating on the PR. I'll make a stable release of ign-physics and run CI one more time before merging.

@ahcorde , @jennuine, have the changes you requested been addressed?

[jennuine]

Everything looks good and great work @peci1 !

The only thing that isn't resolved is this comment I made here: #869 (review)

It seems you can not send individual command velocities to a particular track but can only send it to the whole model. IMO, if not resolved in this PR, it would be good to create an issue to be resolved in future. What are your thoughts @azeey ?

[peci1]

@jennuine you're right this hasn't been resolved yet. But I don't know what would be the best approach. If I group several tracks into a tracked vehicle controller and the controller recieves some commands (or zeros), I would expect the tracks to follow the computed speeds to satisfy the vehicle controller. If I send individual commands, the vehicle controller request would not be satisfied. So what would be the best approach? Should the vehicle controller "yield" the tracks to others after some timeout? Or should the commands in each time step be summed up? I'm inclining towards the latter...

[azeey]

It seems you can not send individual command velocities to a particular track but can only send it to the whole model. IMO, if not resolved in this PR, it would be good to create an issue to be resolved in future. What are your thoughts @azeey ?

This is only the case when the TrackedVehicle system is being used right? If so, I would expect TrackedVehicle to take full control. So IMO, it's working as it should. We have a similar situation with MulticopterMotorModel and MulticopterVelocityControl. If users need to control individual tracks, they can just remove the TrackedVehicle system from the SDFormat file. Perhaps we can add a topic/service to disable the TrackedVehicle system so that switching to individual track control can be done on the fly, but I wouldn't block this PR on that.

[jennuine]

This is only the case when the TrackedVehicle system is being used right? If so, I would expect TrackedVehicle to take full control. So IMO, it's working as it should. We have a similar situation with MulticopterMotorModel and MulticopterVelocityControl. If users need to control individual tracks, they can just remove the TrackedVehicle system from the SDFormat file. Perhaps we can add a topic/service to disable the TrackedVehicle system so that switching to individual track control can be done on the fly, but I wouldn't block this PR on that.

Sounds good to me

[peci1]

@azeey On the real robot, the cmd_vel controller only commands the tracks when an actual velocity command has been received. And the tracks execute the last received velocity command for 0.1 s and then stop (if no other command comes). This is intended as a safety feature, so that if the publisher dies, the robot doesn't go forever. I think a similar approach could be used here (including or not including the command timeout) - the vehicle plugin could publish only when it receives a command.

[peci1]

@azeey Give me a while, I'm gonna give it a try

[peci1]

@azeey @jennuine Got it working. The vehicle plugin only sends commands when it receives new cmd_vel (care had to be taken when the speed limiter is in action).

The track controller also got an optional max_command_age parameter which will stop the track if no commands are received for some time. But it is not needed for the individual track control - it is a new feature.

I had to fix one value in the integration test - in the part where the robot climbs the stairs. There is probably some subtle difference in the workings of the vehicle controller after this update. But I tested manual driving and it behaves well. The gtest check had anyways tolerance 0.15 meters, so the result of the driving was kind of less determined.

The difference could be that the center of rotation (in world coordinates) is now only recomputed when a new command arrives. The position of the center of rotation is a value that is relative to the robot body and given by the desired rotation radius. If the robot does not slide or something, the position of the center of rotation should be the same as it is driving around it - until a command changes. So the difference with the latest update is that the center of rotation will stay in its world-coord place regardless of robot sliding etc. I think this is a minor glitch with which we can live. The worst thing that can happen is that the turning radius will be slightly different on bumpy and slidy terrain. But given the fact that the tracked vehicles are skid-steer, nobody can probably expect some very precise turning motions. Last, I expect that most robot control algorithms will periodically publish to cmd_vel, and will thus force the update of the center of rotation anyways.

[peci1]

I added a test with a conveyor that tests just the track controller, and verifies the speed limiter and command timeout work as intended.

[azeey]

LGTM! It's good that the command timeout is optional. This way it behaves the same as the diffdrive controller by default.

Feedback has been addressed

[peci1]

Thanks a lot to all of the reviewers!