@@ -1079,6 +1079,133 @@ TEST_F(MujocoTest, AttachSame) {
10791079 mj_deleteModel (m_expected);
10801080}
10811081
1082+ TEST_F (MujocoTest, AttachSpatialTendonWithoutSidesite) {
1083+ static constexpr char xml_parent[] = R"(
1084+ <mujoco>
1085+ <worldbody>
1086+ <body name="parent_body">
1087+ <geom size="0.1" type="sphere"/>
1088+ </body>
1089+ </worldbody>
1090+ </mujoco>)" ;
1091+
1092+ static constexpr char xml_child[] = R"(
1093+ <mujoco>
1094+ <worldbody>
1095+ <body name="child_body">
1096+ <geom name="wrap_geom" size="0.05" type="sphere"/>
1097+ <site name="site_A" pos="0 0 0.1"/>
1098+ <site name="site_B" pos="0 0 -0.1"/>
1099+ <site name="side_site" pos="0.05 0 0"/>
1100+ </body>
1101+ </worldbody>
1102+ <tendon>
1103+ <spatial name="tendon_with_sidesite">
1104+ <site site="site_A"/>
1105+ <geom geom="wrap_geom" sidesite="side_site"/>
1106+ <site site="site_B"/>
1107+ </spatial>
1108+ <spatial name="tendon_without_sidesite">
1109+ <site site="site_A"/>
1110+ <geom geom="wrap_geom"/>
1111+ <site site="site_B"/>
1112+ </spatial>
1113+ </tendon>
1114+ </mujoco>)" ;
1115+
1116+ std::array<char , 1000 > er;
1117+ mjSpec* parent = mj_parseXMLString (xml_parent, 0 , er.data (), er.size ());
1118+ ASSERT_THAT (parent, NotNull ()) << er.data ();
1119+ mjSpec* child = mj_parseXMLString (xml_child, 0 , er.data (), er.size ());
1120+ ASSERT_THAT (child, NotNull ()) << er.data ();
1121+
1122+ mjsBody* parent_body = mjs_findBody (parent, " parent_body" );
1123+ ASSERT_THAT (parent_body, NotNull ());
1124+ mjsSite* attach_site = mjs_addSite (parent_body, 0 );
1125+ mjs_setName (attach_site->element , " attach_site" );
1126+
1127+ mjs_attach (attach_site->element ,
1128+ mjs_findBody (child, " child_body" )->element , " " , " _child" );
1129+
1130+ EXPECT_THAT (mjs_findElement (parent, mjOBJ_TENDON,
1131+ " tendon_with_sidesite_child" ), NotNull ());
1132+ EXPECT_THAT (mjs_findElement (parent, mjOBJ_TENDON,
1133+ " tendon_without_sidesite_child" ), NotNull ());
1134+
1135+ mjModel* model = mj_compile (parent, nullptr );
1136+ ASSERT_THAT (model, NotNull ()) << mjs_getError (parent);
1137+ EXPECT_EQ (model->ntendon , 2 );
1138+
1139+ mj_deleteModel (model);
1140+ mj_deleteSpec (parent);
1141+ mj_deleteSpec (child);
1142+ }
1143+
1144+ TEST_F (MujocoTest, AttachSpatialTendonGitHubIssue3119) {
1145+ static constexpr char parent_xml[] = R"(
1146+ <mujoco>
1147+ <worldbody>
1148+ <body name="parent_body">
1149+ <geom size="0.1" type="sphere"/>
1150+ </body>
1151+ </worldbody>
1152+ </mujoco>)" ;
1153+
1154+ static constexpr char child_xml[] = R"(
1155+ <mujoco>
1156+ <worldbody>
1157+ <body name="child_body">
1158+ <geom name="wrap_geom" size="0.05" type="sphere"/>
1159+ <site name="site_A" pos="0 0 0.1"/>
1160+ <site name="site_B" pos="0 0 -0.1"/>
1161+ <site name="side_site" pos="0.05 0 0"/>
1162+ </body>
1163+ </worldbody>
1164+ <tendon>
1165+ <spatial name="tendon_with_sidesite">
1166+ <site site="site_A"/>
1167+ <geom geom="wrap_geom" sidesite="side_site"/>
1168+ <site site="site_B"/>
1169+ </spatial>
1170+ <spatial name="tendon_without_sidesite">
1171+ <site site="site_A"/>
1172+ <geom geom="wrap_geom"/>
1173+ <site site="site_B"/>
1174+ </spatial>
1175+ </tendon>
1176+ </mujoco>)" ;
1177+
1178+ std::array<char , 1000 > er;
1179+ mjSpec* parent_spec =
1180+ mj_parseXMLString (parent_xml, 0 , er.data (), er.size ());
1181+ ASSERT_THAT (parent_spec, NotNull ()) << er.data ();
1182+ mjSpec* child_spec =
1183+ mj_parseXMLString (child_xml, 0 , er.data (), er.size ());
1184+ ASSERT_THAT (child_spec, NotNull ()) << er.data ();
1185+
1186+ mjsBody* parent_body = mjs_findBody (parent_spec, " parent_body" );
1187+ ASSERT_THAT (parent_body, NotNull ());
1188+ mjsSite* attach_site = mjs_addSite (parent_body, 0 );
1189+ mjs_setName (attach_site->element , " attach_site" );
1190+
1191+ mjs_attach (attach_site->element ,
1192+ mjs_findBody (child_spec, " child_body" )->element ,
1193+ " " , " _child" );
1194+
1195+ EXPECT_THAT (mjs_findElement (parent_spec, mjOBJ_TENDON,
1196+ " tendon_with_sidesite_child" ), NotNull ());
1197+ EXPECT_THAT (mjs_findElement (parent_spec, mjOBJ_TENDON,
1198+ " tendon_without_sidesite_child" ), NotNull ());
1199+
1200+ mjModel* model = mj_compile (parent_spec, nullptr );
1201+ ASSERT_THAT (model, NotNull ()) << mjs_getError (parent_spec);
1202+ EXPECT_EQ (model->ntendon , 2 );
1203+
1204+ mj_deleteModel (model);
1205+ mj_deleteSpec (parent_spec);
1206+ mj_deleteSpec (child_spec);
1207+ }
1208+
10821209TEST_F (MujocoTest, AttachDifferent) {
10831210 std::array<char , 1000 > er;
10841211 mjtNum tol = 0 ;
0 commit comments