Fixed index out of bounds at body_jacobians

[GhostMinerPlus]

My program panicked at here, then I added an assert like this:

And it just failed. I didn't unstand these code well, but I noticed the internal_id of the first new link will be updated to self.links.len(), then I gaussed base_internal_id should be set to self.links.len(), or there will be an unused internal_id. I tested this change with my program and the MultibodyJoint example, it seemed not bad.

[Vrixyz]

Thanks, I'm not very comfortable with the code here either ; can you share more context on what you were doing ? Ideally this would help setting up a test for that ☺️

[GhostMinerPlus]

Thanks, I'm not very comfortable with the code here either ; can you share more context on what you were doing ? Ideally this would help setting up a test for that ☺️

Ok, I will do it as soon as possible