Skip to content

Commit 9dc67b5

Browse files
ChuangTseucopybara-github
ChuangTseu
authored and
copybara-github
committed
Write new Mujoco-specific JointAPI data to USD.
PiperOrigin-RevId: 778035661 Change-Id: I945aa38fc58973023d78ab0ab5fad7783d23e1ef
1 parent 862436f commit 9dc67b5

File tree

2 files changed

+144
-7
lines changed

2 files changed

+144
-7
lines changed

‎src/experimental/usd/plugins/mjcf/mujoco_to_usd.cc‎

Lines changed: 77 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -557,13 +557,13 @@ class ModelWriter {
557557
MjcPhysicsTokens->mjcOptionMagnetic, magnetic);
558558

559559
pxr::VtArray<double> o_solref(spec_->option.o_solref,
560-
spec_->option.o_solref + 2);
560+
spec_->option.o_solref + mjNREF);
561561
WriteUniformAttribute(physics_scene_path,
562562
pxr::SdfValueTypeNames->DoubleArray,
563563
MjcPhysicsTokens->mjcOptionO_solref, o_solref);
564564

565565
pxr::VtArray<double> o_solimp(spec_->option.o_solimp,
566-
spec_->option.o_solimp + 5);
566+
spec_->option.o_solimp + mjNIMP);
567567
WriteUniformAttribute(physics_scene_path,
568568
pxr::SdfValueTypeNames->DoubleArray,
569569
MjcPhysicsTokens->mjcOptionO_solimp, o_solimp);
@@ -1716,6 +1716,80 @@ class ModelWriter {
17161716
upper_limit);
17171717
}
17181718
}
1719+
1720+
// Finally write the mjcPhysicsJointAPI attributes.
1721+
ApplyApiSchema(data_, joint_path, MjcPhysicsTokens->PhysicsJointsAPI);
1722+
1723+
WriteUniformAttribute(
1724+
joint_path, pxr::SdfValueTypeNames->DoubleArray,
1725+
MjcPhysicsTokens->mjcSpringdamper,
1726+
pxr::VtArray<double>(joint->springdamper, joint->springdamper + 2));
1727+
1728+
WriteUniformAttribute(joint_path, pxr::SdfValueTypeNames->DoubleArray,
1729+
MjcPhysicsTokens->mjcSolreflimit,
1730+
pxr::VtArray<double>(joint->solref_limit,
1731+
joint->solref_limit + mjNREF));
1732+
1733+
WriteUniformAttribute(joint_path, pxr::SdfValueTypeNames->DoubleArray,
1734+
MjcPhysicsTokens->mjcSolimplimit,
1735+
pxr::VtArray<double>(joint->solimp_limit,
1736+
joint->solimp_limit + mjNIMP));
1737+
1738+
WriteUniformAttribute(
1739+
joint_path, pxr::SdfValueTypeNames->DoubleArray,
1740+
MjcPhysicsTokens->mjcSolreffriction,
1741+
pxr::VtArray<double>(joint->solref_friction,
1742+
joint->solref_friction + mjNREF));
1743+
1744+
WriteUniformAttribute(
1745+
joint_path, pxr::SdfValueTypeNames->DoubleArray,
1746+
MjcPhysicsTokens->mjcSolimpfriction,
1747+
pxr::VtArray<double>(joint->solimp_friction,
1748+
joint->solimp_friction + mjNIMP));
1749+
1750+
WriteUniformAttribute(joint_path, pxr::SdfValueTypeNames->Double,
1751+
MjcPhysicsTokens->mjcStiffness, joint->stiffness);
1752+
1753+
WriteUniformAttribute(joint_path, pxr::SdfValueTypeNames->Double,
1754+
MjcPhysicsTokens->mjcActuatorfrcrangeMin,
1755+
joint->actfrcrange[0]);
1756+
1757+
WriteUniformAttribute(joint_path, pxr::SdfValueTypeNames->Double,
1758+
MjcPhysicsTokens->mjcActuatorfrcrangeMax,
1759+
joint->actfrcrange[1]);
1760+
1761+
pxr::TfToken actuatorfrclimited_token = MjcPhysicsTokens->auto_;
1762+
if (joint->actfrclimited == mjLIMITED_TRUE) {
1763+
actuatorfrclimited_token = MjcPhysicsTokens->true_;
1764+
} else if (joint->actfrclimited == mjLIMITED_FALSE) {
1765+
actuatorfrclimited_token = MjcPhysicsTokens->false_;
1766+
}
1767+
WriteUniformAttribute(joint_path, pxr::SdfValueTypeNames->Token,
1768+
MjcPhysicsTokens->mjcActuatorfrclimited,
1769+
actuatorfrclimited_token);
1770+
1771+
WriteUniformAttribute(joint_path, pxr::SdfValueTypeNames->Bool,
1772+
MjcPhysicsTokens->mjcActuatorgravcomp,
1773+
static_cast<bool>(joint->actgravcomp));
1774+
1775+
WriteUniformAttribute(joint_path, pxr::SdfValueTypeNames->Double,
1776+
MjcPhysicsTokens->mjcMargin, joint->margin);
1777+
1778+
WriteUniformAttribute(joint_path, pxr::SdfValueTypeNames->Double,
1779+
MjcPhysicsTokens->mjcRef, joint->ref);
1780+
1781+
WriteUniformAttribute(joint_path, pxr::SdfValueTypeNames->Double,
1782+
MjcPhysicsTokens->mjcSpringref, joint->springref);
1783+
1784+
WriteUniformAttribute(joint_path, pxr::SdfValueTypeNames->Double,
1785+
MjcPhysicsTokens->mjcArmature, joint->armature);
1786+
1787+
WriteUniformAttribute(joint_path, pxr::SdfValueTypeNames->Double,
1788+
MjcPhysicsTokens->mjcDamping, joint->damping);
1789+
1790+
WriteUniformAttribute(joint_path, pxr::SdfValueTypeNames->Double,
1791+
MjcPhysicsTokens->mjcFrictionloss,
1792+
joint->frictionloss);
17191793
}
17201794
if (joint_id >= 0) {
17211795
joint_paths_[joint_id] = joint_path;
@@ -1824,8 +1898,7 @@ class ModelWriter {
18241898
// If the parent is not the world body, but is child of the world body
18251899
// then we need to apply the articulation root API.
18261900
if (parent_id != kWorldIndex) {
1827-
int parent_parent_id =
1828-
mjs_getId(mjs_getParent(parent->element)->element);
1901+
int parent_parent_id = mjs_getId(mjs_getParent(parent->element)->element);
18291902
if (parent_parent_id == kWorldIndex) {
18301903
ApplyApiSchema(data_, parent_path,
18311904
pxr::UsdPhysicsTokens->PhysicsArticulationRootAPI);

‎test/experimental/usd/plugins/mjcf/mjcf_file_format_test.cc‎

Lines changed: 67 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -19,6 +19,7 @@
1919
#include <gtest/gtest.h>
2020
#include <mujoco/experimental/usd/mjcPhysics/actuatorAPI.h>
2121
#include <mujoco/experimental/usd/mjcPhysics/collisionAPI.h>
22+
#include <mujoco/experimental/usd/mjcPhysics/jointAPI.h>
2223
#include <mujoco/experimental/usd/mjcPhysics/meshCollisionAPI.h>
2324
#include <mujoco/experimental/usd/mjcPhysics/sceneAPI.h>
2425
#include <mujoco/experimental/usd/mjcPhysics/siteAPI.h>
@@ -1169,13 +1170,13 @@ TEST_F(MjcfSdfFileFormatPluginTest, TestPhysicsRigidBody) {
11691170
// test_body_3 is a child of the world but has no children so should not be
11701171
// an articulation root.
11711172
EXPECT_PRIM_API_NOT_APPLIED(stage, "/physics_test/test_body_3",
1172-
pxr::UsdPhysicsArticulationRootAPI);
1173+
pxr::UsdPhysicsArticulationRootAPI);
11731174

11741175
// Articulation root is not applied to other bodies or world body.
11751176
EXPECT_PRIM_API_NOT_APPLIED(stage, "/physics_test",
1176-
pxr::UsdPhysicsArticulationRootAPI);
1177+
pxr::UsdPhysicsArticulationRootAPI);
11771178
EXPECT_PRIM_API_NOT_APPLIED(stage, "/physics_test/test_body/test_body_2",
1178-
pxr::UsdPhysicsArticulationRootAPI);
1179+
pxr::UsdPhysicsArticulationRootAPI);
11791180

11801181
// Geoms should not have RigidBodyAPI applied either.
11811182
EXPECT_PRIM_API_NOT_APPLIED(stage, "/physics_test/test_body/test_geom",
@@ -1589,6 +1590,69 @@ TEST_F(MjcfSdfFileFormatPluginTest, TestMjcPhysicsSliderCrankActuator) {
15891590
ExpectAttributeEqual(stage, "/test/body/crank.mjc:crankLength", 1.23);
15901591
}
15911592

1593+
TEST_F(MjcfSdfFileFormatPluginTest, TestMjcPhysicsJointAPI) {
1594+
static constexpr char xml[] = R"(
1595+
<mujoco model="test">
1596+
<worldbody>
1597+
<body name="parent">
1598+
<body name="child">
1599+
<joint name="my_joint" type="hinge"
1600+
springdamper="1 2"
1601+
solreflimit="0.1 0.2"
1602+
solimplimit="0.3 0.4 0.5 0.6 0.7"
1603+
solreffriction="0.8 0.9"
1604+
solimpfriction="1.0 1.1 1.2 1.3 1.4"
1605+
stiffness="1.5"
1606+
actuatorfrcrange="-1.6 1.7"
1607+
actuatorfrclimited="true"
1608+
actuatorgravcomp="true"
1609+
margin="1.8"
1610+
ref="1.9"
1611+
springref="2.0"
1612+
armature="2.1"
1613+
damping="2.2"
1614+
frictionloss="2.3"
1615+
/>
1616+
<geom type="sphere" size="1"/>
1617+
</body>
1618+
</body>
1619+
</worldbody>
1620+
</mujoco>
1621+
)";
1622+
auto stage = OpenStageWithPhysics(xml);
1623+
1624+
const SdfPath joint_path("/test/parent/child/my_joint");
1625+
EXPECT_PRIM_API_APPLIED(stage, joint_path, pxr::MjcPhysicsJointAPI);
1626+
1627+
ExpectAttributeEqual(stage, "/test/parent/child/my_joint.mjc:springdamper",
1628+
pxr::VtArray<double>({1, 2}));
1629+
ExpectAttributeEqual(stage, "/test/parent/child/my_joint.mjc:solreflimit",
1630+
pxr::VtArray<double>({0.1, 0.2}));
1631+
ExpectAttributeEqual(stage, "/test/parent/child/my_joint.mjc:solimplimit",
1632+
pxr::VtArray<double>({0.3, 0.4, 0.5, 0.6, 0.7}));
1633+
ExpectAttributeEqual(stage, "/test/parent/child/my_joint.mjc:solreffriction",
1634+
pxr::VtArray<double>({0.8, 0.9}));
1635+
ExpectAttributeEqual(stage, "/test/parent/child/my_joint.mjc:solimpfriction",
1636+
pxr::VtArray<double>({1.0, 1.1, 1.2, 1.3, 1.4}));
1637+
ExpectAttributeEqual(stage, "/test/parent/child/my_joint.mjc:stiffness", 1.5);
1638+
ExpectAttributeEqual(
1639+
stage, "/test/parent/child/my_joint.mjc:actuatorfrcrange:min", -1.6);
1640+
ExpectAttributeEqual(
1641+
stage, "/test/parent/child/my_joint.mjc:actuatorfrcrange:max", 1.7);
1642+
ExpectAttributeEqual(stage,
1643+
"/test/parent/child/my_joint.mjc:actuatorfrclimited",
1644+
MjcPhysicsTokens->true_);
1645+
ExpectAttributeEqual(
1646+
stage, "/test/parent/child/my_joint.mjc:actuatorgravcomp", true);
1647+
ExpectAttributeEqual(stage, "/test/parent/child/my_joint.mjc:margin", 1.8);
1648+
ExpectAttributeEqual(stage, "/test/parent/child/my_joint.mjc:ref", 1.9);
1649+
ExpectAttributeEqual(stage, "/test/parent/child/my_joint.mjc:springref", 2.0);
1650+
ExpectAttributeEqual(stage, "/test/parent/child/my_joint.mjc:armature", 2.1);
1651+
ExpectAttributeEqual(stage, "/test/parent/child/my_joint.mjc:damping", 2.2);
1652+
ExpectAttributeEqual(stage, "/test/parent/child/my_joint.mjc:frictionloss",
1653+
2.3);
1654+
}
1655+
15921656
TEST_F(MjcfSdfFileFormatPluginTest, TestPhysicsFloatingAndFixedBaseBody) {
15931657
static constexpr char kXml[] = R"(
15941658
<mujoco model="test">

0 commit comments

Comments
 (0)