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..5f22bfabcf 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)) @@ -185,6 +194,162 @@ bool ODECollision::ParseWheelPlowingParams( return false; } + // Parse nonlinear slip parameters + auto parseNonlinearSlipParams = [&]( + const std::string &_nonlinearSlipElementName, + ODECollisionWheelPlowingParams::NonlinearSlipParams &_nonlinearParams) + -> bool + { + if (wheelElem->HasElement(_nonlinearSlipElementName)) + { + 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(_nonlinearSlipElementName); + 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) + { + // 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 + { + // 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; + _nonlinearParams.lowerDegreesMultipliers.push_back( + {degree, multiplier}); + _nonlinearParams.lowerPerDegrees.push_back(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) + { + 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 + { + // 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; + _nonlinearParams.upperDegreesMultipliers.push_back( + {degree, multiplier}); + _nonlinearParams.upperPerDegrees.push_back(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" + << " " << nonlinearParams.lowerDegreesMultipliers.back().X() << " deg" + << ", " << nonlinearParams.lowerDegreesMultipliers.back().Y() << '\n' + << " " << nonlinearParams.lowerDegreesMultipliers.front().X() << " deg" + << ", " << nonlinearParams.lowerDegreesMultipliers.front().Y() << '\n' + << " Upper:\n" + << " " << nonlinearParams.upperDegreesMultipliers.front().X() << " deg" + << ", " << nonlinearParams.upperDegreesMultipliers.front().Y() << '\n' + << " " << nonlinearParams.upperDegreesMultipliers.back().X() << " deg" + << ", " << nonlinearParams.upperDegreesMultipliers.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 9ef941f393..506fba5f6f 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; @@ -49,6 +56,111 @@ namespace gazebo /// \brief Plowing deadband velocity: the linear wheel velocity [m/s] /// below which no plowing effect occurs. public: double deadbandVelocity = 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. + + 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/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..e023a3e8af 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,12 @@ void ODEPhysics::Collide(ODECollision *_collision1, ODECollision *_collision2, /// << " from surface with smaller mu1\n"; } + // 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) { contact.surface.mode |= dContactFDir1; @@ -1170,7 +1178,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 +1197,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 @@ -1213,9 +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 + // and in lateral/normal plane + ignition::math::Angle longitudinalSlopeAngle( + atan2(longitudinalForce, normalForce)); + ignition::math::Angle lateralSlopeAngle( + atan2(lateralForce, normalForce)); + + // Store average slope degrees + meanLongitudinalSlopeDegrees.InsertData( + longitudinalSlopeAngle.Degree()); + meanLateralSlopeDegrees.InsertData( + lateralSlopeAngle.Degree()); // Compute longitudinal speed (dot product) double wheelSpeedLongitudinal = @@ -1225,7 +1268,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 @@ -1274,9 +1317,100 @@ 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; + } + + // nonlinear longitudinal wheel slip effects + if (meanLongitudinalSlopeDegrees.Count() > 0) + { + 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.longitudinalNonlinearSlipParams.upperDegreesMultipliers; + const auto &lowerDegreesMultipliers = + wheelPlowing.longitudinalNonlinearSlipParams.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.longitudinalNonlinearSlipParams.upperPerDegrees; + const double degreesAboveThreshold = + slopeDegrees - upperDegreesMultipliers[i].X(); + const double multiplier = upperDegreesMultipliers[i].Y() + + upperPerDegrees[i] * degreesAboveThreshold; + contact.surface.slip2 *= 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.longitudinalNonlinearSlipParams.lowerPerDegrees; + const double degreesBelowThreshold = + lowerDegreesMultipliers[i].X() - slopeDegrees; + const double multiplier = lowerDegreesMultipliers[i].Y() + + 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 = 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..fcab725e82 --- /dev/null +++ b/test/models/plowing_nonlinear_slip_trisphere_cycle/model.sdf @@ -0,0 +1,537 @@ + + + + + -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 + + + -10 + 0.0 + + + -6 + 0.25 + + + 6 + 0.25 + + + 10 + 0.0 + + + + + -10 + 0.0 + + + -6 + 0.25 + + + 6 + 0.25 + + + 10 + 0.0 + + + + + + 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 + + + -10 + 0.0 + + + -6 + 0.25 + + + 6 + 0.25 + + + 10 + 0.0 + + + + + -10 + 0.0 + + + -6 + 0.25 + + + 6 + 0.25 + + + 10 + 0.0 + + + + + + 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 + + + -10 + 0.0 + + + -6 + 0.25 + + + 6 + 0.25 + + + 10 + 0.0 + + + + + -10 + 0.0 + + + -6 + 0.25 + + + 6 + 0.25 + + + 10 + 0.0 + + + + + + 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..5518966b14 --- /dev/null +++ b/test/worlds/plowing_nonlinear_slip_tricycle_demo.world @@ -0,0 +1,288 @@ + + + + + -2 0 -9.75 + + model://plowing_effect_ground_plane + + + 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 + 0 0 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_nonlinear_slip_trisphere_cycle + 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 + 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 + + + + +