From 50de1b3c2c62a42b394167b853cba9813f6b99c7 Mon Sep 17 00:00:00 2001 From: Robin Alazard Date: Tue, 1 Jul 2025 07:39:17 -0700 Subject: [PATCH] Read new Mujoco-specific JointAPI when converting USD. PiperOrigin-RevId: 778037519 Change-Id: Idd81dc5dc14e2dfb40ba7db4404fd09f9ff50a9f --- src/experimental/usd/usd_to_mjspec.cc | 169 ++++++++++++++++++++++++++ 1 file changed, 169 insertions(+) diff --git a/src/experimental/usd/usd_to_mjspec.cc b/src/experimental/usd/usd_to_mjspec.cc index 21ef2cb6..da4e4f2a 100644 --- a/src/experimental/usd/usd_to_mjspec.cc +++ b/src/experimental/usd/usd_to_mjspec.cc @@ -25,6 +25,7 @@ #include #include +#include #include #include #include @@ -717,6 +718,170 @@ void ParseMjcPhysicsGeneralActuatorAPI(mjSpec* spec, } } +void ParseMjcPhysicsJointAPI(mjsJoint* mj_joint, + const pxr::MjcPhysicsJointAPI& joint_api) { + auto springdamper_attr = joint_api.GetMjcSpringdamperAttr(); + if (springdamper_attr.HasAuthoredValue()) { + pxr::VtDoubleArray springdamper; + springdamper_attr.Get(&springdamper); + if (springdamper.size() == 2) { + mj_joint->springdamper[0] = springdamper[0]; + mj_joint->springdamper[1] = springdamper[1]; + } else { + mju_warning( + "springdamper attribute for joint %s has incorrect size %zu, " + "expected 2.", + mj_joint->name->c_str(), springdamper.size()); + } + } + + auto solreflimit_attr = joint_api.GetMjcSolreflimitAttr(); + if (solreflimit_attr.HasAuthoredValue()) { + pxr::VtDoubleArray solreflimit; + solreflimit_attr.Get(&solreflimit); + if (solreflimit.size() == mjNREF) { + for (int i = 0; i < mjNREF; ++i) { + mj_joint->solref_limit[i] = solreflimit[i]; + } + } else { + mju_warning( + "solreflimit attribute for joint %s has incorrect size %zu, " + "expected %d.", + mj_joint->name->c_str(), solreflimit.size(), mjNREF); + } + } + + auto solimplimit_attr = joint_api.GetMjcSolimplimitAttr(); + if (solimplimit_attr.HasAuthoredValue()) { + pxr::VtDoubleArray solimplimit; + solimplimit_attr.Get(&solimplimit); + if (solimplimit.size() == mjNIMP) { + for (int i = 0; i < mjNIMP; ++i) { + mj_joint->solimp_limit[i] = solimplimit[i]; + } + } else { + mju_warning( + "solimplimit attribute for joint %s has incorrect size %zu, " + "expected %d.", + mj_joint->name->c_str(), solimplimit.size(), mjNIMP); + } + } + + auto solreffriction_attr = joint_api.GetMjcSolreffrictionAttr(); + if (solreffriction_attr.HasAuthoredValue()) { + pxr::VtDoubleArray solreffriction; + solreffriction_attr.Get(&solreffriction); + if (solreffriction.size() == mjNREF) { + for (int i = 0; i < mjNREF; ++i) { + mj_joint->solref_friction[i] = solreffriction[i]; + } + } else { + mju_warning( + "solreffriction attribute for joint %s has incorrect size %zu, " + "expected %d.", + mj_joint->name->c_str(), solreffriction.size(), mjNREF); + } + } + + auto solimpfriction_attr = joint_api.GetMjcSolimpfrictionAttr(); + if (solimpfriction_attr.HasAuthoredValue()) { + pxr::VtDoubleArray solimpfriction; + solimpfriction_attr.Get(&solimpfriction); + if (solimpfriction.size() == mjNIMP) { + for (int i = 0; i < mjNIMP; ++i) { + mj_joint->solimp_friction[i] = solimpfriction[i]; + } + } else { + mju_warning( + "solimpfriction attribute for joint %s has incorrect size %zu, " + "expected %d.", + mj_joint->name->c_str(), solimpfriction.size(), mjNIMP); + } + } + + auto stiffness_attr = joint_api.GetMjcStiffnessAttr(); + if (stiffness_attr.HasAuthoredValue()) { + double stiffness; + stiffness_attr.Get(&stiffness); + mj_joint->stiffness = stiffness; + } + + auto actuatorfrcrange_min_attr = joint_api.GetMjcActuatorfrcrangeMinAttr(); + if (actuatorfrcrange_min_attr.HasAuthoredValue()) { + double min_val; + actuatorfrcrange_min_attr.Get(&min_val); + mj_joint->actfrcrange[0] = min_val; + } + auto actuatorfrcrange_max_attr = joint_api.GetMjcActuatorfrcrangeMaxAttr(); + if (actuatorfrcrange_max_attr.HasAuthoredValue()) { + double max_val; + actuatorfrcrange_max_attr.Get(&max_val); + mj_joint->actfrcrange[1] = max_val; + } + + auto actuatorfrclimited_attr = joint_api.GetMjcActuatorfrclimitedAttr(); + if (actuatorfrclimited_attr.HasAuthoredValue()) { + pxr::TfToken limited; + actuatorfrclimited_attr.Get(&limited); + if (limited == MjcPhysicsTokens->true_) { + mj_joint->actfrclimited = mjLIMITED_TRUE; + } else if (limited == MjcPhysicsTokens->false_) { + mj_joint->actfrclimited = mjLIMITED_FALSE; + } else if (limited == MjcPhysicsTokens->auto_) { + mj_joint->actfrclimited = mjLIMITED_AUTO; + } + } + + auto actuatorgravcomp_attr = joint_api.GetMjcActuatorgravcompAttr(); + if (actuatorgravcomp_attr.HasAuthoredValue()) { + bool gravcomp; + actuatorgravcomp_attr.Get(&gravcomp); + mj_joint->actgravcomp = gravcomp; + } + + auto margin_attr = joint_api.GetMjcMarginAttr(); + if (margin_attr.HasAuthoredValue()) { + double margin; + margin_attr.Get(&margin); + mj_joint->margin = margin; + } + + auto ref_attr = joint_api.GetMjcRefAttr(); + if (ref_attr.HasAuthoredValue()) { + double ref; + ref_attr.Get(&ref); + mj_joint->ref = ref; + } + + auto springref_attr = joint_api.GetMjcSpringrefAttr(); + if (springref_attr.HasAuthoredValue()) { + double springref; + springref_attr.Get(&springref); + mj_joint->springref = springref; + } + + auto armature_attr = joint_api.GetMjcArmatureAttr(); + if (armature_attr.HasAuthoredValue()) { + double armature; + armature_attr.Get(&armature); + mj_joint->armature = armature; + } + + auto damping_attr = joint_api.GetMjcDampingAttr(); + if (damping_attr.HasAuthoredValue()) { + double damping; + damping_attr.Get(&damping); + mj_joint->damping = damping; + } + + auto frictionloss_attr = joint_api.GetMjcFrictionlossAttr(); + if (frictionloss_attr.HasAuthoredValue()) { + double frictionloss; + frictionloss_attr.Get(&frictionloss); + mj_joint->frictionloss = frictionloss; + } +} + void ParseUsdPhysicsCollider(mjSpec* spec, const pxr::UsdPhysicsCollisionAPI& collision_api, const pxr::UsdPrim& parent_prim, mjsBody* parent, @@ -959,6 +1124,10 @@ void ParseUsdPhysicsJoint(mjSpec* spec, const pxr::UsdPrim& prim, mjsBody* body, ParseMjcPhysicsGeneralActuatorAPI(spec, pxr::MjcPhysicsActuatorAPI(prim), mjtTrn::mjTRN_JOINT, mj_joint->name); } + + if (prim.HasAPI()) { + ParseMjcPhysicsJointAPI(mj_joint, pxr::MjcPhysicsJointAPI(prim)); + } } void ParseMjcPhysicsSite(mjSpec* spec, const pxr::MjcPhysicsSiteAPI& site_api,