Skip to content

Commit

Permalink
Adding piecewise linear limits.
Browse files Browse the repository at this point in the history 
Summary:
Add the ability to specify a range for a linear limit, so we can build piecewise linear functions out of them.

With the new parser, we can parse these functions in a single line

Reviewed By: jeongseok-meta

Differential Revision: D66404724
  • Loading branch information
@facebook-github-bot
Chris Twigg authored and facebook-github-bot committed Nov 26, 2024
1 parent f78f0f6 commit f77a690
Show file tree
Hide file tree
Showing 6 changed files with 360 additions and 51 deletions.
10 changes: 10 additions & 0 deletions momentum/character/parameter_limits.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -93,4 +93,14 @@ bool LimitData::operator==(const LimitData& limitData) const {
return (std::memcmp(this, &limitData, sizeof(LimitData)) == 0);
}

bool isInRange(const LimitLinear& limit, float value) {
// Default initialized limits have all zeros in their members due to the std::fill_n above
// but we want in this case to have the limit apply across the entire range of values:
if (limit.rangeMin == 0 && limit.rangeMax == 0) {
return true;
}

return value >= limit.rangeMin && value < limit.rangeMax;
}

} // namespace momentum
9 changes: 9 additions & 0 deletions momentum/character/parameter_limits.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -41,6 +41,13 @@ struct LimitLinear { // set joints to be similar by a linear relation i.e. p_0 =
size_t targetIndex; // index of target parameter (p_1)
float scale; // linear scale of parameter (x)
float offset; // offset (positive and negative) of acceptable parameter zone

// Range where limit is applied (in target parameter values p1). This can be used to construct
// piecewise linear limits. Note that the minimum value of the range is inclusive but the maximum
// value is noninclusive, this is to allow constructing overlapping limits without
// double-counting.
float rangeMin;
float rangeMax;
};

struct LimitEllipsoid {
Expand Down Expand Up @@ -89,5 +96,7 @@ ParameterLimits getPoseConstraintParameterLimits(
const ParameterTransform& pt,
float weight = 1.0f);

bool isInRange(const LimitLinear& limit, float value);

MOMENTUM_DEFINE_POINTERS(ParameterLimits)
} // namespace momentum
53 changes: 28 additions & 25 deletions momentum/character_solver/limit_error_function.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -73,7 +73,7 @@ double LimitErrorFunctionT<T>::getError(
case MinMaxJoint: {
const auto& data = limit.data.minMaxJoint;
const size_t parameterIndex = data.jointIndex * kParametersPerJoint + data.jointParameter;
MT_CHECK(parameterIndex <= (size_t)state.jointParameters.size());
MT_CHECK(parameterIndex < (size_t)state.jointParameters.size());
if (this->activeJointParams_[parameterIndex]) {
if (state.jointParameters(parameterIndex) < data.limits[0]) {
const T val = data.limits[0] - state.jointParameters[parameterIndex];
Expand All @@ -91,11 +91,12 @@ double LimitErrorFunctionT<T>::getError(
}
case Linear: {
const auto& data = limit.data.linear;
MT_CHECK(data.referenceIndex <= static_cast<size_t>(params.size()));
MT_CHECK(data.targetIndex <= static_cast<size_t>(params.size()));
MT_CHECK(data.referenceIndex < static_cast<size_t>(params.size()));
MT_CHECK(data.targetIndex < static_cast<size_t>(params.size()));

if (this->enabledParameters_.test(data.targetIndex) ||
this->enabledParameters_.test(data.referenceIndex)) {
if ((this->enabledParameters_.test(data.targetIndex) ||
this->enabledParameters_.test(data.referenceIndex)) &&
isInRange(data, params(data.targetIndex))) {
const T residual =
params(data.targetIndex) * data.scale - data.offset - params(data.referenceIndex);
error += residual * residual * limit.weight;
Expand Down Expand Up @@ -163,7 +164,7 @@ double LimitErrorFunctionT<T>::getGradient(
switch (limit.type) {
case MinMax: {
const auto& data = limit.data.minMax;
MT_CHECK(data.parameterIndex <= static_cast<size_t>(params.size()));
MT_CHECK(data.parameterIndex < static_cast<size_t>(params.size()));
if (this->enabledParameters_.test(data.parameterIndex)) {
if (params(data.parameterIndex) < data.limits[0]) {
const T val = params(data.parameterIndex) - data.limits[0];
Expand All @@ -181,7 +182,7 @@ double LimitErrorFunctionT<T>::getGradient(
case MinMaxJoint: {
const auto& data = limit.data.minMaxJoint;
const size_t parameterIndex = data.jointIndex * kParametersPerJoint + data.jointParameter;
MT_CHECK(parameterIndex <= (size_t)state.jointParameters.size());
MT_CHECK(parameterIndex < (size_t)state.jointParameters.size());
if (this->activeJointParams_[parameterIndex]) {
if (state.jointParameters(parameterIndex) < data.limits[0]) {
const T val = state.jointParameters[parameterIndex] - data.limits[0];
Expand Down Expand Up @@ -215,19 +216,20 @@ double LimitErrorFunctionT<T>::getGradient(
}
case Linear: {
const auto& data = limit.data.linear;
MT_CHECK(data.referenceIndex <= static_cast<size_t>(params.size()));
MT_CHECK(data.targetIndex <= static_cast<size_t>(params.size()));
const T residual =
params(data.targetIndex) * data.scale - data.offset - params(data.referenceIndex);
error += residual * residual * limit.weight * tWeight;

if (this->enabledParameters_.test(data.targetIndex)) {
gradient[data.targetIndex] += T(2) * residual * data.scale * tWeight;
}
if (this->enabledParameters_.test(data.referenceIndex)) {
gradient[data.referenceIndex] -= T(2) * residual * tWeight;
}
MT_CHECK(data.referenceIndex < static_cast<size_t>(params.size()));
MT_CHECK(data.targetIndex < static_cast<size_t>(params.size()));
if (isInRange(data, params(data.targetIndex))) {
const T residual =
params(data.targetIndex) * data.scale - data.offset - params(data.referenceIndex);
error += residual * residual * limit.weight * tWeight;

if (this->enabledParameters_.test(data.targetIndex)) {
gradient[data.targetIndex] += T(2) * residual * data.scale * tWeight;
}
if (this->enabledParameters_.test(data.referenceIndex)) {
gradient[data.referenceIndex] -= T(2) * residual * tWeight;
}
}
break;
}
case Ellipsoid: {
Expand Down Expand Up @@ -352,7 +354,7 @@ double LimitErrorFunctionT<T>::getJacobian(
switch (limit.type) {
case MinMax: {
const auto& data = limit.data.minMax;
MT_CHECK(data.parameterIndex <= static_cast<size_t>(params.size()));
MT_CHECK(data.parameterIndex < static_cast<size_t>(params.size()));
if (this->enabledParameters_.test(data.parameterIndex)) {
if (params(data.parameterIndex) < data.limits[0]) {
const T val = params(data.parameterIndex) - data.limits[0];
Expand All @@ -373,7 +375,7 @@ double LimitErrorFunctionT<T>::getJacobian(
// simple case, our jacobians are currently in joint space, just add them up
const auto& data = limit.data.minMaxJoint;
const size_t jointIndex = data.jointIndex * kParametersPerJoint + data.jointParameter;
MT_CHECK(jointIndex <= (size_t)state.jointParameters.size());
MT_CHECK(jointIndex < (size_t)state.jointParameters.size());
if (this->activeJointParams_[jointIndex]) {
if (state.jointParameters(jointIndex) < data.limits[0]) {
const T val = state.jointParameters[jointIndex] - data.limits[0];
Expand Down Expand Up @@ -407,11 +409,12 @@ double LimitErrorFunctionT<T>::getJacobian(
}
case Linear: {
const auto& data = limit.data.linear;
MT_CHECK(data.referenceIndex <= static_cast<size_t>(params.size()));
MT_CHECK(data.targetIndex <= static_cast<size_t>(params.size()));
MT_CHECK(data.referenceIndex < static_cast<size_t>(params.size()));
MT_CHECK(data.targetIndex < static_cast<size_t>(params.size()));

if (this->enabledParameters_.test(data.targetIndex) ||
this->enabledParameters_.test(data.referenceIndex)) {
if ((this->enabledParameters_.test(data.targetIndex) ||
this->enabledParameters_.test(data.referenceIndex)) &&
isInRange(data, params(data.targetIndex))) {
const T res =
params(data.targetIndex) * data.scale - data.offset - params(data.referenceIndex);
error += res * res * limit.weight * tWeight;
Expand Down
Loading

0 comments on commit f77a690

Please sign in to comment.