@Override
public void wasRewound() {
SixDoFJoint rootJoint = fullRobotModel.getRootJoint();
yoRootJointTranslation.get(rootJointTranslation);
rootJoint.setPosition(rootJointTranslation);
yoRootJointRotation.get(rootJointRotation);
rootJoint.setRotation(rootJointRotation);
}
public void recordCurrentState() {
SixDoFJoint rootJoint = fullRobotModel.getRootJoint();
SixDoFJointReferenceFrame rootJointFrame = rootJoint.getFrameAfterJoint();
rootJointFrame.getTraslation(rootJointTranslation);
rootJointFrame.getRotation(rootJointRotation);
yoRootJointTranslation.set(rootJointTranslation);
yoRootJointRotation.set(rootJointRotation);
}