Refactor MjcPhysics EqualityJointAPI polynomial coefficients.
The single array attribute `mjc:polycoef` has been replaced by five individual double attributes: `mjc:coef0`, `mjc:coef1`, `mjc:coef2`, `mjc:coef3`, and `mjc:coef4`. PiperOrigin-RevId: 865947359 Change-Id: If9d14a77b0a06e5dc712eeadd6e9402411570706
This commit is contained in:
Sam Haves
committed by
Copybara-Service
Copybara-Service
parent
7b0b3cf8e3
commit
c63ba211e5
@@ -150,35 +150,124 @@ class MjcPhysicsEqualityJointAPI : public UsdAPISchemaBase {
|
||||
|
||||
public:
|
||||
// --------------------------------------------------------------------- //
|
||||
// POLYCOEF
|
||||
// COEF0
|
||||
// --------------------------------------------------------------------- //
|
||||
/// Coefficients a0 through a4 of the quartic polynomial. If the joint values
|
||||
/// of the constrained joint and reference joint are respectively y and x, and
|
||||
/// their reference positions (corresponding to the joint values in the
|
||||
/// initial model configuration) are y0 and x0, the constraint is: y = y0 + a0
|
||||
/// + a1*(x-x0) + a2*(x-x0)^2 + a3*(x-x0)^3 + a4*(x-x0)^4. Omitting the target
|
||||
/// joint (joint2) is equivalent to setting x = x0, in which case the
|
||||
/// constraint is y = y0 + a0. The default [0, 1, 0, 0, 0] creates a simple
|
||||
/// 1:1 mimic constraint where y tracks x with the same offset from their
|
||||
/// references.
|
||||
/// Constant coefficient a0 of the quartic polynomial. The constraint is:
|
||||
/// y = y0 + a0 + a1*(x-x0) + a2*(x-x0)^2 + a3*(x-x0)^3 + a4*(x-x0)^4.
|
||||
///
|
||||
/// | ||
|
||||
/// | -- | -- |
|
||||
/// | Declaration | `uniform double[] mjc:polycoef = [0, 1, 0, 0, 0]` |
|
||||
/// | C++ Type | VtArray<double> |
|
||||
/// | \ref Usd_Datatypes "Usd Type" | SdfValueTypeNames->DoubleArray |
|
||||
/// | Declaration | `uniform double mjc:coef0 = 0` |
|
||||
/// | C++ Type | double |
|
||||
/// | \ref Usd_Datatypes "Usd Type" | SdfValueTypeNames->Double |
|
||||
/// | \ref SdfVariability "Variability" | SdfVariabilityUniform |
|
||||
MJCPHYSICS_API
|
||||
UsdAttribute GetPolycoefAttr() const;
|
||||
UsdAttribute GetCoef0Attr() const;
|
||||
|
||||
/// See GetPolycoefAttr(), and also
|
||||
/// See GetCoef0Attr(), and also
|
||||
/// \ref Usd_Create_Or_Get_Property for when to use Get vs Create.
|
||||
/// If specified, author \p defaultValue as the attribute's default,
|
||||
/// sparsely (when it makes sense to do so) if \p writeSparsely is \c true -
|
||||
/// the default for \p writeSparsely is \c false.
|
||||
MJCPHYSICS_API
|
||||
UsdAttribute CreatePolycoefAttr(VtValue const& defaultValue = VtValue(),
|
||||
bool writeSparsely = false) const;
|
||||
UsdAttribute CreateCoef0Attr(VtValue const& defaultValue = VtValue(),
|
||||
bool writeSparsely = false) const;
|
||||
|
||||
public:
|
||||
// --------------------------------------------------------------------- //
|
||||
// COEF1
|
||||
// --------------------------------------------------------------------- //
|
||||
/// Linear coefficient a1 of the quartic polynomial.
|
||||
///
|
||||
/// | ||
|
||||
/// | -- | -- |
|
||||
/// | Declaration | `uniform double mjc:coef1 = 1` |
|
||||
/// | C++ Type | double |
|
||||
/// | \ref Usd_Datatypes "Usd Type" | SdfValueTypeNames->Double |
|
||||
/// | \ref SdfVariability "Variability" | SdfVariabilityUniform |
|
||||
MJCPHYSICS_API
|
||||
UsdAttribute GetCoef1Attr() const;
|
||||
|
||||
/// See GetCoef1Attr(), and also
|
||||
/// \ref Usd_Create_Or_Get_Property for when to use Get vs Create.
|
||||
/// If specified, author \p defaultValue as the attribute's default,
|
||||
/// sparsely (when it makes sense to do so) if \p writeSparsely is \c true -
|
||||
/// the default for \p writeSparsely is \c false.
|
||||
MJCPHYSICS_API
|
||||
UsdAttribute CreateCoef1Attr(VtValue const& defaultValue = VtValue(),
|
||||
bool writeSparsely = false) const;
|
||||
|
||||
public:
|
||||
// --------------------------------------------------------------------- //
|
||||
// COEF2
|
||||
// --------------------------------------------------------------------- //
|
||||
/// Quadratic coefficient a2 of the quartic polynomial.
|
||||
///
|
||||
/// | ||
|
||||
/// | -- | -- |
|
||||
/// | Declaration | `uniform double mjc:coef2 = 0` |
|
||||
/// | C++ Type | double |
|
||||
/// | \ref Usd_Datatypes "Usd Type" | SdfValueTypeNames->Double |
|
||||
/// | \ref SdfVariability "Variability" | SdfVariabilityUniform |
|
||||
MJCPHYSICS_API
|
||||
UsdAttribute GetCoef2Attr() const;
|
||||
|
||||
/// See GetCoef2Attr(), and also
|
||||
/// \ref Usd_Create_Or_Get_Property for when to use Get vs Create.
|
||||
/// If specified, author \p defaultValue as the attribute's default,
|
||||
/// sparsely (when it makes sense to do so) if \p writeSparsely is \c true -
|
||||
/// the default for \p writeSparsely is \c false.
|
||||
MJCPHYSICS_API
|
||||
UsdAttribute CreateCoef2Attr(VtValue const& defaultValue = VtValue(),
|
||||
bool writeSparsely = false) const;
|
||||
|
||||
public:
|
||||
// --------------------------------------------------------------------- //
|
||||
// COEF3
|
||||
// --------------------------------------------------------------------- //
|
||||
/// Cubic coefficient a3 of the quartic polynomial.
|
||||
///
|
||||
/// | ||
|
||||
/// | -- | -- |
|
||||
/// | Declaration | `uniform double mjc:coef3 = 0` |
|
||||
/// | C++ Type | double |
|
||||
/// | \ref Usd_Datatypes "Usd Type" | SdfValueTypeNames->Double |
|
||||
/// | \ref SdfVariability "Variability" | SdfVariabilityUniform |
|
||||
MJCPHYSICS_API
|
||||
UsdAttribute GetCoef3Attr() const;
|
||||
|
||||
/// See GetCoef3Attr(), and also
|
||||
/// \ref Usd_Create_Or_Get_Property for when to use Get vs Create.
|
||||
/// If specified, author \p defaultValue as the attribute's default,
|
||||
/// sparsely (when it makes sense to do so) if \p writeSparsely is \c true -
|
||||
/// the default for \p writeSparsely is \c false.
|
||||
MJCPHYSICS_API
|
||||
UsdAttribute CreateCoef3Attr(VtValue const& defaultValue = VtValue(),
|
||||
bool writeSparsely = false) const;
|
||||
|
||||
public:
|
||||
// --------------------------------------------------------------------- //
|
||||
// COEF4
|
||||
// --------------------------------------------------------------------- //
|
||||
/// Quartic coefficient a4 of the quartic polynomial.
|
||||
///
|
||||
/// | ||
|
||||
/// | -- | -- |
|
||||
/// | Declaration | `uniform double mjc:coef4 = 0` |
|
||||
/// | C++ Type | double |
|
||||
/// | \ref Usd_Datatypes "Usd Type" | SdfValueTypeNames->Double |
|
||||
/// | \ref SdfVariability "Variability" | SdfVariabilityUniform |
|
||||
MJCPHYSICS_API
|
||||
UsdAttribute GetCoef4Attr() const;
|
||||
|
||||
/// See GetCoef4Attr(), and also
|
||||
/// \ref Usd_Create_Or_Get_Property for when to use Get vs Create.
|
||||
/// If specified, author \p defaultValue as the attribute's default,
|
||||
/// sparsely (when it makes sense to do so) if \p writeSparsely is \c true -
|
||||
/// the default for \p writeSparsely is \c false.
|
||||
MJCPHYSICS_API
|
||||
UsdAttribute CreateCoef4Attr(VtValue const& defaultValue = VtValue(),
|
||||
bool writeSparsely = false) const;
|
||||
|
||||
public:
|
||||
// ===================================================================== //
|
||||
|
||||
@@ -195,6 +195,26 @@ struct MjcPhysicsTokensType {
|
||||
///
|
||||
/// MjcPhysicsActuator
|
||||
const TfToken mjcBiasType;
|
||||
/// \brief "mjc:coef0"
|
||||
///
|
||||
/// MjcPhysicsEqualityJointAPI
|
||||
const TfToken mjcCoef0;
|
||||
/// \brief "mjc:coef1"
|
||||
///
|
||||
/// MjcPhysicsEqualityJointAPI
|
||||
const TfToken mjcCoef1;
|
||||
/// \brief "mjc:coef2"
|
||||
///
|
||||
/// MjcPhysicsEqualityJointAPI
|
||||
const TfToken mjcCoef2;
|
||||
/// \brief "mjc:coef3"
|
||||
///
|
||||
/// MjcPhysicsEqualityJointAPI
|
||||
const TfToken mjcCoef3;
|
||||
/// \brief "mjc:coef4"
|
||||
///
|
||||
/// MjcPhysicsEqualityJointAPI
|
||||
const TfToken mjcCoef4;
|
||||
/// \brief "mjc:compiler:alignFree"
|
||||
///
|
||||
/// MjcPhysicsSceneAPI
|
||||
@@ -580,10 +600,6 @@ struct MjcPhysicsTokensType {
|
||||
///
|
||||
/// MjcPhysicsTendon
|
||||
const TfToken mjcPathSegments;
|
||||
/// \brief "mjc:polycoef"
|
||||
///
|
||||
/// MjcPhysicsEqualityJointAPI
|
||||
const TfToken mjcPolycoef;
|
||||
/// \brief "mjc:priority"
|
||||
///
|
||||
/// MjcPhysicsCollisionAPI
|
||||
|
||||
@@ -1854,23 +1854,12 @@ void ParseConstraint(mjSpec* spec, const pxr::UsdPrim& prim, mjsBody* body,
|
||||
}
|
||||
// If no target, name2 remains empty, meaning joint1 is fixed to a constant.
|
||||
|
||||
// Parse polycoef attribute for the quartic polynomial coefficients.
|
||||
auto polycoef_attr = eq_joint_api.GetPolycoefAttr();
|
||||
if (polycoef_attr.HasAuthoredValue()) {
|
||||
pxr::VtDoubleArray polycoef;
|
||||
polycoef_attr.Get(&polycoef);
|
||||
size_t num_coefs = std::min(polycoef.size(), static_cast<size_t>(5));
|
||||
for (size_t i = 0; i < num_coefs; ++i) {
|
||||
eq->data[i] = polycoef[i];
|
||||
}
|
||||
} else {
|
||||
// Default polycoef [0, 1, 0, 0, 0] for 1:1 mimic.
|
||||
eq->data[0] = 0;
|
||||
eq->data[1] = 1;
|
||||
eq->data[2] = 0;
|
||||
eq->data[3] = 0;
|
||||
eq->data[4] = 0;
|
||||
}
|
||||
// Parse individual coefficient attributes for the quartic polynomial.
|
||||
eq_joint_api.GetCoef0Attr().Get(&eq->data[0]);
|
||||
eq_joint_api.GetCoef1Attr().Get(&eq->data[1]);
|
||||
eq_joint_api.GetCoef2Attr().Get(&eq->data[2]);
|
||||
eq_joint_api.GetCoef3Attr().Get(&eq->data[3]);
|
||||
eq_joint_api.GetCoef4Attr().Get(&eq->data[4]);
|
||||
|
||||
// Parse solver parameters from MjcEqualityAPI.
|
||||
auto solref_attr = equality_api.GetSolRefAttr();
|
||||
|
||||
@@ -77,14 +77,58 @@ const TfType& MjcPhysicsEqualityJointAPI::_GetTfType() const {
|
||||
return _GetStaticTfType();
|
||||
}
|
||||
|
||||
UsdAttribute MjcPhysicsEqualityJointAPI::GetPolycoefAttr() const {
|
||||
return GetPrim().GetAttribute(MjcPhysicsTokens->mjcPolycoef);
|
||||
UsdAttribute MjcPhysicsEqualityJointAPI::GetCoef0Attr() const {
|
||||
return GetPrim().GetAttribute(MjcPhysicsTokens->mjcCoef0);
|
||||
}
|
||||
|
||||
UsdAttribute MjcPhysicsEqualityJointAPI::CreatePolycoefAttr(
|
||||
UsdAttribute MjcPhysicsEqualityJointAPI::CreateCoef0Attr(
|
||||
VtValue const& defaultValue, bool writeSparsely) const {
|
||||
return UsdSchemaBase::_CreateAttr(
|
||||
MjcPhysicsTokens->mjcPolycoef, SdfValueTypeNames->DoubleArray,
|
||||
MjcPhysicsTokens->mjcCoef0, SdfValueTypeNames->Double,
|
||||
/* custom = */ false, SdfVariabilityUniform, defaultValue, writeSparsely);
|
||||
}
|
||||
|
||||
UsdAttribute MjcPhysicsEqualityJointAPI::GetCoef1Attr() const {
|
||||
return GetPrim().GetAttribute(MjcPhysicsTokens->mjcCoef1);
|
||||
}
|
||||
|
||||
UsdAttribute MjcPhysicsEqualityJointAPI::CreateCoef1Attr(
|
||||
VtValue const& defaultValue, bool writeSparsely) const {
|
||||
return UsdSchemaBase::_CreateAttr(
|
||||
MjcPhysicsTokens->mjcCoef1, SdfValueTypeNames->Double,
|
||||
/* custom = */ false, SdfVariabilityUniform, defaultValue, writeSparsely);
|
||||
}
|
||||
|
||||
UsdAttribute MjcPhysicsEqualityJointAPI::GetCoef2Attr() const {
|
||||
return GetPrim().GetAttribute(MjcPhysicsTokens->mjcCoef2);
|
||||
}
|
||||
|
||||
UsdAttribute MjcPhysicsEqualityJointAPI::CreateCoef2Attr(
|
||||
VtValue const& defaultValue, bool writeSparsely) const {
|
||||
return UsdSchemaBase::_CreateAttr(
|
||||
MjcPhysicsTokens->mjcCoef2, SdfValueTypeNames->Double,
|
||||
/* custom = */ false, SdfVariabilityUniform, defaultValue, writeSparsely);
|
||||
}
|
||||
|
||||
UsdAttribute MjcPhysicsEqualityJointAPI::GetCoef3Attr() const {
|
||||
return GetPrim().GetAttribute(MjcPhysicsTokens->mjcCoef3);
|
||||
}
|
||||
|
||||
UsdAttribute MjcPhysicsEqualityJointAPI::CreateCoef3Attr(
|
||||
VtValue const& defaultValue, bool writeSparsely) const {
|
||||
return UsdSchemaBase::_CreateAttr(
|
||||
MjcPhysicsTokens->mjcCoef3, SdfValueTypeNames->Double,
|
||||
/* custom = */ false, SdfVariabilityUniform, defaultValue, writeSparsely);
|
||||
}
|
||||
|
||||
UsdAttribute MjcPhysicsEqualityJointAPI::GetCoef4Attr() const {
|
||||
return GetPrim().GetAttribute(MjcPhysicsTokens->mjcCoef4);
|
||||
}
|
||||
|
||||
UsdAttribute MjcPhysicsEqualityJointAPI::CreateCoef4Attr(
|
||||
VtValue const& defaultValue, bool writeSparsely) const {
|
||||
return UsdSchemaBase::_CreateAttr(
|
||||
MjcPhysicsTokens->mjcCoef4, SdfValueTypeNames->Double,
|
||||
/* custom = */ false, SdfVariabilityUniform, defaultValue, writeSparsely);
|
||||
}
|
||||
|
||||
@@ -103,7 +147,9 @@ static inline TfTokenVector _ConcatenateAttributeNames(
|
||||
const TfTokenVector& MjcPhysicsEqualityJointAPI::GetSchemaAttributeNames(
|
||||
bool includeInherited) {
|
||||
static TfTokenVector localNames = {
|
||||
MjcPhysicsTokens->mjcPolycoef,
|
||||
MjcPhysicsTokens->mjcCoef0, MjcPhysicsTokens->mjcCoef1,
|
||||
MjcPhysicsTokens->mjcCoef2, MjcPhysicsTokens->mjcCoef3,
|
||||
MjcPhysicsTokens->mjcCoef4,
|
||||
};
|
||||
static TfTokenVector allNames = _ConcatenateAttributeNames(
|
||||
UsdAPISchemaBase::GetSchemaAttributeNames(true), localNames);
|
||||
|
||||
@@ -588,16 +588,26 @@ class "MjcEqualityJointAPI" (
|
||||
or angle. Only scalar joint types (slide and hinge) can be used."""
|
||||
)
|
||||
{
|
||||
uniform double[] mjc:polycoef = [0, 1, 0, 0, 0] (
|
||||
displayName = "Polynomial Coefficients"
|
||||
doc = """Coefficients a0 through a4 of the quartic polynomial. If the joint values
|
||||
of the constrained joint and reference joint are respectively y and x, and their
|
||||
reference positions (corresponding to the joint values in the initial model
|
||||
configuration) are y0 and x0, the constraint is:
|
||||
y = y0 + a0 + a1*(x-x0) + a2*(x-x0)^2 + a3*(x-x0)^3 + a4*(x-x0)^4.
|
||||
Omitting the target joint (joint2) is equivalent to setting x = x0, in which case
|
||||
the constraint is y = y0 + a0. The default [0, 1, 0, 0, 0] creates a simple 1:1
|
||||
mimic constraint where y tracks x with the same offset from their references."""
|
||||
uniform double mjc:coef0 = 0 (
|
||||
displayName = "Coefficient 0"
|
||||
doc = """Constant coefficient a0 of the quartic polynomial. The constraint is:
|
||||
y = y0 + a0 + a1*(x-x0) + a2*(x-x0)^2 + a3*(x-x0)^3 + a4*(x-x0)^4."""
|
||||
)
|
||||
uniform double mjc:coef1 = 1 (
|
||||
displayName = "Coefficient 1"
|
||||
doc = "Linear coefficient a1 of the quartic polynomial."
|
||||
)
|
||||
uniform double mjc:coef2 = 0 (
|
||||
displayName = "Coefficient 2"
|
||||
doc = "Quadratic coefficient a2 of the quartic polynomial."
|
||||
)
|
||||
uniform double mjc:coef3 = 0 (
|
||||
displayName = "Coefficient 3"
|
||||
doc = "Cubic coefficient a3 of the quartic polynomial."
|
||||
)
|
||||
uniform double mjc:coef4 = 0 (
|
||||
displayName = "Coefficient 4"
|
||||
doc = "Quartic coefficient a4 of the quartic polynomial."
|
||||
)
|
||||
}
|
||||
|
||||
|
||||
@@ -1121,19 +1121,45 @@ class "MjcEqualityJointAPI" (
|
||||
inherits = </APISchemaBase>
|
||||
)
|
||||
{
|
||||
uniform double[] mjc:polycoef = [0, 1, 0, 0, 0] (
|
||||
uniform double mjc:coef0 = 0 (
|
||||
customData = {
|
||||
string apiName = "Polycoef"
|
||||
string apiName = "Coef0"
|
||||
}
|
||||
displayName = "Polynomial Coefficients"
|
||||
doc = """Coefficients a0 through a4 of the quartic polynomial. If the joint values
|
||||
of the constrained joint and reference joint are respectively y and x, and their
|
||||
reference positions (corresponding to the joint values in the initial model
|
||||
configuration) are y0 and x0, the constraint is:
|
||||
y = y0 + a0 + a1*(x-x0) + a2*(x-x0)^2 + a3*(x-x0)^3 + a4*(x-x0)^4.
|
||||
Omitting the target joint (joint2) is equivalent to setting x = x0, in which case
|
||||
the constraint is y = y0 + a0. The default [0, 1, 0, 0, 0] creates a simple 1:1
|
||||
mimic constraint where y tracks x with the same offset from their references."""
|
||||
displayName = "Coefficient 0"
|
||||
doc = """Constant coefficient a0 of the quartic polynomial. The constraint is:
|
||||
y = y0 + a0 + a1*(x-x0) + a2*(x-x0)^2 + a3*(x-x0)^3 + a4*(x-x0)^4."""
|
||||
)
|
||||
|
||||
uniform double mjc:coef1 = 1 (
|
||||
customData = {
|
||||
string apiName = "Coef1"
|
||||
}
|
||||
displayName = "Coefficient 1"
|
||||
doc = """Linear coefficient a1 of the quartic polynomial."""
|
||||
)
|
||||
|
||||
uniform double mjc:coef2 = 0 (
|
||||
customData = {
|
||||
string apiName = "Coef2"
|
||||
}
|
||||
displayName = "Coefficient 2"
|
||||
doc = """Quadratic coefficient a2 of the quartic polynomial."""
|
||||
)
|
||||
|
||||
uniform double mjc:coef3 = 0 (
|
||||
customData = {
|
||||
string apiName = "Coef3"
|
||||
}
|
||||
displayName = "Coefficient 3"
|
||||
doc = """Cubic coefficient a3 of the quartic polynomial."""
|
||||
)
|
||||
|
||||
uniform double mjc:coef4 = 0 (
|
||||
customData = {
|
||||
string apiName = "Coef4"
|
||||
}
|
||||
displayName = "Coefficient 4"
|
||||
doc = """Quartic coefficient a4 of the quartic polynomial."""
|
||||
)
|
||||
}
|
||||
|
||||
|
||||
@@ -47,6 +47,11 @@ MjcPhysicsTokensType::MjcPhysicsTokensType()
|
||||
mjcArmature("mjc:armature", TfToken::Immortal),
|
||||
mjcBiasPrm("mjc:biasPrm", TfToken::Immortal),
|
||||
mjcBiasType("mjc:biasType", TfToken::Immortal),
|
||||
mjcCoef0("mjc:coef0", TfToken::Immortal),
|
||||
mjcCoef1("mjc:coef1", TfToken::Immortal),
|
||||
mjcCoef2("mjc:coef2", TfToken::Immortal),
|
||||
mjcCoef3("mjc:coef3", TfToken::Immortal),
|
||||
mjcCoef4("mjc:coef4", TfToken::Immortal),
|
||||
mjcCompilerAlignFree("mjc:compiler:alignFree", TfToken::Immortal),
|
||||
mjcCompilerAngle("mjc:compiler:angle", TfToken::Immortal),
|
||||
mjcCompilerAutoLimits("mjc:compiler:autoLimits", TfToken::Immortal),
|
||||
@@ -150,7 +155,6 @@ MjcPhysicsTokensType::MjcPhysicsTokensType()
|
||||
mjcPathDivisors("mjc:path:divisors", TfToken::Immortal),
|
||||
mjcPathIndices("mjc:path:indices", TfToken::Immortal),
|
||||
mjcPathSegments("mjc:path:segments", TfToken::Immortal),
|
||||
mjcPolycoef("mjc:polycoef", TfToken::Immortal),
|
||||
mjcPriority("mjc:priority", TfToken::Immortal),
|
||||
mjcQpos("mjc:qpos", TfToken::Immortal),
|
||||
mjcQvel("mjc:qvel", TfToken::Immortal),
|
||||
@@ -235,6 +239,11 @@ MjcPhysicsTokensType::MjcPhysicsTokensType()
|
||||
mjcArmature,
|
||||
mjcBiasPrm,
|
||||
mjcBiasType,
|
||||
mjcCoef0,
|
||||
mjcCoef1,
|
||||
mjcCoef2,
|
||||
mjcCoef3,
|
||||
mjcCoef4,
|
||||
mjcCompilerAlignFree,
|
||||
mjcCompilerAngle,
|
||||
mjcCompilerAutoLimits,
|
||||
@@ -331,7 +340,6 @@ MjcPhysicsTokensType::MjcPhysicsTokensType()
|
||||
mjcPathDivisors,
|
||||
mjcPathIndices,
|
||||
mjcPathSegments,
|
||||
mjcPolycoef,
|
||||
mjcPriority,
|
||||
mjcQpos,
|
||||
mjcQvel,
|
||||
|
||||
Reference in New Issue
Block a user