+ * The application provides a {@link RoboticsAPITask#initialize()} and a + * {@link RoboticsAPITask#run()} method, which will be called successively in + * the application lifecycle. The application will terminate automatically after + * the {@link RoboticsAPITask#run()} method has finished or after stopping the + * task. The {@link RoboticsAPITask#dispose()} method will be called, even if an + * exception is thrown during initialization or run. + *
+ * It is imperative to call super.dispose() when overriding the
+ * {@link RoboticsAPITask#dispose()} method.
+ *
+ * @see UseRoboticsAPIContext
+ * @see #initialize()
+ * @see #run()
+ * @see #dispose()
+ */
+public class Iiwa_ros2 extends RoboticsAPIApplication {
+ private Controller _lbrController;
+ private LBR _lbr;
+ private String _clientName;
+ @Inject
+ private MediaFlangeIOGroup _medflange;
+
+ PositionHold posHold;
+ FRIJointOverlay jointOverlay;
+
+ private static final JointPosition INITIAL_POSITION = new JointPosition(0.0,-0.7854,0.0,1.3962,0.0,0.6109,0.0);
+ private static final String CLIENT_IP = "192.170.10.5";
+ private static final double TS = 5; //in ms
+
+ IFRISessionListener listener = new IFRISessionListener(){
+ @Override
+ public void onFRIConnectionQualityChanged(
+ FRIChannelInformation friChannelInformation){
+ getLogger().info("QualityChangedEvent - quality:" +
+ friChannelInformation.getQuality()+"\n Jitter info:" + friChannelInformation.getJitter() +"\n Latency info:" + friChannelInformation.getLatency());
+ }
+ @Override
+ public void onFRISessionStateChanged(
+ FRIChannelInformation friChannelInformation){
+ getLogger().info("SessionStateChangedEvent - session state:" +
+ friChannelInformation.getFRISessionState() +"\n Jitter info:" + friChannelInformation.getJitter() +"\n Latency info:" + friChannelInformation.getLatency());
+ }
+ };
+
+ @Override
+ public void initialize() {
+ _lbrController = (Controller) getContext().getControllers().toArray()[0];
+ _lbr = (LBR) _lbrController.getDevices().toArray()[0];
+ // **********************************************************************
+ // *** change next line to the FRIClient's IP address ***
+ // **********************************************************************
+ _clientName = CLIENT_IP;
+ _lbr.attachTo(_lbr.getFlange());
+ }
+
+
+ @Override
+ public void run() {
+ // Select the type of control
+ String ques = "Select FRI control mode :\n";
+ double res = getApplicationUI().displayModalDialog(ApplicationDialogType.QUESTION,ques , "POSITION","TORQUE","MONITORING","Cancel");
+
+ _medflange.setLEDRed(true);
+
+ _lbr.move(ptp(INITIAL_POSITION).setJointVelocityRel(0.2));
+
+ if(res == 0){
+ PositionControlMode ctrMode = new PositionControlMode();
+ posHold = new PositionHold(ctrMode, -1, TimeUnit.MINUTES);
+ }
+ else if (res == 1 || res == 2){
+ JointImpedanceControlMode ctrMode = new JointImpedanceControlMode(0.0,0.0,0.0,0.0,0.0,0.0,0.0);
+ ctrMode.setStiffnessForAllJoints(0.0);
+ posHold = new PositionHold(ctrMode, -1, TimeUnit.MINUTES);
+ }
+ else return;
+
+ // configure and start FRI session
+ FRIConfiguration friConfiguration = FRIConfiguration.createRemoteConfiguration(_lbr, _clientName);
+ // for torque mode, there has to be a command value at least all 5ms
+ friConfiguration.setSendPeriodMilliSec(TS);
+ friConfiguration.setReceiveMultiplier(1);
+
+ getLogger().info("Creating FRI connection to " + friConfiguration.getHostName());
+ getLogger().info("SendPeriod: " + friConfiguration.getSendPeriodMilliSec() + "ms |"
+ + " ReceiveMultiplier: " + friConfiguration.getReceiveMultiplier());
+
+ FRISession friSession = new FRISession(friConfiguration);
+ friSession.addFRISessionListener(listener);
+
+ // wait until FRI session is ready to switch to command mode
+ try
+ {
+ friSession.await(20, TimeUnit.SECONDS);
+ }
+ catch (final TimeoutException e)
+ {
+ getLogger().error(e.getLocalizedMessage());
+ friSession.close();
+ return;
+ }
+
+ getLogger().info("FRI connection established.");
+
+ getLogger().info("Jitter info: " + friSession.getFRIChannelInformation().getJitter());
+
+ if(res == 0){
+ jointOverlay = new FRIJointOverlay(friSession, ClientCommandMode.POSITION);
+ }
+ else if (res == 1){
+ jointOverlay = new FRIJointOverlay(friSession, ClientCommandMode.TORQUE);
+ }
+ else if(res== 2){
+ jointOverlay = new FRIJointOverlay(friSession, ClientCommandMode.NO_COMMAND_MODE);
+ }
+ else return;
+
+
+ _medflange.setLEDRed(false);
+ _medflange.setLEDGreen(true);
+ BooleanIOCondition _buttonPressed = new BooleanIOCondition(_medflange.getInput("UserButton"), true);
+ if(res == 0 || res == 1) _lbr.move(posHold.addMotionOverlay(jointOverlay).breakWhen(_buttonPressed));
+ else _lbr.move(posHold.breakWhen(_buttonPressed));
+
+ _medflange.setLEDGreen(false);
+ _medflange.setLEDRed(true);
+ // done
+ friSession.close();
+ getLogger().info("FRI connection closed.");
+ getLogger().info("Application stopped.");
+ }
+
+ public static void main(final String[] args)
+ {
+ final Iiwa_ros2 app = new Iiwa_ros2();
+ app.runApplication();
+ }
+}