#2652 Open
Repository
peci1
Branch
tracked_vehicles
Repository
osrf
Branch
gazebo9

Bitbucket cannot automatically merge this request.

The commits that make up this pull request have been removed.

Bitbucket cannot automatically merge this request due to conflicts.

Review the conflicts on the Overview tab. You can then either decline the request or merge it manually on your local system using the following commands:

hg update 297d6947eb79
hg pull -r tracked_vehicles https://bitbucket.org/peci1/gazebo
# Note: This will create a new head!
hg merge dc60c5699f19
hg commit -m 'Merged in peci1/gazebo/tracked_vehicles (pull request #2652)'
Author
  1. Martin Pecka
Reviewers
Description
  • Added the base for tracked vehicles.
  • 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.

Comments (41)

  1. Louise Poubel

    Hi @Martin Pecka , thanks for the contribution, that video looks awesome! I'm going to try this soon!

    Answering some of your questions:

    • Since you didn't change any Gazebo API / ABI, you could even target this to gazebo7 if you want. That is a long term support version.
    • I think the LoadParam function is a great idea! This could probably go into SDFormat.
    • Apparently there isn't an out-of-the-box signum in C++, but we could add one to ignition::math::Helpers
    • I think it should be fine for the model to be embedded into the world as you put it, since it's an example and it's composed of simple shapes.
    • As for generating models programmatically, an option is to use embedded ruby, here's an example from the model database.
    1. Martin Pecka author

      Thanks for comments, Louise.

      • 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:
      list(GET GAZEBO_INCLUDE_DIRS 1 GAZEBO_INCLUDE_HACK)
      include_directories(${Boost_INCLUDE_DIR} ${GAZEBO_INCLUDE_DIRS} ${GAZEBO_INCLUDE_HACK}/gazebo  ${catkin_INCLUDE_DIRS})
      

      but that's not ideal I'd say.

      • 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.
      1. Louise Poubel
        • 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.
        1. Martin Pecka author
          • 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.
          1. Francisco Sánchez

            Hi,

            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?

            1. Martin Pecka author

              Hi Francisco,

              getting this to work with G7 seems to be a lot of extra work. This PR is targeted at G8, where it should merge cleanly. If you're on 16.04, there's (IMO) no point in staying with G7.

              Anyways, the following week, I should push a whole bunch of changes required by the reviewers (mainly tests), and then there's a chance this PR gets merged to the mainstream G8.

              Tell me if you succeded ;)

              1. Francisco Sánchez

                Hi Martin,

                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.
                

                Any suggestion?

  2. Louise Poubel

    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.

    1. Martin Pecka author

      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?

  3. Martin Pecka author

    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] [3], but I think that line follows Google style guide.

    I also tried to solve several issues:

      1. Louise Poubel

        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.

    1. Martin Pecka author

      @Louise Poubel here we go, I've finished writing some unit and integration tests that should cover at least the basic functionality.

      Further changes:

      • I cut out the keyboard control into a standalone plugin that converts keypresses to cmd_vel messages. Might be used/useful in other vehicle worlds.
      • In the model with tracks faked by wheels I changed the way track velocity is applied, because the approach used before was ODE-specific.

      Minor issues will be added as inline comments.

              1. Martin Pecka author

                I see some failures on Mac and Windows which I tried to fix.

                There are also failures in both Linux builds, but they seem to be unrelated. Is that normal?

      1. Louise Poubel

        Awesome, thanks a lot for putting effort into the tests! +1 for splitting the plugin, that should definitely be useful to others. I'll try to review this again soon.

  4. Philip Frieling

    Hey @Martin Pecka,

    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

    1. Martin Pecka author

      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.

  5. Philip Frieling

    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...

    1. Philip Frieling

      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.

      1. Martin Pecka author

        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?

  6. Louise Poubel

    @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?

    1. Martin Pecka author

      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).

        1. Martin Pecka author

          Thanks, I pushed the changes here (without modification). I tested locally, and I saw no related test failures. The GUI part and keyboard control also looked ok.

          1. Louise Poubel

            This is looking good to me overall. It is a large PR, and it will still need a 2nd approval. How do you feel about splitting it into smaller ones to make review easier? Something like this:

            1. Changes to plugin + tests (which I'm ready to approve)
            2. KeysToCmdVelPlugin (which I'm almost ready to approve once my comments are addressed - bonus if it's pimplized, but I'm ok with it either way)
            3. All the tracked vehicle stuff (which is working for me, but I'm still going over the code)
            1. Martin Pecka author

              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.

              1. Louise Poubel

                1 and 2 can be opened in parallel, and you could leave this PR open as 3. As we merge 1 and 2, this branch can be synced with gazebo9 and the diff will get smaller.

                All commits will be there in the history 😁