Got testDrakeJointsComparison to pass. Still need to test joint transform, but everything else is working.