private void compareJointAccelerations(double epsilon)
{
boolean areEqual = true;
String errorMessage = "Joint accelerations are not equal\n";
String jointTriggerErrorMessage = "";
FloatingJoint scsFloatingJoint = (FloatingJoint) robot.getRootJoints().get(0);
SpatialAcceleration expected = extractFromFloatingJoint(scsFloatingJoint, floatingJoint.getFrameBeforeJoint(), floatingJoint.getFrameAfterJoint());
if (!floatingJoint.getJointAcceleration().epsilonEquals(expected, epsilon))
{
areEqual = false;
jointTriggerErrorMessage = " floating-joint";
}
errorMessage += "Floating joint:\nExpected: " + expected + "\nActual : " + floatingJoint.getJointAcceleration();
for (OneDoFJointBasics joint : allOneDoFJoints)
{
String jointName = joint.getName();
OneDegreeOfFreedomJoint scsJoint = (OneDegreeOfFreedomJoint) robot.getJoint(jointName);
if (!EuclidCoreTools.epsilonEquals(scsJoint.getQDD(), joint.getQdd(), epsilon))
{
areEqual = false;
jointTriggerErrorMessage += " " + jointName;
}
errorMessage += "\n" + jointName + ",\texpected: " + String.format(FORMAT, scsJoint.getQDD()) + ",\tactual: "
+ String.format(FORMAT, joint.getQdd()) + ", difference: " + String.format(FORMAT, Math.abs(scsJoint.getQDD() - joint.getQdd()));
}
if (!areEqual)
{
throw new RuntimeException(errorMessage + "\nJoint(s) triggering error:" + jointTriggerErrorMessage);
}
}