From 2e612407d2065e50f75d14abc956df78b5728a75 Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Mon, 28 Aug 2023 11:05:14 -0700 Subject: [PATCH 1/9] Add Model::GetMass API that returns cached mass This breaks ABI, so is not intended for gazebo11. Compute and cache mass in Model::Init. Signed-off-by: Steve Peters --- gazebo/physics/Model.cc | 15 +++++++++++++++ gazebo/physics/Model.hh | 7 +++++++ 2 files changed, 22 insertions(+) diff --git a/gazebo/physics/Model.cc b/gazebo/physics/Model.cc index a23048b98c..4af22743b5 100644 --- a/gazebo/physics/Model.cc +++ b/gazebo/physics/Model.cc @@ -297,6 +297,14 @@ void Model::LoadJoints() ////////////////////////////////////////////////// void Model::Init() { + // Cache the total mass of the model + this->mass = 0.0; + for (uint64_t i = 0; i < this->modelSDFDom->LinkCount(); ++i) + { + this->mass += + this->modelSDFDom->LinkByIndex(i)->Inertial().MassMatrix().Mass(); + } + // Record the model's initial pose (for reseting) this->SetInitialRelativePose(this->SDFPoseRelativeToParent()); this->SetRelativePose(this->SDFPoseRelativeToParent()); @@ -1861,6 +1869,7 @@ void Model::PluginInfo(const common::URI &_pluginUri, << std::endl; } +////////////////////////////////////////////////// std::optional Model::SDFSemanticPose() const { if (nullptr != this->modelSDFDom) @@ -1869,3 +1878,9 @@ std::optional Model::SDFSemanticPose() const } return std::nullopt; } + +////////////////////////////////////////////////// +double Model::GetMass() const +{ + return this->mass; +} diff --git a/gazebo/physics/Model.hh b/gazebo/physics/Model.hh index d39d2faa5b..a1f32fec53 100644 --- a/gazebo/physics/Model.hh +++ b/gazebo/physics/Model.hh @@ -93,6 +93,10 @@ namespace gazebo /// \return The SDF DOM for this model. public: const sdf::Model *GetSDFDom() const; + /// \brief Get the total mass of this model. + /// \return The mass of the model, cached during initialization. + public: double GetMass() const; + /// \internal /// \brief Get the SDF element for the model, without all effects of /// scaling. This is useful in cases when the scale will be applied @@ -567,6 +571,9 @@ namespace gazebo /// \brief SDF Model DOM object private: const sdf::Model *modelSDFDom = nullptr; + + /// \brief Cached mass of the entire model. + private: double mass = 0.0; }; /// \} } From 60ca90128a78eb4f8aa027cc041f5791e56fbcde Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Mon, 28 Aug 2023 12:01:19 -0700 Subject: [PATCH 2/9] Add ODELink::AddedForce accessor This returns the total forces added by the AddForce and AddRelativeForce APIs. This change breaks ABI and is not intended for gazebo11. Signed-off-by: Steve Peters --- gazebo/physics/ode/ODELink.cc | 11 +++++++++++ gazebo/physics/ode/ODELink.hh | 7 +++++++ 2 files changed, 18 insertions(+) diff --git a/gazebo/physics/ode/ODELink.cc b/gazebo/physics/ode/ODELink.cc index 43840ae3fc..d528b08890 100644 --- a/gazebo/physics/ode/ODELink.cc +++ b/gazebo/physics/ode/ODELink.cc @@ -172,6 +172,9 @@ void ODELink::MoveCallback(dBodyID _id) const dReal *dtorque = dBodyGetTorque(_id); self->torque.Set(dtorque[0], dtorque[1], dtorque[2]); } + + // clear cache of AddForce values + self->addedForce.Set(0, 0, 0); } ////////////////////////////////////////////////// @@ -567,6 +570,12 @@ void ODELink::SetTorque(const ignition::math::Vector3d &_torque) << " does not exist, unable to SetTorque" << std::endl; } +////////////////////////////////////////////////// +const ignition::math::Vector3d &ODELink::AddedForce() const +{ + return this->addedForce; +} + ////////////////////////////////////////////////// void ODELink::AddForce(const ignition::math::Vector3d &_force) { @@ -574,6 +583,7 @@ void ODELink::AddForce(const ignition::math::Vector3d &_force) { this->SetEnabled(true); dBodyAddForce(this->linkId, _force.X(), _force.Y(), _force.Z()); + this->addedForce += _force; } else if (!this->IsStatic()) gzlog << "ODE body for link [" << this->GetScopedName() << "]" @@ -587,6 +597,7 @@ void ODELink::AddRelativeForce(const ignition::math::Vector3d &_force) { this->SetEnabled(true); dBodyAddRelForce(this->linkId, _force.X(), _force.Y(), _force.Z()); + this->addedForce += this->WorldPose().Rot().RotateVector(_force); } else if (!this->IsStatic()) gzlog << "ODE body for link [" << this->GetScopedName() << "]" diff --git a/gazebo/physics/ode/ODELink.hh b/gazebo/physics/ode/ODELink.hh index db22dbb153..2ce1fa44d1 100644 --- a/gazebo/physics/ode/ODELink.hh +++ b/gazebo/physics/ode/ODELink.hh @@ -83,6 +83,10 @@ namespace gazebo // Documentation inherited public: virtual void SetTorque(const ignition::math::Vector3d &_torque); + /// Get sum of forces expressed in world frame that have been added by + /// Link::Add*Force during this timestep. + public: const ignition::math::Vector3d &AddedForce() const; + // Documentation inherited public: virtual void AddForce(const ignition::math::Vector3d &_force); @@ -196,6 +200,9 @@ namespace gazebo /// \brief Cache torque applied on body private: ignition::math::Vector3d torque; + + /// \brief Cache force applied by AddForce + private: ignition::math::Vector3d addedForce; }; /// \} } From 65cc16d375e4c872b3402cc8d59ef4c6bae1d7ca Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Mon, 28 Aug 2023 18:18:24 -0700 Subject: [PATCH 3/9] ODECollision: add nonlinear_slip plowing params Apply scaling to slip compliance above and below threshold slopes to induce a nonnlinear effect. Signed-off-by: Steve Peters --- gazebo/physics/ode/ODECollision.cc | 18 ++++++++++++++++++ gazebo/physics/ode/ODECollision.hh | 16 ++++++++++++++++ 2 files changed, 34 insertions(+) diff --git a/gazebo/physics/ode/ODECollision.cc b/gazebo/physics/ode/ODECollision.cc index 6999d9f0f9..02398dfb7d 100644 --- a/gazebo/physics/ode/ODECollision.cc +++ b/gazebo/physics/ode/ODECollision.cc @@ -185,6 +185,24 @@ bool ODECollision::ParseWheelPlowingParams( return false; } + const std::string kNonlinearSlip = "nonlinear_slip"; + if (wheelElem->HasElement(kNonlinearSlip)) + { + sdf::ElementPtr nonlinearSlipElem = wheelElem->GetElement(kNonlinearSlip); + if (nonlinearSlipElem->HasElement("lower")) + { + sdf::ElementPtr lowerElem = nonlinearSlipElem->GetElement("lower"); + _plowing.nonlinearSlipLowerDegree = lowerElem->Get("degree"); + _plowing.nonlinearSlipLowerPerDegree = lowerElem->Get("per_degree"); + } + if (nonlinearSlipElem->HasElement("upper")) + { + sdf::ElementPtr upperElem = nonlinearSlipElem->GetElement("upper"); + _plowing.nonlinearSlipUpperDegree = upperElem->Get("degree"); + _plowing.nonlinearSlipUpperPerDegree = upperElem->Get("per_degree"); + } + } + _plowing.maxAngle = plowingMaxAngle; _plowing.saturationVelocity = plowingSaturationVelocity; _plowing.deadbandVelocity = plowingDeadbandVelocity; diff --git a/gazebo/physics/ode/ODECollision.hh b/gazebo/physics/ode/ODECollision.hh index 9ef941f393..465b665f99 100644 --- a/gazebo/physics/ode/ODECollision.hh +++ b/gazebo/physics/ode/ODECollision.hh @@ -49,6 +49,22 @@ namespace gazebo /// \brief Plowing deadband velocity: the linear wheel velocity [m/s] /// below which no plowing effect occurs. public: double deadbandVelocity = 0.0; + + /// \brief The slope value in degrees below which the nonlinear slip + /// effect is applied. + public: double nonlinearSlipLowerDegree = -361.0; + + /// \brief The slope value in degrees above which the nonlinear slip + /// effect is applied. + public: double nonlinearSlipUpperDegree = 361.0; + + /// \brief The rate of change in slip compliance per degree of slope + /// below the "lower degree" value. + public: double nonlinearSlipLowerPerDegree = 0.0; + + /// \brief The rate of change in slip compliance per degree of slope + /// above the "upper degree" value. + public: double nonlinearSlipUpperPerDegree = 0.0; }; /// \brief Base class for all ODE collisions. From 5101e48707d9639bc44fe9ca3aa4bcf109e293c3 Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Mon, 28 Aug 2023 18:20:35 -0700 Subject: [PATCH 4/9] Fix whitespace in example world Signed-off-by: Steve Peters --- .../worlds/plowing_effect_tricycle_demo.world | 26 +++++++++---------- 1 file changed, 13 insertions(+), 13 deletions(-) diff --git a/test/worlds/plowing_effect_tricycle_demo.world b/test/worlds/plowing_effect_tricycle_demo.world index a62f5780cf..7243a0bc9b 100644 --- a/test/worlds/plowing_effect_tricycle_demo.world +++ b/test/worlds/plowing_effect_tricycle_demo.world @@ -25,7 +25,7 @@ --> - 0 0 -9.75 + 0 0 -9.75 model://plowing_effect_ground_plane @@ -40,18 +40,18 @@ - 3.0 - 3.0 + 3.0 + 3.0 32 - 3.0 - 3.0 + 3.0 + 3.0 32 - 3.0 - 3.0 + 3.0 + 3.0 32 @@ -101,18 +101,18 @@ - 3.0 - 3.0 + 3.0 + 3.0 32 - 3.0 - 3.0 + 3.0 + 3.0 32 - 3.0 - 3.0 + 3.0 + 3.0 32 From 87da65485adabe849d961c7bed3c9b359c8584ed Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Mon, 28 Aug 2023 18:21:30 -0700 Subject: [PATCH 5/9] ODEPhysics: implement nonlineaar slip effect Signed-off-by: Steve Peters --- gazebo/physics/ode/ODEPhysics.cc | 58 +++++++++++++++++++++++++++++--- 1 file changed, 54 insertions(+), 4 deletions(-) diff --git a/gazebo/physics/ode/ODEPhysics.cc b/gazebo/physics/ode/ODEPhysics.cc index 7067861457..9cbbe0ec0d 100644 --- a/gazebo/physics/ode/ODEPhysics.cc +++ b/gazebo/physics/ode/ODEPhysics.cc @@ -26,7 +26,9 @@ #include #include +#include #include +#include #include #include @@ -1158,6 +1160,10 @@ void ODEPhysics::Collide(ODECollision *_collision1, ODECollision *_collision2, /// << " from surface with smaller mu1\n"; } + // Longitudinal slope angle in degrees averaged over each contact point. + ignition::math::SignalMean meanSlopeDegrees; + ODECollisionWheelPlowingParams wheelPlowing; + if (fd != ignition::math::Vector3d::Zero) { contact.surface.mode |= dContactFDir1; @@ -1170,7 +1176,6 @@ void ODEPhysics::Collide(ODECollision *_collision1, ODECollision *_collision2, auto collision2Sdf = _collision2->GetSDF(); const std::string kPlowingTerrain = "gz:plowing_terrain"; - ODECollisionWheelPlowingParams wheelPlowing; ODECollision *wheelCollision = nullptr; sdf::ElementPtr terrainSdf = nullptr; @@ -1190,8 +1195,20 @@ void ODEPhysics::Collide(ODECollision *_collision1, ODECollision *_collision2, if (wheelCollision && terrainSdf) { - // compute slope - double slope = wheelPlowing.maxAngle.Radian() / + // compute chassis world force + ModelPtr wheelModel = wheelCollision->GetModel(); + // initialize from model's gravity force + ignition::math::Vector3d worldForce = + wheelModel->GetMass() * this->world->Gravity(); + // Get the added force applied to the canonical link + LinkPtr link = wheelModel->GetLink(); + ODELinkPtr chassisLink = boost::dynamic_pointer_cast(link); + if (chassisLink) + { + worldForce += chassisLink->AddedForce(); + } + // compute sensitivity of plowing angle to velocity + double plowingSensitivity = wheelPlowing.maxAngle.Radian() / (wheelPlowing.saturationVelocity - wheelPlowing.deadbandVelocity); // Assume origin of collision frame is wheel center @@ -1217,6 +1234,16 @@ void ODEPhysics::Collide(ODECollision *_collision1, ODECollision *_collision2, ignition::math::Vector3d unitLongitudinal = contactNormalCopy.Cross(fdir1); + // Compute normal and longitudinal forces (before plowing) + double normalForce = -worldForce.Dot(contactNormalCopy); + double longitudinalForce = worldForce.Dot(unitLongitudinal); + + // Estimate slope angle from world force in longitudinal/normal plane + ignition::math::Angle slopeAngle(atan2(longitudinalForce, normalForce)); + + // Store average slope degrees + meanSlopeDegrees.InsertData(slopeAngle.Degree()); + // Compute longitudinal speed (dot product) double wheelSpeedLongitudinal = wheelLinearVelocity.Dot(unitLongitudinal); @@ -1225,7 +1252,7 @@ void ODEPhysics::Collide(ODECollision *_collision1, ODECollision *_collision2, double plowingAngle = saturation_deadband(wheelPlowing.maxAngle.Radian(), wheelPlowing.deadbandVelocity, - slope, + plowingSensitivity, wheelSpeedLongitudinal); // Construct axis-angle quaternion using fdir1 and plowing angle @@ -1278,6 +1305,29 @@ void ODEPhysics::Collide(ODECollision *_collision1, ODECollision *_collision2, contact.surface.slip2 *= numc; contact.surface.slip3 *= numc; + if (meanSlopeDegrees.Count() > 0) + { + // multiplying by numc has issues with plowing so undo that operation + contact.surface.slip1 /= numc; + contact.surface.slip2 /= numc; + contact.surface.slip3 /= numc; + + // Increase slip compliance at a specified rate above and below thresholds + // modify slip2 value to affect longitudinal slip + if (meanSlopeDegrees.Value() > wheelPlowing.nonlinearSlipUpperDegree) + { + const double degreesAboveThreshold = + meanSlopeDegrees.Value() - wheelPlowing.nonlinearSlipUpperDegree; + contact.surface.slip2 *= (1 + wheelPlowing.nonlinearSlipUpperPerDegree * degreesAboveThreshold); + } + else if (meanSlopeDegrees.Value() < wheelPlowing.nonlinearSlipLowerDegree) + { + const double degreesBelowThreshold = + wheelPlowing.nonlinearSlipLowerDegree - meanSlopeDegrees.Value(); + contact.surface.slip2 *= (1 + wheelPlowing.nonlinearSlipLowerPerDegree * degreesBelowThreshold); + } + } + // Combine torsional friction patch radius values contact.surface.patch_radius = std::max(surf1->FrictionPyramid()->PatchRadius(), From 9f96569aa42b95f35a7cce881a287fe9ea8dc714 Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Mon, 28 Aug 2023 18:21:57 -0700 Subject: [PATCH 6/9] Example world with nonlinear slip effect Signed-off-by: Steve Peters --- .../model.config | 26 + .../model.sdf | 459 ++++++++++++++++++ ...plowing_nonlinear_slip_tricycle_demo.world | 166 +++++++ 3 files changed, 651 insertions(+) create mode 100644 test/models/plowing_nonlinear_slip_trisphere_cycle/model.config create mode 100644 test/models/plowing_nonlinear_slip_trisphere_cycle/model.sdf create mode 100644 test/worlds/plowing_nonlinear_slip_tricycle_demo.world diff --git a/test/models/plowing_nonlinear_slip_trisphere_cycle/model.config b/test/models/plowing_nonlinear_slip_trisphere_cycle/model.config new file mode 100644 index 0000000000..470f60cac0 --- /dev/null +++ b/test/models/plowing_nonlinear_slip_trisphere_cycle/model.config @@ -0,0 +1,26 @@ + + + + Tricycle with spherical wheels and plowing effect with nonlinear slip + 1.0 + model.sdf + + + Steve Peters + scpeters@osrfoundation.org + + + + Aditya Pande + aditya.pande@openrobotics.org + + + + A tricycle with spherical wheels and front-wheel steering. + The first friction direction for each wheel is parallel to the joint axis + for each wheel spin joint, which corresponds to the wheel's lateral + direction. + The plowing and nonlinear slip parameters are specified in the //collision/gz:plowing_wheel + element for each wheel's sperical collision element. + + diff --git a/test/models/plowing_nonlinear_slip_trisphere_cycle/model.sdf b/test/models/plowing_nonlinear_slip_trisphere_cycle/model.sdf new file mode 100644 index 0000000000..8bb29a457a --- /dev/null +++ b/test/models/plowing_nonlinear_slip_trisphere_cycle/model.sdf @@ -0,0 +1,459 @@ + + + + + -0.40855911616047164 0 0.38502293110800634 0 -0.522020852957719 0 + + 0.0 0 0 0 0 0 + 10 + + 0.22799999999999998 + 0.7435210984814149 + 0.9655210984814149 + 0 + 0 + 0 + + + + -0.4713346258704366 0 0 1.5707963267948966 0 0 + + + 1.0392304845413263 + 0.03 + + + + + -0.4713346258704366 0 0 1.5707963267948966 0 0 + + + 1.0392304845413263 + 0.03 + + + + + + + + 0 0.17155177419583564 0 0 1.5707963267948966 -0.3490658503988659 + + + 1.0031676644991372 + 0.03 + + + + + 0 0.17155177419583564 0 0 1.5707963267948966 -0.3490658503988659 + + + 1.0031676644991372 + 0.03 + + + + + + + + 0 -0.17155177419583564 0 0 1.5707963267948966 0.3490658503988659 + + + 1.0031676644991372 + 0.03 + + + + + 0 -0.17155177419583564 0 0 1.5707963267948966 0.3490658503988659 + + + 1.0031676644991372 + 0.03 + + + + + + + + + 0.04144088383952833 0 0.38502293110800634 0 -0.17453292519943295 0 + + 3 + + 0.15820312499999997 + 0.058359374999999984 + 0.10265624999999999 + 0 + 0 + 0 + + + + 0 0 0.397747564417433 1.5707963267948966 0 0 + + + 0.6363961030678927 + 0.0375 + + + + + 0 0 0.397747564417433 1.5707963267948966 0 0 + + + 0.6363961030678927 + 0.0375 + + + + + + + + 0 0 0.2386485386504598 0 0 0 + + + 0.31819805153394637 + 0.0375 + + + + + 0 0 0.2386485386504598 0 0 0 + + + 0.31819805153394637 + 0.0375 + + + + + + + + 0 0 -0.23864853865045976 1.5707963267948966 0 0 + + + 0.6363961030678927 + 0.015 + + + + + 0 0 -0.23864853865045976 1.5707963267948966 0 0 + + + 0.6363961030678927 + 0.015 + + + + + + + + 0 0.15909902576697318 -0.07954951288348658 0.7853981633974483 0 0 + + + 0.45 + 0.0375 + + + + + 0 0.15909902576697318 -0.07954951288348658 0.7853981633974483 0 0 + + + 0.45 + 0.0375 + + + + + + + + 0 -0.15909902576697318 -0.07954951288348658 -0.7853981633974483 0 0 + + + 0.45 + 0.0375 + + + + + 0 -0.15909902576697318 -0.07954951288348658 -0.7853981633974483 0 0 + + + 0.45 + 0.0375 + + + + + + + + + 0.08288176767905665 0 0.15 0 0 0 + + 0.5 + + 0.0045 + 0.0045 + 0.0045 + 0 + 0 + 0 + + + + + 5 + 0.3 + 0.4 + + + -6 + 0.01 + + + 6 + 0.01 + + + + + + 0.15 + + + + + + 4000 + 16 + 10 + 0.0005 + + + + + 0.7 + 0.7 + 0 1 0 + + + + + + + + 0.15 + + + + + + + + + frame + fork + + 0 0 1 + + 1.0 + + + -0.9599310885968813 + 0.9599310885968813 + + + + + fork + wheel_front + + 0 1 0 + + + + -0.8171182323209433 0.5196152422706631 0.15 0 0 0 + + 0.5 + + 0.0045 + 0.0045 + 0.0045 + 0 + 0 + 0 + + + + + 5 + 0.3 + 0.4 + + + -6 + 0.01 + + + 6 + 0.01 + + + + + + 0.15 + + + + + + 4000 + 16 + 10 + 0.0005 + + + + + 0.7 + 0.7 + 0 1 0 + + + + + + + + 0.15 + + + + + + + + + frame + wheel_rear_left + + 0 1 0 + + + + -0.8171182323209433 -0.5196152422706631 0.15 0 0 0 + + 0.5 + + 0.0045 + 0.0045 + 0.0045 + 0 + 0 + 0 + + + + + 5 + 0.3 + 0.4 + + + -6 + 0.01 + + + 6 + 0.01 + + + + + + 0.15 + + + + + + 4000 + 16 + 10 + 0.0005 + + + + + 0.7 + 0.7 + 0 1 0 + + + + + + + + 0.15 + + + + + + + + + frame + wheel_rear_right + + 0 1 0 + + + + diff --git a/test/worlds/plowing_nonlinear_slip_tricycle_demo.world b/test/worlds/plowing_nonlinear_slip_tricycle_demo.world new file mode 100644 index 0000000000..2cbd675353 --- /dev/null +++ b/test/worlds/plowing_nonlinear_slip_tricycle_demo.world @@ -0,0 +1,166 @@ + + + + + -2 0 -9.75 + + model://plowing_effect_ground_plane + + + model://sun + + + + model://plowing_effect_trisphere_cycle + plowing_tricycle + 0 0 0 0 0 0 + + + + 3.0 + 3.0 + 32 + + + 3.0 + 3.0 + 32 + + + 3.0 + 3.0 + 32 + + + + + + wheel_front_steer + 0 + 9 0 0.1 + + + wheel_rear_left_spin + wheel_rear_right_spin + wheel_front_spin + 6.0 + 9 0 0 + + + wheel_rear_left_spin + wheel_rear_right_spin + 2.15 + + + + + + + + + model://plowing_nonlinear_slip_trisphere_cycle + nonlinear_slip_tricycle + 0 2 0 0 0 0 + + + + 3.0 + 3.0 + 32 + + + 3.0 + 3.0 + 32 + + + 3.0 + 3.0 + 32 + + + + + + wheel_front_steer + 0 + 9 0 0.1 + + + wheel_rear_left_spin + wheel_rear_right_spin + wheel_front_spin + 6.0 + 9 0 0 + + + wheel_rear_left_spin + wheel_rear_right_spin + 2.15 + + + + + + + + + + 1.5 -4 2.5 0 0.5 1.6 + orbit + + + + + From 6a96f1184348d49920793c2ae084a00d2542d13e Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Tue, 10 Oct 2023 02:52:20 -0700 Subject: [PATCH 7/9] Add parameter to disable slip scaling The slip compliance parameters are currently scaled by the number of contact points, but this can cause issues on heightmaps when transitioning between triangle patches. This adds a parameter to disable the scaling behavior, while retaining the current behavior as the default. Signed-off-by: Steve Peters --- gazebo/physics/ode/ODECollision.cc | 9 +++++++++ gazebo/physics/ode/ODECollision.hh | 7 +++++++ gazebo/physics/ode/ODEPhysics.cc | 14 ++++++-------- 3 files changed, 22 insertions(+), 8 deletions(-) diff --git a/gazebo/physics/ode/ODECollision.cc b/gazebo/physics/ode/ODECollision.cc index 02398dfb7d..e0ceafbcf6 100644 --- a/gazebo/physics/ode/ODECollision.cc +++ b/gazebo/physics/ode/ODECollision.cc @@ -105,6 +105,15 @@ bool ODECollision::ParseWheelPlowingParams( } sdf::ElementPtr wheelElem = _sdf->GetElement(kPlowingWheel); + // Check if scaling slip by number of contact points should be disabled. + const std::string kDisableScaling = + "disable_scaling_slip_by_number_of_contact_points"; + if (wheelElem->HasElement(kDisableScaling)) + { + _plowing.disableScalingSlipByNumberOfContactPoints = + wheelElem->Get(kDisableScaling); + } + // Check for required elements: max_degrees and saturation velocity const std::string kPlowingMaxDegrees = "max_degrees"; if (!wheelElem->HasElement(kPlowingMaxDegrees)) diff --git a/gazebo/physics/ode/ODECollision.hh b/gazebo/physics/ode/ODECollision.hh index 465b665f99..01538175f0 100644 --- a/gazebo/physics/ode/ODECollision.hh +++ b/gazebo/physics/ode/ODECollision.hh @@ -39,6 +39,13 @@ namespace gazebo /// \brief Data structure for wheel plowing parameters. class ODECollisionWheelPlowingParams { + /// \brief Flag to disable scaling of slip parameters by the number + /// of contact points. This behavior was added in bitbucket PR 2965, + /// but it can interact poorly with wheels on heightmap collisions. + /// Keep current behavior (scaling on) as default. + /// https://osrf-migration.github.io/gazebo-gh-pages/#!/osrf/gazebo/pull-requests/2965 + public: bool disableScalingSlipByNumberOfContactPoints = false; + /// \brief Maximum angle by which wheel contact points are rotated. public: ignition::math::Angle maxAngle; diff --git a/gazebo/physics/ode/ODEPhysics.cc b/gazebo/physics/ode/ODEPhysics.cc index 9cbbe0ec0d..4f6c7bc125 100644 --- a/gazebo/physics/ode/ODEPhysics.cc +++ b/gazebo/physics/ode/ODEPhysics.cc @@ -1301,17 +1301,15 @@ void ODEPhysics::Collide(ODECollision *_collision1, ODECollision *_collision2, // number of contact points (numc). // To eliminate this dependence on numc, the inverse damping // is multipled by numc. - contact.surface.slip1 *= numc; - contact.surface.slip2 *= numc; - contact.surface.slip3 *= numc; + if (!wheelPlowing.disableScalingSlipByNumberOfContactPoints) + { + contact.surface.slip1 *= numc; + contact.surface.slip2 *= numc; + contact.surface.slip3 *= numc; + } if (meanSlopeDegrees.Count() > 0) { - // multiplying by numc has issues with plowing so undo that operation - contact.surface.slip1 /= numc; - contact.surface.slip2 /= numc; - contact.surface.slip3 /= numc; - // Increase slip compliance at a specified rate above and below thresholds // modify slip2 value to affect longitudinal slip if (meanSlopeDegrees.Value() > wheelPlowing.nonlinearSlipUpperDegree) From 2509e1bb9221297a76e3b1adc49ed532f79464f5 Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Wed, 6 Dec 2023 22:33:06 -0800 Subject: [PATCH 8/9] Add piecewise linear wheel slip parameters This extends the nonlinear wheel slip model with piecewise linear parameters for the slip compliance multipliers. Signed-off-by: Steve Peters --- gazebo/physics/ode/ODECollision.cc | 132 ++++++++++++++++- gazebo/physics/ode/ODECollision.hh | 110 ++++++++++++-- gazebo/physics/ode/ODEPhysics.cc | 33 ++++- .../model.sdf | 36 ++++- ...plowing_nonlinear_slip_tricycle_demo.world | 134 +++++++++++++++++- 5 files changed, 406 insertions(+), 39 deletions(-) diff --git a/gazebo/physics/ode/ODECollision.cc b/gazebo/physics/ode/ODECollision.cc index e0ceafbcf6..4a27e99008 100644 --- a/gazebo/physics/ode/ODECollision.cc +++ b/gazebo/physics/ode/ODECollision.cc @@ -194,24 +194,144 @@ bool ODECollision::ParseWheelPlowingParams( return false; } + // Parse nonlinear slip parameters const std::string kNonlinearSlip = "nonlinear_slip"; if (wheelElem->HasElement(kNonlinearSlip)) { + auto parse_degrees_perDegrees = + [](const std::string &_elementName, sdf::ElementPtr _nonlinearSlipElem, + std::vector &_parsedDegrees, + std::vector &_parsedPerDegrees) -> void + { + if (_nonlinearSlipElem->HasElement(_elementName)) + { + sdf::ElementPtr elem = _nonlinearSlipElem->GetElement(_elementName); + while (elem) + { + _parsedDegrees.push_back(elem->Get("degree")); + _parsedPerDegrees.push_back(elem->Get("per_degree")); + elem = elem->GetNextElement(_elementName); + } + } + }; + sdf::ElementPtr nonlinearSlipElem = wheelElem->GetElement(kNonlinearSlip); if (nonlinearSlipElem->HasElement("lower")) { - sdf::ElementPtr lowerElem = nonlinearSlipElem->GetElement("lower"); - _plowing.nonlinearSlipLowerDegree = lowerElem->Get("degree"); - _plowing.nonlinearSlipLowerPerDegree = lowerElem->Get("per_degree"); + // In XML, all //lower/degree and //upper/degree values should be in + // ascending order, but the //lower/degree values should be in + // descending order in the c++ data structures. So parse them first + // and then iterate over the values in reverse order. + std::vector parsedDegrees; + std::vector parsedPerDegrees; + parse_degrees_perDegrees( + "lower", nonlinearSlipElem, parsedDegrees, parsedPerDegrees); + // + std::optional lastDegree, lastPerDegree; + double multiplier = 1.0; + for (std::size_t i = 0; i < parsedDegrees.size(); ++i) + { + // reversed index + std::size_t r = parsedDegrees.size() - 1 - i; + double degree = parsedDegrees[r]; + double perDegree = parsedPerDegrees[r]; + + if (!lastDegree) + { + // This is the first pair of parameters. + // Replace the first vector values. + _plowing.nonlinearSlipLowerDegreesMultipliers[0] = {degree, 1.0}; + _plowing.nonlinearSlipLowerPerDegrees[0] = perDegree; + } + else + { + // Verify that slope values are in order + if (degree >= *lastDegree) + { + if (verbose) + { + gzerr << "Element <" << kPlowingWheel << "> in collision with name [" + << _scopedNameForErrorMessages << "] has a //nonlinear_slip/lower " + << "parameter with unsorted values: " + << degree << " should be less than " << *lastDegree + << std::endl; + } + return false; + } + double degreesBelowThreshold = *lastDegree - degree; + multiplier += *lastPerDegree * degreesBelowThreshold; + _plowing.nonlinearSlipLowerDegreesMultipliers.push_back( + {degree, multiplier}); + _plowing.nonlinearSlipLowerPerDegrees.push_back(perDegree); + } + lastDegree = degree; + lastPerDegree = perDegree; + } } if (nonlinearSlipElem->HasElement("upper")) { - sdf::ElementPtr upperElem = nonlinearSlipElem->GetElement("upper"); - _plowing.nonlinearSlipUpperDegree = upperElem->Get("degree"); - _plowing.nonlinearSlipUpperPerDegree = upperElem->Get("per_degree"); + std::vector parsedDegrees; + std::vector parsedPerDegrees; + parse_degrees_perDegrees( + "upper", nonlinearSlipElem, parsedDegrees, parsedPerDegrees); + + std::optional lastDegree, lastPerDegree; + double multiplier = 1.0; + for (std::size_t i = 0; i < parsedDegrees.size(); ++i) + { + double degree = parsedDegrees[i]; + double perDegree = parsedPerDegrees[i]; + + if (!lastDegree) + { + // This is the first pair of parameters. + // Replace the first vector values. + _plowing.nonlinearSlipUpperDegreesMultipliers[0] = {degree, 1.0}; + _plowing.nonlinearSlipUpperPerDegrees[0] = perDegree; + } + else + { + // Verify that slope values are in order + if (degree <= *lastDegree) + { + if (verbose) + { + gzerr << "Element <" << kPlowingWheel << "> in collision with name [" + << _scopedNameForErrorMessages << "] has a //nonlinear_slip/upper " + << "parameter with unsorted values: " + << degree << " should be greater than " << *lastDegree + << std::endl; + } + return false; + } + double degreesAboveThreshold = degree - *lastDegree; + multiplier += *lastPerDegree * degreesAboveThreshold; + _plowing.nonlinearSlipUpperDegreesMultipliers.push_back( + {degree, multiplier}); + _plowing.nonlinearSlipUpperPerDegrees.push_back(perDegree); + } + lastDegree = degree; + lastPerDegree = perDegree; + } } } + if (verbose) + { + gzdbg << "Plowing params for " << _scopedNameForErrorMessages << ":\n" + << " Lower:\n" + << " " << _plowing.nonlinearSlipLowerDegreesMultipliers.back().X() << " deg" + << ", " << _plowing.nonlinearSlipLowerDegreesMultipliers.back().Y() << '\n' + << " " << _plowing.nonlinearSlipLowerDegreesMultipliers.front().X() << " deg" + << ", " << _plowing.nonlinearSlipLowerDegreesMultipliers.front().Y() << '\n' + << " Upper:\n" + << " " << _plowing.nonlinearSlipUpperDegreesMultipliers.front().X() << " deg" + << ", " << _plowing.nonlinearSlipUpperDegreesMultipliers.front().Y() << '\n' + << " " << _plowing.nonlinearSlipUpperDegreesMultipliers.back().X() << " deg" + << ", " << _plowing.nonlinearSlipUpperDegreesMultipliers.back().Y() << '\n' + << std::endl; + } + _plowing.maxAngle = plowingMaxAngle; _plowing.saturationVelocity = plowingSaturationVelocity; _plowing.deadbandVelocity = plowingDeadbandVelocity; diff --git a/gazebo/physics/ode/ODECollision.hh b/gazebo/physics/ode/ODECollision.hh index 01538175f0..c50a096158 100644 --- a/gazebo/physics/ode/ODECollision.hh +++ b/gazebo/physics/ode/ODECollision.hh @@ -57,21 +57,101 @@ namespace gazebo /// below which no plowing effect occurs. public: double deadbandVelocity = 0.0; - /// \brief The slope value in degrees below which the nonlinear slip - /// effect is applied. - public: double nonlinearSlipLowerDegree = -361.0; - - /// \brief The slope value in degrees above which the nonlinear slip - /// effect is applied. - public: double nonlinearSlipUpperDegree = 361.0; - - /// \brief The rate of change in slip compliance per degree of slope - /// below the "lower degree" value. - public: double nonlinearSlipLowerPerDegree = 0.0; - - /// \brief The rate of change in slip compliance per degree of slope - /// above the "upper degree" value. - public: double nonlinearSlipUpperPerDegree = 0.0; + /// Nonlinear slip model: + /// The nonlinear slip model applies a multiplier to the slip compliance + /// depending on the terrain slope at the contact point. The slip + /// compliance multipliers are defined as a piecewise linear function of + /// the terrain slope at the contact point in degrees. Two sets of + /// piecewise linear segments are defined as the lower and upper + /// nonlinear parameters. The slip compliance multiplier is 1.0 for + /// terrain slope values in between the largest lower slope value and + /// the smallest upper slope value. + /// + /// A graphical representation of upper piecewise linear segments is + /// provided below. The horizontal axis corresponds to terrain slope in + /// degrees and the vertical axis corresponds to the slip compliance + /// multiplier. This example shows three piecewise linear segments + /// connecting from 0 to 1, 1 to 2, and from 2 onwards. The piecewise + /// linear endpoint coordinates and rates of change are encoded in + /// vectors of the same length for which the values at each index are + /// related: + /// + /// * per_degree[i]: the rate of change of the slip multiplier per + /// degree of change in terrain slope. In this example the final + /// slope is flat (per_degree[2] == 0), but that is not required. + /// * degrees[i]: the slope in degrees above which the slip compliance + /// multiplier can vary according to the rate of change in + /// per_degree[i]. + /// * multipliers[i]: the starting value of the slip compliance + /// multiplier at degrees[i]. Note that multipliers[0] == 1.0 always. + /// + /// The degrees[i] and per_degree[i] values are specified inside + /// XML elements, and the corresponding multiplier[i] values + /// are computed automatically. The XML elements should be + /// defined in order, such that each //upper/degree value is larger than + /// the previous value. + /** \verbatim + | . + slip | per_degree[2] == 0 . + compliance | 2 o o o o o o o o . + multiplier | o (degrees[2], multipliers[2]) . + | o . + | o | . + | o---┘ per_degree[1] . + | o . + | 1-------- . + | o (degrees[1], multipliers[1]) . + | o . + | o| . + | o-┘ per_degree[0] . + | o . + ----------+--0 . + 1.0 | (degrees[0], multipliers[0] == 1.0) . + | . + -------------+-------------------------- slopeDegrees + | + + + + + {degrees[0]} + {per_degree[0]} + + + {degrees[1]} + {per_degree[1]} + + + {degrees[2]} + {per_degree[2]} + + + + \endverbatim */ + /// The lower nonlinear parameters are defined in a similar manner inside + /// XML elements. Again, the elements should be defined in + /// order, such that each //lower/degree value is larger than the + /// previous value. + + /// \brief The pairs of slope in degrees and slip multipliers that + /// define the piecewise linear behavior for the lower slope values. + public: std::vector + nonlinearSlipLowerDegreesMultipliers{{-361.0, 1.0}}; + + /// \brief The pairs of slope in degrees and slip multipliers that + /// define the piecewise linear behavior for the upper slope values. + public: std::vector + nonlinearSlipUpperDegreesMultipliers{{361.0, 1.0}}; + + /// \brief The rates of change in slip compliance multiplier per degree + /// of slope in slope below the lower degrees[i] value for each + /// piecewise linear segment. + public: std::vector nonlinearSlipLowerPerDegrees{0.0}; + + /// \brief The rates of change in slip compliance multiplier per degree + /// of slope in slope above the upper degrees[i] value for each + /// piecewise linear segment. + public: std::vector nonlinearSlipUpperPerDegrees{0.0}; }; /// \brief Base class for all ODE collisions. diff --git a/gazebo/physics/ode/ODEPhysics.cc b/gazebo/physics/ode/ODEPhysics.cc index 4f6c7bc125..2f3bcc0dc4 100644 --- a/gazebo/physics/ode/ODEPhysics.cc +++ b/gazebo/physics/ode/ODEPhysics.cc @@ -1310,19 +1310,40 @@ void ODEPhysics::Collide(ODECollision *_collision1, ODECollision *_collision2, if (meanSlopeDegrees.Count() > 0) { + const double slopeDegrees = meanSlopeDegrees.Value(); // Increase slip compliance at a specified rate above and below thresholds // modify slip2 value to affect longitudinal slip - if (meanSlopeDegrees.Value() > wheelPlowing.nonlinearSlipUpperDegree) + const auto &upperDegreesMultipliers = + wheelPlowing.nonlinearSlipUpperDegreesMultipliers; + const auto &lowerDegreesMultipliers = + wheelPlowing.nonlinearSlipLowerDegreesMultipliers; + if (slopeDegrees > upperDegreesMultipliers[0].X()) { + std::size_t i = 0; + while (i + 1 < upperDegreesMultipliers.size() && + slopeDegrees > upperDegreesMultipliers[i + 1].X()) + { + ++i; + } const double degreesAboveThreshold = - meanSlopeDegrees.Value() - wheelPlowing.nonlinearSlipUpperDegree; - contact.surface.slip2 *= (1 + wheelPlowing.nonlinearSlipUpperPerDegree * degreesAboveThreshold); + slopeDegrees - upperDegreesMultipliers[i].X(); + const double multiplier = upperDegreesMultipliers[i].Y() + + wheelPlowing.nonlinearSlipUpperPerDegrees[i] * degreesAboveThreshold; + contact.surface.slip2 *= multiplier; } - else if (meanSlopeDegrees.Value() < wheelPlowing.nonlinearSlipLowerDegree) + else if (slopeDegrees < lowerDegreesMultipliers[0].X()) { + std::size_t i = 0; + while (i + 1 < lowerDegreesMultipliers.size() && + slopeDegrees < lowerDegreesMultipliers[i + 1].X()) + { + ++i; + } const double degreesBelowThreshold = - wheelPlowing.nonlinearSlipLowerDegree - meanSlopeDegrees.Value(); - contact.surface.slip2 *= (1 + wheelPlowing.nonlinearSlipLowerPerDegree * degreesBelowThreshold); + lowerDegreesMultipliers[i].X() - slopeDegrees; + const double multiplier = lowerDegreesMultipliers[i].Y() + + wheelPlowing.nonlinearSlipLowerPerDegrees[i] * degreesBelowThreshold; + contact.surface.slip2 *= multiplier; } } diff --git a/test/models/plowing_nonlinear_slip_trisphere_cycle/model.sdf b/test/models/plowing_nonlinear_slip_trisphere_cycle/model.sdf index 8bb29a457a..ffcc0967ff 100644 --- a/test/models/plowing_nonlinear_slip_trisphere_cycle/model.sdf +++ b/test/models/plowing_nonlinear_slip_trisphere_cycle/model.sdf @@ -241,13 +241,21 @@ 0.3 0.4 + + -10 + 0.0 + -6 - 0.01 + 0.25 6 - 0.01 + 0.25 + + + 10 + 0.0 @@ -328,13 +336,21 @@ 0.3 0.4 + + -10 + 0.0 + -6 - 0.01 + 0.25 6 - 0.01 + 0.25 + + + 10 + 0.0 @@ -401,13 +417,21 @@ 0.3 0.4 + + -10 + 0.0 + -6 - 0.01 + 0.25 6 - 0.01 + 0.25 + + + 10 + 0.0 diff --git a/test/worlds/plowing_nonlinear_slip_tricycle_demo.world b/test/worlds/plowing_nonlinear_slip_tricycle_demo.world index 2cbd675353..5518966b14 100644 --- a/test/worlds/plowing_nonlinear_slip_tricycle_demo.world +++ b/test/worlds/plowing_nonlinear_slip_tricycle_demo.world @@ -33,6 +33,67 @@ model://sun + + model://plowing_effect_trisphere_cycle + plowing_tricycle_no_slip + 0 -2 0 0 0 0 + + + + 0 + 0 + 32 + + + 0 + 0 + 32 + + + 0 + 0 + 32 + + + + + + wheel_front_steer + 0 + 9 0 0.1 + + + wheel_rear_left_spin + wheel_rear_right_spin + wheel_front_spin + 6.0 + 9 0 0 + + + wheel_rear_left_spin + wheel_rear_right_spin + 2.15 + + + + + + + model://plowing_effect_trisphere_cycle plowing_tricycle @@ -40,18 +101,18 @@ - 3.0 - 3.0 + 1.5 + 1.5 32 - 3.0 - 3.0 + 1.5 + 1.5 32 - 3.0 - 3.0 + 1.5 + 1.5 32 @@ -99,6 +160,67 @@ nonlinear_slip_tricycle 0 2 0 0 0 0 + + + 1.5 + 1.5 + 32 + + + 1.5 + 1.5 + 32 + + + 1.5 + 1.5 + 32 + + + + + + wheel_front_steer + 0 + 9 0 0.1 + + + wheel_rear_left_spin + wheel_rear_right_spin + wheel_front_spin + 6.0 + 9 0 0 + + + wheel_rear_left_spin + wheel_rear_right_spin + 2.15 + + + + + + + + + model://plowing_effect_trisphere_cycle + plowing_tricycle_higher_slip_compliance + 0 4 0 0 0 0 + 3.0 From 89eaf90dcc00f476edb5c8eada30c12a24eff827 Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Thu, 7 Dec 2023 01:14:55 -0800 Subject: [PATCH 9/9] Add lateral nonlinear slip parameters as well * Refactor the data definitions and parsing method to reduce duplication. * Refine some unit vector calculations in ODEPhysics as well. Signed-off-by: Steve Peters --- gazebo/physics/ode/ODECollision.cc | 228 ++++++++++-------- gazebo/physics/ode/ODECollision.hh | 47 ++-- gazebo/physics/ode/ODEPhysics.cc | 87 ++++++- .../model.sdf | 54 +++++ 4 files changed, 281 insertions(+), 135 deletions(-) diff --git a/gazebo/physics/ode/ODECollision.cc b/gazebo/physics/ode/ODECollision.cc index 4a27e99008..5f22bfabcf 100644 --- a/gazebo/physics/ode/ODECollision.cc +++ b/gazebo/physics/ode/ODECollision.cc @@ -195,140 +195,158 @@ bool ODECollision::ParseWheelPlowingParams( } // Parse nonlinear slip parameters - const std::string kNonlinearSlip = "nonlinear_slip"; - if (wheelElem->HasElement(kNonlinearSlip)) + auto parseNonlinearSlipParams = [&]( + const std::string &_nonlinearSlipElementName, + ODECollisionWheelPlowingParams::NonlinearSlipParams &_nonlinearParams) + -> bool { - auto parse_degrees_perDegrees = - [](const std::string &_elementName, sdf::ElementPtr _nonlinearSlipElem, - std::vector &_parsedDegrees, - std::vector &_parsedPerDegrees) -> void - { - if (_nonlinearSlipElem->HasElement(_elementName)) + if (wheelElem->HasElement(_nonlinearSlipElementName)) + { + auto parse_degrees_perDegrees = + [](const std::string &_elementName, sdf::ElementPtr _nonlinearSlipElem, + std::vector &_parsedDegrees, + std::vector &_parsedPerDegrees) -> void { - sdf::ElementPtr elem = _nonlinearSlipElem->GetElement(_elementName); - while (elem) + if (_nonlinearSlipElem->HasElement(_elementName)) { - _parsedDegrees.push_back(elem->Get("degree")); - _parsedPerDegrees.push_back(elem->Get("per_degree")); - elem = elem->GetNextElement(_elementName); + sdf::ElementPtr elem = _nonlinearSlipElem->GetElement(_elementName); + while (elem) + { + _parsedDegrees.push_back(elem->Get("degree")); + _parsedPerDegrees.push_back(elem->Get("per_degree")); + elem = elem->GetNextElement(_elementName); + } } - } - }; + }; - sdf::ElementPtr nonlinearSlipElem = wheelElem->GetElement(kNonlinearSlip); - if (nonlinearSlipElem->HasElement("lower")) - { - // In XML, all //lower/degree and //upper/degree values should be in - // ascending order, but the //lower/degree values should be in - // descending order in the c++ data structures. So parse them first - // and then iterate over the values in reverse order. - std::vector parsedDegrees; - std::vector parsedPerDegrees; - parse_degrees_perDegrees( - "lower", nonlinearSlipElem, parsedDegrees, parsedPerDegrees); - // - std::optional lastDegree, lastPerDegree; - double multiplier = 1.0; - for (std::size_t i = 0; i < parsedDegrees.size(); ++i) + sdf::ElementPtr nonlinearSlipElem = + wheelElem->GetElement(_nonlinearSlipElementName); + if (nonlinearSlipElem->HasElement("lower")) { - // reversed index - std::size_t r = parsedDegrees.size() - 1 - i; - double degree = parsedDegrees[r]; - double perDegree = parsedPerDegrees[r]; - - if (!lastDegree) - { - // This is the first pair of parameters. - // Replace the first vector values. - _plowing.nonlinearSlipLowerDegreesMultipliers[0] = {degree, 1.0}; - _plowing.nonlinearSlipLowerPerDegrees[0] = perDegree; - } - else + // In XML, all //lower/degree and //upper/degree values should be in + // ascending order, but the //lower/degree values should be in + // descending order in the c++ data structures. So parse them first + // and then iterate over the values in reverse order. + std::vector parsedDegrees; + std::vector parsedPerDegrees; + parse_degrees_perDegrees( + "lower", nonlinearSlipElem, parsedDegrees, parsedPerDegrees); + // + std::optional lastDegree, lastPerDegree; + double multiplier = 1.0; + for (std::size_t i = 0; i < parsedDegrees.size(); ++i) { - // Verify that slope values are in order - if (degree >= *lastDegree) + // reversed index + std::size_t r = parsedDegrees.size() - 1 - i; + double degree = parsedDegrees[r]; + double perDegree = parsedPerDegrees[r]; + + if (!lastDegree) + { + // This is the first pair of parameters. + // Replace the first vector values. + _nonlinearParams.lowerDegreesMultipliers[0] = {degree, 1.0}; + _nonlinearParams.lowerPerDegrees[0] = perDegree; + } + else { - if (verbose) + // Verify that slope values are in order + if (degree >= *lastDegree) { - gzerr << "Element <" << kPlowingWheel << "> in collision with name [" - << _scopedNameForErrorMessages << "] has a //nonlinear_slip/lower " - << "parameter with unsorted values: " - << degree << " should be less than " << *lastDegree - << std::endl; + if (verbose) + { + gzerr << "Element <" << kPlowingWheel << "> in collision with name [" + << _scopedNameForErrorMessages << "] has a //nonlinear_slip/lower " + << "parameter with unsorted values: " + << degree << " should be less than " << *lastDegree + << std::endl; + } + return false; } - return false; + double degreesBelowThreshold = *lastDegree - degree; + multiplier += *lastPerDegree * degreesBelowThreshold; + _nonlinearParams.lowerDegreesMultipliers.push_back( + {degree, multiplier}); + _nonlinearParams.lowerPerDegrees.push_back(perDegree); } - double degreesBelowThreshold = *lastDegree - degree; - multiplier += *lastPerDegree * degreesBelowThreshold; - _plowing.nonlinearSlipLowerDegreesMultipliers.push_back( - {degree, multiplier}); - _plowing.nonlinearSlipLowerPerDegrees.push_back(perDegree); + lastDegree = degree; + lastPerDegree = perDegree; } - lastDegree = degree; - lastPerDegree = perDegree; } - } - if (nonlinearSlipElem->HasElement("upper")) - { - std::vector parsedDegrees; - std::vector parsedPerDegrees; - parse_degrees_perDegrees( - "upper", nonlinearSlipElem, parsedDegrees, parsedPerDegrees); - - std::optional lastDegree, lastPerDegree; - double multiplier = 1.0; - for (std::size_t i = 0; i < parsedDegrees.size(); ++i) + if (nonlinearSlipElem->HasElement("upper")) { - double degree = parsedDegrees[i]; - double perDegree = parsedPerDegrees[i]; - - if (!lastDegree) - { - // This is the first pair of parameters. - // Replace the first vector values. - _plowing.nonlinearSlipUpperDegreesMultipliers[0] = {degree, 1.0}; - _plowing.nonlinearSlipUpperPerDegrees[0] = perDegree; - } - else + std::vector parsedDegrees; + std::vector parsedPerDegrees; + parse_degrees_perDegrees( + "upper", nonlinearSlipElem, parsedDegrees, parsedPerDegrees); + + std::optional lastDegree, lastPerDegree; + double multiplier = 1.0; + for (std::size_t i = 0; i < parsedDegrees.size(); ++i) { - // Verify that slope values are in order - if (degree <= *lastDegree) + double degree = parsedDegrees[i]; + double perDegree = parsedPerDegrees[i]; + + if (!lastDegree) + { + // This is the first pair of parameters. + // Replace the first vector values. + _nonlinearParams.upperDegreesMultipliers[0] = {degree, 1.0}; + _nonlinearParams.upperPerDegrees[0] = perDegree; + } + else { - if (verbose) + // Verify that slope values are in order + if (degree <= *lastDegree) { - gzerr << "Element <" << kPlowingWheel << "> in collision with name [" - << _scopedNameForErrorMessages << "] has a //nonlinear_slip/upper " - << "parameter with unsorted values: " - << degree << " should be greater than " << *lastDegree - << std::endl; + if (verbose) + { + gzerr << "Element <" << kPlowingWheel << "> in collision with name [" + << _scopedNameForErrorMessages << "] has a //nonlinear_slip/upper " + << "parameter with unsorted values: " + << degree << " should be greater than " << *lastDegree + << std::endl; + } + return false; } - return false; + double degreesAboveThreshold = degree - *lastDegree; + multiplier += *lastPerDegree * degreesAboveThreshold; + _nonlinearParams.upperDegreesMultipliers.push_back( + {degree, multiplier}); + _nonlinearParams.upperPerDegrees.push_back(perDegree); } - double degreesAboveThreshold = degree - *lastDegree; - multiplier += *lastPerDegree * degreesAboveThreshold; - _plowing.nonlinearSlipUpperDegreesMultipliers.push_back( - {degree, multiplier}); - _plowing.nonlinearSlipUpperPerDegrees.push_back(perDegree); + lastDegree = degree; + lastPerDegree = perDegree; } - lastDegree = degree; - lastPerDegree = perDegree; } } + return true; + }; + bool longitudinalParseResult, lateralParseResult; + longitudinalParseResult = parseNonlinearSlipParams( + "nonlinear_slip", _plowing.longitudinalNonlinearSlipParams); + lateralParseResult = parseNonlinearSlipParams( + "nonlinear_lateral_slip", _plowing.lateralNonlinearSlipParams); + + if (!longitudinalParseResult || !lateralParseResult) + { + return false; } if (verbose) { + const auto &nonlinearParams = _plowing.longitudinalNonlinearSlipParams; gzdbg << "Plowing params for " << _scopedNameForErrorMessages << ":\n" << " Lower:\n" - << " " << _plowing.nonlinearSlipLowerDegreesMultipliers.back().X() << " deg" - << ", " << _plowing.nonlinearSlipLowerDegreesMultipliers.back().Y() << '\n' - << " " << _plowing.nonlinearSlipLowerDegreesMultipliers.front().X() << " deg" - << ", " << _plowing.nonlinearSlipLowerDegreesMultipliers.front().Y() << '\n' + << " " << nonlinearParams.lowerDegreesMultipliers.back().X() << " deg" + << ", " << nonlinearParams.lowerDegreesMultipliers.back().Y() << '\n' + << " " << nonlinearParams.lowerDegreesMultipliers.front().X() << " deg" + << ", " << nonlinearParams.lowerDegreesMultipliers.front().Y() << '\n' << " Upper:\n" - << " " << _plowing.nonlinearSlipUpperDegreesMultipliers.front().X() << " deg" - << ", " << _plowing.nonlinearSlipUpperDegreesMultipliers.front().Y() << '\n' - << " " << _plowing.nonlinearSlipUpperDegreesMultipliers.back().X() << " deg" - << ", " << _plowing.nonlinearSlipUpperDegreesMultipliers.back().Y() << '\n' + << " " << nonlinearParams.upperDegreesMultipliers.front().X() << " deg" + << ", " << nonlinearParams.upperDegreesMultipliers.front().Y() << '\n' + << " " << nonlinearParams.upperDegreesMultipliers.back().X() << " deg" + << ", " << nonlinearParams.upperDegreesMultipliers.back().Y() << '\n' << std::endl; } diff --git a/gazebo/physics/ode/ODECollision.hh b/gazebo/physics/ode/ODECollision.hh index c50a096158..506fba5f6f 100644 --- a/gazebo/physics/ode/ODECollision.hh +++ b/gazebo/physics/ode/ODECollision.hh @@ -133,25 +133,34 @@ namespace gazebo /// order, such that each //lower/degree value is larger than the /// previous value. - /// \brief The pairs of slope in degrees and slip multipliers that - /// define the piecewise linear behavior for the lower slope values. - public: std::vector - nonlinearSlipLowerDegreesMultipliers{{-361.0, 1.0}}; - - /// \brief The pairs of slope in degrees and slip multipliers that - /// define the piecewise linear behavior for the upper slope values. - public: std::vector - nonlinearSlipUpperDegreesMultipliers{{361.0, 1.0}}; - - /// \brief The rates of change in slip compliance multiplier per degree - /// of slope in slope below the lower degrees[i] value for each - /// piecewise linear segment. - public: std::vector nonlinearSlipLowerPerDegrees{0.0}; - - /// \brief The rates of change in slip compliance multiplier per degree - /// of slope in slope above the upper degrees[i] value for each - /// piecewise linear segment. - public: std::vector nonlinearSlipUpperPerDegrees{0.0}; + class NonlinearSlipParams + { + /// \brief The pairs of slope in degrees and slip multipliers that + /// define the piecewise linear behavior for the lower slope values. + public: std::vector + lowerDegreesMultipliers{{-361.0, 1.0}}; + + /// \brief The pairs of slope in degrees and slip multipliers that + /// define the piecewise linear behavior for the upper slope values. + public: std::vector + upperDegreesMultipliers{{361.0, 1.0}}; + + /// \brief The rates of change in slip compliance multiplier per degree + /// of slope in slope below the lower degrees[i] value for each + /// piecewise linear segment. + public: std::vector lowerPerDegrees{0.0}; + + /// \brief The rates of change in slip compliance multiplier per degree + /// of slope in slope above the upper degrees[i] value for each + /// piecewise linear segment. + public: std::vector upperPerDegrees{0.0}; + }; + + /// \brief Nonlinear parameters for longitudinal wheel slip. + public: NonlinearSlipParams longitudinalNonlinearSlipParams; + + /// \brief Nonlinear parameters for lateral wheel slip. + public: NonlinearSlipParams lateralNonlinearSlipParams; }; /// \brief Base class for all ODE collisions. diff --git a/gazebo/physics/ode/ODEPhysics.cc b/gazebo/physics/ode/ODEPhysics.cc index 2f3bcc0dc4..e023a3e8af 100644 --- a/gazebo/physics/ode/ODEPhysics.cc +++ b/gazebo/physics/ode/ODEPhysics.cc @@ -1160,8 +1160,10 @@ void ODEPhysics::Collide(ODECollision *_collision1, ODECollision *_collision2, /// << " from surface with smaller mu1\n"; } - // Longitudinal slope angle in degrees averaged over each contact point. - ignition::math::SignalMean meanSlopeDegrees; + // Slope angle in degrees averaged over each contact point computed in both + // the longitudinal/contact normal and lateral/contact normal planes. + ignition::math::SignalMean meanLongitudinalSlopeDegrees; + ignition::math::SignalMean meanLateralSlopeDegrees; ODECollisionWheelPlowingParams wheelPlowing; if (fd != ignition::math::Vector3d::Zero) @@ -1230,19 +1232,33 @@ void ODEPhysics::Collide(ODECollision *_collision1, ODECollision *_collision2, contactNormalCopy.Set( contactNormal[0], contactNormal[1], contactNormal[2]); + // Compute lateral unit vector as normalized component of fdir1 + // orthogonal to contact normal + ignition::math::Vector3d unitLateral = + fdir1 - fdir1.Dot(contactNormalCopy) * contactNormalCopy; + unitLateral.Normalize(); + // Compute longitudinal unit vector as normal cross fdir1 ignition::math::Vector3d unitLongitudinal = - contactNormalCopy.Cross(fdir1); + contactNormalCopy.Cross(unitLateral); // Compute normal and longitudinal forces (before plowing) double normalForce = -worldForce.Dot(contactNormalCopy); double longitudinalForce = worldForce.Dot(unitLongitudinal); + double lateralForce = worldForce.Dot(unitLateral); // Estimate slope angle from world force in longitudinal/normal plane - ignition::math::Angle slopeAngle(atan2(longitudinalForce, normalForce)); + // and in lateral/normal plane + ignition::math::Angle longitudinalSlopeAngle( + atan2(longitudinalForce, normalForce)); + ignition::math::Angle lateralSlopeAngle( + atan2(lateralForce, normalForce)); // Store average slope degrees - meanSlopeDegrees.InsertData(slopeAngle.Degree()); + meanLongitudinalSlopeDegrees.InsertData( + longitudinalSlopeAngle.Degree()); + meanLateralSlopeDegrees.InsertData( + lateralSlopeAngle.Degree()); // Compute longitudinal speed (dot product) double wheelSpeedLongitudinal = @@ -1308,15 +1324,16 @@ void ODEPhysics::Collide(ODECollision *_collision1, ODECollision *_collision2, contact.surface.slip3 *= numc; } - if (meanSlopeDegrees.Count() > 0) + // nonlinear longitudinal wheel slip effects + if (meanLongitudinalSlopeDegrees.Count() > 0) { - const double slopeDegrees = meanSlopeDegrees.Value(); + const double slopeDegrees = meanLongitudinalSlopeDegrees.Value(); // Increase slip compliance at a specified rate above and below thresholds // modify slip2 value to affect longitudinal slip const auto &upperDegreesMultipliers = - wheelPlowing.nonlinearSlipUpperDegreesMultipliers; + wheelPlowing.longitudinalNonlinearSlipParams.upperDegreesMultipliers; const auto &lowerDegreesMultipliers = - wheelPlowing.nonlinearSlipLowerDegreesMultipliers; + wheelPlowing.longitudinalNonlinearSlipParams.lowerDegreesMultipliers; if (slopeDegrees > upperDegreesMultipliers[0].X()) { std::size_t i = 0; @@ -1325,10 +1342,12 @@ void ODEPhysics::Collide(ODECollision *_collision1, ODECollision *_collision2, { ++i; } + const auto &upperPerDegrees = + wheelPlowing.longitudinalNonlinearSlipParams.upperPerDegrees; const double degreesAboveThreshold = slopeDegrees - upperDegreesMultipliers[i].X(); const double multiplier = upperDegreesMultipliers[i].Y() + - wheelPlowing.nonlinearSlipUpperPerDegrees[i] * degreesAboveThreshold; + upperPerDegrees[i] * degreesAboveThreshold; contact.surface.slip2 *= multiplier; } else if (slopeDegrees < lowerDegreesMultipliers[0].X()) @@ -1339,14 +1358,60 @@ void ODEPhysics::Collide(ODECollision *_collision1, ODECollision *_collision2, { ++i; } + const auto &lowerPerDegrees = + wheelPlowing.longitudinalNonlinearSlipParams.lowerPerDegrees; const double degreesBelowThreshold = lowerDegreesMultipliers[i].X() - slopeDegrees; const double multiplier = lowerDegreesMultipliers[i].Y() + - wheelPlowing.nonlinearSlipLowerPerDegrees[i] * degreesBelowThreshold; + lowerPerDegrees[i] * degreesBelowThreshold; contact.surface.slip2 *= multiplier; } } + // nonlinear lateral wheel slip effects + if (meanLateralSlopeDegrees.Count() > 0) + { + const double slopeDegrees = meanLateralSlopeDegrees.Value(); + // Increase slip compliance at a specified rate above and below thresholds + // modify slip1 value to affect lateral slip + const auto &upperDegreesMultipliers = + wheelPlowing.lateralNonlinearSlipParams.upperDegreesMultipliers; + const auto &lowerDegreesMultipliers = + wheelPlowing.lateralNonlinearSlipParams.lowerDegreesMultipliers; + if (slopeDegrees > upperDegreesMultipliers[0].X()) + { + std::size_t i = 0; + while (i + 1 < upperDegreesMultipliers.size() && + slopeDegrees > upperDegreesMultipliers[i + 1].X()) + { + ++i; + } + const auto &upperPerDegrees = + wheelPlowing.lateralNonlinearSlipParams.upperPerDegrees; + const double degreesAboveThreshold = + slopeDegrees - upperDegreesMultipliers[i].X(); + const double multiplier = upperDegreesMultipliers[i].Y() + + upperPerDegrees[i] * degreesAboveThreshold; + contact.surface.slip1 *= multiplier; + } + else if (slopeDegrees < lowerDegreesMultipliers[0].X()) + { + std::size_t i = 0; + while (i + 1 < lowerDegreesMultipliers.size() && + slopeDegrees < lowerDegreesMultipliers[i + 1].X()) + { + ++i; + } + const auto &lowerPerDegrees = + wheelPlowing.lateralNonlinearSlipParams.lowerPerDegrees; + const double degreesBelowThreshold = + lowerDegreesMultipliers[i].X() - slopeDegrees; + const double multiplier = lowerDegreesMultipliers[i].Y() + + lowerPerDegrees[i] * degreesBelowThreshold; + contact.surface.slip1 *= multiplier; + } + } + // Combine torsional friction patch radius values contact.surface.patch_radius = std::max(surf1->FrictionPyramid()->PatchRadius(), diff --git a/test/models/plowing_nonlinear_slip_trisphere_cycle/model.sdf b/test/models/plowing_nonlinear_slip_trisphere_cycle/model.sdf index ffcc0967ff..fcab725e82 100644 --- a/test/models/plowing_nonlinear_slip_trisphere_cycle/model.sdf +++ b/test/models/plowing_nonlinear_slip_trisphere_cycle/model.sdf @@ -258,6 +258,24 @@ 0.0 + + + -10 + 0.0 + + + -6 + 0.25 + + + 6 + 0.25 + + + 10 + 0.0 + + @@ -353,6 +371,24 @@ 0.0 + + + -10 + 0.0 + + + -6 + 0.25 + + + 6 + 0.25 + + + 10 + 0.0 + + @@ -434,6 +470,24 @@ 0.0 + + + -10 + 0.0 + + + -6 + 0.25 + + + 6 + 0.25 + + + 10 + 0.0 + +