Add four wheels swerve drive to ROS controllers

[gurbain]

Independent steering and control of wheels is a famous technique to control vehicles in robotics. Among other advantages, it provides the user the control of three DOFs (linear speed in X and Y and angular speed in Z) whereas classical steering generally involves two DOFs only. In comparison with differential drive, the orientation of each wheel is controlled at all times to avoid slipping. Also, it can easily generalize to different robot layouts using 3+ wheels.

There is visibly a need for such a controller implemented with the ros_control framework in ROS community.

This pull request contains such an implementation (for a four-wheeled robot only), developed in our startup, Exobotic. It's directly adapted from the four_wheel_steering_controller, and forward and backward models have been inspired from this forum post. The package comes with a default test robot called swervebot and a launch file (both in the test folder). A series of gtest are also included to assess odometry, X, Y linear speeds and Z angular speed. It passes catkin_lint, catkin_make ros_lint and catkin_make run_tests. Could you maybe add it to the repository?

Best, Gabriel

[gurbain]

Hi Bence,

Thank you for the review and the relevant comments. I'm modifying the pull request accordingly. To answer to your other comment on my motivation to provide yet another controller:

The reason is that the four_wheel_steering_controller hypothesizes that the left and right steering mechanisms are physically linked together in the front and in the back, forming, therefore, a back and a front train which turn with the same angle. This constraint leads to simplifications in the controller equations but also restrict the problem to a specific set of robot mechanisms.

The diff_drive_controller also has a skid-steer mode but, in this case, the wheels are not steered independently around the z-axis.

In this swerve_controller, we consider a much more generalistic case, where all four wheels have an independent steering mechanism to turn around the z-axis. This can lead to a larger range of available movements (linear x and y speeds as well as z angular speed). The only constraints that remain in the code concern are geometrical considerations to position all the wheels around the same rotation center (see image below)

[gurbain]

Hi!

Is there any news regarding this pull request? Best,

Gabriel

[AravindaDP]

I also interested if this gets merged (Either as standalone as is or incorporated into four_wheel_steering_controller)

@gurbain While we are at it I would suggest following enhancement to support limited steering range

• Include params min_steer_angle and max_steer_angle (Assumption all wheels support same steer range)
• If target_steer_angle for a wheel exceeds max_steer_angle and target_steer_angle - π > min_steer_angle set target_steer_angle -= π and inverse wheel velocity. And vise versa for target_steer_angle is below min_steer_angle (This would allow functioning if steer angles support at least range -π/2 to π/2)
• If any wheel cannot achieve desired angle either directly or opposite direction, apply brake (I believe that's better approach. or you could try applying best effort angle (where angle error is minimized after clipping to maximum limits) it's up to you)

[gurbain]

Hi @AravindaDP,

Thank you for the feedback! I haven't work for a while on this project but I'm planning to go back to it in a few months. The steering angle limits seem indeed appropriate to decrease large rotation of the wheel over 180° and take the real motor limits into consideration. I implemented it in a new commit that I join to this pull request.

I also followed your advice on braking when the limits are reached, even though this seems a bit harsh to me. I will have a look at other options later if needed (like clipping at the angle limit, even though the desired rotation rate and the velocity are not reached).

Regards, Gabriel

[AravindaDP]

@gurbain Thanks for quickly adding it. From the quick testing it looks work quite good.

As I mentioned, I'm fine with either behavior where requested control is not achievable due to steering limit exceeded, (when range is so limited) Leaving a warning roslog along with letting the arm joint to simply max out also might be ok (specially when using with high level trajectory planners)

[roboticlemon]

Hi @gurbain, first of all thank you for the pull request, for some reason practical implementations of swerve drive (4WIS) controllers are almost non existent in the ROS ecosystem.

One other suggestion I have for this code is minimising steering angles for the robots (like your sample urdf) that have unlimited steering angles, so that the steer motors never make a turn that is > pi/2 radians.

In the test URDF the joint speed limits are almost uncapped which hides the fact that wheels will sometimes do almost 2pi radians of rotation to meet their desired angle. I've forked your work and have a draft solution to the problem that works almost flawlessly and would love to see it integrated with your work. Once it's a little more polished I'll push it up so you can take a look.

Here is an example of the flaw when transitioning from linear x=1 to linear x = -1:

The biggest problem I had was with how gazebo implements the joint position controller. If a joint which we assume works from 0 to 2pi radians is at position 6 radians. If you then want to slightly rotate it in the positive direction our steering angle will be 0.01 radians but gazebo won't wrap the angle and will instead rotate the joint back a full revolution to 0.01 radians. This means to use the controller effectively with Gazebo you need to compute your angles between 0 and 2 pi (or -pi and pi) but then scale them to somewhere within -inf and inf that is closest to the current joint state. As far as I'm aware this problem actually only plagues our controller because none of the other robot controllers like diff drive or ackermann have steering columns that can turn limitlessly.

Another rather strange implementation detail is that the brake() function which gets called at timeout and at 0 velocity resets the steering values to 0. This is undesirable in most cases as slowing the robot down to a stop when it is strafing or turning will yank the steering motors forwards.

Let me know your thoughts!

[AravindaDP]

@gurbain One more enhancement you could do is supporting multi robots under single master.

For that at a minimum you need to support configurable odom frame similar to #205

However I currently use joint parameter reading (track/wheel_radius etc) from robot_description parameter and still couldn't figure out loading without a robot_description. (This is because it always read wheel_steering_y_offset from robot description instead node params for swerve_controller) https://github.com/gurbain/ros_controllers/blob/swerve-drive/swerve_controller/src/swerve_controller.cpp#L164

In that case alternative approach is to let user define a tf_prefix parameter for swerve_controller and append it to frame_ids published by swerve_controller.

Thanks for your time.

[gurbain]

Hi all,

First to answer @ingramator:

• I don't really understand the problem you are mentioning. The steering angle is computed to be in the range [-PI, PI] and now clipped to respect the minimal and maximal steering range as suggested by @AravindaDP. So, the real angle should never be higher than these limits in gazebo. Except in the specific case when you are switching from -PI to PI, I don't see how you observer a full rotation.
• I fully agree about removing the steering angle reset to 0 when braking. I added this below.

About the suggestions from @AravindaDP: I am not sure I fully understand what you mean with the TF prefix but I fixed a few bugs to make sure that all geometric parameters that are written in the configuration file are not parsed in the URDF if not needed. Now it should work even if you are not providing robot_description in ROS params.

All the best, Gabriel

[AravindaDP]

@gurbain Thanks for the update. it now works real great.

Regarding different tf prefix support for multiple robots, I have found a alternate approach where I could republish the odom messages published by swerve_controller to overwrite odom frame name in message. But I guess it wouldn't hurt to make odom_frame also configurable through rosparam. You can similar implementation in following PR (for diff_drive_controller) https://github.com/ros-controls/ros_controllers/pull/205/files#diff-380f38b77ed7f79a1de5cf7de13f429e

You would only need to replicate the changes in *_controller.h and *_controller.cpp in above PR here.

Here is my setup for one robot. For multiple robots each tf tree would identical but with different prefix (say scout_2_tf/...) And all will link to a single parent tf (say map or world) (through static tf broadcaster or amcl mapping)

frames.pdf

[roboticlemon]

Hi @gurbain,

Thank you for the braking fix.

The suboptimality of turning problem is still present albeit less noticeable now that you have steer clipping. The problem is not that a wheel does a full revolution (2π radians), it doesn't seem to be a problem that it will do a half revolution (π radians) either.

This suboptimality problem I'm referring to is really only relevant for continuous joints for instance your sample robot described in URDF where the joint type is continuous. This represents a steer motor that can be positioned between [-∞, +∞] radians. For simplicity we wrap this angle space between (-π, π] since anything more than a full revolution is the same as it's wrapped angle position and we never need to make multiple revolutions in a single joint position command.

The real problem is the swerve controller can do more than a quatre turn positive or negative (±π/2 radians) if it doesn't know where the current position of the steer controller is. For continuous motion this issue doesn't crop up, it only crops up with discontinuous motion, e.g reversing one of the velocity components.

An example is as follows:

Start controller with default (-π, π] min and max angles. Give the controller a Twist with linear x: -0.3 linear y: -0.2 angular z: +0.5

then give it a Twist with (note the negative linear x component now) linear x: +0.3 linear y: -0.2 angular z: +0.5

For the leg_lh_joint the first Twist will yield a position of approx 0.77 radians

For the leg_lh_joint the second Twist will yield a position of approx -1.39 radians

As you can see this difference is greater than π/2 radians. The optimal solution in this case would be to set the leg_lh_joint to 1.75 radians which makes for only around 1 radian of turning needed versus over 2 radians of turning.

Here is the issue illustrated, blue represents the previous joint state, red the controllers output state and green, the ideal state:

Let me know if you need any more clarification! It is initally not very intuitive but hopefully the diagram will help. To fix the problem we need to take the previous state and find which solution is closer, the computed point or the polar opposite point.

Thanks!

[ros-gy]

Hi @gurbain, first of all, it's great to see the work here adding four wheel steering to ros_controls, thank you. I read the linked derivations from "ether." Can you explain how the odometry calculations are done? The equations seem to match dual ackerman odometry calculations, are they the same?

[abhishekove]

Can anyone please tell me how to use this as a plugin in gazebo ROS like a differential drive?

[tomlogan501]

Hi all, any news about this PR ? I have a robot that requires it. At first I used four_wheel_steering, that could work but not for pivot turn.

[gurbain]

I am not working on it for the moment, although I am currently designing a ROS2 version. Nonetheless, I noticed that this is not the only version of this controller anymore, as there is now a duplicate of a swerve drive for ros_control: https://github.com/MarkNaeem/ros_controllers/tree/noetic-devel/swerve_steering_controller https://discourse.ros.org/t/independent-swerve-steering-controller-for-ros-control/21534/21

Maybe faster feedback from the maintainers of ros_control could have helped avoiding duplicates. Swerve drive is very common in robotics, I see a lot of robots based on this principle in agriculture and industry. So, in my own humble opinion, there is an interesting potential for integrating it to ros_control.

[gurbain]

Hi @gurbain, first of all, it's great to see the work here adding four wheel steering to ros_controls, thank you. I read the linked derivations from "ether." Can you explain how the odometry calculations are done? The equations seem to match dual ackerman odometry calculations, are they the same?

Hi @ros-gy the inverse kinematics are somehow similar to the equations presented here: https://www.chiefdelphi.com/uploads/default/original/3X/8/c/8c0451987d09519712780ce18ce6755c21a0acc0.pdf This blog post also provide some code and explanations the forward kinematics (odometry): https://www.chiefdelphi.com/t/paper-4-wheel-independent-drive-independent-steering-swerve/107383