diff --git a/include/mujoco/experimental/usd/mjcPhysics/equalityAPI.h b/include/mujoco/experimental/usd/mjcPhysics/equalityAPI.h new file mode 100644 index 00000000..5f8aa32b --- /dev/null +++ b/include/mujoco/experimental/usd/mjcPhysics/equalityAPI.h @@ -0,0 +1,222 @@ +// Copyright 2025 DeepMind Technologies Limited +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef MJCPHYSICS_GENERATED_EQUALITYAPI_H +#define MJCPHYSICS_GENERATED_EQUALITYAPI_H + +/// \file mjcPhysics/equalityAPI.h + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +PXR_NAMESPACE_OPEN_SCOPE + +class SdfAssetPath; + +// -------------------------------------------------------------------------- // +// MJCEQUALITYAPI // +// -------------------------------------------------------------------------- // + +/// \class MjcPhysicsEqualityAPI +/// +/// Base API for equality constraints. +/// +class MjcPhysicsEqualityAPI : public UsdAPISchemaBase { + public: + /// Compile time constant representing what kind of schema this class is. + /// + /// \sa UsdSchemaKind + static const UsdSchemaKind schemaKind = UsdSchemaKind::SingleApplyAPI; + + /// Construct a MjcPhysicsEqualityAPI on UsdPrim \p prim . + /// Equivalent to MjcPhysicsEqualityAPI::Get(prim.GetStage(), prim.GetPath()) + /// for a \em valid \p prim, but will not immediately throw an error for + /// an invalid \p prim + explicit MjcPhysicsEqualityAPI(const UsdPrim& prim = UsdPrim()) + : UsdAPISchemaBase(prim) {} + + /// Construct a MjcPhysicsEqualityAPI on the prim held by \p schemaObj . + /// Should be preferred over MjcPhysicsEqualityAPI(schemaObj.GetPrim()), + /// as it preserves SchemaBase state. + explicit MjcPhysicsEqualityAPI(const UsdSchemaBase& schemaObj) + : UsdAPISchemaBase(schemaObj) {} + + /// Destructor. + MJCPHYSICS_API + virtual ~MjcPhysicsEqualityAPI(); + + /// Return a vector of names of all pre-declared attributes for this schema + /// class and all its ancestor classes. Does not include attributes that + /// may be authored by custom/extended methods of the schemas involved. + MJCPHYSICS_API + static const TfTokenVector& GetSchemaAttributeNames( + bool includeInherited = true); + + /// Return a MjcPhysicsEqualityAPI holding the prim adhering to this + /// schema at \p path on \p stage. If no prim exists at \p path on + /// \p stage, or if the prim at that path does not adhere to this schema, + /// return an invalid schema object. This is shorthand for the following: + /// + /// \code + /// MjcPhysicsEqualityAPI(stage->GetPrimAtPath(path)); + /// \endcode + /// + MJCPHYSICS_API + static MjcPhysicsEqualityAPI Get(const UsdStagePtr& stage, + const SdfPath& path); + + /// Returns true if this single-apply API schema can be applied to + /// the given \p prim. If this schema can not be a applied to the prim, + /// this returns false and, if provided, populates \p whyNot with the + /// reason it can not be applied. + /// + /// Note that if CanApply returns false, that does not necessarily imply + /// that calling Apply will fail. Callers are expected to call CanApply + /// before calling Apply if they want to ensure that it is valid to + /// apply a schema. + /// + /// \sa UsdPrim::GetAppliedSchemas() + /// \sa UsdPrim::HasAPI() + /// \sa UsdPrim::CanApplyAPI() + /// \sa UsdPrim::ApplyAPI() + /// \sa UsdPrim::RemoveAPI() + /// + MJCPHYSICS_API + static bool CanApply(const UsdPrim& prim, std::string* whyNot = nullptr); + + /// Applies this single-apply API schema to the given \p prim. + /// This information is stored by adding "MjcEqualityAPI" to the + /// token-valued, listOp metadata \em apiSchemas on the prim. + /// + /// \return A valid MjcPhysicsEqualityAPI object is returned upon success. + /// An invalid (or empty) MjcPhysicsEqualityAPI object is returned upon + /// failure. See \ref UsdPrim::ApplyAPI() for conditions + /// resulting in failure. + /// + /// \sa UsdPrim::GetAppliedSchemas() + /// \sa UsdPrim::HasAPI() + /// \sa UsdPrim::CanApplyAPI() + /// \sa UsdPrim::ApplyAPI() + /// \sa UsdPrim::RemoveAPI() + /// + MJCPHYSICS_API + static MjcPhysicsEqualityAPI Apply(const UsdPrim& prim); + + protected: + /// Returns the kind of schema this class belongs to. + /// + /// \sa UsdSchemaKind + MJCPHYSICS_API + UsdSchemaKind _GetSchemaKind() const override; + + private: + // needs to invoke _GetStaticTfType. + friend class UsdSchemaRegistry; + MJCPHYSICS_API + static const TfType& _GetStaticTfType(); + + static bool _IsTypedSchema(); + + // override SchemaBase virtuals. + MJCPHYSICS_API + const TfType& _GetTfType() const override; + + public: + // --------------------------------------------------------------------- // + // SOLREF + // --------------------------------------------------------------------- // + /// Constraint solver parameter for equality constraint simulation. + /// + /// | || + /// | -- | -- | + /// | Declaration | `uniform double[] mjc:solref = [0.02, 1]` | + /// | C++ Type | VtArray | + /// | \ref Usd_Datatypes "Usd Type" | SdfValueTypeNames->DoubleArray | + /// | \ref SdfVariability "Variability" | SdfVariabilityUniform | + MJCPHYSICS_API + UsdAttribute GetSolRefAttr() const; + + /// See GetSolRefAttr(), 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 CreateSolRefAttr(VtValue const& defaultValue = VtValue(), + bool writeSparsely = false) const; + + public: + // --------------------------------------------------------------------- // + // SOLIMP + // --------------------------------------------------------------------- // + /// Constraint solver parameter for equality constraint simulation. + /// + /// | || + /// | -- | -- | + /// | Declaration | `uniform double[] mjc:solimp = [0.9, 0.95, 0.001, 0.5, 2]` + /// | | C++ Type | VtArray | | \ref Usd_Datatypes "Usd Type" | + /// SdfValueTypeNames->DoubleArray | | \ref SdfVariability "Variability" | + /// SdfVariabilityUniform | + MJCPHYSICS_API + UsdAttribute GetSolImpAttr() const; + + /// See GetSolImpAttr(), 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 CreateSolImpAttr(VtValue const& defaultValue = VtValue(), + bool writeSparsely = false) const; + + public: + // --------------------------------------------------------------------- // + // MJCTARGET + // --------------------------------------------------------------------- // + /// Secondary target of the equality constraint. + /// + MJCPHYSICS_API + UsdRelationship GetMjcTargetRel() const; + + /// See GetMjcTargetRel(), and also + /// \ref Usd_Create_Or_Get_Property for when to use Get vs Create + MJCPHYSICS_API + UsdRelationship CreateMjcTargetRel() const; + + public: + // ===================================================================== // + // Feel free to add custom code below this line, it will be preserved by + // the code generator. + // + // Just remember to: + // - Close the class declaration with }; + // - Close the namespace with PXR_NAMESPACE_CLOSE_SCOPE + // - Close the include guard with #endif + // ===================================================================== // + // --(BEGIN CUSTOM CODE)-- +}; + +PXR_NAMESPACE_CLOSE_SCOPE + +#endif diff --git a/include/mujoco/experimental/usd/mjcPhysics/equalityWeldAPI.h b/include/mujoco/experimental/usd/mjcPhysics/equalityWeldAPI.h new file mode 100644 index 00000000..98a7ae7a --- /dev/null +++ b/include/mujoco/experimental/usd/mjcPhysics/equalityWeldAPI.h @@ -0,0 +1,187 @@ +// Copyright 2025 DeepMind Technologies Limited +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef MJCPHYSICS_GENERATED_EQUALITYWELDAPI_H +#define MJCPHYSICS_GENERATED_EQUALITYWELDAPI_H + +/// \file mjcPhysics/equalityWeldAPI.h + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +PXR_NAMESPACE_OPEN_SCOPE + +class SdfAssetPath; + +// -------------------------------------------------------------------------- // +// MJCEQUALITYWELDAPI // +// -------------------------------------------------------------------------- // + +/// \class MjcPhysicsEqualityWeldAPI +/// +/// API providing extension attributes to represent equality/weld constraints. +/// +class MjcPhysicsEqualityWeldAPI : public UsdAPISchemaBase { + public: + /// Compile time constant representing what kind of schema this class is. + /// + /// \sa UsdSchemaKind + static const UsdSchemaKind schemaKind = UsdSchemaKind::SingleApplyAPI; + + /// Construct a MjcPhysicsEqualityWeldAPI on UsdPrim \p prim . + /// Equivalent to MjcPhysicsEqualityWeldAPI::Get(prim.GetStage(), + /// prim.GetPath()) for a \em valid \p prim, but will not immediately throw an + /// error for an invalid \p prim + explicit MjcPhysicsEqualityWeldAPI(const UsdPrim& prim = UsdPrim()) + : UsdAPISchemaBase(prim) {} + + /// Construct a MjcPhysicsEqualityWeldAPI on the prim held by \p schemaObj . + /// Should be preferred over MjcPhysicsEqualityWeldAPI(schemaObj.GetPrim()), + /// as it preserves SchemaBase state. + explicit MjcPhysicsEqualityWeldAPI(const UsdSchemaBase& schemaObj) + : UsdAPISchemaBase(schemaObj) {} + + /// Destructor. + MJCPHYSICS_API + virtual ~MjcPhysicsEqualityWeldAPI(); + + /// Return a vector of names of all pre-declared attributes for this schema + /// class and all its ancestor classes. Does not include attributes that + /// may be authored by custom/extended methods of the schemas involved. + MJCPHYSICS_API + static const TfTokenVector& GetSchemaAttributeNames( + bool includeInherited = true); + + /// Return a MjcPhysicsEqualityWeldAPI holding the prim adhering to this + /// schema at \p path on \p stage. If no prim exists at \p path on + /// \p stage, or if the prim at that path does not adhere to this schema, + /// return an invalid schema object. This is shorthand for the following: + /// + /// \code + /// MjcPhysicsEqualityWeldAPI(stage->GetPrimAtPath(path)); + /// \endcode + /// + MJCPHYSICS_API + static MjcPhysicsEqualityWeldAPI Get(const UsdStagePtr& stage, + const SdfPath& path); + + /// Returns true if this single-apply API schema can be applied to + /// the given \p prim. If this schema can not be a applied to the prim, + /// this returns false and, if provided, populates \p whyNot with the + /// reason it can not be applied. + /// + /// Note that if CanApply returns false, that does not necessarily imply + /// that calling Apply will fail. Callers are expected to call CanApply + /// before calling Apply if they want to ensure that it is valid to + /// apply a schema. + /// + /// \sa UsdPrim::GetAppliedSchemas() + /// \sa UsdPrim::HasAPI() + /// \sa UsdPrim::CanApplyAPI() + /// \sa UsdPrim::ApplyAPI() + /// \sa UsdPrim::RemoveAPI() + /// + MJCPHYSICS_API + static bool CanApply(const UsdPrim& prim, std::string* whyNot = nullptr); + + /// Applies this single-apply API schema to the given \p prim. + /// This information is stored by adding "MjcEqualityWeldAPI" to the + /// token-valued, listOp metadata \em apiSchemas on the prim. + /// + /// \return A valid MjcPhysicsEqualityWeldAPI object is returned upon success. + /// An invalid (or empty) MjcPhysicsEqualityWeldAPI object is returned upon + /// failure. See \ref UsdPrim::ApplyAPI() for conditions + /// resulting in failure. + /// + /// \sa UsdPrim::GetAppliedSchemas() + /// \sa UsdPrim::HasAPI() + /// \sa UsdPrim::CanApplyAPI() + /// \sa UsdPrim::ApplyAPI() + /// \sa UsdPrim::RemoveAPI() + /// + MJCPHYSICS_API + static MjcPhysicsEqualityWeldAPI Apply(const UsdPrim& prim); + + protected: + /// Returns the kind of schema this class belongs to. + /// + /// \sa UsdSchemaKind + MJCPHYSICS_API + UsdSchemaKind _GetSchemaKind() const override; + + private: + // needs to invoke _GetStaticTfType. + friend class UsdSchemaRegistry; + MJCPHYSICS_API + static const TfType& _GetStaticTfType(); + + static bool _IsTypedSchema(); + + // override SchemaBase virtuals. + MJCPHYSICS_API + const TfType& _GetTfType() const override; + + public: + // --------------------------------------------------------------------- // + // TORQUESCALE + // --------------------------------------------------------------------- // + /// A constant that scales the angular residual (angular constraint + /// violation). Notionally in units of torque/force = length. Intuitively this + /// coefficient defines how much the weld “cares” about rotational + /// displacements vs. translational displacements. + /// + /// | || + /// | -- | -- | + /// | Declaration | `uniform float mjc:torqueScale = 1` | + /// | C++ Type | float | + /// | \ref Usd_Datatypes "Usd Type" | SdfValueTypeNames->Float | + /// | \ref SdfVariability "Variability" | SdfVariabilityUniform | + MJCPHYSICS_API + UsdAttribute GetTorqueScaleAttr() const; + + /// See GetTorqueScaleAttr(), 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 CreateTorqueScaleAttr(VtValue const& defaultValue = VtValue(), + bool writeSparsely = false) const; + + public: + // ===================================================================== // + // Feel free to add custom code below this line, it will be preserved by + // the code generator. + // + // Just remember to: + // - Close the class declaration with }; + // - Close the namespace with PXR_NAMESPACE_CLOSE_SCOPE + // - Close the include guard with #endif + // ===================================================================== // + // --(BEGIN CUSTOM CODE)-- +}; + +PXR_NAMESPACE_CLOSE_SCOPE + +#endif diff --git a/include/mujoco/experimental/usd/mjcPhysics/tokens.h b/include/mujoco/experimental/usd/mjcPhysics/tokens.h index f1e2d28e..716f17eb 100644 --- a/include/mujoco/experimental/usd/mjcPhysics/tokens.h +++ b/include/mujoco/experimental/usd/mjcPhysics/tokens.h @@ -634,7 +634,7 @@ struct MjcPhysicsTokensType { const TfToken mjcSliderSite; /// \brief "mjc:solimp" /// - /// MjcPhysicsCollisionAPI + /// MjcPhysicsCollisionAPI, MjcPhysicsEqualityAPI const TfToken mjcSolimp; /// \brief "mjc:solimpfriction" /// @@ -650,7 +650,7 @@ struct MjcPhysicsTokensType { const TfToken mjcSolmix; /// \brief "mjc:solref" /// - /// MjcPhysicsCollisionAPI + /// MjcPhysicsCollisionAPI, MjcPhysicsEqualityAPI const TfToken mjcSolref; /// \brief "mjc:solreffriction" /// @@ -678,8 +678,12 @@ struct MjcPhysicsTokensType { const TfToken mjcStiffness; /// \brief "mjc:target" /// - /// MjcPhysicsActuator + /// MjcPhysicsActuator, MjcPhysicsEqualityAPI const TfToken mjcTarget; + /// \brief "mjc:torqueScale" + /// + /// MjcPhysicsEqualityWeldAPI + const TfToken mjcTorqueScale; /// \brief "mjc:torsionalfriction" /// /// MjcPhysicsMaterialAPI @@ -764,6 +768,14 @@ struct MjcPhysicsTokensType { /// /// Schema identifier and family for MjcPhysicsCollisionAPI const TfToken MjcCollisionAPI; + /// \brief "MjcEqualityAPI" + /// + /// Schema identifier and family for MjcPhysicsEqualityAPI + const TfToken MjcEqualityAPI; + /// \brief "MjcEqualityWeldAPI" + /// + /// Schema identifier and family for MjcPhysicsEqualityWeldAPI + const TfToken MjcEqualityWeldAPI; /// \brief "MjcImageableAPI" /// /// Schema identifier and family for MjcPhysicsImageableAPI diff --git a/plugin/usd_decoder/CMakeLists.txt b/plugin/usd_decoder/CMakeLists.txt index 2c113311..8f6e6007 100644 --- a/plugin/usd_decoder/CMakeLists.txt +++ b/plugin/usd_decoder/CMakeLists.txt @@ -11,8 +11,6 @@ # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. # See the License for the specific language governing permissions and # limitations under the License. -find_package(pxr REQUIRED) - set(CMAKE_INSTALL_RPATH_USE_LINK_PATH TRUE) set(USD_DECODER_SRCS usd_decoder.cc @@ -20,7 +18,6 @@ set(USD_DECODER_SRCS kinematic_tree.h material_parsing.cc material_parsing.h - utils.h ) add_library(usd_decoder_plugin SHARED ${USD_DECODER_SRCS}) diff --git a/plugin/usd_decoder/kinematic_tree.cc b/plugin/usd_decoder/kinematic_tree.cc index c579f412..f4eec7a2 100644 --- a/plugin/usd_decoder/kinematic_tree.cc +++ b/plugin/usd_decoder/kinematic_tree.cc @@ -212,6 +212,16 @@ std::unique_ptr BuildKinematicTree(const pxr::UsdStageRefPtr stage) { return nullptr; } + // If we encounter a joint that does not participate in articulation, we + // should treat it as a constraint instead. + // For example, a weld constraint is represented by a fixed joint. + bool excluded_from_articulation; + joint.GetExcludeFromArticulationAttr().Get(&excluded_from_articulation); + if (excluded_from_articulation) { + extraction.nodes[to_idx]->constraints.push_back(joint.GetPath()); + continue; + } + children[from_idx][to_idx] = true; parent_joints[to_idx].push_back(joint.GetPath()); // Now that we know all the bodies, we can assign joints to respective diff --git a/plugin/usd_decoder/kinematic_tree.h b/plugin/usd_decoder/kinematic_tree.h index b80acc02..47fcc87d 100644 --- a/plugin/usd_decoder/kinematic_tree.h +++ b/plugin/usd_decoder/kinematic_tree.h @@ -28,6 +28,7 @@ struct Node { pxr::SdfPath body_path; pxr::SdfPath physics_scene; std::vector actuators; + std::vector constraints; std::vector joints; std::vector visual_gprims; std::vector colliders; diff --git a/plugin/usd_decoder/usd_decoder.cc b/plugin/usd_decoder/usd_decoder.cc index 59179e99..22f5366b 100644 --- a/plugin/usd_decoder/usd_decoder.cc +++ b/plugin/usd_decoder/usd_decoder.cc @@ -25,6 +25,8 @@ #include #include +#include +#include #include #include #include @@ -39,6 +41,7 @@ #include "material_parsing.h" #include #include +#include #include #include #include @@ -1821,16 +1824,197 @@ void ParseUsdPhysicsCollider(mjSpec* spec, } } +void ParseConstraint(mjSpec* spec, const pxr::UsdPrim& prim, mjsBody* body, + pxr::UsdGeomXformCache& xform_cache) { + if (!prim.IsA()) { + mju_warning("Constraint %s is not a fixed joint, skipping.", + prim.GetPath().GetAsString().c_str()); + return; + } + pxr::UsdPhysicsJoint joint(prim); + // A fixed joint means the bodies are welded. + pxr::UsdRelationship body0_rel = joint.GetBody0Rel(); + pxr::UsdRelationship body1_rel = joint.GetBody1Rel(); + pxr::SdfPathVector targets0, targets1; + body0_rel.GetTargets(&targets0); + body1_rel.GetTargets(&targets1); + + pxr::SdfPath body0_path; + if (!targets0.empty()) body0_path = targets0[0]; + pxr::SdfPath body1_path; + if (!targets1.empty()) body1_path = targets1[0]; + + auto stage = prim.GetStage(); + + auto body0_prim = stage->GetPrimAtPath(body0_path); + auto body1_prim = stage->GetPrimAtPath(body1_path); + + bool body0_is_site = false; + if (!body0_path.IsEmpty()) { + body0_is_site = body0_prim.HasAPI(); + } + bool body1_is_site = false; + if (!body1_path.IsEmpty()) { + body1_is_site = body1_prim.HasAPI(); + } + + if (body0_is_site != body1_is_site) { + mju_warning( + "Weld constraint %s has mismatch between site and body targets, " + "skipping", + prim.GetPath().GetAsString().c_str()); + return; + } + + mjsEquality* eq = mjs_addEquality(spec, nullptr); + eq->type = mjEQ_WELD; + mjs_setName(eq->element, prim.GetPath().GetAsString().c_str()); + SetUsdPrimPathUserValue(eq->element, prim.GetPath()); + + if (body0_is_site) { + mjs_setString(eq->name1, body0_path.GetAsString().c_str()); + mjs_setString(eq->name2, body1_path.GetAsString().c_str()); + eq->objtype = mjOBJ_SITE; + } else { + mjs_setString(eq->name1, body0_path.GetAsString().c_str()); + mjs_setString(eq->name2, body1_path.GetAsString().c_str()); + eq->objtype = mjOBJ_BODY; + } + + // In USD, joints have a reference frame that is shared between the two + // connecting bodies. This reference frame is defined relative to both + // bodies, in localPos/Rot 0 and 1. A fixed joint removes all degrees of + // freedom for the joint, ensuring the reference frame is fixed in place + // This means the bodies should also be fixed, but relative to the joint + // depending on their respective localPos/Rot. + // In MuJoCo fixed joint frames are not explicitly defined relative to their + // connecting bodies. Instead, we define a weld constraint providing the + // weld point (anchor) relative to body 2 and then we specify the position + // of body 2 relative to body 1. + + // Here is the mapping of terms concretely: + // T(bodyX) = transform of bodyX relative to joint frame (localPos/Rot). + // anchor = localPos1 (position of the weld point relative to mjc body 2) + // relpose = T(body0)*T(body1)^-1 + + // relpose is float[7], pos(3) + quat(4) + // anchor is float[3], pos(3) + // in mjsEquality data: anchor 0-2, relpose 3-9. + + auto body0_xform = xform_cache.GetLocalToWorldTransform(body0_prim); + auto body1_xform = xform_cache.GetLocalToWorldTransform(body1_prim); + + pxr::GfVec3d body0_scale, body1_scale; + + { + pxr::GfMatrix4d scale_orient, rot, persp; + pxr::GfVec3d translation; + if (!body0_xform.Factor(&scale_orient, &body0_scale, &rot, + &translation, &persp)) { + // unable to decompose, emit warning and set scale to identity + mju_warning( + "Unable to decompose matrix for body 0: %s.", + body0_path.GetAsString().c_str()); + body0_scale = pxr::GfVec3f(1, 1, 1); + } + + if (!body1_xform.Factor(&scale_orient, &body1_scale, &rot, + &translation, &persp)) { + // unable to decompose, emit warning and set scale to identity + mju_warning( + "Unable to decompose matrix for body 1: %s.", + body1_path.GetAsString().c_str()); + body1_scale = pxr::GfVec3f(1, 1, 1); + } + } + + pxr::GfVec3f localPos1; + joint.GetLocalPos1Attr().Get(&localPos1); + localPos1[0] *= body1_scale[0]; + localPos1[1] *= body1_scale[1]; + localPos1[2] *= body1_scale[2]; + pxr::GfQuatf localRot1; + joint.GetLocalRot1Attr().Get(&localRot1); + + pxr::GfVec3f localPos0; + joint.GetLocalPos0Attr().Get(&localPos0); + localPos0[0] *= body0_scale[0]; + localPos0[1] *= body0_scale[1]; + localPos0[2] *= body0_scale[2]; + pxr::GfQuatf localRot0; + joint.GetLocalRot0Attr().Get(&localRot0); + + auto relpose_quat = localRot0 * localRot1.GetConjugate(); + relpose_quat.Normalize(); + auto relpose_pos = localPos0 - relpose_quat.Transform(localPos1); + + eq->data[0] = localPos1[0]; + eq->data[1] = localPos1[1]; + eq->data[2] = localPos1[2]; + eq->data[3] = relpose_pos[0]; + eq->data[4] = relpose_pos[1]; + eq->data[5] = relpose_pos[2]; + eq->data[6] = relpose_quat.GetReal(); + eq->data[7] = relpose_quat.GetImaginary()[0]; + eq->data[8] = relpose_quat.GetImaginary()[1]; + eq->data[9] = relpose_quat.GetImaginary()[2]; + + if (prim.HasAPI()) { + // MjcPhysicsEqualityAPI is always automatically applied + // by MjcPhysicsEqualityWeldAPI. + pxr::MjcPhysicsEqualityAPI equality_api(prim); + auto solref_attr = equality_api.GetSolRefAttr(); + if (solref_attr.HasAuthoredValue()) { + pxr::VtDoubleArray solref; + solref_attr.Get(&solref); + if (solref.size() == mjNREF) { + for (int i = 0; i < mjNREF; ++i) { + eq->solref[i] = solref[i]; + } + } else { + mju_warning( + "solref attribute for weld equality %s has incorrect size " + "%zu, expected %d.", + prim.GetPath().GetAsString().c_str(), solref.size(), mjNREF); + } + } + + auto solimp_attr = equality_api.GetSolImpAttr(); + if (solimp_attr.HasAuthoredValue()) { + pxr::VtDoubleArray solimp; + solimp_attr.Get(&solimp); + if (solimp.size() == mjNIMP) { + for (int i = 0; i < mjNIMP; ++i) { + eq->solimp[i] = solimp[i]; + } + } else { + mju_warning( + "solimp attribute for weld equality %s has incorrect size " + "%zu, expected %d.", + prim.GetPath().GetAsString().c_str(), solimp.size(), mjNIMP); + } + } + + pxr::MjcPhysicsEqualityWeldAPI weld_api(prim); + auto torque_scale_attr = weld_api.GetTorqueScaleAttr(); + if (torque_scale_attr.HasAuthoredValue()) { + float torque_scale; + torque_scale_attr.Get(&torque_scale); + eq->data[10] = torque_scale; + } + } +} + void ParseUsdPhysicsJoint(mjSpec* spec, const pxr::UsdPrim& prim, mjsBody* body, pxr::UsdGeomXformCache& xform_cache) { - pxr::UsdPhysicsJoint joint(prim); - // A fixed joint means the bodies are welded. if (prim.IsA()) { // No joint needed for welded bodies. return; } + pxr::UsdPhysicsJoint joint(prim); + mjtJoint type; if (prim.IsA()) { type = mjJNT_HINGE; @@ -2093,6 +2277,11 @@ void PopulateSpecFromTree(pxr::UsdStageRefPtr stage, mjSpec* spec, free_joint->type = mjJNT_FREE; } + for (const auto& constraint_path : current_node->constraints) { + ParseConstraint(spec, stage->GetPrimAtPath(constraint_path), + current_mj_body, caches.xform_cache); + } + pxr::UsdPrim body_prim_for_xform = current_node->body_path.IsEmpty() ? stage->GetPseudoRoot() diff --git a/src/experimental/usd/CMakeLists.txt b/src/experimental/usd/CMakeLists.txt index f2d01f0f..46d8e18b 100644 --- a/src/experimental/usd/CMakeLists.txt +++ b/src/experimental/usd/CMakeLists.txt @@ -127,6 +127,8 @@ add_library(${MJC_PHYSICS_PLUGIN_TARGET_NAME} SHARED) target_sources(${MJC_PHYSICS_PLUGIN_TARGET_NAME} PRIVATE mjcPhysics/actuator.cpp mjcPhysics/collisionAPI.cpp + mjcPhysics/equalityAPI.cpp + mjcPhysics/equalityWeldAPI.cpp mjcPhysics/imageableAPI.cpp mjcPhysics/jointAPI.cpp mjcPhysics/keyframe.cpp @@ -162,19 +164,19 @@ target_compile_options( # Configure RPATH for macOS and UNIX. if(APPLE) - set_target_properties($(MJCF_PLUGIN_TARGET_NAME) PROPERTIES + set_target_properties(${MJCF_PLUGIN_TARGET_NAME} PROPERTIES BUILD_RPATH "${USD_INSTALL_ROOT}" INSTALL_RPATH "@loader_path") - set_target_properties($(MJC_PHYSICS_PLUGIN_TARGET_NAME) PROPERTIES + set_target_properties(${MJC_PHYSICS_PLUGIN_TARGET_NAME} PROPERTIES BUILD_RPATH "${USD_INSTALL_ROOT}" INSTALL_RPATH "@loader_path") elseif(UNIX) - set_target_properties($(MJCF_PLUGIN_TARGET_NAME) PROPERTIES + set_target_properties(${MJCF_PLUGIN_TARGET_NAME} PROPERTIES BUILD_RPATH "${USD_INSTALL_ROOT}" INSTALL_RPATH "$ORIGIN") - set_target_properties($(MJC_PHYSICS_PLUGIN_TARGET_NAME) PROPERTIES + set_target_properties(${MJC_PHYSICS_PLUGIN_TARGET_NAME} PROPERTIES BUILD_RPATH "${USD_INSTALL_ROOT}" INSTALL_RPATH "$ORIGIN") endif() diff --git a/src/experimental/usd/mjcPhysics/equalityAPI.cpp b/src/experimental/usd/mjcPhysics/equalityAPI.cpp new file mode 100644 index 00000000..8caad788 --- /dev/null +++ b/src/experimental/usd/mjcPhysics/equalityAPI.cpp @@ -0,0 +1,144 @@ +// Copyright 2025 DeepMind Technologies Limited +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include +#include +#include + +PXR_NAMESPACE_OPEN_SCOPE + +// Register the schema with the TfType system. +TF_REGISTRY_FUNCTION(TfType) { + TfType::Define >(); +} + +/* virtual */ +MjcPhysicsEqualityAPI::~MjcPhysicsEqualityAPI() {} + +/* static */ +MjcPhysicsEqualityAPI MjcPhysicsEqualityAPI::Get(const UsdStagePtr& stage, + const SdfPath& path) { + if (!stage) { + TF_CODING_ERROR("Invalid stage"); + return MjcPhysicsEqualityAPI(); + } + return MjcPhysicsEqualityAPI(stage->GetPrimAtPath(path)); +} + +/* virtual */ +UsdSchemaKind MjcPhysicsEqualityAPI::_GetSchemaKind() const { + return MjcPhysicsEqualityAPI::schemaKind; +} + +/* static */ +bool MjcPhysicsEqualityAPI::CanApply(const UsdPrim& prim, std::string* whyNot) { + return prim.CanApplyAPI(whyNot); +} + +/* static */ +MjcPhysicsEqualityAPI MjcPhysicsEqualityAPI::Apply(const UsdPrim& prim) { + if (prim.ApplyAPI()) { + return MjcPhysicsEqualityAPI(prim); + } + return MjcPhysicsEqualityAPI(); +} + +/* static */ +const TfType& MjcPhysicsEqualityAPI::_GetStaticTfType() { + static TfType tfType = TfType::Find(); + return tfType; +} + +/* static */ +bool MjcPhysicsEqualityAPI::_IsTypedSchema() { + static bool isTyped = _GetStaticTfType().IsA(); + return isTyped; +} + +/* virtual */ +const TfType& MjcPhysicsEqualityAPI::_GetTfType() const { + return _GetStaticTfType(); +} + +UsdAttribute MjcPhysicsEqualityAPI::GetSolRefAttr() const { + return GetPrim().GetAttribute(MjcPhysicsTokens->mjcSolref); +} + +UsdAttribute MjcPhysicsEqualityAPI::CreateSolRefAttr( + VtValue const& defaultValue, bool writeSparsely) const { + return UsdSchemaBase::_CreateAttr( + MjcPhysicsTokens->mjcSolref, SdfValueTypeNames->DoubleArray, + /* custom = */ false, SdfVariabilityUniform, defaultValue, writeSparsely); +} + +UsdAttribute MjcPhysicsEqualityAPI::GetSolImpAttr() const { + return GetPrim().GetAttribute(MjcPhysicsTokens->mjcSolimp); +} + +UsdAttribute MjcPhysicsEqualityAPI::CreateSolImpAttr( + VtValue const& defaultValue, bool writeSparsely) const { + return UsdSchemaBase::_CreateAttr( + MjcPhysicsTokens->mjcSolimp, SdfValueTypeNames->DoubleArray, + /* custom = */ false, SdfVariabilityUniform, defaultValue, writeSparsely); +} + +UsdRelationship MjcPhysicsEqualityAPI::GetMjcTargetRel() const { + return GetPrim().GetRelationship(MjcPhysicsTokens->mjcTarget); +} + +UsdRelationship MjcPhysicsEqualityAPI::CreateMjcTargetRel() const { + return GetPrim().CreateRelationship(MjcPhysicsTokens->mjcTarget, + /* custom = */ false); +} + +namespace { +static inline TfTokenVector _ConcatenateAttributeNames( + const TfTokenVector& left, const TfTokenVector& right) { + TfTokenVector result; + result.reserve(left.size() + right.size()); + result.insert(result.end(), left.begin(), left.end()); + result.insert(result.end(), right.begin(), right.end()); + return result; +} +} // namespace + +/*static*/ +const TfTokenVector& MjcPhysicsEqualityAPI::GetSchemaAttributeNames( + bool includeInherited) { + static TfTokenVector localNames = { + MjcPhysicsTokens->mjcSolref, + MjcPhysicsTokens->mjcSolimp, + }; + static TfTokenVector allNames = _ConcatenateAttributeNames( + UsdAPISchemaBase::GetSchemaAttributeNames(true), localNames); + + if (includeInherited) + return allNames; + else + return localNames; +} + +PXR_NAMESPACE_CLOSE_SCOPE + +// ===================================================================== // +// Feel free to add custom code below this line. It will be preserved by +// the code generator. +// +// Just remember to wrap code in the appropriate delimiters: +// 'PXR_NAMESPACE_OPEN_SCOPE', 'PXR_NAMESPACE_CLOSE_SCOPE'. +// ===================================================================== // +// --(BEGIN CUSTOM CODE)-- diff --git a/src/experimental/usd/mjcPhysics/equalityWeldAPI.cpp b/src/experimental/usd/mjcPhysics/equalityWeldAPI.cpp new file mode 100644 index 00000000..d8cd5afa --- /dev/null +++ b/src/experimental/usd/mjcPhysics/equalityWeldAPI.cpp @@ -0,0 +1,125 @@ +// Copyright 2025 DeepMind Technologies Limited +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include +#include +#include + +PXR_NAMESPACE_OPEN_SCOPE + +// Register the schema with the TfType system. +TF_REGISTRY_FUNCTION(TfType) { + TfType::Define >(); +} + +/* virtual */ +MjcPhysicsEqualityWeldAPI::~MjcPhysicsEqualityWeldAPI() {} + +/* static */ +MjcPhysicsEqualityWeldAPI MjcPhysicsEqualityWeldAPI::Get( + const UsdStagePtr& stage, const SdfPath& path) { + if (!stage) { + TF_CODING_ERROR("Invalid stage"); + return MjcPhysicsEqualityWeldAPI(); + } + return MjcPhysicsEqualityWeldAPI(stage->GetPrimAtPath(path)); +} + +/* virtual */ +UsdSchemaKind MjcPhysicsEqualityWeldAPI::_GetSchemaKind() const { + return MjcPhysicsEqualityWeldAPI::schemaKind; +} + +/* static */ +bool MjcPhysicsEqualityWeldAPI::CanApply(const UsdPrim& prim, + std::string* whyNot) { + return prim.CanApplyAPI(whyNot); +} + +/* static */ +MjcPhysicsEqualityWeldAPI MjcPhysicsEqualityWeldAPI::Apply( + const UsdPrim& prim) { + if (prim.ApplyAPI()) { + return MjcPhysicsEqualityWeldAPI(prim); + } + return MjcPhysicsEqualityWeldAPI(); +} + +/* static */ +const TfType& MjcPhysicsEqualityWeldAPI::_GetStaticTfType() { + static TfType tfType = TfType::Find(); + return tfType; +} + +/* static */ +bool MjcPhysicsEqualityWeldAPI::_IsTypedSchema() { + static bool isTyped = _GetStaticTfType().IsA(); + return isTyped; +} + +/* virtual */ +const TfType& MjcPhysicsEqualityWeldAPI::_GetTfType() const { + return _GetStaticTfType(); +} + +UsdAttribute MjcPhysicsEqualityWeldAPI::GetTorqueScaleAttr() const { + return GetPrim().GetAttribute(MjcPhysicsTokens->mjcTorqueScale); +} + +UsdAttribute MjcPhysicsEqualityWeldAPI::CreateTorqueScaleAttr( + VtValue const& defaultValue, bool writeSparsely) const { + return UsdSchemaBase::_CreateAttr( + MjcPhysicsTokens->mjcTorqueScale, SdfValueTypeNames->Float, + /* custom = */ false, SdfVariabilityUniform, defaultValue, writeSparsely); +} + +namespace { +static inline TfTokenVector _ConcatenateAttributeNames( + const TfTokenVector& left, const TfTokenVector& right) { + TfTokenVector result; + result.reserve(left.size() + right.size()); + result.insert(result.end(), left.begin(), left.end()); + result.insert(result.end(), right.begin(), right.end()); + return result; +} +} // namespace + +/*static*/ +const TfTokenVector& MjcPhysicsEqualityWeldAPI::GetSchemaAttributeNames( + bool includeInherited) { + static TfTokenVector localNames = { + MjcPhysicsTokens->mjcTorqueScale, + }; + static TfTokenVector allNames = _ConcatenateAttributeNames( + UsdAPISchemaBase::GetSchemaAttributeNames(true), localNames); + + if (includeInherited) + return allNames; + else + return localNames; +} + +PXR_NAMESPACE_CLOSE_SCOPE + +// ===================================================================== // +// Feel free to add custom code below this line. It will be preserved by +// the code generator. +// +// Just remember to wrap code in the appropriate delimiters: +// 'PXR_NAMESPACE_OPEN_SCOPE', 'PXR_NAMESPACE_CLOSE_SCOPE'. +// ===================================================================== // +// --(BEGIN CUSTOM CODE)-- diff --git a/src/experimental/usd/mjcPhysics/generatedSchema.usda b/src/experimental/usd/mjcPhysics/generatedSchema.usda index 41ad2ac7..d33301a7 100644 --- a/src/experimental/usd/mjcPhysics/generatedSchema.usda +++ b/src/experimental/usd/mjcPhysics/generatedSchema.usda @@ -550,6 +550,34 @@ class "MjcMaterialAPI" ( ) } +class "MjcEqualityAPI" ( + doc = "Base API for equality constraints." +) +{ + uniform double[] mjc:solimp = [0.9, 0.95, 0.001, 0.5, 2] ( + displayName = "SolImp" + doc = "Constraint solver parameter for equality constraint simulation." + ) + uniform double[] mjc:solref = [0.02, 1] ( + displayName = "SolRef" + doc = "Constraint solver parameter for equality constraint simulation." + ) + rel mjc:target ( + doc = "Secondary target of the equality constraint." + ) +} + +class "MjcEqualityWeldAPI" ( + apiSchemas = ["MjcEqualityAPI"] + doc = "API providing extension attributes to represent equality/weld constraints." +) +{ + uniform float mjc:torqueScale = 1 ( + displayName = "Torque Scale" + doc = "A constant that scales the angular residual (angular constraint violation). Notionally in units of torque/force = length. Intuitively this coefficient defines how much the weld “cares” about rotational displacements vs. translational displacements." + ) +} + class MjcTendon "MjcTendon" ( doc = "Type describing fixed and spatial tendons." ) diff --git a/src/experimental/usd/mjcPhysics/plugInfo.json b/src/experimental/usd/mjcPhysics/plugInfo.json index e8593d3c..7f1eb33e 100644 --- a/src/experimental/usd/mjcPhysics/plugInfo.json +++ b/src/experimental/usd/mjcPhysics/plugInfo.json @@ -23,6 +23,26 @@ ], "schemaKind": "singleApplyAPI" }, + "MjcPhysicsEqualityAPI": { + "alias": { + "UsdSchemaBase": "MjcEqualityAPI" + }, + "autoGenerated": true, + "bases": [ + "UsdAPISchemaBase" + ], + "schemaKind": "singleApplyAPI" + }, + "MjcPhysicsEqualityWeldAPI": { + "alias": { + "UsdSchemaBase": "MjcEqualityWeldAPI" + }, + "autoGenerated": true, + "bases": [ + "UsdAPISchemaBase" + ], + "schemaKind": "singleApplyAPI" + }, "MjcPhysicsImageableAPI": { "alias": { "UsdSchemaBase": "MjcImageableAPI" diff --git a/src/experimental/usd/mjcPhysics/schema.usda b/src/experimental/usd/mjcPhysics/schema.usda index 1e0904dc..6dfa20f0 100644 --- a/src/experimental/usd/mjcPhysics/schema.usda +++ b/src/experimental/usd/mjcPhysics/schema.usda @@ -1057,6 +1057,55 @@ class "MjcMaterialAPI" ) } +class "MjcEqualityAPI" ( + customData = { + string className = "EqualityAPI" + } + doc = """Base API for equality constraints.""" + + inherits = +) +{ + rel mjc:target ( + doc = "Secondary target of the equality constraint." + ) + + uniform double[] mjc:solref = [0.02, 1.0] ( + customData = { + string apiName = "SolRef" + } + displayName = "SolRef" + doc = """Constraint solver parameter for equality constraint simulation.""" + ) + + uniform double[] mjc:solimp = [0.9, 0.95, 0.001, 0.5, 2.0] ( + customData = { + string apiName = "SolImp" + } + displayName = "SolImp" + doc = """Constraint solver parameter for equality constraint simulation.""" + ) +} + +class "MjcEqualityWeldAPI" ( + customData = { + string className = "EqualityWeldAPI" + } + doc = """API providing extension attributes to represent equality/weld constraints.""" + + prepend apiSchemas = ["MjcEqualityAPI"] + inherits = +) +{ + uniform float mjc:torqueScale = 1.0 ( + customData = { + string apiName = "TorqueScale" + } + displayName = "Torque Scale" + doc = """A constant that scales the angular residual (angular constraint violation). Notionally in units of torque/force = length. Intuitively this coefficient defines how much the weld “cares” about rotational displacements vs. translational displacements.""" + ) +} + class MjcTendon "MjcTendon" ( customData = { diff --git a/src/experimental/usd/mjcPhysics/tokens.cpp b/src/experimental/usd/mjcPhysics/tokens.cpp index afb4afc1..e09af755 100644 --- a/src/experimental/usd/mjcPhysics/tokens.cpp +++ b/src/experimental/usd/mjcPhysics/tokens.cpp @@ -175,6 +175,7 @@ MjcPhysicsTokensType::MjcPhysicsTokensType() mjcSpringref("mjc:springref", TfToken::Immortal), mjcStiffness("mjc:stiffness", TfToken::Immortal), mjcTarget("mjc:target", TfToken::Immortal), + mjcTorqueScale("mjc:torqueScale", TfToken::Immortal), mjcTorsionalfriction("mjc:torsionalfriction", TfToken::Immortal), mjcType("mjc:type", TfToken::Immortal), mjcWidth("mjc:width", TfToken::Immortal), @@ -192,6 +193,8 @@ MjcPhysicsTokensType::MjcPhysicsTokensType() user("user", TfToken::Immortal), MjcActuator("MjcActuator", TfToken::Immortal), MjcCollisionAPI("MjcCollisionAPI", TfToken::Immortal), + MjcEqualityAPI("MjcEqualityAPI", TfToken::Immortal), + MjcEqualityWeldAPI("MjcEqualityWeldAPI", TfToken::Immortal), MjcImageableAPI("MjcImageableAPI", TfToken::Immortal), MjcJointAPI("MjcJointAPI", TfToken::Immortal), MjcKeyframe("MjcKeyframe", TfToken::Immortal), @@ -351,6 +354,7 @@ MjcPhysicsTokensType::MjcPhysicsTokensType() mjcSpringref, mjcStiffness, mjcTarget, + mjcTorqueScale, mjcTorsionalfriction, mjcType, mjcWidth, @@ -368,6 +372,8 @@ MjcPhysicsTokensType::MjcPhysicsTokensType() user, MjcActuator, MjcCollisionAPI, + MjcEqualityAPI, + MjcEqualityWeldAPI, MjcImageableAPI, MjcJointAPI, MjcKeyframe,