Differential drive for Rover & Gazebo R1 Rover port

[PerFrivik]

Solved Problem

This PR addresses two key points:

Differential Module:

1. The existing RoverPositionControl module, handling both Differential and Ackermann rover configurations, limits the potential of each configuration due to their distinct maneuvering capabilities.

2. The module's interface elements are more suited for drones, leading to inefficiencies in rover control.

Gazebo R1:

1. With the deprecation and firction issues of Gazebo-classic, we are want to update the testing environment and lay the foundation for validating the current and upcoming rover architecture types.

Solution

Differential Module:

1. To seperate the individual rover types, we have started to restructure the control architecture into separate modules and vehicles types (differential, ackermann, mecanum, etc.), starting with the introduction of differential_drive_control module along side the DifferentialDriveKinematics library. The module is specifically tailored for differential rovers and, in its current release, supports only manual mode. Figure 1 presents our initial implementation plan, with areas requiring further development highlighted in red.

$$Figure. 1$$

Symbol	Description	Unit
$$V_x$$	Linear Velocity in the X-direction (relative to the rover's body frame)	m/s
$$\omega$$	Angular Velocity around the Z-axis	rad/s
$$\phi_{l,r}$$	Angular Wheel Velocity for the left (l) and right (r) wheels	rad/s

2. In the context of rovers, it is possible to directly map their angular wheel speeds to body velocities. Therefore we have implemented the DifferentialDriveKinematics library and with this approach, the controller's inputs are the linear and angular body velocities, and the outputs are the wheel angular speeds, which are directly published to actuator_motors. This way of control, allows us to bypass the control allocation module and directly control the wheels. The calculation of wheel speeds is based on the inverse kinematic equations for a differential drive rover, taking the requested body velocity rates as inputs.

Here are the inverse kinematics equations:

$$ \dot{\phi}_l = \frac{v_x}{r} - \frac{L}{2} \cdot \frac{\omega}{r} $$

$$ \dot{\phi}_r = \frac{v_x}{r} + \frac{L}{2} \cdot \frac{\omega}{r} $$

and for future references, the forwards kinematic equations:

$$ v_x = \frac{r}{2} (\dot{\phi}_r + \dot{\phi}_l) $$

$$ \omega = \frac{r}{L} (\dot{\phi}_r - \dot{\phi}_l) $$

Symbol	Description	Unit
$$L$$	Wheelbase, distance from left to right wheel	m
$$r$$	Radius of wheel	m

The kinematics equations used here are based on the motion model described by Frank Dellaert and Seth Hutchinson in the "Introduction to Robotics and Perception" course, specifically in the section on the differential drive robot's motion model. Further details and examples can be found in their course material.

Source: Motion Model for the Differential Drive Robot

Gazebo R1:

1. Ported the SDF from gazebo-classic to the newer Gazebo GZ version and introduced GZMixingInterfaceMotor, a new interface class for motor control. For the motor velocity control, we are using the joint_controller plugin provided by Gazebo (GZ).

Changelog Entry

For release notes:

Feature: Introduced base architecture for new rover modules
Feature: Introduced `differential_drive_control` module and `DifferentialDriveKinematics` library for enhanced control of differential rovers, currently supporting manual mode only.
Feature: Ported glazebo-classic R1 rover to Gazebo (GZ)

Alternatives

Open to any suggestions.

Test coverage

Unit/integration test: DifferentialDriveKinematicsTest
SITL tested

Context

Excuse the laggy videos...

Gazebo-classic:

gazebo_classic_r1_rover_short_git.mp4

Gazebo (GZ):

gz_r1_rover.mp4

[DronecodeBot]

This pull request has been mentioned on Discussion Forum for PX4, Pixhawk, QGroundControl, MAVSDK, MAVLink. There might be relevant details there:

https://discuss.px4.io/t/rc-speed-boat-with-px4-episode-1-using-px4-to-control-the-boat/28429/5

[slgrobotics]

I am not sure that any model would work if there's substantial inertias/masses and slippery surfaces involved, like it happens in real life.

Such vehicles can use torque/thrust values directly, just mapped to the wheels power/speed - and be controlled reactively by Speed PID and Yaw Rate controllers.

Would these types benefit from Differential Drive implementation? Will there be a way to model them in SITL/GZ? Any thoughts?

BTW, it is easy to see in GZ or Classic if adding some slippage to wheels and mass/inertia to body.

[PerFrivik]

I am not sure that any model would work if there's substantial inertias/masses and slippery surfaces involved, like it happens in real life.

Hey, I fully agree with your point. Once the speed, inertia, and masses are too high, or if surfaces are slippery, this model will indeed fail due to various reasons. But we must also consider cases where we want to drive slowly and with precision.

Such vehicles can use torque/thrust values directly, just mapped to the wheels power/speed - and be controlled reactively by Speed PID and Yaw Rate controllers.

Yes, I agree it would be possible to use torque/thrust values mapped directly to the wheel's power/speed, and control them closed loop with a wheel speed PID and Yaw Rate controllers. However, what is the benefit? If we close the loop on a rate controller, we achieve the same effect as using torque/thrust values, but we don't lose out on the possibility to directly map wheel velocities to body velocities. This could not only be a very cost-effective way to get a good estimation but also a very reliable model under good conditions. By using thrust and torque, we also lose a physical correlation to reality. For example, what does 50 NM of thrust mean for a rover? It will depend on the ground state, the wheels, the weight, inertia, etc. But if you tell your rover to spin their wheels at 10 rpm, you get a direct correlation to its body velocity.

Would these types benefit from Differential Drive implementation? Will there be a way to model them in SITL/GZ? Any thoughts?

Regarding the question about Differential Drive implementation and whether these types could be modeled in SITL/GZ: I'm not entirely clear on this question. I already have an SITL implementation. If you use 'make px4_sitl gz_r1_rover' in this branch, you can test it.

BTW, it is easy to see in GZ or Classic if adding some slippage to wheels and mass/inertia to body.

Once again, I fully agree with your point that there will be errors. But in my opinion, we gain nothing from using thrust and torque, instead of rates. Closing the loop will have the same effect either way; it's all about PID tuning.

Thank you for your comments @slgrobotics ! I'd love to hear what you think about this; I am still learning.

[PerFrivik]

P.S I'll be in the community call tomorrow, so we can talk again there :)

[slgrobotics]

@PerFrivik - thanks for explanation, I am trying to understand the nature of this PR, just learning.

For my question: "Would these types benefit from Differential Drive implementation? Will there be a way to model them in SITL/GZ?" - sorry for confusion. What I meant is this:

1. There is a "wall" between the code that belongs to the robot and the code that belongs to simulation. Code belonging to the robot will, of course, work in real life. Code on the simulation side (like GZ plugins, including an existing DD plugin there) is somewhat obscuring real behaviors and, IMHO, should be very lean and not attempt to do vehicle-related logic (like kinematics).
2. if the new DD implementation stays on the "robot side" and will work in real life (whatever the control loops are) - I am all for it and just looking forward to try it in sim and in reality.
3. if this is an attempt to fatten the "sim side" - I am not sure of the benefits (to my project at least). That's why I couldn't use the existing GZ DD plugin, and hope to figure out more direct JointController use there.

When it comes to PID loops, I just look at parameters that we can independently measure and may need to control. For example, in turns we may measure heading error or crosstrack distance and yaw rate. First PID of the cascade takes heading error or crosstrack distance and produces yaw rate setpoint. Second PID (a.k.a. Rate Controller) stabilizes the yaw rate with its torque output. That allows me to turn to the next waypoint and then stick to the AB line. Meanwhile Speed PID takes x velocity and outputs thrust. Control Allocation delivers torque and thrust to the wheels.

[PerFrivik]

@slgrobotics - Thanks for your response.

Same Code for Simulation and Real Rover: I want to point out that the code for both the simulation and the real rover is the same. I don't use any differential drive (DD) plugin in Gazebo. Here's a link to a video showing it working on the actual rover: Real Rover Video.

Differential Drive Library: Like I mentioned, the differential drive library I added works the same in simulation and in real life. There's no difference in how it's used.

Using JointController in GZ Simulation: In my Gazebo (GZ) simulation, I'm currently using the JointController to control wheel speeds in gazebo.

By the way, I've also written some guidance logic for the differential drive rover that's not in this PR. It all runs with my DD library. You can see it here: Guidance Logic Video.

Here is the logic, this is just a test and it's be no means perfect, but thats not what this PR is about:

About your last point on PID: I agree that closing the loop as you mentioned, by first calculating an error and then a yaw rate setpoint, is the right approach. But my main problem with using thrust and torques, like you said, is understanding what they actually mean physically.

Control Allocation delivers torque and thrust to the wheels.

What does this really mean? And if we're already calculating a yaw rate setpoint ("First PID of the cascade takes heading error or crosstrack distance and produces yaw rate setpoint."), why should we change that to torque when we can control the motors directly to get that yaw rate?

I think the big plus of rate controls and not using torque and thrust is that you get a better feel for your system. In manual mode, if you have encoders and know your forward kinematics, you can guess how fast you're going pretty well. You can set your controls to drive at, say, 5 m/s, and then your system can work out the wheel speeds needed for that. If you use torques and thrusts, what are you going to tell your system? "Drive with 10 NM forward"?

I believe that working directly with rates open a door to more accurately control your system, also this new base will allow better support for vehicles with more complex and accurate kinematic estimation, like the mecanum wheel robot.

Let me know if this was clear :)!

[slgrobotics]

@PerFrivik - Thanks for clarification, this discussion illustrates how things that are obvious to the author aren't clear to a casual user.

Some points you've mentioned:

"..main problem with using thrust and torques, like you said, is understanding what they actually mean physically" ... "If you use torques and thrusts, what are you going to tell your system? 'Drive with 10 NM forward'?" - in PX4 Control Allocation logic torque and thrust are vectors in VehicleTorqueSetpoint.msg and VehicleThrustSetpoint.msg, normalized. So, there is no physical meaning - except for "percentage of actuators ability". It becomes meaningful only when PIDs start comparing physical measurements (speed, angle, distance to AB line, yaw rate) to setpoints and adjusting torque and thrust to achieve the desired values. So, I am thinking of these outputs as a direct way to move levers on my lawnmower back and forth. The same way I use R/C sticks.

Generally speaking, PIDs are a poor man's way to avoid modelling at all, especially when modelling is not possible. In my case, I don't even have to use wheel base or wheel diameter as parameters. Encoders? We have blades instead ;-)

"the big plus of rate controls and not using torque and thrust is that you get a better feel for your system" - correct me if I am wrong, but the lib/rate_control.cpp is just a fancy PID controller. You probably meant something else by rate controls.

While VERY rusty on math, I do understand what you are trying to do and am looking forward to try it, if at all possible. Id' like to hear from you - how would you approach modeling my full size gasoline powered zero turn mower in terms of Diff Drive parameters - well, not modelling in general, but just using your Diff Drive module for it. There is a large gap between true modelling and applying helpful abstractions to real life, and "it all depends" ;-) - sometimes we can't do it. Even applying a Diff Drive abstraction to a four wheeled R1 robot is a bit of a stretch, I guess.

Honestly, I need to dive a bit in your code and see what's happening. I'll do it soon. Meanwhile don't take my ramblings too seriously, what you do leads to better control and is very valuable.

[slgrobotics]

@PerFrivik - I tested "make px4_sitl gz_r1_rover" on your branch (Ubuntu 22.04, Intel I5), manual works fine, attempting a Mission - no movement.

[PerFrivik]

@PerFrivik - I tested "make px4_sitl gz_r1_rover" on your branch (Ubuntu 22.04, Intel I5), manual works fine, attempting a Mission - no movement.

It currently only supports manual mode. The video of the mission mode, are some next coming steps, but it might take some time.

[PerFrivik]

Generally speaking, PIDs are a poor man's way to avoid modelling at all, especially when modelling is not possible. In my case, I don't even have to use wheel base or wheel diameter as parameters. Encoders? We have blades instead ;-)

"the big plus of rate controls and not using torque and thrust is that you get a better feel for your system" - correct me if I am wrong, but the lib/rate_control.cpp is just a fancy PID controller. You probably meant something else by rate controls.

While VERY rusty on math, I do understand what you are trying to do and am looking forward to try it, if at all possible. Id' like to hear from you - how would you approach modeling my full size gasoline powered zero turn mower in terms of Diff Drive parameters - well, not modelling in general, but just using your Diff Drive module for it. There is a large gap between true modelling and applying helpful abstractions to real life, and "it all depends" ;-) - sometimes we can't do it. Even applying a Diff Drive abstraction to a four wheeled R1 robot is a bit of a stretch, I guess.

The only two parameters I need are wheelbase and wheel diameter! I am not quite sure why the R1 abstraction is not sufficient, maybe you could elaborate on that part, but I would strongly suggest you look at this page, which I found to be very helpful:

https://www.roboticsbook.org/S52_diffdrive_actions.html

What you are saying regarding the models is true, but there is a famous quote that I love.

Honestly, I need to dive a bit in your code and see what's happening. I'll do it soon. Meanwhile don't take my ramblings too seriously, what you do leads to better control and is very valuable.

Alright, that sounds good, if you have any questions about it, please let me know! :) I think if you read the code, you will understand what I mean by rate controller, sorry if that was not quite clear here. I really appriciate the feedback, hope to hear more from you soon.

[slgrobotics]

@PerFrivik - I looked at the code and reviewed the PR description. Here are random thoughts.

1. Diff Drive (DD) formulas and model are usually on the first several pages of any Robotics course, it has value and it is a good idea to have it in the library. Whether it is applicable to a particular robot - is "in the eyes of the beholder".
2. I am wondering if you see your DD Controller as a thin layer, subscribing to "body control" messages (similar to attitude setpoints) coming from higher level navigation and position control modules (currently operating in terms of torque and thrust).
3. Your model outputs (Linear and Angular velocities) after the required normalization are indistinguishable from PX4 thrust and torque setpoints (those are usually fed to Control Allocation). I am not sure if filling actuator_motors_s directly is better than filling vehicle_thrust_setpoint_s and vehicle_torque_setpoint_s - neither is expressed in physical units anyway.
4. "Rate Controller" term is already taken in PX4, using it in DD context is confusing to me.
5. I am looking forward to see your code performing Missions, hopefully it will align well with existing PX4 architecture and style. I hope it becomes a good example/base for customizations and experimentation.

Touching on your question on why R1 is not a good DD example - well, it has four wheels ;-) DD model works well for two-wheelers (balancers or those with casters), four- and six- wheelers suck at turning on grippy surfaces, and don't comply to models on slippery.

With all said, I am just a bystander, not a reviewer or a team member. Hope I am helping a bit - while trying to learn the ropes.

[PerFrivik]

@slgrobotics Good morning!

1. For a differential drive module you are right, it is quite trivial, but what about a mecanum wheel setup? The idea of this PR is to lay the foundation/template for all rover types.

2. Yes that is exactly how I want to use it. With the new ROS 2: Library with dynamic modes API #20707 ROS2 Modes PR being merged, the idea is to restructure not only the differential drive but all rovers to better support ROS2 Modes. To demonstrate, I wrote a cruise control (used in cars) mode to demonstrate: Video Link

Of course we also want missions without external modes, which will follow in a later PR.

3. Your model outputs (Linear and Angular velocities) after the required normalization are indistinguishable from PX4 thrust and torque setpoints (those are usually fed to Control Allocation).

My model outputs wheel velocities, not quite sure I get this part. There are two kinematic equations, the forwards and inverse kinematics. To get my wheel velocities I just use the Inverse kinematics.

Requested linear & angular velocity -> | Inverse Kinematics| -> Wheel velocities

I am not sure if filling actuator_motors_s directly is better than filling vehicle_thrust_setpoint_s and vehicle_torque_setpoint_s - neither is expressed in physical units anyway.

They are completely different setpoints, I am not quite sure what you mean here, the goal is to directly control the wheel motors, instead of having to abstract the logic into the current control allocation implementation. I could pass the wheel velocities through the control allocation and use a unity matrix to directly map the wheel velocities to actuator outputs.

4. I could change the name to Rover Rate controller, but in general it is just a controller that regulates the angular and linear velocities.

5. I look forward to that too! But I believe that the idea of this PR to not nessearily try to integrate a Rover into the Drone section and recycle drone controls/modules, but rather branch off a little but as you mention align with the current PX4 architecture and style.

Bystander, reviewer or team member, does not matter to me. I appreciate all the feedback and it helps me understand what I should improve on also lets me know what is unclear! I also totally agree that a 4/6 wheeled system is not optimal, but in most everyday normal surfaces cases it is sufficient and usable with the model.

[slgrobotics]

@PerFrivik -
"...My model outputs wheel velocities" - well, the DD model does, but then you divide its output by wheels max speeds, effectively normalizing the outputs (to -1...+1 range, lines 143-144). That's what I meant.

Speaking of architecture, I am looking at https://docs.px4.io/main/en/concept/control_allocation.html - and the first picture there suggests that the interface between generic controllers and vehicle models should be in terms of "desired torque and thrust":

If you follow this pattern, should the DD model be somewhere in the "mixing" part of Control Allocation?

And just to clarify, the "desired torque and thrust", the way I understand it, is the "body torque (~yaw acceleration or rate?) and thrust (~linear acceleration or speed?)" - I'd guess, yaw rate and speed rather than accelerations. They are also normalized, -1...+1 to maximum possible for a specific robot.

[PerFrivik]

Greetings @slgrobotics !

If you follow this pattern, should the DD model be somewhere in the "mixing" part of Control Allocation?

This is indeed something I have considered and also discussed with @dagar. The question would be, how do we rewrite the mixing module to support a totally new implementation? The current way it is structured it is very much not possible without a large re-factor of the mixing module to support custom kinematic equations and linear/angular velocity inputs. That's why I opted for a library that can be imported into modules.

[PerFrivik]

DD model does, but then you divide its output by wheels max speeds, effectively normalizing the outputs (to -1...+1 range, lines 143-144). That's what I meant.

I see what you mean now, thank you for clarifying! Yes, this is true, but the meaning behind the setpoint I publish to actuator_motors is completely different to the normalized thrust and torque setpoint. What I am publishing is the normalized wheel speed and not the normalized linear/angular body velocities.

My architecture is designed to support vehicles that also have more than 2 control setpoints. Imagine a mecanum wheeled system with 4 wheels that need to be controlled individually to achieve certain requested linear/angular velocities. In this case you cannot just publish a torque or thrust setpoint and use the current control matrix implementation, this is also the reason for the kinematic models, that directly map to individual wheel velocities.

[PerFrivik]

I can really see where a lot of the confusion comes from, I apologize for some bad naming of variables and functions @slgrobotics, I'll update it and hopefully make it more clear, thank you for your input sofar!

[PerFrivik]

The commit 15fe000 adds support for the new #20707 ROS 2 Modes. As seen in the image below, the module has one setpoint topic differential_drive_setpoint which all control interfaces pass through, this includes internal manual control setpoints.

[sfuhrer]

Very nice work, with that you start to lay the foundation of a clean rover implementation!
I basically have (a lot of) small comments here and there. Fundamentally I find this good for a first iteration, though the non-manual ability for DD rovers should then follow soon to not have this feature removed for too long.

About the wheel velocity vs wheel torque setpoint controversy: How it is done now is enabling rovers without/low slip to be controlled much more preciously. If you have a lot of slip (think: driving on ice), then it's though applicable anymore. If I see it correctly we though don't loose anything by doing the no-slip transformations also in this case. The interfaces are just no longer correct (the absolute vehicle velocity setpoint my be 1m/s, but to get to this speed the differential_drive_setpoint.velocity field may need to go up to 5m/s). Maybe we need to add a comment in the message saying it is expected that the system can only hold the correct setpoints in no-slip conditions.

[MaEtUgR]

Conclusion from yesterday's call: We move forward with separate modules and since it won't all fit into e.g. v6x_default flash we'll make a separate build for rover.

[MaEtUgR]

There were two .vscode settings files related to ROS2 that slipped in. Would be nice to stop that from happening e.g. with .gitignore.

Hello Jaey

[slgrobotics]

@PerFrivik @MaEtUgR - hey, gentlemen - you broke my Lawnmower! ;-)

Removing ActuatorEffectivenessRoverDifferential was a bit premature, IMHO. I have a diff drive rover, but still want to use Control Allocator the traditional torque/thrust way (while your DD is being evaluated). I am also stuck for now with Gazebo Classic, as GZ plugin isn't working for me yet (that's on my todo list).

In your current setup Control Allocator isn't even run for VEHICLE_TYPE=rover, and CA_AIRFRAME=6 skips setting ActuatorEffectiveness altogether. Not running CA causes a flow of "ERROR [simulator_mavlink] poll timeout 0, 22" messages on pxh> console. Not setting Effectiveness matrix, obviously, zeroes the motors and servos output.

I can sure work around it in my fork, or just configure a Custom airframe - but you might have a better idea.

[PerFrivik]

@slgrobotics Hey!

What broke in GZ? Rather what plugin are you talking about?

[slgrobotics]

@PerFrivik "What broke in GZ? Rather what plugin are you talking about?" - when I tried porting my lawnmower from Gazebo Classic to GZ, using JointController plugin proved inadequate due to problems with scaling "servo" values 1000...2000 to smooth wheels rotation. I need to try again some time later - as there were some changes which could provide a fix.

But this PR is not about it, and I hope for an answer to my questions above.

BTW, ControlAllocator in a robot will surely be used for other actuators, servos... Not running it for a Diff Drive rover sets a bad example.

[PerFrivik]

@slgrobotics Hey I am back! I was on a trip to India to present a project, sorry for the long wait.

Not running CA causes a flow of "ERROR [simulator_mavlink] poll timeout 0, 22" messages on pxh> console.

I need to investiage this issue, I don't think I got this while testing.

Regarding your comment on removing the Control Allocator in general. Yes, this might be a big change to someone who has extensively been using the Control Allocator for their rovers, but I believe that the number of users in this regard is quite low. The new DD PR, should make it simpler for new users to start using the Rover module in PX4 and interface it with the new ROS 2 Modes. I really plan on trying to increase the user base, by also extending the different supported vehicle types and making it all easier to use.

BTW, ControlAllocator in a robot will surely be used for other actuators, servos... Not running it for a Diff Drive rover sets a bad example.

I am not quite sure what you mean by used for other actuators, servos. The actuator_motors/actuator_servos part controlling the motors/actuators is still running, so you can control your system using pwm etc. just like before.

[slgrobotics]

@PerFrivik - Not running CA causes a flow of "ERROR [simulator_mavlink] poll timeout 0, 22" messages on pxh> console.
That happens with Gazebo Classic, not GZ.

"removing the Control Allocator" - I understand that Control Allocator looks obsolete from your DD-focused perspective, but if you consider robots having servos and motors beyond wheels, - CA still has a role - as long as it doesn't conflict with your wheel channels. Was there even a discussion about this, from the architectural standpoint? (@MaEtUgR) And yes, people can run it in their startup files, but that looks strange to me, if CA is considered preferred/mainstream way of controlling actuators.

Removing ActuatorEffectivenessRoverDifferential was a major inconvenience to me. If you aren't using CA in your sample code, these aren't called anyway. For people still relying on CA and wishing to SLOWLY migrate to your DD implementation leaving these objects in place would avoid some trouble.

[slgrobotics]

@PerFrivik - would you mind trying "pxh> differential_drive_control stop" ? It causes Segmentation fault on my Intel Ubuntu 22.04 machine with GZ.

[PerFrivik]

@PerFrivik - would you mind trying "pxh> differential_drive_control stop" ? It causes Segmentation fault on my Intel Ubuntu 22.04 machine with GZ.

I get the same issue, I am checking it out. Thanks for the info!