From 5e099f0cb6c88063a17afa772f1d23a2e75fe8d8 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Krist=C3=B3fi=20Mih=C3=A1ly?= <krmihaly42@gmail.com>
Date: Mon, 14 Oct 2024 16:49:18 +0200
Subject: [PATCH] add cart pose mode to robot application

---
 .../src/ros2/modules/FRIManager.java            | 17 ++++++++++++++---
 1 file changed, 14 insertions(+), 3 deletions(-)

diff --git a/kuka_sunrise_fri_driver/robot_application/src/ros2/modules/FRIManager.java b/kuka_sunrise_fri_driver/robot_application/src/ros2/modules/FRIManager.java
index 47651f31..17cd6dc9 100755
--- a/kuka_sunrise_fri_driver/robot_application/src/ros2/modules/FRIManager.java
+++ b/kuka_sunrise_fri_driver/robot_application/src/ros2/modules/FRIManager.java
@@ -205,12 +205,23 @@ public CommandResult endFRI(){
 		}
 		@Override
 		public CommandResult activateControl(){
-			FRIJointOverlay friJointOverlay =
+		  System.out.println("ClientCommandMode: " + FRIManager.this._clientCommandMode + ".");
+		  if (FRIManager.this._clientCommandMode==ClientCommandMode.CARTESIAN_POSE) {
+		    FRICartesianOverlay friCartesianOverlay =
+          new FRICartesianOverlay(FRIManager.this._FRISession);
+		    PositionHold motion =
+	          new PositionHold(FRIManager.this._controlMode, -1, null);
+	        FRIManager.this._motionContainer =
+	          FRIManager.this._lbr.getFlange().moveAsync(motion.addMotionOverlay(friCartesianOverlay));
+		  }
+		  else {
+		    FRIJointOverlay friJointOverlay =
 					new FRIJointOverlay(FRIManager.this._FRISession, FRIManager.this._clientCommandMode);
-			PositionHold motion =
+		    PositionHold motion =
 					new PositionHold(FRIManager.this._controlMode, -1, null);
-			FRIManager.this._motionContainer =
+		    FRIManager.this._motionContainer =
 					FRIManager.this._lbr.getFlange().moveAsync(motion.addMotionOverlay(friJointOverlay));
+		  }
 			return CommandResult.EXECUTED;
 		}
 		@Override