Added the simple tracked vehicle utilizing Contact Surface Motion method of track simulation.
I'm gonna publish a paper at IROS 2017 about this method, so I wanted to share it to the community.
This is just a first shot at the API. I'll welcome any comments. What I'm most unsure about is:
I wanted to create a common interface all tracked vehicle plugins could use. However, I had some problems with linking or dynamic loading when I just dynamically linked the "bottom" plugin against the API plugin. Some discussion about rpath has already been there, and the agreement was that for plugins it's gonna stay there. But I'm not sure if it is expected to work "inside" Gazebo, or only for external code. For now, I'm building the API library statically, which I know is not ideal, but is the only thing that worked for me.
Is gazebo8 the right target? I didn't change anything outside the plugins dir (+ changelog and worlds).
I resorted to writing my own SDF parameter loading helpers, because the way they are read in other plugins seems really cumbersome to me (bascially copy&paste 10 lines of code for each parameter). Shouldn't that code be moved somewhere "higher" in the class hierarchy?
Is there no 3-valued signum function available in standard C/C++ library or ignition?
Should the example model and obstacle be in this repo, or should they be moved to the OSRF model library?
If I'll find more time, I'll add the other models suggested in https://bitbucket.org/osrf/gazebo/issues/863/tracksdriveplugin (I already have the implementations, but they need some cleanup). One thing that really stopped me for a long while was programmatic building of the model (generating the track pieces). I ended up writing SDF in C strings and calling Link::Load() or Joint::Load(). And removing links/joints seems to be even bigger pain in *** . Is there some tutorial on programmatic building/destroying of the world? There also seems to be a (possibly unresolvable) problem that in SDF you can't specify a link connected to a joint that will be auto-generated by a plugin.
Gazebo 7 is unfortunately a bit trickier, since I need to #include <gazebo/ode/contact.h> in SimpleTrackedVehicle.cc to get access to dContact. In G7, the ODE headers were not moved under the gazebo/ode include path, which makes it error-prone when there is a system install of ODE present. I was able to hack this in my G7 build file using:
I can create the LoadParam and signum PRs. I assume if I target them to the default branches of these libraries, this will be another show-stopper for G7.
Ruby is tricky if you want to have URDF as the initial input. I seek for run-time model generation/editing. My idea was to allow the user to specify a placeholder link (used e.g. in URDF) that will be deleted when the model is loaded and substituted with a track generated by the plugin.
Oh interesting, I hadn't noticed this was ODE-specific. Targeting Gazebo 8 should be fine. But now I'm wondering if there could be a way to make this physics-engine-agnostic… I'll need to look closer into the code for that
Adding functions usually doesn't break ABI, so you can try adding LoadParamand signum into sdf5 and `ign-math3` respectively, if you want. Those would be great contributions, but I wouldn't let them block this PR. Having the functions here is ok too.
Oh I see, so you want to generate the track inside the plugin C++ code? I've seen this being done before. Maybe a naive way to do it could be to have a world plugin which generates an SDF file and spawns a model from that, where that model has it's own model plugin.
Yes, it is very ODE-spcific. If you'd like to search for support in other engines, what's needed is support for friction velocity in both first and second friction direction.
Is there a mapping/wiki that would explicitly state "Gazebo 7 works with sdformat 5 and ign-math3" and so on? I'm a bit lost in these version numbers.
What I do now is to compose a piece of SDF in a string, load it, and then attach the loaded link/joint to the existing model. It's not ideal, but it works. What's bigger problem is deleting links/joints. I've never managed to delete them completely including visuals. I suspect I need the create and publish a "visual disappeared" message, but I don't know how. I assume the model editor does it somehow, but I haven't investigated into that too much.
I am trying to merge the changes to the source of Gazebo7 distributed in Ubuntu 16.04. I am having problems with the sdf::Element::<T>Get() function:
/home/fjsanchez/gazebo-7.0.0+dfsg/gazebo/common/Plugin.hh:208:62: error: no matching function for call to ‘sdf::Element::Get(const string&, double&)’
auto result = _sdf->Get<V>(_name, _defaultValue);
I think the function in the SDF library used by gazebo7 do not have support for default values. Also, in gazebo::physics::Link it cannot find the member BoundingBox and gazebo::physics::World do not have a member called Physics... Possibly more things are required to get it to work in G7 but I am new to Gazebo... any hint? Should I just move to Gazebo8 only to get that? What is the current plan for that PR?
thank you for the answer. I only wanted to use G7 for the LTS but I think that G8 should be ok for now. I have cloned the 8.1.1 tag from the repository and merged your changes. When I try to run your *.world file in Gazebo I get this:
gzserver: /tmp/gazebo/plugins/WheelTrackedVehiclePlugin.cc:93: void gazebo::WheelTrackedVehiclePlugin::LoadWheel(gazebo::physics::ModelPtr&, gazebo::Tracks&, const string&): Assertion `(joint->GetType() == physics::Base::HINGE_JOINT)&&(("Joint " + _jointName + " is not a hinge (revolute) joint.").c_str())' failed.
Also a heads up that there will be some style changes needed before this can be merged in. Here's our style guide for your reference. Just glancing through the code I can already observe some things which need change, like 50 / in front of functions, input parameters prefixed by _, pointer's *close to the variable and so on…
I realize that adapting to a new style can be tedious. I can try helping with that when I have some time.
I forgot to mention I've created this PR in a time pressure created by IROS deadline =) I wanted to refer it in my paper. Of course style changes are on my list, and I know about the style-checker script, which is now probably crazy from this PR :) I'll do the changes. Don't you have a CLion code-style config by the way?
I updated the code using the style guide as much as I can.
The only thing code_checker yells about is ./plugins/SimpleTrackedVehiclePlugin.hh:193: All parameters should be named in a function [readability/function] , but I think that line follows Google style guide.
I also tried to solve several issues:
I added the LoadParam function to Plugin.hh (where it makes more sense than in sdformat, because it is expected to write using gzdbg). Tell me if you think this way it's okay regarding ABI changes.
I changed plugins/CMakeLists.txt to address two issues:
Most include dirs are now included as SYSTEM, because e.g. protobuf has produced lots of warnings in my build
Yeah I know, it's not very clear where to find these things. Plugin tests are usually integration tests which load a world that already contains the plugin. You can see an example for the harness plugin here.
i have used you code to simulate a robot with 2 tracks and 4 flippers in gazebo 7. I modified the plugin, so that the variable "tracks" of SimpleTrackedVehiclePlugin can now store 6, instead of 2 LinkPtr's. Also BELT_CATEGORY, ROBOT_CATEGORY and, on the left side, LEFT_CATEGORY will be set on the flippers. Everything works on simple surfaces, but i found some bugs:
If the robot is driving/rotating e.g. on ramps, std::swap() (used in SimpleTrackedVehiclePlugin::DriveTracks) will be triggered. This causes the friction direction to point in the opposite direction. I have fixed this by setting beltDirection to -beltDirection, if std::swap has been used before. You can see the bug in the following video. The result of frictionDirection will be displayed as markers in rviz, with the length of the calculated value of motion1. If motion1 is not equal to zero, the arrows will become red. Also every contact point on the tracks is shown by a blue sphere. The linear and angular velocity, which i have manually controled with a controller, will be visible in the console: https://www.youtube.com/watch?v=u1lSbLIL9a4 In that example video i'am driving the robot onto aramp, and then just turn. But as you can see, the robot is moving sideways, instead of turning.
There is also a second bug. When the chassis touches the surface, and the tracks have no contact, the simulation will become really unstable and the robot will fly away. Have you encounterd a similar problem? You can see this in this video (the first bug is already fixed in this video, that's why the robot can now rotate on the ramp): https://www.youtube.com/watch?v=oPy5BFjr4g4
Hi @Philip Frieling , good to hear you more or less successfully use this plugin.
Regarding the first bug: it seems I can't get to a case where the swap would trigger in my scenarios. Could you share your world files? Nevertheless, it seems it makes sense to flip some vectors if the order of the bodies changes. However, it seems to me it'd be more proper the negate the contact normal, which would then affect more parts of the code that also access the contact normal data...
And regarding the second one - I can't replicate the behavior you describe. I tried it both in Gazebo 7 and Gazebo 8, and the problem doesn't manifest. Is the weight and inertia of your robot correct? Mine weighs about 20 kg, which adds a lot of stability.
Thanks for the quick reply.
I simplified my robot model, so that i can send it to you, but now i'am unable to replicate bug 1. I will try it tomorrow again...
I have investigated bug 2 a bit more, and it seems that it only happens on models, which have meshes for collision. If i try to build a similar ramp out of simple boxes, everything works good. I have uploaded a second video, where you can see a similar behavior, caused by other obstacles. I have uploaded the used sdf model in my dropbox, you can find it in the "object_robocup_metalbar" folder. I also uploaded my robot model, it it is written as a xacro xml file and launched by "getjag_new/launch/getjag_world.launch". Your can also see the source code, which i modified, located in "getjag_new/src/" - I don't think the inertia or weight is causing this issue, because i tested the same robot with wheels, behaving normal. I guess it has something to do with my meshes, used for the collision, but then it would also affect the wheeled version...
Today i have used the complete robot, and reproduced bug1 again. As you can see in my video, the swapping only takes places on custom collision meshes (the visual mesh was also used as collision element for the floor model). The effect causes the normal to flip in the opposite direction, as you can see from the arrows on my first screen and in the console. Sometimes this effects stays. You can see it at 56sec, which made my contros inverted until the robot touches normal ground again. - Also the contact normals, showed by gazebo, point into the wrong direction (between 56sec and 64sec).
Would a flipped face cause this affect? Maybe, somehow all the objects i'am using have flipped faces, which cause this affect. But why is this only affecting the robot after it turned? I also modelled the objects in blender, with backface culling enabled, so i would have spotted flipped faces...
I also think bug 2 is related to this, because it also only occures on custom meshes. But why does it only occur, when a non-track object, which is never touched by the plugin, hits a custom mesh? - Edit: If i only use box and cylinder collisions for my robot, bug 2 is no longer happening and the simulation is stable.
Cheers @Philip Frieling after some time. Today I "managed" to reproduce the issue with a rather simple scenario - I substituted an obstacle composed of a few boxes with just a single cylinder. And for some reason this cylinder started to be the first collision body when colliding with main tracks (but not flippers). I think I once tried to track down where is this collision body order given, and I think it looks like it's "random" (but consistent, as you can see).
I implemented a fix that tries to be more "intelligent" and might address both of your issues. Basically, instead of just flipping the normal when the track is not the first collision body, it makes sure that in any case the normal points "inside the track". So I think it should work well even on mesh surfaces with flipped normals.
If you have time, could you please test it and report your findings?
@Martin Pecka , I'd like to try and get this merged this week and I can help fixing conflicts and addressing final feedback if you don't have the time. It's targeted at gazebo 8, which is going EOL in a bit more than a month, can we retarget to gazebo9?
Great, Louise Of course, feel free to retarget to gazebo 9. This week it’s possible I’ll have some time to also work on it, so let’s see who’s gonna be faster. Just leave here a comment and a link to the new branch (or I’ll do that if I’m faster).
Okay, I can do that. Just how should I proceed technically? The PRs depend on each other (fortunately in a linear ordering ).
My suggestion: I’ll prepare 1. ASAP. Then, I’ll do the fixes required to get 2. approved over the weekend. And then, 3. will be left alone. I’m not sure what the commit history would look like then, but that’s probably a minor problem.