SITL R1 Rover Mission Mode spin around itself

[Sasysan]

Describe the bug

In SITL mode when i try to do a mission with the rover, i am able to load the mission in QGC but when i start the mission the rover don't go in the position but start to revolve around itself

To Reproduce

Steps to reproduce the behavior: 1.Pull down main (do the normal git sub-module and such) 2.run make px4_sitl gazebo_r1_rover 3.Open QGC

4. Start Mission

[Jaeyoung-Lim]

@Sasysan Does the same happen also for the normal rover?

[Sasysan]

No the normal rover is working as expected. The issue is only with the r1

[dev10110]

does anyone have an update on how to fix this issue? I am seeing it as well.

[Jaeyoung-Lim]

@dev10110 Would you be able to make a fix? This is probably due to a mistuned vehicle

[leogeo13]

Any progress on this? I'm running into the same issue. I'll start by tuning the gains and see if that fixes it.

[leogeo13]

I got rid of the spin. However, I still get drift and errors such as 'horizontal velocity unstable' and 'velocity estimate error.'

I deleted these two lines in 1061_gazebo-classic_r1_rover:

param set-default PWM_MAIN_FUNC6 102
param set-default PWM_MAIN_FUNC7 102

and added the following:

param set-default GND_MAX_ANG 2
param set-default PWM_MAIN_FUNC1 101
param set-default PWM_MAIN_FUNC2 102

[Jaeyoung-Lim]

@leogeo13 Awesome! Would you be able to make a pull request for the fix?

Thanks!

[dev10110]

Cool - awesome find @leogeo13

@kalebbennaveed can you try this as well?

@Jaeyoung-Lim sorry I somehow missed these messages. If it works for us, I can raise a PR.

[kalebbennaveed]

So @leogeo13 's fix doesnt really work for us.

Instead in ROMFS/px4fmu_common/init.d-posix/airframes/1061_gazebo-classic_r1_rover we noticed that the param GND_WHEEL_BASE was set to 2.0, instead of 0.3 and the GND_L1_DIST was 5.0 instead of a more reasonable 1.0

Making these changes meant that the rover was able not spin when we give it a position control setpoint, and the manual control setpoint (though QGC virtual joysticks) was reasonable.

However, we still notice the EKF drifting when using the position controller - it works for a little bit of time but then it eventually diverges.

Since the gazebo_classic-rover target works well in sim, we don't think there is an issue with the underlying rover_pos_control but rather with the tuning parameters of the controller. I am however rather confused as to why the ekf has issues.

[liu-hao000]

I also found this problem and tested it on 1.12.3 with the same result

[YongraeKim]

@Sasysan @Jaeyoung-Lim @dev10110

I found the way to solve this problem!

On r1_rover.sdf.jinja file, add these line under for each 'xx_wheel_link_collision'

          <slip1>0.05</slip1>
          <slip2>0.02</slip2>

For example,

  <collision name='rf_wheel_link_collision'>
    <pose>0 0 0 1.57079632679 0 0</pose>
    <geometry>
      <cylinder>
        <radius>0.0686</radius>
        <length>0.0762</length>
      </cylinder>
    </geometry>
    <max_contacts>1</max_contacts>
    <surface>
      <friction>
        <torsional>
          <coefficient>0.25</coefficient>
          <use_patch_radius>1</use_patch_radius>
          <surface_radius>0.0686</surface_radius>
          <patch_radius>0.015</patch_radius>
        </torsional>
        <ode>
          <mu>1.0</mu>
          <mu2>0.45</mu2>
          <fdir1>1 0 0</fdir1>
          <slip1>0.05</slip1>
          <slip2>0.02</slip2>
        </ode>
      </friction>

As Default r1_rover.sdf model doesn't contain and that prevent the wheel model not to slip to the lateral of the vehicle if they are set to zero. If you add and set and over 0.0, the vehicle moves what you expect to be without EKF2 diverging. So just adjust these values to your r1_rover.sdf.jinja file. I recommend slip1 to '0.05' and slip2 to '0.02'

I attach SITL log file. -before adding slip1 and slip2 params https://review.px4.io/plot_app?log=e4340823-e912-43e9-9dc1-25a0768a61e4 -after adding them https://review.px4.io/plot_app?log=a77bff45-85aa-4b54-a474-bf6d9708986e

This will help for sure!

[Jaeyoung-Lim]

@YongraeKim Thanks for the update. Please consider making a PR if you think this is an improvement

[liu-hao000]

@Sasysan @Jaeyoung-Lim @dev10110

I found the way to solve this problem!

On r1_rover.sdf.jinja file, add these line under for each 'xx_wheel_link_collision'

          <slip1>0.05</slip1>
          <slip2>0.02</slip2>

For example,

  <collision name='rf_wheel_link_collision'>
    <pose>0 0 0 1.57079632679 0 0</pose>
    <geometry>
      <cylinder>
        <radius>0.0686</radius>
        <length>0.0762</length>
      </cylinder>
    </geometry>
    <max_contacts>1</max_contacts>
    <surface>
      <friction>
        <torsional>
          <coefficient>0.25</coefficient>
          <use_patch_radius>1</use_patch_radius>
          <surface_radius>0.0686</surface_radius>
          <patch_radius>0.015</patch_radius>
        </torsional>
        <ode>
          <mu>1.0</mu>
          <mu2>0.45</mu2>
          <fdir1>1 0 0</fdir1>
          <slip1>0.05</slip1>
          <slip2>0.02</slip2>
        </ode>
      </friction>

As Default r1_rover.sdf model doesn't contain and that prevent the wheel model not to slip to the lateral of the vehicle if they are set to zero. If you add and set and over 0.0, the vehicle moves what you expect to be without EKF2 diverging. So just adjust these values to your r1_rover.sdf.jinja file. I recommend slip1 to '0.05' and slip2 to '0.02'

I attach SITL log file. -before adding slip1 and slip2 params https://review.px4.io/plot_app?log=e4340823-e912-43e9-9dc1-25a0768a61e4 -after adding them https://review.px4.io/plot_app?log=a77bff45-85aa-4b54-a474-bf6d9708986e

This will help for sure!

I tried your suggestion and the UGV ran hard, I mean, it wobbled towards the next waypoint. I will attach my logs, I use the V1.14.0 and make px4_sitl gazebo-classic_r1_rover to build。logs

[YongraeKim]

@liu-hao000 I suggest you to set GND_MAX_ANG over 60degrees(1rad) to reduce steering command of the vehicle. If you increase GND_MAX_ANG value then steering command will be smaller that will attentuate wobble things on your vehicle sim. On your log looks EKF2 didn't diverge so this will makes the situation better.

[liu-hao000]

@liu-hao000 我建议您将 GND_MAX_ANG 设置为超过 60 度（1rad），以减少车辆的转向指令。 如果您增加 GND_MAX_ANG 值，则转向命令会更小，这会减弱车辆 sim 上的摆动。 在你的日志中看起来 EKF2 没有发散，所以这将使情况变得更好。

Thank you for your suggestion. In addition, I referred to the GND_XXX_XXX configuration in your log. Now the UGV can basically follow the waypoints.logs

[YongraeKim]

@liu-hao000 Good!

[YongraeKim]

@YongraeKim Thanks for the update. Please consider making a PR if you think this is an improvement

I'll make a PR for this solution for the issue soon as it looks effective. Some preps are needed for a PR so it takes some time. Thank you!