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; }; /// \} } 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. 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; }; /// \} } 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(), 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_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 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 + + + + +