private RigidBody(String bodyName, JointBasics parentJoint, RigidBodyTransform inertiaPose) { if (bodyName == null) throw new IllegalArgumentException("Name can not be null"); name = bodyName; this.parentJoint = parentJoint; ReferenceFrame frameAfterJoint = parentJoint.getFrameAfterJoint(); bodyFixedFrame = MovingReferenceFrame.constructFrameFixedInParent(bodyName + "CoM", frameAfterJoint, inertiaPose); inertia = new SpatialInertia(bodyFixedFrame, bodyFixedFrame); // inertia should be expressed in body frame, otherwise it will change inertia.getBodyFrame().checkReferenceFrameMatch(inertia.getReferenceFrame()); parentJoint.setSuccessor(this); nameId = parentJoint.getPredecessor().getNameId() + NAME_ID_SEPARATOR + bodyName; }