diff --git a/include/mujoco/experimental/usd/mjcPhysics/equalityJointAPI.h b/include/mujoco/experimental/usd/mjcPhysics/equalityJointAPI.h index ebceafd2..adfca61b 100644 --- a/include/mujoco/experimental/usd/mjcPhysics/equalityJointAPI.h +++ b/include/mujoco/experimental/usd/mjcPhysics/equalityJointAPI.h @@ -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 | - /// | \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: // ===================================================================== // diff --git a/include/mujoco/experimental/usd/mjcPhysics/tokens.h b/include/mujoco/experimental/usd/mjcPhysics/tokens.h index 88dc37c2..37dff9c4 100644 --- a/include/mujoco/experimental/usd/mjcPhysics/tokens.h +++ b/include/mujoco/experimental/usd/mjcPhysics/tokens.h @@ -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 diff --git a/plugin/usd_decoder/usd_decoder.cc b/plugin/usd_decoder/usd_decoder.cc index 07071a76..a35971d2 100644 --- a/plugin/usd_decoder/usd_decoder.cc +++ b/plugin/usd_decoder/usd_decoder.cc @@ -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(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(); diff --git a/src/experimental/usd/mjcPhysics/equalityJointAPI.cpp b/src/experimental/usd/mjcPhysics/equalityJointAPI.cpp index b26b0ee5..92607cfd 100644 --- a/src/experimental/usd/mjcPhysics/equalityJointAPI.cpp +++ b/src/experimental/usd/mjcPhysics/equalityJointAPI.cpp @@ -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); diff --git a/src/experimental/usd/mjcPhysics/generatedSchema.usda b/src/experimental/usd/mjcPhysics/generatedSchema.usda index b072bd0a..ff607a84 100644 --- a/src/experimental/usd/mjcPhysics/generatedSchema.usda +++ b/src/experimental/usd/mjcPhysics/generatedSchema.usda @@ -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." ) } diff --git a/src/experimental/usd/mjcPhysics/schema.usda b/src/experimental/usd/mjcPhysics/schema.usda index 63ed804e..5738b5a0 100644 --- a/src/experimental/usd/mjcPhysics/schema.usda +++ b/src/experimental/usd/mjcPhysics/schema.usda @@ -1121,19 +1121,45 @@ class "MjcEqualityJointAPI" ( inherits = ) { - 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.""" ) } diff --git a/src/experimental/usd/mjcPhysics/tokens.cpp b/src/experimental/usd/mjcPhysics/tokens.cpp index 37d0cca5..f87aaa61 100644 --- a/src/experimental/usd/mjcPhysics/tokens.cpp +++ b/src/experimental/usd/mjcPhysics/tokens.cpp @@ -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,