FEEL I source code

In a previous post, I shared the logic and inspiration behind the FEEL I application that we (João Pedro Sousa and I) developed for the KUKA iiwa robotic arm for FISTA 2019 at ISCTE-IUL. Behind the scenes, the robotic arm is running a simple application written in JAVA, that we developed specially for this aim. More specifically, the application is being run in a computer that commands the arm and enforces security rules to safely operate the arm in a collaborative environment. Yet, the arm is as collaborative as the application that it runs. Ultimately, it is the responsibility first of the programmers and then of the operators to ensure that all safety measures are in place. With that out of the way this is the application:

package application;

import java.util.ArrayList;
import java.util.List;

import com.kuka.generated.ioAccess.GripperIOGroup;
import com.kuka.roboticsAPI.applicationModel.RoboticsAPIApplication;
import com.kuka.roboticsAPI.conditionModel.ForceCondition;
import com.kuka.roboticsAPI.conditionModel.ICondition;
import com.kuka.roboticsAPI.deviceModel.LBR;
import com.kuka.roboticsAPI.geometricModel.Frame;
import com.kuka.roboticsAPI.geometricModel.Tool;
import com.kuka.roboticsAPI.geometricModel.World;
import com.kuka.roboticsAPI.geometricModel.math.CoordinateAxis;
import com.kuka.roboticsAPI.geometricModel.math.Transformation;
import com.kuka.roboticsAPI.motionModel.BasicMotions;
import com.kuka.roboticsAPI.motionModel.IMotionContainer;

public class PickAndPlace_Fista extends RoboticsAPIApplication {

  private LBR _lbr;
  private Tool _gripper;
  private GripperIOGroup _iogroup;
  private Frame _pt;
  private List<Frame> _listPt = new ArrayList<Frame>();
  private List<Boolean> _rotation = new ArrayList<Boolean>();

  @Override
  public void initialize() {
    _lbr = getContext().getDeviceFromType(LBR.class);
    _gripper = (Tool) createFromTemplate("MyVirtualGripper");
    _gripper.attachTo(_lbr.getFlange());
    _iogroup = new GripperIOGroup(_lbr.getController());
  }

  public void run() throws Exception {
    double cellXY = 63.0;
    double cellZ = 20.0;
    int numCellX = 5;
    int numCellY = 5;
    int numCellZ = 4;
    int count = 0;
    double tol = 3.0;
    double vel1 = 20; // cartesian slow
    double vel2 = 90; // cartesian fast
    double jointVel1 = 0.07; // joint slow
    double jointVel2 = 0.18; // joint fast
    double breakForce = 5;
    
    _pt = getFrame("/baseFista/P2").copy();

    //generate all the positions that the robot will visit
    for (int i = 0; i < numCellZ; i++) {
      for (int j = 0; j < numCellX; j++) {
        for (int k = 0; k < numCellY; k++) {
          Frame tempPt = _pt.copy();
          tempPt.setZ(_pt.getZ() + cellZ * i + cellZ / 2);
          tempPt.setX(_pt.getX() + cellXY * j);
          tempPt.setY(_pt.getY() + cellXY * k);
          count = i + j + k;
          if (count % 2 < 1) {//now rotate the gripper 
            _rotation.add(false);
          } else {
            _rotation.add(true);
            tempPt.setAlphaRad(tempPt.getAlphaRad() + Math.PI / 2);
          }
          // we don't really need to add the points to a list because the robot will visit each position as it's created
          _listPt.add(tempPt); 

          // Ensure that the gripper is open before we try to pick something
          _iogroup.setOut_Close(false); 
          _iogroup.setOut_Open(true);

          // now move to the station (P1 is a point 20mm above the piece we want to pick)
          _gripper.move(BasicMotions.ptp(getFrame("/baseFista/P1").copy().transform(Transformation.ofTranslation(0, 0, -30))).setJointVelocityRel(jointVel2)); 
          _gripper.move(BasicMotions.lin(
              getFrame("/baseFista/P1").copy().transform(
                  Transformation.ofTranslation(0, 0, -10)))
              .setCartVelocity(vel1));

          // Now we pick the piece
          _iogroup.setOut_Open(false);
          _iogroup.setOut_Close(true);

          // We step back from the station to a safe point
          _gripper.move(BasicMotions.lin(
              getFrame("/baseFista/P1").copy().transform(
                  Transformation.ofTranslation(0, 0, -100)))
              .setJointVelocityRel(jointVel2));

          // first move to a position a little bit above the desired position.
          _gripper.move(BasicMotions.ptp(
              tempPt.copy().transform(
                  Transformation.ofTranslation(0, 0, - cellZ * (numCellZ + 3))))
              .setJointVelocityRel(jointVel2));

          // Create the condition that if broken will cause the robot to react
          ICondition cond = ForceCondition
              .createNormalForceCondition(
                  _gripper.getDefaultMotionFrame(),
                  CoordinateAxis.Z, breakForce);
          Frame fr = _lbr.getCurrentCartesianPosition(_gripper.getDefaultMotionFrame());

          IMotionContainer container = _lbr.move(BasicMotions
              .linRel(0, 0, - cellZ * (numCellZ + 2) - tol)
              .setReferenceFrame(World.Current.getRootFrame())
              .setJointVelocityRel(jointVel1).breakWhen(cond));
          
          if (container.getFiredBreakConditionInfo() != null) {
            // if the condition is broken before reaching the desired position; determine the distance to the desired position and divide it by cellZ
            // if the remainder of the division by two is 1, change the boolean
            // place the object at the new location
            double x = _lbr.getCurrentCartesianPosition(
                _gripper.getDefaultMotionFrame())
                .distanceTo(fr.copy());
            double y = (cellZ * (numCellZ + 2) - x - tol) / cellZ;
            getLogger().info("x:" + x);
            getLogger().info("y:" + y);
            _lbr.move(BasicMotions
                .linRel(0, 0, cellZ*4)
                .setReferenceFrame(World.Current.getRootFrame())
                .setCartVelocity(vel2)); // move back a bit
            if (y % 2 < 1) {
                _gripper.move(BasicMotions.linRel(0,0,0, Math.PI / 2 , 0,0));
            }
            
            _gripper.move(BasicMotions.linRel(0, 0, cellZ * 4)
                .setCartVelocity(vel1).breakWhen(cond)); // this motion is using the TCP of the gripper
            _iogroup.setOut_Close(false);
            _iogroup.setOut_Open(true);
            _gripper.move(BasicMotions.linRel(0, 0,	-(cellZ * 3 + y))
                .setCartVelocity(vel2)); // this motion is using the TCP of the gripper
            continue;
          }

          //we have come most of the way to where we wanted to be and still there is nothing in the way
          // now move to a position that is a little bellow the desired position
          // if you feel something in the way place the object in the desired position
          container = _lbr.move(BasicMotions.linRel(0, 0, -cellZ)
              .setReferenceFrame(World.Current.getRootFrame())
              .setJointVelocityRel(jointVel1)
              .breakWhen(cond));
          if (container.getFiredBreakConditionInfo() != null) {
            _iogroup.setOut_Close(false);
            _iogroup.setOut_Open(true);
            _gripper.move(BasicMotions.linRel(0, 0, -(cellZ * (numCellZ + 3)))
                .setCartVelocity(vel2));
            continue;
          }

          //Wait! There is no support for the piece the robot is carrying!
          // now move until you touch something (up to the base plane)
          container = _lbr.move(BasicMotions
           		.linRel(0, 0, -cellZ * numCellZ)
              .setReferenceFrame(World.Current.getRootFrame())
              .setJointVelocityRel(jointVel1)
              .breakWhen(cond));
          if (container.getFiredBreakConditionInfo() != null) {
            double x = _lbr.getCurrentCartesianPosition(
                _gripper.getDefaultMotionFrame())
                .distanceTo(fr.copy());
            double y = (x + tol - cellZ * (numCellZ + 3)) / cellZ;
            _lbr.move(BasicMotions
                .linRel(0, 0, cellZ*4)
                .setReferenceFrame(World.Current.getRootFrame())
                .setCartVelocity(vel2)); // move back a bit
            if (!(y % 2 < 1)) {
                _gripper.move(BasicMotions.linRel(0,0,0, Math.PI / 2 , 0,0));

            }
            
            _gripper.move(BasicMotions.linRel(0, 0, cellZ * 4)
                .setCartVelocity(vel1)
                .breakWhen(cond));// this motion is using the TCP of the gripper
            _iogroup.setOut_Close(false);
            _iogroup.setOut_Open(true);
            _gripper.move(BasicMotions.linRel(0, 0, -(cellZ * 4 + y))
                .setCartVelocity(vel2));// this motion is using the TCP of the gripper
          }
        }
      }
    }

  }

  @Override
  public void dispose() {
    // TODO Auto-generated method stub
    super.dispose();
  }
}