[mjcf] Use the frame of the parent body of the sites

Megane Millan · Megane Millan · commit cf3a416b694c · 2025-01-07T16:55:17.000+01:00
diff --git a/src/parsers/mjcf/mjcf-graph.cpp b/src/parsers/mjcf/mjcf-graph.cpp
@@ -1070,12 +1070,13 @@ namespace pinocchio
             rangeCompo.armature;
         }
 
-        FrameIndex previousFrameId = urdfVisitor.model.frames.size();
+        FrameIndex bodyId = urdfVisitor.model.getFrameId(nameOfBody, BODY);
+        frame = urdfVisitor.model.frames[bodyId];
         for (const auto & site : currentBody.siteChildren)
         {
           SE3 placement = bodyInJoint * site.sitePlacement;
-          previousFrameId = urdfVisitor.model.addFrame(
-            Frame(site.siteName, frame.parentJoint, previousFrameId, placement, OP_FRAME));
+          FrameIndex previousFrameId = urdfVisitor.model.addFrame(
+            Frame(site.siteName, frame.parentJoint, bodyId, placement, OP_FRAME));
         }
       }
 

Original file line number	Diff line number	Diff line change
`@@ -1070,12 +1070,13 @@ namespace pinocchio`
`1070`	`1070`	`rangeCompo.armature;`
`1071`	`1071`	`}`
`1072`	`1072`
`1073`		`- FrameIndex previousFrameId = urdfVisitor.model.frames.size();`
	`1073`	`+ FrameIndex bodyId = urdfVisitor.model.getFrameId(nameOfBody, BODY);`
	`1074`	`+ frame = urdfVisitor.model.frames[bodyId];`
`1074`	`1075`	`for (const auto & site : currentBody.siteChildren)`
`1075`	`1076`	`{`
`1076`	`1077`	`SE3 placement = bodyInJoint * site.sitePlacement;`
`1077`		`- previousFrameId = urdfVisitor.model.addFrame(`
`1078`		`- Frame(site.siteName, frame.parentJoint, previousFrameId, placement, OP_FRAME));`
	`1078`	`+ FrameIndex previousFrameId = urdfVisitor.model.addFrame(`
	`1079`	`+ Frame(site.siteName, frame.parentJoint, bodyId, placement, OP_FRAME));`
`1079`	`1080`	`}`
`1080`	`1081`	`}`
`1081`	`1082`