Expand 7th joint limit with arm rotation [with code]

package application;

import com.kuka.roboticsAPI.deviceModel.LBR;
import com.kuka.roboticsAPI.deviceModel.LBRE1Redundancy;
import com.kuka.roboticsAPI.deviceModel.JointPosition;
import com.kuka.roboticsAPI.geometricModel.Frame;
import com.kuka.roboticsAPI.geometricModel.ObjectFrame;

/**
 * Use the redundancy of robot in order to be able to have a pseudo 360° J7
 * 
 * @author 
 *
 */
public class ExpandJ7 {

    private LBR robot;
    private final double aStep = Math.toRadians(5);
    private final int maxStep = (int) Math.ceil(2.0*Math.PI/aStep);
    private LBRE1Redundancy default_red = null;
    
    
    public LBRE1Redundancy getDefault_red() {
        return this.default_red;
    }

    public void setDefault_red(LBRE1Redundancy default_red) {
        this.default_red = default_red;
    }
    
    public void setDefault_red_as_current() { 
        this.default_red = getCurrent_red();    
    }
    
    public void resetDefault_red() {
        this.default_red = null;
    }

    public ExpandJ7(LBR robot) {
        super();
        this.robot = robot;
    }

    public LBRE1Redundancy getCurrent_red() {
        Frame frob = robot.getForwardKinematic(robot.getCurrentJointPosition()); 
        return (LBRE1Redundancy) frob.getRedundancyInformationForDevice(robot);    
    }
    
    /**
     *  Try to transform the Frame using the redundancy of robot in order to be able to have a pseudo 360° J7
     * @param f frame to transform with redundancy to make it reachable
     * @param toolFrame Frame of the tool (connected to the flange) that will be used to reach the frame f
     * @return a copy of f with modified redundancy if necessary
     */
    public Frame makeFrameReacheable(Frame f, ObjectFrame toolFrame) throws IllegalStateException{
        LBRE1Redundancy red;
        JointPosition jp = null;
        Frame fr = f.copyWithRedundancy();
        
        // add the transformation of the tool because IK calculation is done for the flange only
        fr.transform(toolFrame.getTransformationFromParent().invert());    
        
        // get current Redundancy information
        red = (LBRE1Redundancy) fr.getRedundancyInformationForDevice(robot);
        if (red == null) { // frame without Redundancy
            if (default_red != null){ 
                red = default_red.copy();    // use default redundancy if provided
            }else {
                red = getCurrent_red();        // use current Redundancy from robot position
            }
            fr.setRedundancyInformation(robot, red);
        }
        
        for (int i = 0 ; i <= maxStep; ++i) {
            jp = robot.getInverseKinematicFromFrameAndRedundancy(fr);
            if (isInRange(jp)){break;}    // yes
            // let's test another E1
            red.setE1(getOtherE1(red));
            fr.setRedundancyInformation(robot, red);
        }
        
        if (!isInRange(jp)){
            System.err.println("No solution found :(");
            throw new IllegalStateException("Frame is unreachable!");
            //return null;
        }
        // remove the transformation of the tool
        fr.transform(toolFrame.getTransformationFromParent());
        
        return fr;
    }

    private double getOtherE1(final LBRE1Redundancy red) {
        final double limit = Math.PI;
        double value = red.getE1() - aStep;
        if (value > limit) {value = -limit;}
        if (value < -limit) {value = +limit;}
        return value;
    }
    
    private boolean isInRange(JointPosition jp, int j){
        if ( (jp.get(j) > robot.getJointLimits().getMaxJointPosition().get(j))
                || jp.get(j) < robot.getJointLimits().getMinJointPosition().get(j)){
            //System.err.println(j.toString() + " out of range: " + Math.toDegrees(jp.get(j)));
            return false;
        }
        return true;
    }
    private boolean isInRange(JointPosition jp){
        for (int i = 0; i < robot.getJointCount(); ++i){
            if (!isInRange(jp, i)){
                //System.err.println(j.toString() + " out of range: " + Math.toDegrees(jp.get(j)));
                return false;
            }
        }
        return true;
    }
}

    private void J7extTest(){
        Frame f = getApplicationData().getFrame("/SomeBase/SomePoint").copyWithRedundancy();
        Frame f_rot, f_mod;
        double fakeAngle;
        
        ExpandJ7 ex7 = new ExpandJ7(robot);    // init
        gripper.move(ptp(f));                        // go to test pint
        ex7.setDefault_red_as_current();    
        f_rot = f.copyWithRedundancy();
        for (int i = 0; i < 360; i++){
            fakeAngle = i;
            if(fakeAngle>180){fakeAngle-=360;}
            f_rot.setAlphaRad(f.getAlphaRad() + Math.toRadians(i));
            f_mod = ex7.makeFrameReacheable(f_rot, gripper);
            gripper.move(ptp(f_mod));                        
        }
    }