private Matrix3d computeDesiredFootRotation(
      double angleToDestination, RobotSide swingLegSide, ReferenceFrame supportFootFrame) {
    RigidBodyTransform supportFootToWorldTransform =
        supportFootFrame.getTransformToDesiredFrame(ReferenceFrame.getWorldFrame());
    Matrix3d supportFootToWorldRotation = new Matrix3d();
    supportFootToWorldTransform.get(supportFootToWorldRotation);

    double maxTurnInAngle = 0.25;
    double maxTurnOutAngle = 0.4;

    double amountToYaw;
    switch (blindWalkingDirection.getEnumValue()) {
      case BACKWARD:
        {
          amountToYaw = AngleTools.computeAngleDifferenceMinusPiToPi(angleToDestination, Math.PI);
          break;
        }
      case LEFT:
        {
          amountToYaw =
              AngleTools.computeAngleDifferenceMinusPiToPi(angleToDestination, Math.PI / 2.0);
          break;
        }
      case RIGHT:
        {
          amountToYaw =
              AngleTools.computeAngleDifferenceMinusPiToPi(angleToDestination, -Math.PI / 2.0);
          break;
        }
      case FORWARD:
        {
          amountToYaw = angleToDestination;
          break;
        }
      default:
        {
          throw new RuntimeException("Shouldn't get here!");
        }
    }

    if (swingLegSide == RobotSide.LEFT) {
      amountToYaw = MathTools.clipToMinMax(amountToYaw, -maxTurnInAngle, maxTurnOutAngle);

    } else {
      amountToYaw = MathTools.clipToMinMax(amountToYaw, -maxTurnOutAngle, maxTurnInAngle);
    }

    Matrix3d yawRotation = new Matrix3d();
    yawRotation.rotZ(amountToYaw);

    Matrix3d ret = new Matrix3d();
    ret.mul(yawRotation, supportFootToWorldRotation);

    return ret;
  }
 public Matrix3d getViewMatrix(int index) {
   Matrix3d m = new Matrix3d();
   switch (index) {
     case 0:
       m.setIdentity(); // C4 vertex-centered
       break;
     case 1:
       m.rotX(-0.5 * TETRAHEDRAL_ANGLE); // C3 face-centered  2.0*Math.PI/3
       Matrix3d m1 = new Matrix3d();
       m1.rotZ(Math.PI / 4);
       m.mul(m1);
       break;
     case 2:
       m.rotY(Math.PI / 4); // side face-centered
       break;
     default:
       throw new IllegalArgumentException("getViewMatrix: index out of range:" + index);
   }
   return m;
 }
  private void computeOrientationStateOutputBlock() {
    estimationFrame.getTransformToDesiredFrame(tempTransform, ReferenceFrame.getWorldFrame());
    tempTransform.getRotation(rotationFromEstimationToWorld);

    tempCenterOfMassPosition.setIncludingFrame(centerOfMassPositionPort.getData());
    tempCenterOfMassPosition.changeFrame(estimationFrame);

    ReferenceFrame referenceFrame =
        referenceFrameMap.getFrameByName(
            pointPositionMeasurementInputPort.getData().getBodyFixedReferenceFrameName());
    tempFramePoint.setIncludingFrame(
        referenceFrame,
        pointPositionMeasurementInputPort.getData().getMeasurementPointInBodyFrame());

    tempFramePoint.changeFrame(estimationFrame);
    tempFramePoint.sub(tempCenterOfMassPosition);
    tempFramePoint.scale(-1.0);

    MatrixTools.toTildeForm(tempMatrix, tempFramePoint.getPoint());
    tempMatrix.mul(rotationFromEstimationToWorld, tempMatrix);
    MatrixTools.setDenseMatrixFromMatrix3d(0, 0, tempMatrix, getOutputMatrixBlock(orientationPort));
  }