Skip to content

Commit

Permalink
Add lateral nonlinear slip parameters as well
Browse files Browse the repository at this point in the history 
* Refactor the data definitions and parsing method
  to reduce duplication.
* Refine some unit vector calculations in ODEPhysics
  as well.

Signed-off-by: Steve Peters <[email protected]>
  • Loading branch information
@scpeters
scpeters committed Dec 9, 2023
1 parent 2509e1b commit 89eaf90
Show file tree
Hide file tree
Showing 4 changed files with 281 additions and 135 deletions.
228 changes: 123 additions & 105 deletions gazebo/physics/ode/ODECollision.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<double> &_parsedDegrees,
std::vector<double> &_parsedPerDegrees) -> void
{
if (_nonlinearSlipElem->HasElement(_elementName))
if (wheelElem->HasElement(_nonlinearSlipElementName))
{
auto parse_degrees_perDegrees =
[](const std::string &_elementName, sdf::ElementPtr _nonlinearSlipElem,
std::vector<double> &_parsedDegrees,
std::vector<double> &_parsedPerDegrees) -> void
{
sdf::ElementPtr elem = _nonlinearSlipElem->GetElement(_elementName);
while (elem)
if (_nonlinearSlipElem->HasElement(_elementName))
{
_parsedDegrees.push_back(elem->Get<double>("degree"));
_parsedPerDegrees.push_back(elem->Get<double>("per_degree"));
elem = elem->GetNextElement(_elementName);
sdf::ElementPtr elem = _nonlinearSlipElem->GetElement(_elementName);
while (elem)
{
_parsedDegrees.push_back(elem->Get<double>("degree"));
_parsedPerDegrees.push_back(elem->Get<double>("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<double> parsedDegrees;
std::vector<double> parsedPerDegrees;
parse_degrees_perDegrees(
"lower", nonlinearSlipElem, parsedDegrees, parsedPerDegrees);
//
std::optional<double> 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<double> parsedDegrees;
std::vector<double> parsedPerDegrees;
parse_degrees_perDegrees(
"lower", nonlinearSlipElem, parsedDegrees, parsedPerDegrees);
//
std::optional<double> 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 <degree> 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 <degree> 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<double> parsedDegrees;
std::vector<double> parsedPerDegrees;
parse_degrees_perDegrees(
"upper", nonlinearSlipElem, parsedDegrees, parsedPerDegrees);

std::optional<double> 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<double> parsedDegrees;
std::vector<double> parsedPerDegrees;
parse_degrees_perDegrees(
"upper", nonlinearSlipElem, parsedDegrees, parsedPerDegrees);

std::optional<double> 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 <degree> 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 <degree> 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;
}

Expand Down
47 changes: 28 additions & 19 deletions gazebo/physics/ode/ODECollision.hh
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<ignition::math::Vector2d>
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<ignition::math::Vector2d>
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<double> 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<double> 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<ignition::math::Vector2d>
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<ignition::math::Vector2d>
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<double> 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<double> 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.
Expand Down
Loading

0 comments on commit 89eaf90

Please sign in to comment.