Skid steer controller

[mikepurvis]

This is being partly tracked in #55, but I'm especially interested in plans for a skid steer controller, for example for Husky, and similar vehicles.

I've started experimenting a bit with diff_drive_controller, and I like how it's set up— I'll start there, but there a couple big missing pieces to make it suitable for use as a skid steer controller:

• close loop around vel feedback from an IMU
• allow for more than one left-side and right-side wheel joint
• estimate slip, and feed that forward; provide a parameter to initialize the estimate

Would it be worthwhile trying to add this stuff to diff_drive_controller, or should a brand new skid_steer_controller be created? I believe these things would benefit a differential-drive chassis as well, and there'd be the obvious win of shared tests, tutorials, documentation, etc.

I don't have the resources to take on a major project myself in the short term, but I'd be delighted to collaborate on it with someone, or contribute some of these things to diff_drive_controller with design guidance.

[adolfo-rt]

Hey Mike,

It's great to see some activity on this front!

I'd suggest a unified implementation if the overlap between a skid-steer and a diff-drive controller is large, both conceptually and implementation-wise.

On the missing pieces:

• close loop around vel feedback from an IMU

This would be a great feature. I'd make it optional, so robots without IMU can still benefit from the implementation.

• allow for more than one left-side and right-side wheel joint

Would you recommend having different velocity IK implementations (twist → joint) to support diff-drive and skid-steered vehicles, or would it make more sense to have one IK that generalizes to both cases?.

• estimate slip, and feed that forward; provide a parameter to initialize the estimate

Could you briefly outline what the slip estimator would look like?

I'll ask for input from the diff_drive_controller authors/contributors, which are more knowledgeable than me on the subject, @bmagyar, @efernandez, @po1

[bmagyar]

Hi all,
I also think that the skid-steer could be incorporated in the diff-drive-controller in a relatively straightforward way, I would try to go in that direction.

As you can imagine, we don't have too many issues with REEM in terms of slip, but I suppose this can be significant with the Husky running in mud :)

If you don't have time to take on the project we should at least discuss the above mentioned details and perhaps the new configuration items that will be necessary so with having the idea and implementation laid out, someone can take care of it when the time comes.

[mikepurvis]

Would you recommend having different velocity IK implementations (twist → joint) to support diff-drive and skid-steered vehicles, or would it make more sense to have one IK that generalizes to both cases?

To be perfectly honest, I'm not totally sure what you're meaning here— as it stands, diff_drive_controller is just using the URDF to extract a wheel radius and separation, so I'm picturing a starting point would be allowing it do things like infer for each wheel the component of the applied force which is in the direction of desired movement vs. skidding. The degenerate case of that with one wheel per side is that all the forces are going into moving the vehicle.

Another aspect of the "multiple wheels" question is that Husky, Pioneer, and similar vehicles have only one motor per side (the wheels are belted together), whereas Grizzly and some other platforms have one motor per wheel, so in theory all four are independently driveable, though our present controllers for Grizzly don't take advantage of that capability.

Could you briefly outline what the slip estimator would look like?

At its most basic, there could be no estimator at all— just a multiplier applied to any attempted angular velocity over and above the ideal of (curr_cmd.ang * ws / 2.0) / wr. I don't have specific thoughts about an implementation, but it would seem that if the controller observed consistent error in angular speed as compared to the IMU, it should be able to incrementally adjust that multiplier.

Of course, the amount of slip is also a function of linear velocity, too, so this is all a bit more speculative. There may be some papers out there from groups who have studied these techniques in more detail.

[efernandez]

I see a direct implementation in the diff_driver_controller by extending the configuration to indicate a vector a left and right wheels (actually motors). Then, we have to use a vector in the code to send the same velocity to each motor (if one motor drives all the wheels in a lateral, the control will be the same one we have now). This is quite straightforward to implement (no more than 1-2 hours).

Regarding the odometry, I don't see the IMU or any estimation here. The controller should only output the wheels encoder odometry. You can have an estimator to provided better odometry using the encoder odometry, the IMU yaw, and also other source of odometry (visual, GPS, whatever). For instance, you can use this: http://wiki.ros.org/robot_localization

Since the controller relies on the URDF, and makes some assumptions, it'd be useful to have a skid steer robot in URDF as a reference. For example, the husky description has the 2x2 wheels in the URDF. However, if there's only one motor to control each side pair in the real robot, I think we have a conflict there. @bmagyar @po1 What do you think?

[adolfo-rt]

@mikepurvis, the IK computation maps from twist to joint velocities, and as such it needs to know about the wheels and their locations. Currently, the diff_drive_controller configuration requires specifying the left and right wheel names, which are parsed during controller initialization to instantiate an IK mapping specific to the robot at hand.

An IK solver that generalizes to skid-steer platforms should be able to handle any number of wheels (some assumptions on their layout may apply), and the controller configuration should change accordingly (ie. specify a list of left/right wheels).

I see that @efernandez was faster to post than me, with a more detailed answer :)

[po1]

For controlling the wheels, I support @efernandez, the controller should probably do something simple in that regard. Forwarding the same commands to a vector of joint velocity interfaces might be enough to cover most cases.
However, the case of the odometry is different. We need a policy to merge odometry input from potentially many encoders, and this is where an estimator would be needed.
I also support the idea that the IMU should be kept outside of this if we can live without it. The data fusion can further be done by other components.

[bmagyar]

How about following a plugin-like approach for odometry? They don't necesseraly have to be plugins, we could use an interface and just instantiate the proper implementation according to a line in the yaml file.

This way we can keep the current implementation as it is, and only override the functions needed for a different policy.

[mikepurvis]

It's not going to be possible to accurately control a skid steer vehicle based on encoders alone, so it comes down to two possible implementations:

1. First, the controller can subscribe to the IMU, close its velocity control around angular_velocity/z (but miss out on potential angular information from GPS, VO, etc). It would still publish an Odometry based solely on the wheel encoders.
2. Second, the controller can supply Odometry, but control angular/linear velocity based on the fused pose/vel estimate coming back from an external sensor fusion component.

The second is more correct, and I think that's what's being advocated in this thread. But that approach has a few key drawbacks. Namely, it makes the ability to drive/control the vehicle at all dependent on the presence of a functioning localization component. More importantly, it depends on the localization component actually supplying an estimate of angular and linear velocity, which the classical ROS one (robot_pose_ekf) doesn't even do!

Going route #2 would definitely require that the controller have a fallback open-loop mode it can enter if the external state estimation goes stale (and suitably inform the user).

[po1]

It's not going to be possible to accurately control a skid steer vehicle based on encoders alone

I see. In that case, I would probably advocate a separate controller that would work with direct input from an IMU. I don't know how much code could be factorized between the two controllers, but it seems that there are fundamental differences in the model that in my opinion will result in very different implementations.

Because it solves a simple problem, I would like to leave the diff_drive_controller as untouched as possible. If the new controller ends up solving a more general problem, then we can deprecate the simple controller in favor of the new one.

In the mean time, I suggest using the diff_drive_controller as an implementation example and write a separate skid_steer_controller. Since the diff drive controller is not very big, not much code would be duplicated anyway.

[mikepurvis]

I've thought about this a bit more and done some initial experiments with diff_drive_controller on my four-wheeled (Pioneer-like) platform. I think what would be great would be:

• for diff_drive_controller to retain its present open loop behaviour, but to gain the ability to command 4- and 6-wheeled vehicles. Perhaps one could specify either left_wheel: x or left_wheels: [x, ...].
  • This would be great for simulation, where Gazebo doesn't understand belted-together wheels,
  • And also great for being able to use the joint_state_controller.
• for a separate skid_steer_controller to extend that by closing around an odom -> base_footprint TF (parametrized, though).
  • This would keep diff_drive_controller simple and focused.

[efernandez]

@mikepurvis We could starting by supporting several wheels per side.
This also needs several radius multipliers, one per wheel, but I'm not sure if it makes sense.
The equations to convert vehicle-space velocities to wheel-space velocities will remain the same. :)

However, to obtain the odometry, I'm not sure if it makes sense to have feedback from all the wheels or not; indeed, the equations might be different for the wheels that aren't at the center.

What do you think?
@bmagyar @po1

[mikepurvis]

Is there an example of a skid-steer vehicle where there are multiple driven wheels of differing sizes? Doesn't seem like a typical configuration— I'd be fine to specify multiple left-side wheels, and have the controller throw a fatal error if it discovers from the URDF that they're not all the same size.

The issue of how to compute odometry is more tricky, particularly accounting for vehicles like Grizzly and Summit, where the front-right and rear-right wheels might not have moved quite the same amount. I'm fine with just assuming an average; some user needing something more sophisticated could implement it.

[efernandez]

@mikepurvis Regarding the wheel radius you're right. However, I was talking about some multipliers we have to correct possible imperfections. Anyway, we can start doing something and later we can see if we have to consider some particularities or not; they can be probably neglected.

Therefore, the first step seems to be extending the configuration and the code to support several joints for left and right wheels. Something like:

left_wheel: [left_wheel_1, left_wheel_2, left_wheel_3]
right_wheel: [right_wheel_1, right_wheel_2, right_wheel_3]

Then, the code should accept them and send the wheel-space velocities to all the joints.
For the odometry we should also read from all the joints and take the average.
I think that will be enough.

I've done a quick implementation in pal-robotics-forks#42, so we can continue discussing there the details; also note that testing is pending, and the unit tests must be updated with a new one using several wheels (I kept backwards compatibility for the left_wheel and right_wheel params, allowing them to be a list or simply a string).

[mikepurvis]

Whoa, thanks for taking a stab at this. I've attempted to use it with my platform, but I'm getting a segfault on startup when loading it inside of the gazebo_ros_control plugin. Will investigate further and report findings in your PR.

[skohlbr]

Slightly related, I made a (non ros_control) gazebo plugin for multi wheel robots some time ago (see diffdrive_plugin_multi_wheel.cpp). I used space separated strings for the left and right wheel joint names parameters there. Mainly used for experimenting with simulation of tracked vehicles (with mixed success, see Gazebo Answers).

[rgariepy]

Should this issue be closed? It's been working on Jackal for a while for us.

[mikepurvis]

I'm content with the current implementation. There may still be room for a distinct skid steer controller which estimates and compensates for slip internally, but having diff_drive_controller push it up to the higher level localization is working well for us at present.

[Whytehorse]

Hi everyone, can I get the current status of skid steering? I'm building a human-operated electric ATV and need to control 4 hub-motors inependently with skid steering. Will this controller work? If so, does it have all the bells and whistles from control systems theory to make it stable at high speeds, minimize use of power, etc?

[bmagyar]

Will it work: yes.
Does it have all bells and whistles: no.

For everything else I'd recommend reading the code. If you miss something you'd like to add, we are open to review PRs.

[Whytehorse]

Cool! I'll use it and once I'm familiar with the code, add the bells and whistles. There are a bunch of publications with the exact equations of motion

[senoa95]

I'm content with the current implementation. There may still be room for a distinct skid steer controller which estimates and compensates for slip internally, but having diff_drive_controller push it up to the higher level localization is working well for us at present.

I am using working on an outdoor differential drive robot. I anticipate to operate the robot on wet grass and therefor expect lots of slip. I find this thread very relevant for my implementation; @mikepurvis I am interested in learning how you were able to use robot_localization to overcome the slip issue while still using diff_drive_controller (without compensating for slip internally).