Add four wheels swerve drive to ROS controllers #441
Conversation
There was a problem hiding this comment.
I've left a couple of general review point for you out of a shallow review.
Could you please post a bit of reasoning as to why this can't be done as an extension of the four_wheel_steering_controller? Reason: diff_drive_controller also has a skid-steer mode, saves a lot of duplicated code.
swerve_controller/test/bin/pub_cmd
Outdated
|
|
||
| final_x = copy.copy(last_x) | ||
|
|
||
| print "DIFF: " + str(final_x - init_x) |
There was a problem hiding this comment.
I think you already have a rostest for this, right?
There was a problem hiding this comment.
Indeed, we can actually remove this file!
| msg.pose.pose.position.z]) | ||
|
|
||
| self.world_ori = [msg.pose.pose.orientation.x, | ||
| msg.pose.pose.orientation.y, |
There was a problem hiding this comment.
there seems to be a tabs vs spaces situation here, please replace all tabs with 2 or 4 spaces, up to you
| import tf | ||
| from nav_msgs.msg import Odometry | ||
|
|
||
| class Link2TF(): |
There was a problem hiding this comment.
It is not required indeed but I've included this for visualization purposes. It helps to show the odometry in RVIZ, which I found interesting as part of the tests.
| } | ||
|
|
||
| /** | ||
| * \brief return le last odometry value |
There was a problem hiding this comment.
a one line brief of a one line getter with a typo seems to be an overkill, could remove it altogether
| } | ||
|
|
||
| /** | ||
| * \brief Desctructor. Shuts off the subscriber |
There was a problem hiding this comment.
subscribers know how to shut down on their own usually
There was a problem hiding this comment.
I've just copied your code and commented it for my own understanding. I will remove it.
| POSSIBILITY OF SUCH DAMAGE. | ||
| --> | ||
|
|
||
| <!-- Revolute-Revolute Manipulator --> |
There was a problem hiding this comment.
Totally unrelevant indeed, I'm not sure how it ended up there. I will remove it
| <plot row="0" col="1"> | ||
| <range left="0.007000" bottom="-0.000007" top="0.000006" right="1.056000"/> | ||
| <limitY/> | ||
| <curve G="119" custom_transform="noTransform" B="180" name="/swervebot/joint_states/leg_rf_joint/pos" R="31"/> |
There was a problem hiding this comment.
It is indeed mainly for me. I will remove it from the pull request
swerve_controller/CMakeLists.txt
Outdated
| project(swerve_controller) | ||
|
|
||
| # Default to C++14 | ||
| if(NOT CMAKE_CXX_STANDARD) |
There was a problem hiding this comment.
in melodic we have this by default, please remove
There was a problem hiding this comment.
It's copy-paste from the four_wheel_steering_controller btw
swerve_controller/CMakeLists.txt
Outdated
| endif() | ||
|
|
||
| # Package dependencies | ||
| set(${PROJECT_NAME}_CATKIN_DEPS |
There was a problem hiding this comment.
No. Do it like it's done everywhere else please.
There was a problem hiding this comment.
Ok, I only changed it to avoid catkin_lint to complain with:
swerve_controller: CMakeLists.txt(5): list COMPONENTS should be sorted
swerve_controller/CMakeLists.txt
Outdated
|
|
||
| # Source and headers for build | ||
| include_directories(include) | ||
| include_directories(${catkin_INCLUDE_DIRS}) |
There was a problem hiding this comment.
only one include_directories should be there
|
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)
|
|
Hi! Is there any news regarding this pull request? Gabriel |
|
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
|
|
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, |
|
@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) |
|
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! |
|
@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. |
|
Hi all, First to answer @ingramator:
About the suggestions from @AravindaDP: All the best, |
|
@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 |
|
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. then give it a Twist with (note the negative linear x component now) 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! |
|
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? |
|
Can anyone please tell me how to use this as a plugin in gazebo ROS like a differential drive? |
|
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. |
|
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: 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. |
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 |
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