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.

Sign up for GitHub

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

Min and max parameters for velocity, acceleration, and jerk apply to linear and angular separately. #1229

Merged
merged 6 commits into from
Jan 11, 2022
Merged

Min and max parameters for velocity, acceleration, and jerk apply to linear and angular separately. #1229

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
6 commits
Select commit Hold shift + click to select a range
687d378
Min and max parameters for velocity, acceleration, and jerk apply to …
roni-kreinin Dec 2, 2021
63fbab7
Updated DiffDrive.cc to retain old parameters. They are overwritten i…
roni-kreinin Dec 13, 2021
a32e01d
Documented parameter changes
roni-kreinin Dec 14, 2021
4573602
Merge branch 'ign-gazebo5' into roni/5
chapulina Jan 11, 2022
679503b
Satisfy code checker
chapulina Jan 11, 2022
83fc98b
more codecheck
chapulina Jan 11, 2022
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
16 changes: 16 additions & 0 deletions examples/worlds/diff_drive.sdf
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -252,6 +252,14 @@
<wheel_separation>1.25</wheel_separation>
<wheel_radius>0.3</wheel_radius>
<odom_publish_frequency>1</odom_publish_frequency>
<max_linear_acceleration>1</max_linear_acceleration>
<min_linear_acceleration>-1</min_linear_acceleration>
<max_angular_acceleration>2</max_angular_acceleration>
<min_angular_acceleration>-2</min_angular_acceleration>
<max_linear_velocity>0.5</max_linear_velocity>
<min_linear_velocity>-0.5</min_linear_velocity>
<max_angular_velocity>1</max_angular_velocity>
<min_angular_velocity>-1</min_angular_velocity>
</plugin>

</model>
Expand Down Expand Up @@ -431,6 +439,14 @@
<right_joint>right_wheel_joint</right_joint>
<wheel_separation>1.25</wheel_separation>
<wheel_radius>0.3</wheel_radius>
<max_linear_acceleration>1</max_linear_acceleration>
<min_linear_acceleration>-1</min_linear_acceleration>
<max_angular_acceleration>2</max_angular_acceleration>
<min_angular_acceleration>-2</min_angular_acceleration>
<max_linear_velocity>0.5</max_linear_velocity>
<min_linear_velocity>-0.5</min_linear_velocity>
<max_angular_velocity>1</max_angular_velocity>
<min_angular_velocity>-1</min_angular_velocity>
</plugin>

</model>
Expand Down
8 changes: 8 additions & 0 deletions examples/worlds/diff_drive_skid.sdf
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -346,6 +346,14 @@
<right_joint>rear_right_wheel_joint</right_joint>
<wheel_separation>1.25</wheel_separation>
<wheel_radius>0.3</wheel_radius>
<max_linear_acceleration>1</max_linear_acceleration>
<min_linear_acceleration>-1</min_linear_acceleration>
<max_angular_acceleration>2</max_angular_acceleration>
<min_angular_acceleration>-2</min_angular_acceleration>
<max_linear_velocity>0.5</max_linear_velocity>
<min_linear_velocity>-0.5</min_linear_velocity>
<max_angular_velocity>1</max_angular_velocity>
<min_angular_velocity>-1</min_angular_velocity>
</plugin>

</model>
Expand Down
72 changes: 72 additions & 0 deletions src/systems/diff_drive/DiffDrive.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -210,42 +210,114 @@ void DiffDrive::Configure(const Entity &_entity,
this->dataPtr->limiterAng = std::make_unique<ignition::math::SpeedLimiter>();

// Parse speed limiter parameters.

// Min Velocity
if (_sdf->HasElement("min_velocity"))
chapulina marked this conversation as resolved.
Show resolved Hide resolved
{
const double minVel = _sdf->Get<double>("min_velocity");
this->dataPtr->limiterLin->SetMinVelocity(minVel);
this->dataPtr->limiterAng->SetMinVelocity(minVel);
}
if (_sdf->HasElement("min_linear_velocity"))
{
const double minLinVel = _sdf->Get<double>("min_linear_velocity");
this->dataPtr->limiterLin->SetMinVelocity(minLinVel);
}
if (_sdf->HasElement("min_angular_velocity"))
{
const double minAngVel = _sdf->Get<double>("min_angular_velocity");
this->dataPtr->limiterAng->SetMinVelocity(minAngVel);
}

// Max Velocity
if (_sdf->HasElement("max_velocity"))
{
const double maxVel = _sdf->Get<double>("max_velocity");
this->dataPtr->limiterLin->SetMaxVelocity(maxVel);
this->dataPtr->limiterAng->SetMaxVelocity(maxVel);
}
if (_sdf->HasElement("max_linear_velocity"))
{
const double maxLinVel = _sdf->Get<double>("max_linear_velocity");
this->dataPtr->limiterLin->SetMaxVelocity(maxLinVel);
}
if (_sdf->HasElement("max_angular_velocity"))
{
const double maxAngVel = _sdf->Get<double>("max_angular_velocity");
this->dataPtr->limiterAng->SetMaxVelocity(maxAngVel);
}

// Min Acceleration
if (_sdf->HasElement("min_acceleration"))
{
const double minAccel = _sdf->Get<double>("min_acceleration");
this->dataPtr->limiterLin->SetMinAcceleration(minAccel);
this->dataPtr->limiterAng->SetMinAcceleration(minAccel);
}
if (_sdf->HasElement("min_linear_acceleration"))
{
const double minLinAccel = _sdf->Get<double>("min_linear_acceleration");
this->dataPtr->limiterLin->SetMinAcceleration(minLinAccel);
}
if (_sdf->HasElement("min_angular_acceleration"))
{
const double minAngAccel = _sdf->Get<double>("min_angular_acceleration");
this->dataPtr->limiterAng->SetMinAcceleration(minAngAccel);
}

// Max Acceleration
if (_sdf->HasElement("max_acceleration"))
{
const double maxAccel = _sdf->Get<double>("max_acceleration");
this->dataPtr->limiterLin->SetMaxAcceleration(maxAccel);
this->dataPtr->limiterAng->SetMaxAcceleration(maxAccel);
}
if (_sdf->HasElement("max_linear_acceleration"))
{
const double maxLinAccel = _sdf->Get<double>("max_linear_acceleration");
this->dataPtr->limiterLin->SetMaxAcceleration(maxLinAccel);
}
if (_sdf->HasElement("max_angular_acceleration"))
{
const double maxAngAccel = _sdf->Get<double>("max_angular_acceleration");
this->dataPtr->limiterAng->SetMaxAcceleration(maxAngAccel);
}

// Min Jerk
if (_sdf->HasElement("min_jerk"))
{
const double minJerk = _sdf->Get<double>("min_jerk");
this->dataPtr->limiterLin->SetMinJerk(minJerk);
this->dataPtr->limiterAng->SetMinJerk(minJerk);
}
if (_sdf->HasElement("min_linear_jerk"))
{
const double minLinJerk = _sdf->Get<double>("min_linear_jerk");
this->dataPtr->limiterLin->SetMinJerk(minLinJerk);
}
if (_sdf->HasElement("min_angular_jerk"))
{
const double minAngJerk = _sdf->Get<double>("min_angular_jerk");
this->dataPtr->limiterAng->SetMinJerk(minAngJerk);
}

// Max Jerk
if (_sdf->HasElement("max_jerk"))
{
const double maxJerk = _sdf->Get<double>("max_jerk");
this->dataPtr->limiterLin->SetMaxJerk(maxJerk);
this->dataPtr->limiterAng->SetMaxJerk(maxJerk);
}
if (_sdf->HasElement("max_linear_jerk"))
{
const double maxLinJerk = _sdf->Get<double>("max_linear_jerk");
this->dataPtr->limiterLin->SetMaxJerk(maxLinJerk);
}
if (_sdf->HasElement("max_angular_jerk"))
{
const double maxAngJerk = _sdf->Get<double>("max_angular_jerk");
this->dataPtr->limiterAng->SetMaxJerk(maxAngJerk);
}

double odomFreq = _sdf->Get<double>("odom_publish_frequency", 50).first;
if (odomFreq > 0)
Expand Down
52 changes: 52 additions & 0 deletions src/systems/diff_drive/DiffDrive.hh
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -77,6 +77,58 @@ namespace systems
/// `ignition.msgs.Pose_V` message and the `<odom_topic>`
/// `ignition.msgs.Odometry` message. This element if optional,
/// and the default value is `{name_of_model}/{name_of_link}`.
///
/// `<min_velocity>`: Sets both the minimum linear and minimum angular
/// velocity.
///
/// `<max_velocity>`: Sets both the maximum linear and maximum angular
/// velocity.
///
/// `<min_acceleration>`: Sets both the minimum linear and minimum angular
/// acceleration.
///
/// `<max_acceleration>`: Sets both the maximum linear and maximum angular
/// acceleration.
///
/// `<min_jerk>`: Sets both the minimum linear and minimum angular jerk.
///
/// `<max_jerk>`: Sets both the maximum linear and maximum angular jerk.
///
/// `<min_linear_velocity>`: Sets the minimum linear velocity. Overrides
/// `<min_velocity>` if set.
///
/// `<max_linear_velocity>`: Sets the maximum linear velocity. Overrides
/// `<max_velocity>` if set.
///
/// `<min_angular_velocity>`: Sets the minimum angular velocity. Overrides
/// `<min_velocity>` if set.
///
/// `<max_angular_velocity>`: Sets the maximum angular velocity. Overrides
/// `<max_velocity>` if set.
///
/// `<min_linear_acceleration>`: Sets the minimum linear acceleration.
/// Overrides `<min_acceleration>` if set.
///
/// `<max_linear_acceleration>`: Sets the maximum linear acceleration.
/// Overrides `<max_acceleration>` if set.
///
/// `<min_angular_acceleration>`: Sets the minimum angular acceleration.
/// Overrides `<min_acceleration>` if set.
///
/// `<max_angular_acceleration>`: Sets the maximum angular acceleration.
/// Overrides `<max_acceleration>` if set.
///
/// `<min_linear_jerk>`: Sets the minimum linear jerk. Overrides `<min_jerk>`
/// if set.
///
/// `<max_linear_jerk>`: Sets the maximum linear jerk. Overrides `<max_jerk>`
/// if set.
///
/// `<min_angular_jerk>`: Sets the minimum angular jerk. Overrides
/// `<min_jerk>` if set.
///
/// `<max_angular_jerk>`: Sets the maximum angular jerk. Overrides
/// `<max_jerk>` if set.
class DiffDrive
: public System,
public ISystemConfigure,
Expand Down
12 changes: 8 additions & 4 deletions test/worlds/diff_drive.sdf
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -227,10 +227,14 @@
<right_joint>right_wheel_joint</right_joint>
<wheel_separation>1.25</wheel_separation>
<wheel_radius>0.3</wheel_radius>
<max_acceleration>1</max_acceleration>
<min_acceleration>-1</min_acceleration>
<max_velocity>0.5</max_velocity>
<min_velocity>-0.5</min_velocity>
<max_linear_acceleration>1</max_linear_acceleration>
<min_linear_acceleration>-1</min_linear_acceleration>
<max_angular_acceleration>2</max_angular_acceleration>
<min_angular_acceleration>-2</min_angular_acceleration>
<max_linear_velocity>0.5</max_linear_velocity>
<min_linear_velocity>-0.5</min_linear_velocity>
<max_angular_velocity>1</max_angular_velocity>
<min_angular_velocity>-1</min_angular_velocity>
<!-- no odom_publisher_frequency defaults to 50 Hz -->
</plugin>

Expand Down
12 changes: 8 additions & 4 deletions test/worlds/diff_drive_custom_topics.sdf
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -229,10 +229,14 @@
<wheel_radius>0.3</wheel_radius>
<topic>/model/foo/cmdvel</topic>
<odom_topic>/model/bar/odom</odom_topic>
<max_acceleration>1</max_acceleration>
<max_velocity>0.5</max_velocity>
<min_acceleration>-1</min_acceleration>
<min_velocity>-0.5</min_velocity>
<max_linear_acceleration>1</max_linear_acceleration>
<min_linear_acceleration>-1</min_linear_acceleration>
<max_angular_acceleration>2</max_angular_acceleration>
<min_angular_acceleration>-2</min_angular_acceleration>
<max_linear_velocity>0.5</max_linear_velocity>
<min_linear_velocity>-0.5</min_linear_velocity>
<max_angular_velocity>1</max_angular_velocity>
<min_angular_velocity>-1</min_angular_velocity>
<!-- no odom_publisher_frequency defaults to 50 Hz -->
</plugin>

Expand Down