Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

AckermannSteering Plugin limits the minimum angular velocity based on the minimum linear velocity, preventing steering to the right when set to 0 #2734

Open
Dyst-0 opened this issue Jan 26, 2025 · 2 comments · May be fixed by #2748
Labels
bug Something isn't working

Comments

@Dyst-0
Copy link

Dyst-0 commented Jan 26, 2025

Environment

  • OS Version: Ubuntu 24.04
  • Source build of gz-sim8
    Latest commit: 4c2797d

Description

When setting the minimum linear velocity to 0, then testing the steering commands, the following happens:

  • Expected behavior: Front wheels turn anti-clockwise, then clockwise until the steering limit has been reached
  • Actual behavior: Front wheels do not reach the steering limit in the clockwise direction. Instead, the wheels return to pointing straight.

In AckermannSteering.cc#L349, the minimum angular velocity is set to the same value as the minimum linear velocity. In the case when the value is 0, the wheels cannot turn right, since a negative yaw rate would be capped at 0, hence making wheels point straight.

Steps to reproduce

  1. Change the min_velocity value to 0 in ackermann_steering.sdf by replacing the line with the following:
<min_velocity>0</min_velocity>
  1. Build and source the workspace
  2. Run gz sim ackermann_steering.sdf -r
  3. Publish the steering commands
    1 - gz topic -t "/model/vehicle_blue/cmd_vel" -m gz.msgs.Twist -p "angular: {z: 0.1}"
    then
    2 - gz topic -t "/model/vehicle_blue/cmd_vel" -m gz.msgs.Twist -p "angular: {z: -0.1}"

Output

Output after running the first steering command:

Image

Output after running the second steering command:

Image

Proposed Solution

  1. Separate angular speed limiter parameters from linear ones in AckermannSteering.cc by replacing the code block with the following:
  // Parse speed limiter parameters.
  if (_sdf->HasElement("min_velocity"))
  {
    const double minVel = _sdf->Get<double>("min_velocity");
    this->dataPtr->limiterLin->SetMinVelocity(minVel);
  }
  if (_sdf->HasElement("min_angular_velocity"))
  {
    const double minAngVel = _sdf->Get<double>("min_angular_velocity");
    this->dataPtr->limiterAng->SetMinVelocity(minAngVel);
  }
  if (_sdf->HasElement("max_velocity"))
  {
    const double maxVel = _sdf->Get<double>("max_velocity");
    this->dataPtr->limiterLin->SetMaxVelocity(maxVel);
  }
  if (_sdf->HasElement("max_angular_velocity"))
  {
    const double maxAngVel = _sdf->Get<double>("max_angular_velocity");
    this->dataPtr->limiterAng->SetMaxVelocity(maxAngVel);
  }
  if (_sdf->HasElement("min_acceleration"))
  {
    const double minAccel = _sdf->Get<double>("min_acceleration");
    this->dataPtr->limiterLin->SetMinAcceleration(minAccel);
  }
  if (_sdf->HasElement("min_angular_acceleration"))
  {
    const double minAngAccel = _sdf->Get<double>("min_angular_acceleration");
    this->dataPtr->limiterAng->SetMinAcceleration(minAngAccel);
  }
  if (_sdf->HasElement("max_acceleration"))
  {
    const double maxAccel = _sdf->Get<double>("max_acceleration");
    this->dataPtr->limiterLin->SetMaxAcceleration(maxAccel);
  }
  if (_sdf->HasElement("max_angular_acceleration"))
  {
    const double maxAngAccel = _sdf->Get<double>("max_angular_acceleration");
    this->dataPtr->limiterAng->SetMaxAcceleration(maxAngAccel);
  }
  if (_sdf->HasElement("min_jerk"))
  {
    const double minJerk = _sdf->Get<double>("min_jerk");
    this->dataPtr->limiterLin->SetMinJerk(minJerk);
  }
  if (_sdf->HasElement("min__angular_jerk"))
  {
    const double minAngJerk = _sdf->Get<double>("min_angular_jerk");
    this->dataPtr->limiterAng->SetMinJerk(minAngJerk);
  }
  if (_sdf->HasElement("max_jerk"))
  {
    const double maxJerk = _sdf->Get<double>("max_jerk");
    this->dataPtr->limiterLin->SetMaxJerk(maxJerk);
  }
  if (_sdf->HasElement("max_angular_jerk"))
  {
    const double maxAngJerk = _sdf->Get<double>("max_angular_jerk");
    this->dataPtr->limiterAng->SetMaxJerk(maxAngJerk);
  }
  1. Add the following tag to ackermann_steering.sdf anywhere inside the <plugin> tag
<min_angular_velocity>-1</min_angular_velocity>
  1. Build and source the workspace
  2. Run gz sim ackermann_steering.sdf -r
  3. Publish the steering commands
    1 - gz topic -t "/model/vehicle_blue/cmd_vel" -m gz.msgs.Twist -p "angular: {z: 0.1}"
    then
    2 - gz topic -t "/model/vehicle_blue/cmd_vel" -m gz.msgs.Twist -p "angular: {z: -0.1}"

Output after running the first steering command:

Image

Output after running the second steering command:

Image
@bperseghetti
Copy link
Member

I think you might be experiencing the conundrum of trying to control a non-holonomic vehicle with the same type of command input of a holonomic one.

I'm not sure the assumption that the wheels should go to their min/max when given any angular rate but with no linear velocity makes any more sense then them pointing forward for a non-holonomic vehicle.
What does a commanded 10 rad/s angular rate mean to a stationary ackermann vehicle?

Simply put from a purely mathematical perspective, there is no angular rate for an ackermann if there is no linear velocity component.

Furthermore, what steering angle should it even be pointing to? If I give it a command of 0.000000001 rad/s at 0 m/s linear velocity.

If you are looking for the ability to control the steering angle I would suggest using <steering_only> option and then control the wheel speed with the jointcontroller. If you decide to go that route I would also recommend using it with the actuator message for simplicity.

@Dyst-0
Copy link
Author

Dyst-0 commented Jan 31, 2025

Just to clarify, the main issue is that turning to the right is impossible with <min_velocity> set to 0, even when the linear velocity is positive. I know it is not mentioned in the original post, but running

gz topic -t "/model/vehicle_blue/cmd_vel" -m gz.msgs.Twist -p "linear: {x: 1.0}, angular: {z: -0.1}"

given that the <min_velocity> tag is set to 0, will result in the vehicle not steering to the right, while

gz topic -t "/model/vehicle_blue/cmd_vel" -m gz.msgs.Twist -p "linear: {x: 1.0}, angular: {z: 0.1}"

works just fine.

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
bug Something isn't working
Projects
Status: Inbox
2 participants