From 687d378d5ef8cbcd54d7eeb9f16741c8e3449d16 Mon Sep 17 00:00:00 2001 From: Roni Kreinin Date: Thu, 2 Dec 2021 13:15:08 -0500 Subject: [PATCH 1/5] Min and max parameters for velocity, acceleration, and jerk apply to linear and angular separately. Signed-off-by: Roni Kreinin --- src/systems/diff_drive/DiffDrive.cc | 74 +++++++++++++++++++---------- 1 file changed, 50 insertions(+), 24 deletions(-) diff --git a/src/systems/diff_drive/DiffDrive.cc b/src/systems/diff_drive/DiffDrive.cc index 3ec64cae66..13abc37d6d 100644 --- a/src/systems/diff_drive/DiffDrive.cc +++ b/src/systems/diff_drive/DiffDrive.cc @@ -210,43 +210,69 @@ void DiffDrive::Configure(const Entity &_entity, this->dataPtr->limiterAng = std::make_unique(); // Parse speed limiter parameters. - if (_sdf->HasElement("min_velocity")) + if (_sdf->HasElement("min_linear_velocity")) { - const double minVel = _sdf->Get("min_velocity"); - this->dataPtr->limiterLin->SetMinVelocity(minVel); - this->dataPtr->limiterAng->SetMinVelocity(minVel); + const double minLinVel = _sdf->Get("min_linear_velocity"); + this->dataPtr->limiterLin->SetMinVelocity(minLinVel); } - if (_sdf->HasElement("max_velocity")) + if (_sdf->HasElement("max_linear_velocity")) { - const double maxVel = _sdf->Get("max_velocity"); - this->dataPtr->limiterLin->SetMaxVelocity(maxVel); - this->dataPtr->limiterAng->SetMaxVelocity(maxVel); + const double maxLinVel = _sdf->Get("max_linear_velocity"); + this->dataPtr->limiterLin->SetMaxVelocity(maxLinVel); } - if (_sdf->HasElement("min_acceleration")) + if (_sdf->HasElement("min_linear_acceleration")) { - const double minAccel = _sdf->Get("min_acceleration"); - this->dataPtr->limiterLin->SetMinAcceleration(minAccel); - this->dataPtr->limiterAng->SetMinAcceleration(minAccel); + const double minLinAccel = _sdf->Get("min_linear_acceleration"); + this->dataPtr->limiterLin->SetMinAcceleration(minLinAccel); } - if (_sdf->HasElement("max_acceleration")) + if (_sdf->HasElement("max_linear_acceleration")) { - const double maxAccel = _sdf->Get("max_acceleration"); - this->dataPtr->limiterLin->SetMaxAcceleration(maxAccel); - this->dataPtr->limiterAng->SetMaxAcceleration(maxAccel); + const double maxLinAccel = _sdf->Get("max_linear_acceleration"); + this->dataPtr->limiterLin->SetMaxAcceleration(maxLinAccel); } - if (_sdf->HasElement("min_jerk")) + if (_sdf->HasElement("min_linear_jerk")) { - const double minJerk = _sdf->Get("min_jerk"); - this->dataPtr->limiterLin->SetMinJerk(minJerk); - this->dataPtr->limiterAng->SetMinJerk(minJerk); + const double minLinJerk = _sdf->Get("min_linear_jerk"); + this->dataPtr->limiterLin->SetMinJerk(minLinJerk); } - if (_sdf->HasElement("max_jerk")) + if (_sdf->HasElement("max_linear_jerk")) { - const double maxJerk = _sdf->Get("max_jerk"); - this->dataPtr->limiterLin->SetMaxJerk(maxJerk); - this->dataPtr->limiterAng->SetMaxJerk(maxJerk); + const double maxLinJerk = _sdf->Get("max_linear_jerk"); + this->dataPtr->limiterLin->SetMaxJerk(maxLinJerk); } + if (_sdf->HasElement("min_angular_velocity")) + { + const double minAngVel = _sdf->Get("min_angular_velocity"); + this->dataPtr->limiterAng->SetMinVelocity(minAngVel); + } + if (_sdf->HasElement("max_angular_velocity")) + { + const double maxAngVel = _sdf->Get("max_angular_velocity"); + this->dataPtr->limiterAng->SetMaxVelocity(maxAngVel); + } + if (_sdf->HasElement("min_angular_acceleration")) + { + const double minAngAccel = _sdf->Get("min_angular_acceleration"); + this->dataPtr->limiterAng->SetMinAcceleration(minAngAccel); + } + if (_sdf->HasElement("max_angular_acceleration")) + { + const double maxAngAccel = _sdf->Get("max_angular_acceleration"); + this->dataPtr->limiterAng->SetMaxAcceleration(maxAngAccel); + } + if (_sdf->HasElement("min_angular_jerk")) + { + const double minAngJerk = _sdf->Get("min_angular_jerk"); + this->dataPtr->limiterAng->SetMinJerk(minAngJerk); + } + if (_sdf->HasElement("max_angular_jerk")) + { + const double maxAngJerk = _sdf->Get("max_angular_jerk"); + this->dataPtr->limiterAng->SetMaxJerk(maxAngJerk); + } + + double odomFreq = _sdf->Get("odom_publish_frequency", 50).first; if (odomFreq > 0) { From 63fbab70ddb7b1c59ba733734e16811395a8c801 Mon Sep 17 00:00:00 2001 From: Roni Kreinin Date: Mon, 13 Dec 2021 17:32:47 -0500 Subject: [PATCH 2/5] Updated DiffDrive.cc to retain old parameters. They are overwritten if new parameters are used. Added new parameters to example diff_drive worlds Added new parameters to test diff_drive worlds Signed-off-by: Roni Kreinin --- examples/worlds/diff_drive.sdf | 16 ++++ examples/worlds/diff_drive_skid.sdf | 8 ++ src/systems/diff_drive/DiffDrive.cc | 93 ++++++++++++++++++------ test/worlds/diff_drive.sdf | 12 ++- test/worlds/diff_drive_custom_topics.sdf | 12 ++- 5 files changed, 110 insertions(+), 31 deletions(-) diff --git a/examples/worlds/diff_drive.sdf b/examples/worlds/diff_drive.sdf index 647509944d..79fb0ba80b 100644 --- a/examples/worlds/diff_drive.sdf +++ b/examples/worlds/diff_drive.sdf @@ -252,6 +252,14 @@ 1.25 0.3 1 + 1 + -1 + 2 + -2 + 0.5 + -0.5 + 1 + -1 @@ -431,6 +439,14 @@ right_wheel_joint 1.25 0.3 + 1 + -1 + 2 + -2 + 0.5 + -0.5 + 1 + -1 diff --git a/examples/worlds/diff_drive_skid.sdf b/examples/worlds/diff_drive_skid.sdf index 4edf93497d..4a8b15b412 100644 --- a/examples/worlds/diff_drive_skid.sdf +++ b/examples/worlds/diff_drive_skid.sdf @@ -346,6 +346,14 @@ rear_right_wheel_joint 1.25 0.3 + 1 + -1 + 2 + -2 + 0.5 + -0.5 + 1 + -1 diff --git a/src/systems/diff_drive/DiffDrive.cc b/src/systems/diff_drive/DiffDrive.cc index 13abc37d6d..df35f41d17 100644 --- a/src/systems/diff_drive/DiffDrive.cc +++ b/src/systems/diff_drive/DiffDrive.cc @@ -210,62 +210,109 @@ void DiffDrive::Configure(const Entity &_entity, this->dataPtr->limiterAng = std::make_unique(); // Parse speed limiter parameters. + + // Min Velocity + if (_sdf->HasElement("min_velocity")) + { + const double minVel = _sdf->Get("min_velocity"); + this->dataPtr->limiterLin->SetMinVelocity(minVel); + this->dataPtr->limiterAng->SetMinVelocity(minVel); + } if (_sdf->HasElement("min_linear_velocity")) { const double minLinVel = _sdf->Get("min_linear_velocity"); this->dataPtr->limiterLin->SetMinVelocity(minLinVel); } - if (_sdf->HasElement("max_linear_velocity")) - { - const double maxLinVel = _sdf->Get("max_linear_velocity"); - this->dataPtr->limiterLin->SetMaxVelocity(maxLinVel); - } - if (_sdf->HasElement("min_linear_acceleration")) + if (_sdf->HasElement("min_angular_velocity")) { - const double minLinAccel = _sdf->Get("min_linear_acceleration"); - this->dataPtr->limiterLin->SetMinAcceleration(minLinAccel); + const double minAngVel = _sdf->Get("min_angular_velocity"); + this->dataPtr->limiterAng->SetMinVelocity(minAngVel); } - if (_sdf->HasElement("max_linear_acceleration")) + + // Max Velocity + if (_sdf->HasElement("max_velocity")) { - const double maxLinAccel = _sdf->Get("max_linear_acceleration"); - this->dataPtr->limiterLin->SetMaxAcceleration(maxLinAccel); + const double maxVel = _sdf->Get("max_velocity"); + this->dataPtr->limiterLin->SetMaxVelocity(maxVel); + this->dataPtr->limiterAng->SetMaxVelocity(maxVel); } - if (_sdf->HasElement("min_linear_jerk")) + if (_sdf->HasElement("max_linear_velocity")) { - const double minLinJerk = _sdf->Get("min_linear_jerk"); - this->dataPtr->limiterLin->SetMinJerk(minLinJerk); + const double maxLinVel = _sdf->Get("max_linear_velocity"); + this->dataPtr->limiterLin->SetMaxVelocity(maxLinVel); } - if (_sdf->HasElement("max_linear_jerk")) + if (_sdf->HasElement("max_angular_velocity")) { - const double maxLinJerk = _sdf->Get("max_linear_jerk"); - this->dataPtr->limiterLin->SetMaxJerk(maxLinJerk); + const double maxAngVel = _sdf->Get("max_angular_velocity"); + this->dataPtr->limiterAng->SetMaxVelocity(maxAngVel); } - if (_sdf->HasElement("min_angular_velocity")) + // Min Acceleration + if (_sdf->HasElement("min_acceleration")) { - const double minAngVel = _sdf->Get("min_angular_velocity"); - this->dataPtr->limiterAng->SetMinVelocity(minAngVel); + const double minAccel = _sdf->Get("min_acceleration"); + this->dataPtr->limiterLin->SetMinAcceleration(minAccel); + this->dataPtr->limiterAng->SetMinAcceleration(minAccel); } - if (_sdf->HasElement("max_angular_velocity")) + if (_sdf->HasElement("min_linear_acceleration")) { - const double maxAngVel = _sdf->Get("max_angular_velocity"); - this->dataPtr->limiterAng->SetMaxVelocity(maxAngVel); + const double minLinAccel = _sdf->Get("min_linear_acceleration"); + this->dataPtr->limiterLin->SetMinAcceleration(minLinAccel); } if (_sdf->HasElement("min_angular_acceleration")) { const double minAngAccel = _sdf->Get("min_angular_acceleration"); this->dataPtr->limiterAng->SetMinAcceleration(minAngAccel); } + + // Max Acceleration + if (_sdf->HasElement("max_acceleration")) + { + const double maxAccel = _sdf->Get("max_acceleration"); + this->dataPtr->limiterLin->SetMaxAcceleration(maxAccel); + this->dataPtr->limiterAng->SetMaxAcceleration(maxAccel); + } + if (_sdf->HasElement("max_linear_acceleration")) + { + const double maxLinAccel = _sdf->Get("max_linear_acceleration"); + this->dataPtr->limiterLin->SetMaxAcceleration(maxLinAccel); + } if (_sdf->HasElement("max_angular_acceleration")) { const double maxAngAccel = _sdf->Get("max_angular_acceleration"); this->dataPtr->limiterAng->SetMaxAcceleration(maxAngAccel); } + + // Min Jerk + if (_sdf->HasElement("min_jerk")) + { + const double minJerk = _sdf->Get("min_jerk"); + this->dataPtr->limiterLin->SetMinJerk(minJerk); + this->dataPtr->limiterAng->SetMinJerk(minJerk); + } + if (_sdf->HasElement("min_linear_jerk")) + { + const double minLinJerk = _sdf->Get("min_linear_jerk"); + this->dataPtr->limiterLin->SetMinJerk(minLinJerk); + } if (_sdf->HasElement("min_angular_jerk")) { const double minAngJerk = _sdf->Get("min_angular_jerk"); this->dataPtr->limiterAng->SetMinJerk(minAngJerk); } + + // Max Jerk + if (_sdf->HasElement("max_jerk")) + { + const double maxJerk = _sdf->Get("max_jerk"); + this->dataPtr->limiterLin->SetMaxJerk(maxJerk); + this->dataPtr->limiterAng->SetMaxJerk(maxJerk); + } + if (_sdf->HasElement("max_linear_jerk")) + { + const double maxLinJerk = _sdf->Get("max_linear_jerk"); + this->dataPtr->limiterLin->SetMaxJerk(maxLinJerk); + } if (_sdf->HasElement("max_angular_jerk")) { const double maxAngJerk = _sdf->Get("max_angular_jerk"); diff --git a/test/worlds/diff_drive.sdf b/test/worlds/diff_drive.sdf index dcb6a1fc2e..2f544baba0 100644 --- a/test/worlds/diff_drive.sdf +++ b/test/worlds/diff_drive.sdf @@ -227,10 +227,14 @@ right_wheel_joint 1.25 0.3 - 1 - -1 - 0.5 - -0.5 + 1 + -1 + 2 + -2 + 0.5 + -0.5 + 1 + -1 diff --git a/test/worlds/diff_drive_custom_topics.sdf b/test/worlds/diff_drive_custom_topics.sdf index 15a414d1e3..5c45c946fb 100644 --- a/test/worlds/diff_drive_custom_topics.sdf +++ b/test/worlds/diff_drive_custom_topics.sdf @@ -229,10 +229,14 @@ 0.3 /model/foo/cmdvel /model/bar/odom - 1 - 0.5 - -1 - -0.5 + 1 + -1 + 2 + -2 + 0.5 + -0.5 + 1 + -1 From a32e01dbd15d9fcd4dba9d5f7b8dc2cc846b711e Mon Sep 17 00:00:00 2001 From: Roni Kreinin Date: Tue, 14 Dec 2021 13:05:10 -0500 Subject: [PATCH 3/5] Documented parameter changes Signed-off-by: Roni Kreinin --- src/systems/diff_drive/DiffDrive.hh | 37 +++++++++++++++++++++++++++++ 1 file changed, 37 insertions(+) diff --git a/src/systems/diff_drive/DiffDrive.hh b/src/systems/diff_drive/DiffDrive.hh index 35002495d5..e51b19fe6b 100644 --- a/src/systems/diff_drive/DiffDrive.hh +++ b/src/systems/diff_drive/DiffDrive.hh @@ -77,6 +77,43 @@ namespace systems /// `ignition.msgs.Pose_V` message and the `` /// `ignition.msgs.Odometry` message. This element if optional, /// and the default value is `{name_of_model}/{name_of_link}`. + /// + /// ``: Sets both the minimum linear and minimum angular velocity. + /// + /// ``: Sets both the maximum linear and maximum angular velocity. + /// + /// ``: Sets both the minimum linear and minimum angular acceleration. + /// + /// ``: Sets both the maximum linear and maximum angular acceleration. + /// + /// ``: Sets both the minimum linear and minimum angular jerk. + /// + /// ``: Sets both the maximum linear and maximum angular jerk. + /// + /// ``: Sets the minimum linear velocity. Overrides `` if set. + /// + /// ``: Sets the maximum linear velocity. Overrides `` if set. + /// + /// ``: Sets the minimum angular velocity. Overrides `` if set. + /// + /// ``: Sets the maximum angular velocity. Overrides `` if set. + /// + /// ``: Sets the minimum linear acceleration. Overrides `` if set. + /// + /// ``: Sets the maximum linear acceleration. Overrides `` if set. + /// + /// ``: Sets the minimum angular acceleration. Overrides `` if set. + /// + /// ``: Sets the maximum angular acceleration. Overrides `` if set. + /// + /// ``: Sets the minimum linear jerk. Overrides `` if set. + /// + /// ``: Sets the maximum linear jerk. Overrides `` if set. + /// + /// ``: Sets the minimum angular jerk. Overrides `` if set. + /// + /// ``: Sets the maximum angular jerk. Overrides `` if set. + class DiffDrive : public System, public ISystemConfigure, From 679503b081bf4312a58ef194401efc9108677ad5 Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Mon, 10 Jan 2022 16:25:48 -0800 Subject: [PATCH 4/5] Satisfy code checker Signed-off-by: Louise Poubel --- src/systems/diff_drive/DiffDrive.cc | 1 - src/systems/diff_drive/DiffDrive.hh | 49 +++++++++++++++++++---------- 2 files changed, 32 insertions(+), 18 deletions(-) diff --git a/src/systems/diff_drive/DiffDrive.cc b/src/systems/diff_drive/DiffDrive.cc index df35f41d17..ec8e5a46d2 100644 --- a/src/systems/diff_drive/DiffDrive.cc +++ b/src/systems/diff_drive/DiffDrive.cc @@ -318,7 +318,6 @@ void DiffDrive::Configure(const Entity &_entity, const double maxAngJerk = _sdf->Get("max_angular_jerk"); this->dataPtr->limiterAng->SetMaxJerk(maxAngJerk); } - double odomFreq = _sdf->Get("odom_publish_frequency", 50).first; if (odomFreq > 0) diff --git a/src/systems/diff_drive/DiffDrive.hh b/src/systems/diff_drive/DiffDrive.hh index e51b19fe6b..1f745626ca 100644 --- a/src/systems/diff_drive/DiffDrive.hh +++ b/src/systems/diff_drive/DiffDrive.hh @@ -78,42 +78,57 @@ namespace systems /// `ignition.msgs.Odometry` message. This element if optional, /// and the default value is `{name_of_model}/{name_of_link}`. /// - /// ``: Sets both the minimum linear and minimum angular velocity. + /// ``: Sets both the minimum linear and minimum angular + /// velocity. /// - /// ``: Sets both the maximum linear and maximum angular velocity. + /// ``: Sets both the maximum linear and maximum angular + /// velocity. /// - /// ``: Sets both the minimum linear and minimum angular acceleration. + /// ``: Sets both the minimum linear and minimum angular + /// acceleration. /// - /// ``: Sets both the maximum linear and maximum angular acceleration. + /// ``: Sets both the maximum linear and maximum angular + /// acceleration. /// /// ``: Sets both the minimum linear and minimum angular jerk. /// /// ``: Sets both the maximum linear and maximum angular jerk. /// - /// ``: Sets the minimum linear velocity. Overrides `` if set. + /// ``: Sets the minimum linear velocity. Overrides + /// `` if set. /// - /// ``: Sets the maximum linear velocity. Overrides `` if set. + /// ``: Sets the maximum linear velocity. Overrides + /// `` if set. /// - /// ``: Sets the minimum angular velocity. Overrides `` if set. + /// ``: Sets the minimum angular velocity. Overrides + /// `` if set. /// - /// ``: Sets the maximum angular velocity. Overrides `` if set. + /// ``: Sets the maximum angular velocity. Overrides + /// `` if set. /// - /// ``: Sets the minimum linear acceleration. Overrides `` if set. + /// ``: Sets the minimum linear acceleration. + /// Overrides `` if set. /// - /// ``: Sets the maximum linear acceleration. Overrides `` if set. + /// ``: Sets the maximum linear acceleration. + /// Overrides `` if set. /// - /// ``: Sets the minimum angular acceleration. Overrides `` if set. + /// ``: Sets the minimum angular acceleration. + /// Overrides `` if set. /// - /// ``: Sets the maximum angular acceleration. Overrides `` if set. + /// ``: Sets the maximum angular acceleration. + /// Overrides `` if set. /// - /// ``: Sets the minimum linear jerk. Overrides `` if set. + /// ``: Sets the minimum linear jerk. Overrides `` + /// if set. /// - /// ``: Sets the maximum linear jerk. Overrides `` if set. + /// ``: Sets the maximum linear jerk. Overrides `` + /// if set. /// - /// ``: Sets the minimum angular jerk. Overrides `` if set. + /// ``: Sets the minimum angular jerk. Overrides `` + /// if set. /// - /// ``: Sets the maximum angular jerk. Overrides `` if set. - + /// ``: Sets the maximum angular jerk. Overrides `` + /// if set. class DiffDrive : public System, public ISystemConfigure, From 83fc98b32aab698bd9648ba2e854a30f37bff9a4 Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Mon, 10 Jan 2022 17:43:43 -0800 Subject: [PATCH 5/5] more codecheck Signed-off-by: Louise Poubel --- src/systems/diff_drive/DiffDrive.hh | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/systems/diff_drive/DiffDrive.hh b/src/systems/diff_drive/DiffDrive.hh index 1f745626ca..c9d45e6617 100644 --- a/src/systems/diff_drive/DiffDrive.hh +++ b/src/systems/diff_drive/DiffDrive.hh @@ -124,11 +124,11 @@ namespace systems /// ``: Sets the maximum linear jerk. Overrides `` /// if set. /// - /// ``: Sets the minimum angular jerk. Overrides `` - /// if set. + /// ``: Sets the minimum angular jerk. Overrides + /// `` if set. /// - /// ``: Sets the maximum angular jerk. Overrides `` - /// if set. + /// ``: Sets the maximum angular jerk. Overrides + /// `` if set. class DiffDrive : public System, public ISystemConfigure,