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