Ackermann steering odometry bug?

[franzrammerstorfer]

In my understanding, the current computation of the steering angles (left vs. right) in the ackermann odometry is wrong.

Disclaimer

Unfortunately, I could not find documentation on the frames/sign convention used for that calc, so this is more a question than a bug report.

IMO, we need to change the signs in the current implementation

ros2_controllers/steering_controllers_library/src/steering_odometry.cpp

Lines 269 to 275 in 23f944f

	double numerator = 2 * wheelbase_ * std::sin(alpha);
	double denominator_first_member = 2 * wheelbase_ * std::cos(alpha);
	double denominator_second_member = wheel_track_ * std::sin(alpha);
	double alpha_r = std::atan2(numerator, denominator_first_member - denominator_second_member);
	double alpha_l = std::atan2(numerator, denominator_first_member + denominator_second_member);
	steering_commands = {alpha_r, alpha_l};

to something like

      double alpha_r = std::atan2(numerator, denominator_first_member + denominator_second_member);
      double alpha_l = std::atan2(numerator, denominator_first_member - denominator_second_member);

Here is why (sorry for not providing a proper formatted document, hopefully somebody is able to read this ...)

[christophfroehlich]

Hi Franz! Thanks for pointing this out.

In general, I'd appreciate more documentation of the kinematics in the docs for all controllers here. Is there any publicly available reference/book for these mobile robot kinematics we could refer to?
I'd happy to help with writing the docs, and maybe writing proper tests for the implementations. But I'm not "fluent" with these type of kinematics, I'd need a textbook for that.

[franzrammerstorfer]

Hi Christoph!
I do not have a 'bible' for kinematics in mind -- if needed, I look things up in my old "Vorlesungsmitschrift" :)
Maybe Matlab helps out (also for creating test cases?!), e.g. here. There, they refer to "Lynch, Kevin M., and Frank C. Park. Modern Robotics: Mechanics, Planning, and Control 1st ed. Cambridge, MA: Cambridge University Press, 2017" -- I do not have the book, but a MOOC is avail.

Anyway, what I pointed out above is just trigonometry. As a sign convention, I used a right-handed coordinate system, x pointing forwards, hence a positive rotation is counterclockwise.
Where does the orig implementation come from?

[aizmailovs]

Hi all!
I did decide to check steering angles in a simulation using a car-like robot together with ackermann_steering_controller. Long story short, it seems like angles are calculated incorrectly indeed.

I have decided to draw similar drawing as Franz, but in SOLIDWORKS to check steering angles of each wheel. As it was assumed, the steering wheel that is closer to the centre of rotation, has to turn more than the wheel which is further away from the centre of rotation.

Then I launched a simulation and applied reference Twist command using steering gui.

When, turning left (positive rotation around Z axis), the right steering joint turned more than the left one.

Similar behaviour can be seen when turning right (negative rotation around Z axis), the left joint turned more than the right one.

If I am correct, it should be other way around.

Just in case, I will provide my .yaml file that is similar to the parameter file.

controller_manager:
  ros__parameters:
    update_rate: 50

    use_sim_time: true

    joint_state_controller:
      type: joint_state_broadcaster/JointStateBroadcaster

    ackermann_steering_controller:
      type: ackermann_steering_controller/AckermannSteeringController

ackermann_steering_controller:
  ros__parameters:

    reference_timeout: 2.0
    front_steering: true
    open_loop: false
    velocity_rolling_window_size: 10
    position_feedback: true
    use_stamped_vel: true   # Use true to make controller topic /ackermann_steering_controller/reference listen to TwistStamped type data
                            # Use false ro make controller topic /ackermann_steering_controller/reference_unstamped listen to Twist type data
    rear_wheels_names: [right_rear_joint, left_rear_joint]
    front_wheels_names: [front_steer_right_joint, front_steer_left_joint]

    wheelbase: 0.65               # Distance between front and rear wheel axes
    front_wheel_track: 0.605      # Distance between front wheels
    rear_wheel_track: 0.605       # Distance between rear wheels
    front_wheels_radius: 0.165
    rear_wheels_radius: 0.165

joint_state_controller:
  ros__parameters:
    type: joint_state_controller/JointStateController

[roncapat]

What if you swap the left & right steer in the parameters? Just asking, since this may arise from that ambiguity.

[aizmailovs]

Swapping steer joints solves the problem with angles of the steering wheels. However, it makes definition of the parameters inconsistent.

    rear_wheels_names: [right_rear_joint, left_rear_joint]
    front_wheels_names: [front_steer_left_joint, front_steer_right_joint]

Difference in velocities between rear joints seems to be ok, the wheel that is further from the centre of rotation is going faster than the wheel that is closer.

[franzrammerstorfer]

Thanks, @aleksandrsizmailovs, for providing the diagrams. So, IMO we should fix it in the steering lib and provide at least a unit test with the numbers written in the diagram, right?
Furthermore, we should rewrite/redesign the parameters for left/right wheels -- to fix this ambiguity. I stumbled across this as well, like @roncapat pointed out. @christophfroehlich: How to proceed?

[roncapat]

Somewhat related to #692 since we are talking of "clarity issues" of the parameter interface.

[christophfroehlich]

Furthermore, we should rewrite/redesign the parameters for left/right wheels -- to fix this ambiguity. I stumbled across this as well, like @roncapat pointed out. @christophfroehlich: How to proceed?

Somewhat related to #692 since we are talking of "clarity issues" of the parameter interface.

good idea to fix this at once, but with the proposals from #692 there is still only a vector for steering joints, where it is not clear if it is [right,left] or [left,right]. Am I right?

Thanks, @aleksandrsizmailovs, for providing the diagrams. So, IMO we should fix it in the steering lib and provide at least a unit test with the numbers written in the diagram, right?

👍

[roncapat]

@christophfroehlich exaclty... This is an "orthogonal" issue. We should distinguish both left/right & drive/steer IMHO.