Skip to content

Commit 72f323d

Browse files
committed
core: Fix appendUniverseToModel bug
1 parent 1a9c4c2 commit 72f323d

File tree

1 file changed

+3
-8
lines changed

1 file changed

+3
-8
lines changed

include/pinocchio/algorithm/model.hxx

Lines changed: 3 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -102,14 +102,9 @@ namespace pinocchio
102102

103103
// Modify frame placement
104104
frame.placement = pframe.placement * pfMAB * frame.placement;
105-
// Some frames may have some inertia attached to them. In this case, we need to remove it
106-
// from the parent joint. To prevent introducing NaNs, we check if the frame inertia is
107-
// not NaN and is not zero.
108-
if (frame.inertia == frame.inertia && frame.inertia != Inertia::Zero())
109-
{
110-
model.inertias[frame.parentJoint] -= frame.inertia;
111-
}
112-
model.addFrame(frame);
105+
// Inertias are already computed in model.appendBodyToJoint call.
106+
// No need to append them again.
107+
model.addFrame(frame, false);
113108
}
114109
}
115110

0 commit comments

Comments
 (0)