Add AMENT_IGNORE file and initial implementation of AAServer and Iiwa_ros2 classes

2026-04-09 13:55:32 +10:00
parent 845f4ca89c
commit 47cafe7ea1
3 changed files with 185 additions and 0 deletions
@@ -0,0 +1,185 @@
+// Copyright 2022, ICube Laboratory, University of Strasbourg
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+package application;
+
+import java.util.concurrent.TimeUnit;
+import java.util.concurrent.TimeoutException;
+
+import javax.inject.Inject;
+import javax.inject.Named;
+
+import com.kuka.roboticsAPI.applicationModel.RoboticsAPIApplication;
+import static com.kuka.roboticsAPI.motionModel.BasicMotions.*;
+
+import com.kuka.roboticsAPI.conditionModel.BooleanIOCondition;
+import com.kuka.roboticsAPI.controllerModel.Controller;
+import com.kuka.roboticsAPI.deviceModel.JointPosition;
+import com.kuka.roboticsAPI.deviceModel.LBR;
+import com.kuka.roboticsAPI.geometricModel.Tool;
+import com.kuka.roboticsAPI.motionModel.PositionHold;
+import com.kuka.roboticsAPI.motionModel.controlModeModel.JointImpedanceControlMode;
+import com.kuka.roboticsAPI.motionModel.controlModeModel.PositionControlMode;
+import com.kuka.roboticsAPI.uiModel.ApplicationDialogType;
+import com.kuka.connectivity.fastRobotInterface.ClientCommandMode;
+import com.kuka.connectivity.fastRobotInterface.FRIChannelInformation;
+import com.kuka.connectivity.fastRobotInterface.FRIConfiguration;
+import com.kuka.connectivity.fastRobotInterface.FRIJointOverlay;
+import com.kuka.connectivity.fastRobotInterface.FRISession;
+import com.kuka.connectivity.fastRobotInterface.IFRISessionListener;
+import com.kuka.generated.flexfellow.FlexFellow;
+import com.kuka.generated.ioAccess.MediaFlangeIOGroup;
+import com.kuka.grippertoolbox.api.gripper.AbstractGripper;
+
+/**
+ * Implementation of a robot application.
+ * <p>
+ * 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.
+ * <p>
+ * <b>It is imperative to call <code>super.dispose()</code> when overriding the
+ * {@link RoboticsAPITask#dispose()} method.</b>
+ *
+ * @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();
+    }
+}