/** Computes using Twists, ignores linear part */
public void compute(Twist twistToPack, FramePose desiredPose, Twist desiredTwist) {
checkBodyFrames(desiredTwist, twistToPack);
checkBaseFrames(desiredTwist, twistToPack);
checkExpressedInFrames(desiredTwist, twistToPack);
twistToPack.setToZero(bodyFrame, desiredTwist.getBaseFrame(), bodyFrame);
desiredPose.getOrientationIncludingFrame(desiredOrientation);
desiredTwist.getAngularPart(desiredAngularVelocity);
desiredTwist.getAngularPart(feedForwardAngularAction);
compute(
angularActionFromOrientationController,
desiredOrientation,
desiredAngularVelocity,
null,
feedForwardAngularAction);
twistToPack.setAngularPart(angularActionFromOrientationController.getVector());
}
private void checkExpressedInFrames(Twist desiredTwist, Twist currentTwist) {
desiredTwist.getExpressedInFrame().checkReferenceFrameMatch(bodyFrame);
currentTwist.getExpressedInFrame().checkReferenceFrameMatch(bodyFrame);
}
private void checkBaseFrames(Twist desiredTwist, Twist currentTwist) {
desiredTwist.getBaseFrame().checkReferenceFrameMatch(currentTwist.getBaseFrame());
}