diff --git a/src/systems/ackermann_steering/AckermannSteering.cc b/src/systems/ackermann_steering/AckermannSteering.cc index 5c432f7e68..184744cb0d 100644 --- a/src/systems/ackermann_steering/AckermannSteering.cc +++ b/src/systems/ackermann_steering/AckermannSteering.cc @@ -197,6 +197,9 @@ class gz::sim::systems::AckermannSteeringPrivate /// \brief Last target angle requested. public: msgs::Double targetAng; + /// \brief P gain for angular position. + public: double gainPAng{1.0}; + /// \brief A mutex to protect the target velocity command. public: std::mutex mutex; @@ -290,6 +293,11 @@ void AckermannSteering::Configure(const Entity &_entity, this->dataPtr->steeringLimit = _sdf->Get("steering_limit", this->dataPtr->steeringLimit).first; + // Get proportional gain for steering angle. + if (_sdf->HasElement("steer_p_gain")) + { + this->dataPtr->gainPAng = _sdf->Get("steer_p_gain"); + } // Instantiate the speed limiters. this->dataPtr->limiterLin = std::make_unique(); @@ -927,11 +935,10 @@ void AckermannSteeringPrivate::UpdateAngle( double leftDelta = leftSteeringJointAngle - leftSteeringPos->Data()[0]; double rightDelta = rightSteeringJointAngle - rightSteeringPos->Data()[0]; - // Simple proportional control with a gain of 1 + // Simple proportional control with settable gain. // Adding programmable PID values might be a future feature. - // Works as is for tested cases - this->leftSteeringJointSpeed = leftDelta; - this->rightSteeringJointSpeed = rightDelta; + this->leftSteeringJointSpeed = this->gainPAng * leftDelta; + this->rightSteeringJointSpeed = this->gainPAng * rightDelta; } ////////////////////////////////////////////////// diff --git a/src/systems/ackermann_steering/AckermannSteering.hh b/src/systems/ackermann_steering/AckermannSteering.hh index fbba1ef087..2630cd0d6f 100644 --- a/src/systems/ackermann_steering/AckermannSteering.hh +++ b/src/systems/ackermann_steering/AckermannSteering.hh @@ -43,6 +43,9 @@ namespace systems /// and wheel_separation. Uses gz::msg::Double on default topic name /// `/model/{name_of_model}/steer_angle` /// + /// ``: Float used to control the steering angle P gain. + /// Only used for when in steering_only mode. + /// /// ``: Name of a joint that controls a left wheel. This /// element can appear multiple times, and must appear at least once. /// diff --git a/test/worlds/ackermann_steering_only.sdf b/test/worlds/ackermann_steering_only.sdf index a85bbbf963..b41297a888 100644 --- a/test/worlds/ackermann_steering_only.sdf +++ b/test/worlds/ackermann_steering_only.sdf @@ -364,6 +364,7 @@ front_left_wheel_steering_joint front_right_wheel_steering_joint true + 10.0 0.5 1.0 1.25