Change in how RigidBodyAddedMass operates: Now added new variables into RigidBody, including two separate spatial inertia matrices. RigidBodyAddedMass only affects one of them, while the other is the original mass properties of the link. Requesting RigidBody.I gives the sum of the two spatial inertia matrices. Also simplified some of the the functions inside RigidBodyAddedMass