R1 model incorrect

[bperseghetti]

R1 SDF is making gazebo run in non-realtime.

[Jaeyoung-Lim]

@bperseghetti What gazebo version are you running?

[bperseghetti]

@Jaeyoung-Lim we are intentionally running gazebo 11. It all seems to work fine in gazebo 11 on commit 8468271. Are you guys planning to officially switch to gazebo 11 anytime soon? Would be nice to use the intended ros-noetic gazebo target.

[TSC21]

@Jaeyoung-Lim we are intentionally running gazebo 11. It all seems to work fine in gazebo 11 on commit 8468271. Are you guys planning to officially switch to gazebo 11 anytime soon? Would be nice to use the intended ros-noetic gazebo target.

@bperseghetti we are not supporting officialy yet since there are some problems to solve still, including in the build process and CI. But it's an ongoing work.

[TSC21]

Regarding your problem: it might require to bump the SDF version of the model to 1.7. I would try that and check if it works or if it still complains about the DOM.

[bperseghetti]

@TSC21 Side note, have you tested the r1_rover recently? If you run it with verbose:=true and gui:=true I think you will notice that after about a minute it drops sim time down to ~.02 (sometimes lower), In comparison I can put 10 iris models in the sim and still run it at 1.00 (2 Titans in SLI, 24 cores and 256GB ram make all of them barely register on my machine). I think it might have to do with the number of contact points in the wheel mesh.

[TSC21]

@TSC21 Side note, have you tested the r1_rover recently? If you run it with verbose:=true and gui:=true I think you will notice that after about a minute it drops sim time down to ~.2 (sometimes lower), In comparison I can put 10 iris models in the sim and still run it at 1.00 (2 Titans in SLI, 24 cores and 256GB ram make all of them barely register on my machine). I think it might have to do with the number of contact points in the wheel mesh.

Yep we have noted that on recent tests. @Jaeyoung-Lim has been investigating it as well.

[bperseghetti]

@TSC21 @Jaeyoung-Lim I took a look at the model's wheel contact STL and it looks overly complex. I think if we simply replaced it with a reduced mesh model it might work significantly better. Or why not use something like this for a 6in diameter wheel with 3in width:

<collision name="collision">
  <geometry>
    <cylinder>
      <radius>0.0762</radius>
      <length>0.0762</length>
    </cylinder>
  </geometry>
</collision>

[Jaeyoung-Lim]

@bperseghetti Have you tried it with the cylinder?

The reason we are using collision as a mesh is

1. This is a port from https://github.com/aionrobotics/r1_ugv_sim and that is what they had
2. I did try simplifying to a cylinder, but this broke the dynamics so I didn't move forward on this.

[bperseghetti]

@Jaeyoung-Lim I can modify and test. I created an autonomous tractor for a customer in Gazebo 3 years ago and it had very large complex treaded wheels. I used the method I posted above and it worked well. Give me a few to edit the SDF.

[bperseghetti]

@Jaeyoung-Lim @TSC21 Good news, Real Time Factor stays at 1.00 after 10 minutes+.
Not as good news the model does vibrate somewhat. I think we need to modify the SDF for dampening. But at least you can run it without it grinding the sim down.

[bperseghetti]

[bperseghetti]

@TSC21 @Jaeyoung-Lim
So I have fixed it that it now "drives around" in manual mode with Real Time Factor. However, it does not seem to be mapped as what I would expect. I would have thought throttle would make it go forward/backward (instead yaw makes it go forward/backward) And it doesn't seem to allow for differential/skid steering, even when adding/changing torsional friction to the SDF). It no longer bounces at least.

[bperseghetti]

@Jaeyoung-Lim @TSC21 https://youtu.be/jE5jdurBPUw

[Jaeyoung-Lim]

@bperseghetti Yes that is exactly what I was seeing, and that was why I reverted to mesh collision for the wheels. You can look at the discussion here: #394

[bperseghetti]

@Jaeyoung-Lim Ok, well with the current reduced wheel mesh on my just shy of super computer it doesn't even get to .1 Real Time Factor and also doesn't move in manual mode. I think adding the slip parameters to the SDF also might allow the rest of the wheels to overcome the frictional resistance. Also here is an example of a skid steer project in gazebo that is frequently referenced: https://github.com/husky/husky

[Jaeyoung-Lim]

@bperseghetti I didn't mean it to sound as it is not possible.

I think the husky model uses a differntial drive plugin, which models the differential drive dynamics (please correct me if I am wrong)

Would you be able to take this further? It would be awesome if we can get this working.

[TSC21]

@bperseghetti does the initial reported problem got solved to you? If yes, let's close this and move this discussion to a separate thread as this has diverged from the initial issue.

[bperseghetti]

@TSC21 Sadly no, bumping to 1.7 still provided same error.
We can move the rest of this to new thread or maybe I should just rename this thread since there is a bunch already in it for the R1.

[bperseghetti]

@TSC21 @Jaeyoung-Lim I found that the motors are not hooked up correctly and I believe the mixing is wrong. Using QGC mavlink inspector I noticed that servo1_raw and servo2_raw are linked together and respond together to yaw input. Furthermore, servo6_raw and servo7_raw are linked together and respond to throttle input. In the SDF there is no correct connection to these. I connected them correctly by changing what is currently in the SDF to:

      <control_channels>
        <channel name="left_front_wheel_drive">
          <input_index>0</input_index>
          <input_offset>0</input_offset>
          <input_scaling>40</input_scaling>
          <zero_position_disarmed>0</zero_position_disarmed>
          <zero_position_armed>0</zero_position_armed>
          <joint_control_type>velocity</joint_control_type>
          <joint_name>lf_wheel_joint</joint_name>
          <joint_control_pid>
            <p>0.1</p>
            <i>0</i>
            <d>0</d>
            <iMax>800</iMax>
            <iMin>-800</iMin>
            <cmdMax>1200</cmdMax>
            <cmdMin>-1200</cmdMin>
          </joint_control_pid>
        </channel>
        <channel name="left_back_wheel_drive">
          <input_index>1</input_index>
          <input_offset>0</input_offset>
          <input_scaling>40</input_scaling>
          <zero_position_disarmed>0</zero_position_disarmed>
          <zero_position_armed>0</zero_position_armed>
          <joint_control_type>velocity</joint_control_type>
          <joint_name>lb_wheel_joint</joint_name>
          <joint_control_pid>
            <p>0.1</p>
            <i>0</i>
            <d>0</d>
            <iMax>800</iMax>
            <iMin>-800</iMin>
            <cmdMax>1200</cmdMax>
            <cmdMin>-1200</cmdMin>
          </joint_control_pid>
        </channel>
        <channel name="right_front_wheel_drive">
          <input_index>5</input_index>
          <input_offset>0</input_offset>
          <input_scaling>40</input_scaling>
          <zero_position_disarmed>0</zero_position_disarmed>
          <zero_position_armed>0</zero_position_armed>
          <joint_control_type>velocity</joint_control_type>
          <joint_name>rf_wheel_joint</joint_name>
          <joint_control_pid>
            <p>0.1</p>
            <i>0</i>
            <d>0</d>
            <iMax>800</iMax>
            <iMin>-800</iMin>
            <cmdMax>1200</cmdMax>
            <cmdMin>-1200</cmdMin>
          </joint_control_pid>
        </channel>
        <channel name="right_back_wheel_drive">
          <input_index>6</input_index>
          <input_offset>0</input_offset>
          <input_scaling>40</input_scaling>
          <zero_position_disarmed>0</zero_position_disarmed>
          <zero_position_armed>0</zero_position_armed>
          <joint_control_type>velocity</joint_control_type>
          <joint_name>rb_wheel_joint</joint_name>
          <joint_control_pid>
            <p>0.1</p>
            <i>0</i>
            <d>0</d>
            <iMax>800</iMax>
            <iMin>-800</iMin>
            <cmdMax>1200</cmdMax>
            <cmdMin>-1200</cmdMin>
          </joint_control_pid>
        </channel>
      </control_channels> 

[bperseghetti]

@TSC21 @Jaeyoung-Lim I believe the correct mixing for these should be:
throttle_max | m0,m1 -> 2000 | m5,m6 -> 2000
throttle_min | m0,m1 -> 1000 | m5,m6 -> 1000
yaw_max | m0,m1 -> 2000 | m5,m6 -> 1000
yaw_min | m0,m1 -> 1000 | m5,m6 -> 2000
the vehicle ever so slowly rotates when simultaneously increasing throttle and decreasing yaw.
If I add a small rotation to the revolute joints to be chiral from left to right and all be tangential along an ellipse that allows symmetric forward to always cancel rotational out but if need be you can oppose the right and left and the components should allow it to rotate much better. Does anyone know who is in charge of the mixer for the R1?

[bperseghetti]

@TSC21 @Jaeyoung-Lim By doing what I mentioned above I am able to make the vehicle "skid steer" I rotated the links chiral by .2 rad and was able to do this: (yes I know it's not "perfectly pretty" but it at least moves and runs in real time)
https://youtu.be/1ZbkIDfGflc

[bperseghetti]

Appears the mixer used is: https://github.com/PX4/Firmware/blob/dfa5ca17104d1174d3e9aeeb3103d1cc592b1632/ROMFS/px4fmu_common/mixers-sitl/rover_sitl.main.mix and is what is loaded in 1060_rover. It should be more based off of: https://github.com/PX4/Firmware/blob/master/ROMFS/px4fmu_common/mixers/generic_diff_rover.main.mix

Here is a corrected version that I have tested:

Mixer for SITL rover
=========================================================

Output 0
---------------------------------------
M: 2
S: 0 2  10000   10000      0 -10000  10000
S: 0 3  10000   10000      0 -10000  10000

Output 1
---------------------------------------
M: 2
S: 0 2  10000   10000      0 -10000  10000
S: 0 3  10000   10000      0 -10000  10000

Output 2
---------------------------------------
Z:

Output 3
---------------------------------------
Z:

Output 4
---------------------------------------
Z:

Output 5
---------------------------------------
M: 2
S: 0 2  -10000  -10000      0 -10000  10000
S: 0 3   10000   10000      0 -10000  10000

Output 6
---------------------------------------
M: 2
S: 0 2  -10000  -10000      0 -10000  10000
S: 0 3   10000   10000      0 -10000  10000

Output 7
---------------------------------------
Z:

Output 8
---------------------------------------
Z:

[bperseghetti]

@Jaeyoung-Lim @TSC21
I think the wheelbase is off in both of these:
https://github.com/PX4/Firmware/blob/master/ROMFS/px4fmu_common/init.d-posix/1060_rover
https://github.com/PX4/Firmware/blob/master/ROMFS/px4fmu_common/init.d-posix/1061_r1_rover
Note that what actually gets loaded is what is in 1060 and not 1061 as when I modify rover_sitl.main.mix it actually get's reflected when I launch the R1.

[bperseghetti]

@LorenzMeier @TSC21 @Jaeyoung-Lim Changed:

• Correctly aligned fdir vector
• Altered the friction models
• Correctly linked the motors
• Removed the collision mesh for properly rotated cylinder
• Changed the inertia values to be correct
• Changed the mixing file

Now you have a working R1 that no longer has a "Real Time Factor" of 0.02 (that's what I got when testing on a standard system with 1080Ti, 10 core processor, etc).

Here's a video of me driving it around with the fixes using QGC and a logitech gamepad: https://youtu.be/PczMFFk1Ltc

Things to add to make it better: Joint friction, potential fixes for IMU as it seems rather unstable (I added visualization in sdf if uncommented) and also I believe the wheel base and some other values are off for 1060_rover/1061_r1_rover (though this seems to use the 1060_rover currently).

My changes were modified off commit 8468271 in sitl_gazebo and commit 2bbdef2 in Firmware (Beta2 tag). It all works in ros-noetic and gazebo 11.

PRs PX4/PX4-Autopilot#15209 and #534

[bperseghetti]

Oh and if anyone is wonder the wheels are no longer needed to be slightly rotated (as you can see from above video). Like I showed earlier in the day. Yay!

[bperseghetti]

@TSC21 @Jaeyoung-Lim Side note, has anyone ever tested the controller for skid steer rover? The only really thing you need is a simple PID for your attitude (yaw) and vehicle velocity (throttle). I'm halfway just tempted to use the output from the gazebo body state to then feed back in to a simple PID for what I need my custom skid steer rover to do. If there is some sort of "skid steer outer loop" doesn't seem to be working very well, maybe it's setup with the assumption of ackerman steering where you can't do a turn in place? I don't even see how wheelbase information is relevant for any estimate for skid steer. At any point you can essentially tell it to "change orientation" and there is no real "penalty" that you have to plan an arc/path for. Anyone know who might be responsible for skid-steer control?

[Jaeyoung-Lim]

@bperseghetti If you are talking about the rover PX4 firmware, actually that is the reason I was planning to add a body rate controller for steering. This would be a more sensible way of handling a "steering input" in my opinion.

[bperseghetti]

@TSC21 have any insight into this, this what I get when compiling current master and using the latest apt-based mavlink/mavros:

/home/user_name/git/project_name/src/sitl_gazebo/src/gazebo_mavlink_interface.cpp: In member function ‘void gazebo::GazeboMavlinkInterface::LidarCallback(gazebo::LidarPtr&, const int&)’:
/home/user_name/git/project_name/src/sitl_gazebo/src/gazebo_mavlink_interface.cpp:1048:14: error: ‘mavlink_distance_sensor_t’ {aka ‘struct __mavlink_distance_sensor_t’} has no member named ‘signal_quality’
 1048 |   sensor_msg.signal_quality = lidar_message->signal_quality();
      |              ^~~~~~~~~~~~~~
/home/user_name/git/project_name/src/sitl_gazebo/src/gazebo_mavlink_interface.cpp: In member function ‘void gazebo::GazeboMavlinkInterface::SonarCallback(gazebo::SonarPtr&, const int&)’:
/home/user_name/git/project_name/src/sitl_gazebo/src/gazebo_mavlink_interface.cpp:1120:14: error: ‘mavlink_distance_sensor_t’ {aka ‘struct __mavlink_distance_sensor_t’} has no member named ‘signal_quality’
 1120 |   sensor_msg.signal_quality = sonar_message->signal_quality();
      |              ^~~~~~~~~~~~~~
make[2]: *** [sitl_gazebo/CMakeFiles/gazebo_mavlink_interface.dir/build.make:63: sitl_gazebo/CMakeFiles/gazebo_mavlink_interface.dir/src/gazebo_mavlink_interface.cpp.o] Error 1
make[1]: *** [CMakeFiles/Makefile2:6673: sitl_gazebo/CMakeFiles/gazebo_mavlink_interface.dir/all] Error 2
make[1]: *** Waiting for unfinished jobs....
[ 98%] Linking CXX shared library /home/user_name/git/project_name/devel/lib/libLiftDragPlugin.so
[ 98%] Built target LiftDragPlugin
make: *** [Makefile:163: all] Error 2
Invoking "make -j48 -l48" failed

[bperseghetti]

@Jaeyoung-Lim Yes, it appears there is no specific PX4 Firmware controller for it? Hahaha... Or if there is... Seems like it needs a lot of help.

[bperseghetti]

@Jaeyoung-Lim you use the PX4 slack channel? If so want to discuss this on there more?

[Jaeyoung-Lim]

@bperseghetti Sure I am on slack

[bperseghetti]

@Jaeyoung-Lim driving rover back and forth in sim for an hour and a half has not slowed down the sim run time from 1.00

[bperseghetti]

@Jaeyoung-Lim I can upload an image of the old models performance if you want me to but it hits Real Time Factor of 0.03 100% of the time after 2.5 minutes.

Are we good on the PR #534

[Jaeyoung-Lim]

@bperseghetti Sure, it seems like the wrong numbering of the autostart script was the cause. Also the generic rover now has realtime factor close to 1

[bperseghetti]

@Jaeyoung-Lim Awesome. Looks like after ~3hrs it's still running strong at realtime:

[bperseghetti]

Fixed with PR PX4/PX4-Autopilot#15209 and #534