FEEL I código fonte

Num artigo anterior, partilhei a lógica e a inspiração para a aplicação FEEL que desenvolvemos (eu e o João Pedro Sousa) para a KUKA iiwa 7 executar durante o FISTA 2019 no ISCTE-IUL. Atrás da cortina, o braço robótico está a correr uma aplicação simples escrita em JAVA, desenvolvida especificamente para este propósito. Especificamente, a aplicação é corrida por um computador que comanda o braço e o “vigia” para garantir que este cumpre as regras de segurança que garantem o ambiente colaborativo. No entanto, o braço é tão colaborativo quanto a aplicação que corre. É a responsabilidade do programador e do operador garantir que todas as medidas de segurança estão em vigor. Terminado o sermão segue-se a aplicação:

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();
  }
}