├── Cartesian motion open loop ├── iiwa │ ├── SimulinkIIWADirectServoCartesian.java │ └── SimulinkIIWASmartServoCartesian.java └── simulink │ ├── smartDirectServoCartesian.slx │ └── smartDirectServoCartesian.slxc ├── Cartesian motion with feedback ├── iiwa │ ├── SimulinkFIIWADirectServoCartesian.java │ └── SimulinkFIIWASmartServoCartesian.java └── simulink │ ├── feedBackSmartDirectServoCartesian1.slx │ ├── feedBackSmartDirectServoCartesian1.slxc │ ├── freeHandGuiding_01.slx │ └── freeHandGuiding_01.slxc ├── Impedance control open loop ├── iiwa │ └── SimulinkIIWA_impedanceJoints.java └── simulink │ ├── impedanceJoints.slx │ └── impedanceJoints.slxc ├── Impedance control with feedback ├── iiwa │ └── SimulinkFIIWA_impedanceJoints.java └── simulink │ ├── feedbackImpedanceJoints.slx │ └── feedbackImpedanceJoints.slxc ├── Joint space motion open loop ├── iiwa │ ├── SimulinkIIWADirectServoJoints.java │ └── SimulinkIIWASmartServoJoints.java └── simulink │ ├── smartDirectServoJoints.slx │ └── smartDirectServoJoints.slxc ├── Joint space motion with feedback ├── iiwa │ ├── SimulinkFIIWADirectServoJoints.java │ └── SimulinkFIIWASmartServoJoints.java └── simulink │ ├── feedBackSmartDirectServoJoints.slx │ └── feedBackSmartDirectServoJoints.slxc ├── LICENSE ├── Photos └── SimulinkIIWAcover.jpg └── README.md /Cartesian motion open loop/iiwa/SimulinkIIWADirectServoCartesian.java: -------------------------------------------------------------------------------- 1 | package lbrExampleApplications; 2 | 3 | 4 | import static com.kuka.roboticsAPI.motionModel.BasicMotions.lin; 5 | import static com.kuka.roboticsAPI.motionModel.BasicMotions.ptp; 6 | 7 | import java.io.IOException; 8 | import java.net.ServerSocket; 9 | import java.net.Socket; 10 | import java.util.Scanner; 11 | import java.util.StringTokenizer; 12 | import java.util.logging.Logger; 13 | 14 | import com.kuka.connectivity.motionModel.directServo.DirectServo; 15 | import com.kuka.connectivity.motionModel.directServo.IDirectServoRuntime; 16 | import com.kuka.generated.ioAccess.MediaFlangeIOGroup; 17 | import com.kuka.roboticsAPI.applicationModel.RoboticsAPIApplication; 18 | import com.kuka.roboticsAPI.controllerModel.Controller; 19 | import com.kuka.roboticsAPI.deviceModel.JointPosition; 20 | import com.kuka.roboticsAPI.deviceModel.LBR; 21 | import com.kuka.roboticsAPI.geometricModel.Frame; 22 | 23 | 24 | import java.io.IOException; 25 | import java.net.DatagramPacket; 26 | import java.net.DatagramSocket; 27 | import java.net.SocketException; 28 | import java.nio.ByteBuffer; 29 | 30 | 31 | /* 32 | * SimulinkIIWA interface, soft real-time control in Cartesian position mode 33 | * Copyright: Mohammad SAFEEA, 22nd/May/2018 34 | * License: MIT license 35 | * 36 | * This is a UDP server that receives motion commands from Simulink. 37 | * This server works together with the Simulink example: (smartDirectServoCartesian.slx) 38 | * 39 | * 40 | * Below you can find quick start instructions, for more info, please refer to the youtube 41 | * video tutorials, a link for which is available in the repository main page. 42 | * 43 | * Hardware setup: 44 | * 1- KUKA iiwa 7R800 only, modifications in Simulink schematic are required for 14R820 45 | * 2- External PC 46 | * 3- A network between the PC and the robot, the connector X66 47 | * of the robot shall be used 48 | * 49 | * Required software: 50 | * 1- MATLAB with Simulink, the code was written using MATLAB2018a, it 51 | * is recommended to use the same version. 52 | * 2- Sunrise.Workbench, this program will be used to synchronize the java 53 | * code to the robot. 54 | * 3- The Sunrise code was tested on (KUKA Sunrise.OS 1.11.0.7) 55 | * 56 | * Setup instructions: 57 | * 1- Add this class to a new project using kuka's Sunrsie.Workbench. 58 | * 2- Add reference to the Direct/SmartServo from inside the (StationSetup.cat) file. 59 | * 3- Change the following variables according to preference: 60 | * * externa_PC_IP: change this variable according to the IP of the PC used for control. 61 | * 62 | * 63 | * Utilization instructions: 64 | * 1- Synchronize the project to the controller of the robot. 65 | * 2- Run this application from the teach-pad of the robot, 66 | * This application has a time out of 10 seconds, if a connection 67 | * is not established during the 10 seconds interval the server will 68 | * turn off. 69 | * 3- Start the Simulink example (smartDirectServoCartesian.slx) 70 | * from the external PC. 71 | * 4- To change the timeout value, change the argument for the instruction 72 | * soc.setSoTimeout(10000), the argument is in milliseconds. 73 | * 74 | * 75 | * Updated on 11th/June/2019 76 | * Bug fix (the bug happened only in 14R820) 77 | * To fix the bug hard-coding of the Cartesian home position was performed. 78 | */ 79 | 80 | 81 | public class SimulinkIIWADirectServoCartesian extends RoboticsAPIApplication 82 | { 83 | private LBR _lbr; 84 | 85 | 86 | public static double EEfServoPos[]; 87 | 88 | public static boolean loopFlag; 89 | 90 | public static double jDispMax[]; 91 | public static double updateCycleJointPos[]; 92 | private UDPServer udpServer; 93 | int _port; 94 | 95 | ServerSocket ss; 96 | Socket soc; 97 | 98 | 99 | 100 | 101 | @Override 102 | public void initialize() 103 | { 104 | _lbr = getContext().getDeviceFromType(LBR.class); 105 | EEfServoPos=new double[6]; 106 | for(int i=0;i<6;i++) 107 | { 108 | EEfServoPos[i]=0; 109 | } 110 | loopFlag=true; 111 | } 112 | 113 | /** 114 | * Move to an initial Position WARNING: MAKE SURE, THAT the pose is collision free. 115 | */ 116 | private void moveToInitialPosition() 117 | { 118 | _lbr.move( 119 | ptp(0., Math.PI / 180 * 30.,0.,-Math.PI / 180 * 80.,0.,Math.PI / 180 * 70., 0.).setJointVelocityRel(0.15)); 120 | // Bug fix (the bug happened only in 14R820) 121 | // Hard-code the Cartesian home position. 122 | Frame daframe= new Frame(575.87,0.05,397.56,Math.PI,0,Math.PI); 123 | _lbr.move( 124 | lin(daframe).setJointVelocityRel(0.15)); 125 | 126 | /* Note: The Validation itself justifies, that in this very time instance, the load parameter setting was 127 | * sufficient. This does not mean by far, that the parameter setting is valid in the sequel or lifetime of this 128 | * program */ 129 | try 130 | { 131 | 132 | } 133 | catch (IllegalStateException e) 134 | { 135 | getLogger().info("Omitting validation failure for this sample\n" 136 | + e.getMessage()); 137 | } 138 | } 139 | 140 | 141 | /** 142 | * Main Application Routine 143 | */ 144 | @Override 145 | public void run() 146 | { 147 | moveToInitialPosition(); 148 | 149 | System.out.print("You have ten seconds to establish a connection with iiwa"); 150 | int portTemp=30002; 151 | udpServer=new UDPServer(portTemp); 152 | directServoStartCartezian(); 153 | 154 | } 155 | 156 | 157 | private void directServoStartCartezian() { 158 | 159 | boolean doDebugPrints = false; 160 | 161 | DirectServo aDirectServoMotion = new DirectServo( 162 | _lbr.getCurrentJointPosition()); 163 | 164 | aDirectServoMotion.setMinimumTrajectoryExecutionTime(40e-3); 165 | 166 | getLogger().info("Starting DirectServo motion in position control mode"); 167 | _lbr.moveAsync(aDirectServoMotion); 168 | 169 | getLogger().info("Get the runtime of the DirectServo motion"); 170 | IDirectServoRuntime theDirectServoRuntime = aDirectServoMotion 171 | .getRuntime(); 172 | 173 | Frame aFrame = theDirectServoRuntime.getCurrentCartesianDestination(_lbr.getFlange()); 174 | Frame destFrame = aFrame.copyWithRedundancy(); 175 | // Initiate the initial position 176 | EEfServoPos[0]=aFrame.getX(); 177 | EEfServoPos[1]=aFrame.getY(); 178 | EEfServoPos[2]=aFrame.getZ(); 179 | EEfServoPos[3]=aFrame.getAlphaRad(); 180 | EEfServoPos[4]=aFrame.getBetaRad(); 181 | EEfServoPos[5]=aFrame.getGammaRad(); 182 | 183 | try 184 | { 185 | // do a cyclic loop 186 | // Do some timing... 187 | // in nanosec 188 | 189 | while(loopFlag==true) 190 | { 191 | 192 | // /////////////////////////////////////////////////////// 193 | // Insert your code here 194 | // e.g Visual Servoing or the like 195 | // Synchronize with the realtime system 196 | theDirectServoRuntime.updateWithRealtimeSystem(); 197 | Frame msrPose = theDirectServoRuntime 198 | .getCurrentCartesianDestination(_lbr.getFlange()); 199 | 200 | if (doDebugPrints) 201 | { 202 | getLogger().info("Current cartesian goal " + aFrame); 203 | getLogger().info("Current joint destination " 204 | + theDirectServoRuntime.getCurrentJointDestination()); 205 | } 206 | 207 | Thread.sleep(1); 208 | 209 | for(int kk=0;kk<6;kk++) 210 | { 211 | EEfServoPos[kk]=udpServer.EEFpos[kk]; 212 | } 213 | // update Cartesian positions 214 | destFrame.setX(EEfServoPos[0]); 215 | destFrame.setY(EEfServoPos[1]); 216 | destFrame.setZ(EEfServoPos[2]); 217 | destFrame.setAlphaRad(EEfServoPos[3]); 218 | destFrame.setBetaRad(EEfServoPos[4]); 219 | destFrame.setGammaRad(EEfServoPos[5]); 220 | 221 | if (doDebugPrints) 222 | { 223 | getLogger().info("New cartesian goal " + destFrame); 224 | getLogger().info("LBR position " 225 | + _lbr.getCurrentCartesianPosition(_lbr 226 | .getFlange())); 227 | getLogger().info("Measured cartesian pose from runtime " 228 | + msrPose); 229 | } 230 | 231 | theDirectServoRuntime.setDestination(destFrame); 232 | 233 | } 234 | } 235 | catch (Exception e) 236 | { 237 | getLogger().info(e.getLocalizedMessage()); 238 | e.printStackTrace(); 239 | //Print statistics and parameters of the motion 240 | getLogger().info("Simple Cartesian Test \n" + theDirectServoRuntime.toString()); 241 | 242 | getLogger().info("Stop the DirectServo motion"); 243 | 244 | } 245 | theDirectServoRuntime.stopMotion(); 246 | getLogger().info("Stop the DirectServo motion for the stop instruction was sent"); 247 | 248 | } 249 | 250 | 251 | /** 252 | * Main routine, which starts the application 253 | */ 254 | public static void main(String[] args) 255 | { 256 | SimulinkIIWADirectServoCartesian app = new SimulinkIIWADirectServoCartesian(); 257 | app.runApplication(); 258 | } 259 | 260 | public class UDPServer implements Runnable{ 261 | 262 | public double[] EEFpos={575.87,0.05,397.56,Math.PI,0,Math.PI}; 263 | 264 | double stime=0; 265 | double endtime=0; 266 | 267 | int _port; 268 | int vectorSize=6; 269 | int packetCounter=0; 270 | 271 | byte[] buffer=new byte[8*7]; 272 | UDPServer(int port) 273 | { 274 | Thread t=new Thread(this); 275 | t.setDaemon(true); 276 | t.start(); 277 | _port=port; 278 | } 279 | 280 | public void run() 281 | { 282 | System.out.println("Program will terminate if comuncation is not established in 10 seconds"); 283 | DatagramSocket soc=null; 284 | try { 285 | soc = new DatagramSocket(_port); 286 | soc.setSoTimeout(10000); // 5 seconds, if a time out occurred, turn off program 287 | DatagramPacket response=new DatagramPacket(buffer, buffer.length); 288 | while(true) 289 | { 290 | soc.receive(response); 291 | packetCounter=packetCounter+1; 292 | // String s= new String(buffer,0,response.getLength()) 293 | // System.out.println(response.getLength()); 294 | if(response.getLength()==8*vectorSize) 295 | { 296 | if(packetCounter==1) 297 | { 298 | stime=System.currentTimeMillis(); 299 | } 300 | else 301 | { 302 | endtime=System.currentTimeMillis(); 303 | } 304 | byte[] daB=new byte[8]; 305 | int counter=0; 306 | int index=0; 307 | while(counter <8*vectorSize) 308 | { 309 | for(int i=0;i<8;i++) 310 | { 311 | daB[7-i]=buffer[counter]; 312 | counter=counter+1; 313 | } 314 | EEFpos[index]=bytesToDouble(daB); 315 | index=index+1; 316 | //System.out.println(bytesToDouble(daB)); 317 | } 318 | } 319 | } 320 | } catch (SocketException e) { 321 | // TODO Auto-generated catch block 322 | e.printStackTrace(); 323 | //System.out.println(e.toString()); 324 | } catch (IOException e) { 325 | // TODO Auto-generated catch block 326 | e.printStackTrace(); 327 | System.out.println(e.toString()); 328 | } 329 | if(soc==null) 330 | {} 331 | else 332 | { 333 | if(soc.isClosed()) 334 | {} 335 | else 336 | { 337 | soc.close(); 338 | } 339 | } 340 | SimulinkIIWADirectServoCartesian.loopFlag=false; //Turn off main loop 341 | double time=(endtime-stime)/1000; 342 | double rate=packetCounter/time; 343 | System.out.println("update rate, packets/second"); 344 | System.out.println(rate); 345 | } 346 | } 347 | 348 | 349 | public double bytesToDouble(byte[] b) 350 | { 351 | return ByteBuffer.wrap(b).getDouble(); 352 | } 353 | 354 | } 355 | -------------------------------------------------------------------------------- /Cartesian motion open loop/iiwa/SimulinkIIWASmartServoCartesian.java: -------------------------------------------------------------------------------- 1 | package lbrExampleApplications; 2 | 3 | 4 | import static com.kuka.roboticsAPI.motionModel.BasicMotions.lin; 5 | import static com.kuka.roboticsAPI.motionModel.BasicMotions.ptp; 6 | 7 | import java.io.IOException; 8 | import java.net.ServerSocket; 9 | import java.net.Socket; 10 | import java.util.Scanner; 11 | import java.util.StringTokenizer; 12 | import java.util.logging.Logger; 13 | 14 | import com.kuka.common.ThreadUtil; 15 | import com.kuka.connectivity.motionModel.directServo.DirectServo; 16 | import com.kuka.connectivity.motionModel.directServo.IDirectServoRuntime; 17 | import com.kuka.connectivity.motionModel.smartServo.ISmartServoRuntime; 18 | import com.kuka.connectivity.motionModel.smartServo.SmartServo; 19 | import com.kuka.generated.ioAccess.MediaFlangeIOGroup; 20 | import com.kuka.roboticsAPI.applicationModel.RoboticsAPIApplication; 21 | import com.kuka.roboticsAPI.controllerModel.Controller; 22 | import com.kuka.roboticsAPI.deviceModel.JointPosition; 23 | import com.kuka.roboticsAPI.deviceModel.LBR; 24 | import com.kuka.roboticsAPI.geometricModel.Frame; 25 | import com.kuka.roboticsAPI.geometricModel.LoadData; 26 | import com.kuka.roboticsAPI.geometricModel.ObjectFrame; 27 | import com.kuka.roboticsAPI.geometricModel.Tool; 28 | import com.kuka.roboticsAPI.geometricModel.math.XyzAbcTransformation; 29 | 30 | 31 | import java.io.IOException; 32 | import java.net.DatagramPacket; 33 | import java.net.DatagramSocket; 34 | import java.net.SocketException; 35 | import java.nio.ByteBuffer; 36 | 37 | /* 38 | * SimulinkIIWA interface, soft real-time control in Cartesian position mode 39 | * Copyright: Mohammad SAFEEA, 22nd/May/2018 40 | * License: MIT license 41 | * 42 | * This is a UDP server that receives motion commands from Simulink. 43 | * This server works together with the Simulink example: (smartDirectServoCartesian.slx) 44 | * 45 | * 46 | * Below you can find quick start instructions, for more info, please refer to the youtube 47 | * video tutorials, a link for which is available in the repository main page. 48 | * 49 | * Hardware setup: 50 | * 1- KUKA iiwa 7R800, modifications are required for the Simulink schematics for 14R820 51 | * 2- External PC 52 | * 3- A network between the PC and the robot, the connector X66 53 | * of the robot shall be used 54 | * 55 | * Required software: 56 | * 1- MATLAB with Simulink, the code was written using MATLAB2018a, it 57 | * is recommended to use the same version. 58 | * 2- Sunrise.Workbench, this program will be used to synchronize the java 59 | * code to the robot. 60 | * 3- The Sunrise code was tested on (KUKA Sunrise.OS 1.11.0.7) 61 | * 62 | * Setup instructions: 63 | * 1- Add this class to a new project using kuka's Sunrsie.Workbench. 64 | * 2- Add reference to the Direct/SmartServo from inside the (StationSetup.cat) file. 65 | * 3- Change the following variables according to your preference: 66 | * * externa_PC_IP: change this variable according to the IP of the PC used for control. 67 | * * TRANSLATION_OF_TOOL: translation of tool center point with respect to flange frame. 68 | * * massOfTool: mass of the tool 69 | * * toolsCOMCoordiantes: coordinates of center of mass of tool with regards to the frame of the flange. 70 | * 71 | * 72 | * Utilization instructions: 73 | * 1- Synchronize the project to the controller of the robot. 74 | * 2- Run this application from the teach-pad of the robot, 75 | * This application has a time out of 10 seconds, if a connection 76 | * is not established during the 10 seconds interval the server will 77 | * turn off. 78 | * 3- Start the Simulink example (smartDirectServoCartesian.slx) 79 | * from the external PC. 80 | * 4- To change the timeout value, change the argument for the instruction 81 | * soc.setSoTimeout(10000), the argument is in milliseconds. 82 | * 83 | * Updated on 11th/June/2019 84 | * Bug fix (the bug happened only in 14R820) 85 | * To fix the bug hard-coding of the Cartesian home position was performed. 86 | * 87 | */ 88 | 89 | public class SimulinkIIWASmartServoCartesian extends RoboticsAPIApplication 90 | { 91 | private LBR _lbr; 92 | 93 | 94 | public static double EEfServoPos[]; 95 | 96 | public static boolean loopFlag; 97 | 98 | 99 | public static double updateCycleJointPos[]; 100 | private UDPServer udpServer; 101 | // Tool Data 102 | private Tool _toolAttachedToLBR; 103 | private LoadData _loadData; 104 | private static final String TOOL_FRAME = "toolFrame"; 105 | private static final double[] TRANSLATION_OF_TOOL = { 0, 0, 0 }; 106 | private static final double MASS = 0.4; 107 | private static final double[] CENTER_OF_MASS_IN_MILLIMETER = { 0, 0, 75 }; 108 | 109 | private static final int MILLI_SLEEP_TO_EMULATE_COMPUTATIONAL_EFFORT = 1; 110 | 111 | 112 | 113 | @Override 114 | public void initialize() 115 | { 116 | _lbr = getContext().getDeviceFromType(LBR.class); 117 | EEfServoPos=new double[6]; 118 | for(int i=0;i<6;i++) 119 | { 120 | EEfServoPos[i]=0; 121 | } 122 | loopFlag=true; 123 | addLoadData(); 124 | } 125 | 126 | private void addLoadData() 127 | { 128 | // Create a Tool by Hand this is the tool we want to move with some mass 129 | // properties and a TCP-Z-offset of 100. 130 | _loadData = new LoadData(); 131 | _loadData.setMass(MASS); 132 | _loadData.setCenterOfMass( 133 | CENTER_OF_MASS_IN_MILLIMETER[0], CENTER_OF_MASS_IN_MILLIMETER[1], 134 | CENTER_OF_MASS_IN_MILLIMETER[2]); 135 | _toolAttachedToLBR = new Tool("Tool", _loadData); 136 | 137 | XyzAbcTransformation trans = XyzAbcTransformation.ofTranslation( 138 | TRANSLATION_OF_TOOL[0], TRANSLATION_OF_TOOL[1], 139 | TRANSLATION_OF_TOOL[2]); 140 | ObjectFrame aTransformation = _toolAttachedToLBR.addChildFrame(TOOL_FRAME 141 | + "(TCP)", trans); 142 | _toolAttachedToLBR.setDefaultMotionFrame(aTransformation); 143 | // Attach tool to the robot 144 | _toolAttachedToLBR.attachTo(_lbr.getFlange()); 145 | } 146 | 147 | /** 148 | * Move to an initial Position WARNING: MAKE SURE, THAT the pose is collision free. 149 | */ 150 | private void moveToInitialPosition() 151 | { 152 | _lbr.move( 153 | ptp(0., Math.PI / 180 * 30.,0.,-Math.PI / 180 * 80.,0.,Math.PI / 180 * 70., 0.).setJointVelocityRel(0.15)); 154 | // Bug fix (the bug happened only in 14R820) 155 | // Hard-code the Cartesian home position. 156 | Frame daframe= new Frame(575.87,0.05,397.56,Math.PI,0,Math.PI); 157 | _lbr.move( 158 | lin(daframe).setJointVelocityRel(0.15)); 159 | /* Note: The Validation itself justifies, that in this very time instance, the load parameter setting was 160 | * sufficient. This does not mean by far, that the parameter setting is valid in the sequel or lifetime of this 161 | * program */ 162 | try 163 | { 164 | 165 | } 166 | catch (IllegalStateException e) 167 | { 168 | getLogger().info("Omitting validation failure for this sample\n" 169 | + e.getMessage()); 170 | } 171 | } 172 | 173 | 174 | /** 175 | * Main Application Routine 176 | */ 177 | @Override 178 | public void run() 179 | { 180 | moveToInitialPosition(); 181 | 182 | System.out.print("You have ten seconds to establish a connection with iiwa"); 183 | int portTemp=30002; 184 | udpServer=new UDPServer(portTemp); 185 | smartServoStartCartezian(); 186 | 187 | } 188 | 189 | 190 | private void smartServoStartCartezian() { 191 | 192 | boolean doDebugPrints = false; 193 | 194 | SmartServo aSmartServoMotion = new SmartServo( 195 | _lbr.getCurrentJointPosition()); 196 | 197 | aSmartServoMotion.useTrace(true); 198 | 199 | aSmartServoMotion.setMinimumTrajectoryExecutionTime(5e-3); 200 | 201 | getLogger().info("Starting SmartServo motion in position control mode"); 202 | _toolAttachedToLBR.moveAsync(aSmartServoMotion); 203 | 204 | getLogger().info("Get the runtime of the SmartServo motion"); 205 | ISmartServoRuntime theServoRuntime = aSmartServoMotion 206 | .getRuntime(); 207 | 208 | Frame aFrame = theServoRuntime.getCurrentCartesianDestination(_toolAttachedToLBR.getDefaultMotionFrame()); 209 | Frame destFrame = aFrame.copyWithRedundancy(); 210 | // Initiate the initial position 211 | EEfServoPos[0]=aFrame.getX(); 212 | EEfServoPos[1]=aFrame.getY(); 213 | EEfServoPos[2]=aFrame.getZ(); 214 | EEfServoPos[3]=aFrame.getAlphaRad(); 215 | EEfServoPos[4]=aFrame.getBetaRad(); 216 | EEfServoPos[5]=aFrame.getGammaRad(); 217 | 218 | 219 | try 220 | { 221 | // do a cyclic loop 222 | // Do some timing... 223 | // in nanosec 224 | while(loopFlag) 225 | { 226 | // Insert your code here 227 | // e.g Visual Servoing or the like 228 | 229 | // Synchronize with the realtime system 230 | theServoRuntime.updateWithRealtimeSystem(); 231 | 232 | // Get the measured position 233 | Frame msrPose = theServoRuntime 234 | .getCurrentCartesianDestination(_lbr.getFlange()); 235 | 236 | if (doDebugPrints) 237 | { 238 | getLogger().info("Current cartesian goal " + aFrame); 239 | getLogger().info("Current joint destination " 240 | + theServoRuntime.getCurrentJointDestination()); 241 | } 242 | 243 | // Do some Computation 244 | // emulate some computational effort - or waiting for external 245 | // stuff 246 | ThreadUtil.milliSleep(MILLI_SLEEP_TO_EMULATE_COMPUTATIONAL_EFFORT); 247 | 248 | // compute a new commanded position 249 | for(int kk=0;kk<6;kk++) 250 | { 251 | EEfServoPos[kk]=udpServer.EEFpos[kk]; 252 | } 253 | // update Cartesian positions 254 | destFrame.setX(EEfServoPos[0]); 255 | destFrame.setY(EEfServoPos[1]); 256 | destFrame.setZ(EEfServoPos[2]); 257 | destFrame.setAlphaRad(EEfServoPos[3]); 258 | destFrame.setBetaRad(EEfServoPos[4]); 259 | destFrame.setGammaRad(EEfServoPos[5]); 260 | 261 | if (doDebugPrints) 262 | { 263 | getLogger().info("New cartesian goal " + destFrame); 264 | getLogger().info("LBR position " 265 | + _lbr.getCurrentCartesianPosition(_lbr 266 | .getFlange())); 267 | getLogger().info("Measured cartesian pose from runtime " 268 | + msrPose); 269 | } 270 | 271 | theServoRuntime.setDestination(destFrame); 272 | } 273 | } 274 | catch (Exception e) 275 | { 276 | getLogger().error(e.toString()); 277 | e.printStackTrace(); 278 | } 279 | 280 | //Print statistics and parameters of the motion 281 | getLogger().info("Simple cartesian test " + theServoRuntime.toString()); 282 | 283 | getLogger().info("Stop the SmartServo motion"); 284 | theServoRuntime.stopMotion(); 285 | } 286 | 287 | 288 | /** 289 | * Main routine, which starts the application 290 | */ 291 | public static void main(String[] args) 292 | { 293 | SimulinkIIWASmartServoCartesian app = new SimulinkIIWASmartServoCartesian(); 294 | app.runApplication(); 295 | } 296 | 297 | public class UDPServer implements Runnable{ 298 | 299 | public double[] EEFpos={575.87,0.05,397.56,Math.PI,0,Math.PI}; 300 | 301 | double stime=0; 302 | double endtime=0; 303 | 304 | int _port; 305 | int vectorSize=6; 306 | int packetCounter=0; 307 | 308 | byte[] buffer=new byte[8*7]; 309 | UDPServer(int port) 310 | { 311 | Thread t=new Thread(this); 312 | t.setDaemon(true); 313 | t.start(); 314 | _port=port; 315 | } 316 | 317 | public void run() 318 | { 319 | System.out.println("Program will terminate if comuncation is not established in 10 seconds"); 320 | DatagramSocket soc=null; 321 | try { 322 | soc = new DatagramSocket(_port); 323 | soc.setSoTimeout(10000); // 5 seconds, if a time out occurred, turn off program 324 | DatagramPacket response=new DatagramPacket(buffer, buffer.length); 325 | while(true) 326 | { 327 | soc.receive(response); 328 | packetCounter=packetCounter+1; 329 | // String s= new String(buffer,0,response.getLength()) 330 | // System.out.println(response.getLength()); 331 | if(response.getLength()==8*vectorSize) 332 | { 333 | if(packetCounter==1) 334 | { 335 | stime=System.currentTimeMillis(); 336 | } 337 | else 338 | { 339 | endtime=System.currentTimeMillis(); 340 | } 341 | byte[] daB=new byte[8]; 342 | int counter=0; 343 | int index=0; 344 | while(counter <8*vectorSize) 345 | { 346 | for(int i=0;i<8;i++) 347 | { 348 | daB[7-i]=buffer[counter]; 349 | counter=counter+1; 350 | } 351 | EEFpos[index]=bytesToDouble(daB); 352 | index=index+1; 353 | //System.out.println(bytesToDouble(daB)); 354 | } 355 | } 356 | } 357 | } catch (SocketException e) { 358 | // TODO Auto-generated catch block 359 | e.printStackTrace(); 360 | //System.out.println(e.toString()); 361 | } catch (IOException e) { 362 | // TODO Auto-generated catch block 363 | e.printStackTrace(); 364 | System.out.println(e.toString()); 365 | } 366 | if(soc==null) 367 | {} 368 | else 369 | { 370 | if(soc.isClosed()) 371 | {} 372 | else 373 | { 374 | soc.close(); 375 | } 376 | } 377 | SimulinkIIWASmartServoCartesian.loopFlag=false; //Turn off main loop 378 | double time=(endtime-stime)/1000; 379 | double rate=packetCounter/time; 380 | System.out.println("update rate, packets/second"); 381 | System.out.println(rate); 382 | } 383 | } 384 | 385 | 386 | public double bytesToDouble(byte[] b) 387 | { 388 | return ByteBuffer.wrap(b).getDouble(); 389 | } 390 | 391 | } 392 | -------------------------------------------------------------------------------- /Cartesian motion open loop/simulink/smartDirectServoCartesian.slx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Modi1987/Simulink-iiwa-interface/5d0b6c1a84492946bbb26aa03efa3ab85d25d530/Cartesian motion open loop/simulink/smartDirectServoCartesian.slx -------------------------------------------------------------------------------- /Cartesian motion open loop/simulink/smartDirectServoCartesian.slxc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Modi1987/Simulink-iiwa-interface/5d0b6c1a84492946bbb26aa03efa3ab85d25d530/Cartesian motion open loop/simulink/smartDirectServoCartesian.slxc -------------------------------------------------------------------------------- /Cartesian motion with feedback/iiwa/SimulinkFIIWADirectServoCartesian.java: -------------------------------------------------------------------------------- 1 | package lbrExampleApplications; 2 | 3 | 4 | import static com.kuka.roboticsAPI.motionModel.BasicMotions.lin; 5 | import static com.kuka.roboticsAPI.motionModel.BasicMotions.ptp; 6 | 7 | import java.io.IOException; 8 | import java.net.ServerSocket; 9 | import java.net.Socket; 10 | import java.util.Scanner; 11 | import java.util.StringTokenizer; 12 | import java.util.logging.Logger; 13 | 14 | import com.kuka.connectivity.motionModel.directServo.DirectServo; 15 | import com.kuka.connectivity.motionModel.directServo.IDirectServoRuntime; 16 | import com.kuka.generated.ioAccess.MediaFlangeIOGroup; 17 | import com.kuka.roboticsAPI.applicationModel.RoboticsAPIApplication; 18 | import com.kuka.roboticsAPI.controllerModel.Controller; 19 | import com.kuka.roboticsAPI.deviceModel.JointPosition; 20 | import com.kuka.roboticsAPI.deviceModel.LBR; 21 | import com.kuka.roboticsAPI.geometricModel.Frame; 22 | import com.kuka.roboticsAPI.geometricModel.World; 23 | import com.kuka.roboticsAPI.sensorModel.ForceSensorData; 24 | import com.kuka.roboticsAPI.sensorModel.TorqueSensorData; 25 | 26 | 27 | import java.io.IOException; 28 | import java.net.DatagramPacket; 29 | import java.net.DatagramSocket; 30 | import java.net.InetAddress; 31 | import java.net.SocketException; 32 | import java.net.UnknownHostException; 33 | import java.nio.ByteBuffer; 34 | 35 | /* 36 | * SimulinkIIWA interface, soft real-time control in Cartesian position mode 37 | * Copyright: Mohammad SAFEEA, 22nd/May/2018 38 | * License: MIT license 39 | * 40 | * This is a UDP server that receives motion commands from Simulink. 41 | * This server works together with the Simulink example: (feedBackSmartDirectServoCartesian1.slx) 42 | * 43 | * Also this script returns back a feedback about the (1)actual joints positions, 44 | * (2)the external torques and (3)the measured torques of all the joints to Simulink. 45 | * (4) EEF force moment. 46 | * 47 | * Below you can find quick start instructions, for more info, please refer to the youtube 48 | * video tutorials, a link for which is available in the repository main page. 49 | * 50 | * Hardware setup: 51 | * 1- KUKA iiwa 7R800 only, modifications in Simulink schematic are required for 14R820 52 | * 2- External PC 53 | * 3- A network between the PC and the robot, the connector X66 54 | * of the robot shall be used 55 | * 56 | * Required software: 57 | * 1- MATLAB with Simulink, the code was written using MATLAB2018a, it 58 | * is recommended to use the same version. 59 | * 2- Sunrise.Workbench, this program will be used to synchronize the java 60 | * code to the robot. 61 | * 3- The Sunrise code was tested on (KUKA Sunrise.OS 1.11.0.7) 62 | * 63 | * Setup instructions: 64 | * 1- Add this class to a new project using kuka's Sunrsie.Workbench. 65 | * 2- Add reference to the Direct/SmartServo from inside the (StationSetup.cat) file. 66 | * 3- Change the following variables according to preference: 67 | * * externa_PC_IP: change this variable according to the IP of the PC used for control. 68 | * 69 | * 70 | * Utilization instructions: 71 | * 1- Synchronize the project to the controller of the robot. 72 | * 2- Run this application from the teach-pad of the robot, 73 | * This application has a time out of 10 seconds, if a connection 74 | * is not established during the 10 seconds interval the server will 75 | * turn off. 76 | * 3- Start the Simulink example (feedBackSmartDirectServoCartesian1.slx) 77 | * from the external PC. 78 | * 4- To change the timeout value, change the argument for the instruction 79 | * soc.setSoTimeout(10000), the argument is in milliseconds. 80 | * 81 | * Updated on 11th/June/2019 82 | * Bug fix (the bug happened only in 14R820) 83 | * To fix the bug hard-coding of the Cartesian home position was performed. 84 | * 85 | */ 86 | 87 | 88 | public class SimulinkFIIWADirectServoCartesian extends RoboticsAPIApplication 89 | { 90 | 91 | private static String externa_PC_IP="172.31.69.55"; 92 | private LBR _lbr; 93 | public static double EEfServoPos[]; 94 | public static boolean loopFlag; 95 | public static double jDispMax[]; 96 | public static double updateCycleJointPos[]; 97 | private UDPServer udpServer; 98 | int _port; 99 | 100 | ServerSocket ss; 101 | Socket soc; 102 | 103 | 104 | 105 | 106 | @Override 107 | public void initialize() 108 | { 109 | _lbr = getContext().getDeviceFromType(LBR.class); 110 | EEfServoPos=new double[6]; 111 | for(int i=0;i<6;i++) 112 | { 113 | EEfServoPos[i]=0; 114 | } 115 | loopFlag=true; 116 | } 117 | 118 | /** 119 | * Move to an initial Position WARNING: MAKE SURE, THAT the pose is collision free. 120 | */ 121 | private void moveToInitialPosition() 122 | { 123 | _lbr.move( 124 | ptp(0., Math.PI / 180 * 30.,0.,-Math.PI / 180 * 80.,0.,Math.PI / 180 * 70., 0.).setJointVelocityRel(0.15)); 125 | // Bug fix (the bug happened only in 14R820) 126 | // Newly added, hard-coding of the Cartesian home position. 127 | Frame daframe= new Frame(575.87,0.05,397.56,Math.PI,0,Math.PI); 128 | _lbr.move( 129 | lin(daframe).setJointVelocityRel(0.15)); 130 | /* Note: The Validation itself justifies, that in this very time instance, the load parameter setting was 131 | * sufficient. This does not mean by far, that the parameter setting is valid in the sequel or lifetime of this 132 | * program */ 133 | try 134 | { 135 | 136 | } 137 | catch (IllegalStateException e) 138 | { 139 | getLogger().info("Omitting validation failure for this sample\n" 140 | + e.getMessage()); 141 | } 142 | } 143 | 144 | 145 | /** 146 | * Main Application Routine 147 | */ 148 | @Override 149 | public void run() 150 | { 151 | moveToInitialPosition(); 152 | 153 | System.out.print("You have ten seconds to establish a connection with iiwa"); 154 | int portTemp=30002; 155 | udpServer=new UDPServer(portTemp); 156 | new UDPPublisher(externa_PC_IP); 157 | directServoStartCartezian(); 158 | 159 | } 160 | 161 | 162 | private void directServoStartCartezian() { 163 | 164 | boolean doDebugPrints = false; 165 | 166 | DirectServo aDirectServoMotion = new DirectServo( 167 | _lbr.getCurrentJointPosition()); 168 | 169 | aDirectServoMotion.setMinimumTrajectoryExecutionTime(40e-3); 170 | 171 | getLogger().info("Starting DirectServo motion in position control mode"); 172 | _lbr.moveAsync(aDirectServoMotion); 173 | 174 | getLogger().info("Get the runtime of the DirectServo motion"); 175 | IDirectServoRuntime theDirectServoRuntime = aDirectServoMotion 176 | .getRuntime(); 177 | 178 | Frame aFrame = theDirectServoRuntime.getCurrentCartesianDestination(_lbr.getFlange()); 179 | Frame destFrame = aFrame.copyWithRedundancy(); 180 | // Initiate the initial position 181 | EEfServoPos[0]=aFrame.getX(); 182 | EEfServoPos[1]=aFrame.getY(); 183 | EEfServoPos[2]=aFrame.getZ(); 184 | EEfServoPos[3]=aFrame.getAlphaRad(); 185 | EEfServoPos[4]=aFrame.getBetaRad(); 186 | EEfServoPos[5]=aFrame.getGammaRad(); 187 | 188 | try 189 | { 190 | // do a cyclic loop 191 | // Do some timing... 192 | // in nanosec 193 | 194 | while(loopFlag==true) 195 | { 196 | 197 | // /////////////////////////////////////////////////////// 198 | // Insert your code here 199 | // e.g Visual Servoing or the like 200 | // Synchronize with the realtime system 201 | theDirectServoRuntime.updateWithRealtimeSystem(); 202 | Frame msrPose = theDirectServoRuntime 203 | .getCurrentCartesianDestination(_lbr.getFlange()); 204 | 205 | if (doDebugPrints) 206 | { 207 | getLogger().info("Current cartesian goal " + aFrame); 208 | getLogger().info("Current joint destination " 209 | + theDirectServoRuntime.getCurrentJointDestination()); 210 | } 211 | 212 | Thread.sleep(1); 213 | 214 | for(int kk=0;kk<6;kk++) 215 | { 216 | EEfServoPos[kk]=udpServer.EEFpos[kk]; 217 | } 218 | // update Cartesian positions 219 | destFrame.setX(EEfServoPos[0]); 220 | destFrame.setY(EEfServoPos[1]); 221 | destFrame.setZ(EEfServoPos[2]); 222 | destFrame.setAlphaRad(EEfServoPos[3]); 223 | destFrame.setBetaRad(EEfServoPos[4]); 224 | destFrame.setGammaRad(EEfServoPos[5]); 225 | 226 | if (doDebugPrints) 227 | { 228 | getLogger().info("New cartesian goal " + destFrame); 229 | getLogger().info("LBR position " 230 | + _lbr.getCurrentCartesianPosition(_lbr 231 | .getFlange())); 232 | getLogger().info("Measured cartesian pose from runtime " 233 | + msrPose); 234 | } 235 | 236 | theDirectServoRuntime.setDestination(destFrame); 237 | 238 | } 239 | } 240 | catch (Exception e) 241 | { 242 | getLogger().info(e.getLocalizedMessage()); 243 | e.printStackTrace(); 244 | //Print statistics and parameters of the motion 245 | getLogger().info("Simple Cartesian Test \n" + theDirectServoRuntime.toString()); 246 | 247 | getLogger().info("Stop the DirectServo motion"); 248 | 249 | } 250 | theDirectServoRuntime.stopMotion(); 251 | getLogger().info("Stop the DirectServo motion for the stop instruction was sent"); 252 | 253 | } 254 | 255 | 256 | /** 257 | * Main routine, which starts the application 258 | */ 259 | public static void main(String[] args) 260 | { 261 | SimulinkFIIWADirectServoCartesian app = new SimulinkFIIWADirectServoCartesian(); 262 | app.runApplication(); 263 | } 264 | 265 | public class UDPServer implements Runnable{ 266 | 267 | public double[] EEFpos={575.87,0.05,397.56,Math.PI,0,Math.PI}; 268 | 269 | double stime=0; 270 | double endtime=0; 271 | 272 | int _port; 273 | int vectorSize=6; 274 | int packetCounter=0; 275 | 276 | byte[] buffer=new byte[8*7]; 277 | UDPServer(int port) 278 | { 279 | Thread t=new Thread(this); 280 | t.setDaemon(true); 281 | t.start(); 282 | _port=port; 283 | } 284 | 285 | public void run() 286 | { 287 | System.out.println("Program will terminate if comuncation is not established in 10 seconds"); 288 | DatagramSocket soc=null; 289 | try { 290 | soc = new DatagramSocket(_port); 291 | soc.setSoTimeout(10000); // 10 seconds, if a time out occurred, turn off program 292 | DatagramPacket response=new DatagramPacket(buffer, buffer.length); 293 | while(true) 294 | { 295 | soc.receive(response); 296 | packetCounter=packetCounter+1; 297 | // String s= new String(buffer,0,response.getLength()) 298 | // System.out.println(response.getLength()); 299 | if(response.getLength()==8*vectorSize) 300 | { 301 | if(packetCounter==1) 302 | { 303 | stime=System.currentTimeMillis(); 304 | } 305 | else 306 | { 307 | endtime=System.currentTimeMillis(); 308 | } 309 | byte[] daB=new byte[8]; 310 | int counter=0; 311 | int index=0; 312 | while(counter <8*vectorSize) 313 | { 314 | for(int i=0;i<8;i++) 315 | { 316 | daB[7-i]=buffer[counter]; 317 | counter=counter+1; 318 | } 319 | EEFpos[index]=bytesToDouble(daB); 320 | index=index+1; 321 | //System.out.println(bytesToDouble(daB)); 322 | } 323 | } 324 | } 325 | } catch (SocketException e) { 326 | // TODO Auto-generated catch block 327 | e.printStackTrace(); 328 | //System.out.println(e.toString()); 329 | } catch (IOException e) { 330 | // TODO Auto-generated catch block 331 | e.printStackTrace(); 332 | System.out.println(e.toString()); 333 | } 334 | if(soc==null) 335 | {} 336 | else 337 | { 338 | if(soc.isClosed()) 339 | {} 340 | else 341 | { 342 | soc.close(); 343 | } 344 | } 345 | SimulinkFIIWADirectServoCartesian.loopFlag=false; //Turn off main loop 346 | double time=(endtime-stime)/1000; 347 | double rate=packetCounter/time; 348 | System.out.println("update rate, packets/second"); 349 | System.out.println(rate); 350 | } 351 | } 352 | 353 | 354 | public class UDPPublisher implements Runnable{ 355 | 356 | String ip; 357 | UDPPublisher(String ip) 358 | { 359 | this.ip=ip; 360 | Thread t=new Thread(this); 361 | t.setDaemon(true); 362 | t.start(); 363 | } 364 | public void run() 365 | { 366 | broadcastData(); 367 | } 368 | private void broadcastData() { 369 | // TODO Auto-generated method stub 370 | String message="Publishing robot state using UDP at port 30008 to destination: "+ ip; 371 | System.out.println(message); 372 | message="Streamed message: 7 actual joint angles, 7 measured torques, 7 external torques,"; 373 | System.out.println(message); 374 | message="State streaming frequency is approximatly at 25HZ"; 375 | System.out.println(message); 376 | int port=30008; 377 | InetAddress host; 378 | int numOfDoubles=7+7+7+6;// 7 actual joint angles, 7 measured torques, 7 external torques, 7 force at flange 379 | double[] doubleVals=new double[numOfDoubles]; 380 | 381 | byte[] buffer= new byte[8*numOfDoubles]; 382 | 383 | try { 384 | host = InetAddress.getByName(ip); 385 | DatagramSocket soc=new DatagramSocket(); 386 | while(loopFlag) 387 | { 388 | // Feedback of actual joints positions 389 | JointPosition jpos=_lbr.getCurrentJointPosition(); 390 | int valsCounter=0; 391 | for(int i=0;i<7;i++) 392 | { 393 | doubleVals[valsCounter]=jpos.get(i); 394 | valsCounter=valsCounter+1; 395 | } 396 | // Feedback of measured torques 397 | double[] m_tor=_lbr.getMeasuredTorque().getTorqueValues(); 398 | for(int i=0;i<7;i++) 399 | { 400 | doubleVals[valsCounter]=m_tor[i]; 401 | valsCounter=valsCounter+1; 402 | } 403 | // Feedback of external torques 404 | double[] ex_tor=_lbr.getExternalTorque().getTorqueValues(); 405 | for(int i=0;i<7;i++) 406 | { 407 | doubleVals[valsCounter]=ex_tor[i]; 408 | valsCounter=valsCounter+1; 409 | } 410 | // Feedback of forces at flange 411 | ForceSensorData f_at_flange=_lbr.getExternalForceTorque(_lbr.getFlange(),World.Current.getRootFrame()); 412 | doubleVals[valsCounter]=f_at_flange.getForce().getX(); 413 | valsCounter=valsCounter+1; 414 | doubleVals[valsCounter]=f_at_flange.getForce().getY(); 415 | valsCounter=valsCounter+1; 416 | doubleVals[valsCounter]=f_at_flange.getForce().getZ(); 417 | valsCounter=valsCounter+1; 418 | doubleVals[valsCounter]=f_at_flange.getTorque().getX(); 419 | valsCounter=valsCounter+1; 420 | doubleVals[valsCounter]=f_at_flange.getTorque().getY(); 421 | valsCounter=valsCounter+1; 422 | doubleVals[valsCounter]=f_at_flange.getTorque().getZ(); 423 | valsCounter=valsCounter+1; 424 | 425 | int count=0; 426 | for(int i=0;ijDispMax[k]) 224 | { 225 | jDispMax[k]=absDisp; 226 | } 227 | destination.set(k, temp); 228 | updateCycleJointPos[k]=temp; 229 | 230 | } 231 | 232 | /*terminateFlag=checkCollisionWithEEF(updateCycleJointPos); 233 | if(terminateFlag==true) 234 | { 235 | dabak.terminateBool=true; 236 | dabak.soc.close(); 237 | dabak.ss.close(); 238 | 239 | break; 240 | }*/ 241 | 242 | theDirectServoRuntime.setDestination(destination); 243 | 244 | } 245 | } 246 | catch (Exception e) 247 | { 248 | getLogger().info(e.getLocalizedMessage()); 249 | e.printStackTrace(); 250 | //Print statistics and parameters of the motion 251 | getLogger().info("Simple Cartesian Test \n" + theDirectServoRuntime.toString()); 252 | 253 | getLogger().info("Stop the DirectServo motion"); 254 | 255 | 256 | } 257 | theDirectServoRuntime.stopMotion(); 258 | 259 | } 260 | 261 | 262 | double getTheDisplacment(double dj) 263 | { 264 | 265 | return dj; 266 | /* 267 | double a=0.07; 268 | double b=a*0.75; 269 | double exponenet=-Math.pow(dj/b, 2); 270 | return Math.signum(dj)*a*(1-Math.exp(exponenet)); 271 | */ 272 | } 273 | 274 | 275 | /** 276 | * Main routine, which starts the application 277 | */ 278 | public static void main(String[] args) 279 | { 280 | SimulinkIIWADirectServoJoints app = new SimulinkIIWADirectServoJoints(); 281 | app.runApplication(); 282 | } 283 | 284 | public class UDPServer implements Runnable{ 285 | 286 | public double[] jpos=new double[7]; 287 | 288 | double stime=0; 289 | double endtime=0; 290 | 291 | int _port; 292 | int vectorSize=7; 293 | int packetCounter=0; 294 | 295 | byte[] buffer=new byte[8*vectorSize]; 296 | UDPServer(int port) 297 | { 298 | for(int i=0;i<7;i++) 299 | { 300 | jpos[i]=0; 301 | } 302 | Thread t=new Thread(this); 303 | t.setDaemon(true); 304 | t.start(); 305 | _port=port; 306 | } 307 | 308 | public void run() 309 | { 310 | System.out.println("Program will terminate if comuncation is not established in 10 seconds"); 311 | DatagramSocket soc=null; 312 | try { 313 | soc = new DatagramSocket(_port); 314 | soc.setSoTimeout(10000); // 5 seconds, if a time out occurred, turn off program 315 | DatagramPacket response=new DatagramPacket(buffer, buffer.length); 316 | while(true) 317 | { 318 | soc.receive(response); 319 | packetCounter=packetCounter+1; 320 | // String s= new String(buffer,0,response.getLength()) 321 | // System.out.println(response.getLength()); 322 | if(response.getLength()==8*vectorSize) 323 | { 324 | if(packetCounter==1) 325 | { 326 | stime=System.currentTimeMillis(); 327 | } 328 | else 329 | { 330 | endtime=System.currentTimeMillis(); 331 | } 332 | byte[] daB=new byte[8]; 333 | int counter=0; 334 | int jointNum=0; 335 | while(counter <8*vectorSize) 336 | { 337 | for(int i=0;i<8;i++) 338 | { 339 | daB[7-i]=buffer[counter]; 340 | counter=counter+1; 341 | } 342 | jpos[jointNum]=bytesToDouble(daB); 343 | jointNum=jointNum+1; 344 | //System.out.println(bytesToDouble(daB)); 345 | } 346 | } 347 | } 348 | } catch (SocketException e) { 349 | // TODO Auto-generated catch block 350 | e.printStackTrace(); 351 | //System.out.println(e.toString()); 352 | } catch (IOException e) { 353 | // TODO Auto-generated catch block 354 | e.printStackTrace(); 355 | System.out.println(e.toString()); 356 | } 357 | if(soc==null) 358 | {} 359 | else 360 | { 361 | if(soc.isClosed()) 362 | {} 363 | else 364 | { 365 | soc.close(); 366 | } 367 | } 368 | SimulinkIIWADirectServoJoints.loopFlag=false; //Turn off main loop 369 | double time=(endtime-stime)/1000; 370 | double rate=packetCounter/time; 371 | System.out.println("update rate, packets/second"); 372 | System.out.println(rate); 373 | } 374 | } 375 | 376 | 377 | public double bytesToDouble(byte[] b) 378 | { 379 | return ByteBuffer.wrap(b).getDouble(); 380 | } 381 | 382 | } 383 | -------------------------------------------------------------------------------- /Joint space motion open loop/iiwa/SimulinkIIWASmartServoJoints.java: -------------------------------------------------------------------------------- 1 | package lbrExampleApplications; 2 | 3 | 4 | import static com.kuka.roboticsAPI.motionModel.BasicMotions.ptp; 5 | 6 | import java.io.IOException; 7 | import java.net.ServerSocket; 8 | import java.net.Socket; 9 | import java.util.Scanner; 10 | import java.util.StringTokenizer; 11 | import java.util.logging.Logger; 12 | 13 | import com.kuka.common.StatisticTimer; 14 | import com.kuka.common.ThreadUtil; 15 | import com.kuka.common.StatisticTimer.OneTimeStep; 16 | import com.kuka.connectivity.motionModel.directServo.DirectServo; 17 | import com.kuka.connectivity.motionModel.directServo.IDirectServoRuntime; 18 | import com.kuka.connectivity.motionModel.smartServo.ISmartServoRuntime; 19 | import com.kuka.connectivity.motionModel.smartServo.SmartServo; 20 | import com.kuka.generated.ioAccess.MediaFlangeIOGroup; 21 | import com.kuka.roboticsAPI.applicationModel.RoboticsAPIApplication; 22 | import com.kuka.roboticsAPI.controllerModel.Controller; 23 | import com.kuka.roboticsAPI.deviceModel.JointPosition; 24 | import com.kuka.roboticsAPI.deviceModel.LBR; 25 | import com.kuka.roboticsAPI.geometricModel.Frame; 26 | import com.kuka.roboticsAPI.geometricModel.LoadData; 27 | import com.kuka.roboticsAPI.geometricModel.ObjectFrame; 28 | import com.kuka.roboticsAPI.geometricModel.Tool; 29 | import com.kuka.roboticsAPI.geometricModel.math.XyzAbcTransformation; 30 | 31 | 32 | import java.io.IOException; 33 | import java.net.DatagramPacket; 34 | import java.net.DatagramSocket; 35 | import java.net.SocketException; 36 | import java.nio.ByteBuffer; 37 | 38 | import lbrExampleApplications.SimulinkIIWADirectServoJoints.UDPServer; 39 | 40 | /* 41 | * SimulinkIIWA interface, soft real-time control in joint position mode 42 | * Copyright: Mohammad SAFEEA, 22nd/May/2018 43 | * License: MIT license 44 | * 45 | * This is a UDP server that receives motion commands from Simulink. 46 | * This server works together with the Simulink example: (smartDirectServoJoints.slx) 47 | * 48 | * Below you can find quick start instructions, for more info, please refer to the youtube 49 | * video tutorials, a link for which is available in the repository main page. 50 | * 51 | * Hardware setup: 52 | * 1- KUKA iiwa 7R800 or 14R820 53 | * 2- External PC 54 | * 3- A network between the PC and the robot, the connector X66 55 | * of the robot shall be used 56 | * 57 | * Required software: 58 | * 1- MATLAB with Simulink, the code was written using MATLAB2018a, it 59 | * is recommended to use the same version. 60 | * 2- Sunrise.Workbench, this program will be used to synchronize the java 61 | * code to the robot. 62 | * 3- The Sunrise code was tested on (KUKA Sunrise.OS 1.11.0.7) 63 | * 64 | * Setup instructions: 65 | * 1- Add this class to a new project using kuka's Sunrsie.Workbench. 66 | * 2- Add reference to the Direct/SmartServo from inside the (StationSetup.cat) file. 67 | * 3- Change the following variables according to your preference: 68 | * * externa_PC_IP: change this variable according to the IP of the PC used for control. 69 | * * TRANSLATION_OF_TOOL: translation of tool center point with respect to flange frame. 70 | * * massOfTool: mass of the tool 71 | * * toolsCOMCoordiantes: coordinates of center of mass of tool with regards to the frame of the flange. 72 | * 73 | * 74 | * Utilization instructions: 75 | * 1- Synchronize the project to the controller of the robot. 76 | * 2- Run this application from the teach-pad of the robot, 77 | * This application has a time out of 10 seconds, if a connection 78 | * is not established during the 10 seconds interval the server will 79 | * turn off. 80 | * 3- Start the Simulink example (smartDirectServoJoints.slx) 81 | * from the external PC. 82 | * 4- To change the timeout value, change the argument for the instruction 83 | * soc.setSoTimeout(10000), the argument is in milliseconds. 84 | * 85 | */ 86 | 87 | 88 | public class SimulinkIIWASmartServoJoints extends RoboticsAPIApplication 89 | { 90 | private LBR _lbr; 91 | private ISmartServoRuntime _theSmartServoRuntime = null; 92 | 93 | // Tool Data 94 | private Tool _toolAttachedToLBR; 95 | private LoadData _loadData; 96 | private static final String TOOL_FRAME = "toolFrame"; 97 | private static final double[] TRANSLATION_OF_TOOL = { 0, 0, 100 }; 98 | private static final double MASS = 0.4; 99 | private static final double[] CENTER_OF_MASS_IN_MILLIMETER = { 0, 0, 100 }; 100 | 101 | private int _count = 0; 102 | 103 | private static final int MILLI_SLEEP_TO_EMULATE_COMPUTATIONAL_EFFORT = 1; 104 | private int _steps = 0; 105 | 106 | public static boolean loopFlag; 107 | public static double[] jpos; 108 | private UDPServer udpServer; 109 | 110 | 111 | 112 | 113 | 114 | @Override 115 | public void initialize() 116 | { 117 | _lbr = getContext().getDeviceFromType(LBR.class); 118 | jpos=new double[7]; 119 | for(int i=0;i<7;i++) 120 | { 121 | jpos[i]=0; 122 | } 123 | loopFlag=true; 124 | addLoadData(); 125 | } 126 | 127 | private void addLoadData() 128 | { 129 | // Create a Tool by Hand this is the tool we want to move with some mass 130 | // properties and a TCP-Z-offset of 100. 131 | _loadData = new LoadData(); 132 | _loadData.setMass(MASS); 133 | _loadData.setCenterOfMass( 134 | CENTER_OF_MASS_IN_MILLIMETER[0], CENTER_OF_MASS_IN_MILLIMETER[1], 135 | CENTER_OF_MASS_IN_MILLIMETER[2]); 136 | _toolAttachedToLBR = new Tool("Tool", _loadData); 137 | 138 | XyzAbcTransformation trans = XyzAbcTransformation.ofTranslation( 139 | TRANSLATION_OF_TOOL[0], TRANSLATION_OF_TOOL[1], 140 | TRANSLATION_OF_TOOL[2]); 141 | ObjectFrame aTransformation = _toolAttachedToLBR.addChildFrame(TOOL_FRAME 142 | + "(TCP)", trans); 143 | _toolAttachedToLBR.setDefaultMotionFrame(aTransformation); 144 | // Attach tool to the robot 145 | _toolAttachedToLBR.attachTo(_lbr.getFlange()); 146 | } 147 | 148 | /** 149 | * Move to an initial Position WARNING: MAKE SURE, THAT the pose is collision free. 150 | */ 151 | private void moveToInitialPosition() 152 | { 153 | _lbr.move( 154 | ptp(0., 0.,0.,0.,0.,0., 0.).setJointVelocityRel(0.15)); 155 | /* Note: The Validation itself justifies, that in this very time instance, the load parameter setting was 156 | * sufficient. This does not mean by far, that the parameter setting is valid in the sequel or lifetime of this 157 | * program */ 158 | try 159 | { 160 | 161 | } 162 | catch (IllegalStateException e) 163 | { 164 | getLogger().info("Omitting validation failure for this sample\n" 165 | + e.getMessage()); 166 | } 167 | } 168 | 169 | /** 170 | * Main Application Routine 171 | */ 172 | @Override 173 | public void run() 174 | { 175 | moveToInitialPosition(); 176 | System.out.print("You have ten seconds to establish a connection with iiwa"); 177 | int portTemp=30002; 178 | udpServer=new UDPServer(portTemp); 179 | startSmartServo(); 180 | } 181 | 182 | private void startSmartServo(){ 183 | 184 | boolean doDebugPrints = false; 185 | 186 | JointPosition initialPosition = new JointPosition( 187 | _lbr.getCurrentJointPosition()); 188 | SmartServo aSmartServoMotion = new SmartServo(initialPosition); 189 | 190 | // Set the motion properties to 20% of systems abilities 191 | aSmartServoMotion.setJointAccelerationRel(0.2); 192 | aSmartServoMotion.setJointVelocityRel(0.2); 193 | 194 | aSmartServoMotion.setMinimumTrajectoryExecutionTime(20e-3); 195 | 196 | getLogger().info("Starting SmartServo motion in position control mode"); 197 | _toolAttachedToLBR.getDefaultMotionFrame().moveAsync(aSmartServoMotion); 198 | 199 | getLogger().info("Get the runtime of the SmartServo motion"); 200 | _theSmartServoRuntime = aSmartServoMotion.getRuntime(); 201 | 202 | // create an JointPosition Instance, to play with 203 | JointPosition destination = new JointPosition( 204 | _lbr.getJointCount()); 205 | for(int i=0;i<7;i++){ 206 | jpos[i]=destination.get(i); 207 | } 208 | getLogger().info("Start loop"); 209 | // For Roundtrip time measurement... 210 | StatisticTimer timing = new StatisticTimer(); 211 | try 212 | { 213 | // do a cyclic loop 214 | // Refer to some timing... 215 | // in nanosec 216 | 217 | long startTimeStamp = System.nanoTime(); 218 | while(loopFlag==true) 219 | { 220 | _steps=_steps+1; 221 | // Timing - draw one step 222 | OneTimeStep aStep = timing.newTimeStep(); 223 | // /////////////////////////////////////////////////////// 224 | // Insert your code here 225 | // e.g Visual Servoing or the like 226 | // emulate some computational effort - or waiting for external 227 | // stuff 228 | ThreadUtil.milliSleep(MILLI_SLEEP_TO_EMULATE_COMPUTATIONAL_EFFORT); 229 | _theSmartServoRuntime.updateWithRealtimeSystem(); 230 | // Get the measured position 231 | JointPosition curMsrJntPose = _theSmartServoRuntime 232 | .getAxisQMsrOnController(); 233 | 234 | for (int k = 0; k < destination.getAxisCount(); ++k) 235 | { 236 | jpos[k]=udpServer.jpos[k]; 237 | destination.set(k, jpos[k]); 238 | } 239 | _theSmartServoRuntime.setDestination(destination); 240 | 241 | // //////////////////////////////////////////////////////////// 242 | if (doDebugPrints) 243 | { 244 | getLogger().info("Step " + _steps + " New Goal " 245 | + destination); 246 | getLogger().info("Fine ipo finished " + _theSmartServoRuntime.isDestinationReached()); 247 | if (_theSmartServoRuntime.isDestinationReached()) 248 | { 249 | _count++; 250 | } 251 | getLogger().info("Ipo state " + _theSmartServoRuntime.getFineIpoState()); 252 | getLogger().info("Remaining time " + _theSmartServoRuntime.getRemainingTime()); 253 | getLogger().info("LBR Position " 254 | + _lbr.getCurrentJointPosition()); 255 | getLogger().info(" Measured LBR Position " 256 | + curMsrJntPose); 257 | if (_steps % 100 == 0) 258 | { 259 | // Some internal values, which can be displayed 260 | getLogger().info("Simple Joint Test - step " + _steps + _theSmartServoRuntime.toString()); 261 | 262 | } 263 | } 264 | aStep.end(); 265 | 266 | } 267 | } 268 | catch (Exception e) 269 | { 270 | getLogger().info(e.getLocalizedMessage()); 271 | e.printStackTrace(); 272 | } 273 | ThreadUtil.milliSleep(1000); 274 | 275 | //Print statistics and parameters of the motion 276 | getLogger().info("Displaying final states after loop "); 277 | getLogger().info(getClass().getName() + _theSmartServoRuntime.toString()); 278 | // Stop the motion 279 | 280 | getLogger().info("Stop the SmartServo motion"); 281 | _theSmartServoRuntime.stopMotion(); 282 | 283 | getLogger().info(_count + " times was the destination reached."); 284 | getLogger().info("Statistic Timing of Overall Loop " + timing); 285 | if (timing.getMeanTimeMillis() > 150) 286 | { 287 | getLogger().info("Statistic Timing is unexpected slow, you should try to optimize TCP/IP Transfer"); 288 | getLogger().info("Under Windows, you should play with the registry, see the e.g. the SmartServo Class javaDoc for details"); 289 | } 290 | } 291 | 292 | 293 | double getTheDisplacment(double dj) 294 | { 295 | 296 | return dj; 297 | /* 298 | double a=0.07; 299 | double b=a*0.75; 300 | double exponenet=-Math.pow(dj/b, 2); 301 | return Math.signum(dj)*a*(1-Math.exp(exponenet)); 302 | */ 303 | } 304 | 305 | 306 | /** 307 | * Main routine, which starts the application 308 | */ 309 | public static void main(String[] args) 310 | { 311 | SimulinkIIWASmartServoJoints app = new SimulinkIIWASmartServoJoints(); 312 | app.runApplication(); 313 | } 314 | 315 | public class UDPServer implements Runnable{ 316 | 317 | public double[] jpos=new double[7]; 318 | 319 | double stime=0; 320 | double endtime=0; 321 | 322 | int _port; 323 | int vectorSize=7; 324 | int packetCounter=0; 325 | 326 | byte[] buffer=new byte[8*vectorSize]; 327 | UDPServer(int port) 328 | { 329 | for(int i=0;i<7;i++) 330 | { 331 | jpos[i]=0; 332 | } 333 | Thread t=new Thread(this); 334 | t.setDaemon(true); 335 | t.start(); 336 | _port=port; 337 | } 338 | 339 | public void run() 340 | { 341 | System.out.println("Program will terminate if comuncation is not established in 10 seconds"); 342 | DatagramSocket soc=null; 343 | try { 344 | soc = new DatagramSocket(_port); 345 | soc.setSoTimeout(10000); // 5 seconds, if a time out occurred, turn off program 346 | DatagramPacket response=new DatagramPacket(buffer, buffer.length); 347 | while(true) 348 | { 349 | soc.receive(response); 350 | packetCounter=packetCounter+1; 351 | // String s= new String(buffer,0,response.getLength()) 352 | // System.out.println(response.getLength()); 353 | if(response.getLength()==8*vectorSize) 354 | { 355 | if(packetCounter==1) 356 | { 357 | stime=System.currentTimeMillis(); 358 | } 359 | else 360 | { 361 | endtime=System.currentTimeMillis(); 362 | } 363 | byte[] daB=new byte[8]; 364 | int counter=0; 365 | int jointNum=0; 366 | while(counter <8*vectorSize) 367 | { 368 | for(int i=0;i<8;i++) 369 | { 370 | daB[7-i]=buffer[counter]; 371 | counter=counter+1; 372 | } 373 | jpos[jointNum]=bytesToDouble(daB); 374 | jointNum=jointNum+1; 375 | //System.out.println(bytesToDouble(daB)); 376 | } 377 | } 378 | } 379 | } catch (SocketException e) { 380 | // TODO Auto-generated catch block 381 | e.printStackTrace(); 382 | System.out.println(e.toString()); 383 | } catch (IOException e) { 384 | // TODO Auto-generated catch block 385 | e.printStackTrace(); 386 | System.out.println(e.toString()); 387 | } 388 | if(soc==null) 389 | {} 390 | else 391 | { 392 | if(soc.isClosed()) 393 | {} 394 | else 395 | { 396 | soc.close(); 397 | } 398 | } 399 | SimulinkIIWASmartServoJoints.loopFlag=false; //Turn off main loop 400 | double time=(endtime-stime)/1000; 401 | double rate=packetCounter/time; 402 | System.out.println("update rate, packets/second"); 403 | System.out.println(rate); 404 | } 405 | } 406 | 407 | 408 | public double bytesToDouble(byte[] b) 409 | { 410 | return ByteBuffer.wrap(b).getDouble(); 411 | } 412 | 413 | } 414 | -------------------------------------------------------------------------------- /Joint space motion open loop/simulink/smartDirectServoJoints.slx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Modi1987/Simulink-iiwa-interface/5d0b6c1a84492946bbb26aa03efa3ab85d25d530/Joint space motion open loop/simulink/smartDirectServoJoints.slx -------------------------------------------------------------------------------- /Joint space motion open loop/simulink/smartDirectServoJoints.slxc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Modi1987/Simulink-iiwa-interface/5d0b6c1a84492946bbb26aa03efa3ab85d25d530/Joint space motion open loop/simulink/smartDirectServoJoints.slxc -------------------------------------------------------------------------------- /Joint space motion with feedback/iiwa/SimulinkFIIWADirectServoJoints.java: -------------------------------------------------------------------------------- 1 | package lbrExampleApplications; 2 | 3 | 4 | import static com.kuka.roboticsAPI.motionModel.BasicMotions.ptp; 5 | 6 | import java.io.IOException; 7 | import java.net.ServerSocket; 8 | import java.net.Socket; 9 | import java.util.Scanner; 10 | import java.util.StringTokenizer; 11 | import java.util.logging.Logger; 12 | 13 | import com.kuka.connectivity.motionModel.directServo.DirectServo; 14 | import com.kuka.connectivity.motionModel.directServo.IDirectServoRuntime; 15 | import com.kuka.generated.ioAccess.MediaFlangeIOGroup; 16 | import com.kuka.roboticsAPI.applicationModel.RoboticsAPIApplication; 17 | import com.kuka.roboticsAPI.controllerModel.Controller; 18 | import com.kuka.roboticsAPI.deviceModel.JointPosition; 19 | import com.kuka.roboticsAPI.deviceModel.LBR; 20 | import com.kuka.roboticsAPI.geometricModel.Frame; 21 | import com.kuka.roboticsAPI.geometricModel.World; 22 | import com.kuka.roboticsAPI.sensorModel.ForceSensorData; 23 | 24 | 25 | import java.io.IOException; 26 | import java.net.DatagramPacket; 27 | import java.net.DatagramSocket; 28 | import java.net.InetAddress; 29 | import java.net.SocketException; 30 | import java.net.UnknownHostException; 31 | import java.nio.ByteBuffer; 32 | 33 | import lbrExampleApplications.SimulinkFIIWADirectServoCartesian.UDPPublisher; 34 | 35 | /* 36 | * SimulinkIIWA interface, soft real-time control in joints position mode 37 | * Copyright: Mohammad SAFEEA, 22nd/May/2018 38 | * License: MIT license 39 | * 40 | * This is a UDP server that receives motion commands from Simulink. 41 | * This server works together with the Simulink example: (feedBackSmartDirectServoJoints.slx) 42 | * 43 | * Also this script returns back a feedback about the (1)actual joints positions, 44 | * (2)the external torques and (3)the measured torques of all the joints to Simulink. 45 | * (4) EEF force moment. 46 | * 47 | * Below you can find quick start instructions, for more info, please refer to the youtube 48 | * video tutorials, a link for which is available in the repository main page. 49 | * 50 | * Hardware setup: 51 | * 1- KUKA iiwa 7R800 or 14R820 52 | * 2- External PC 53 | * 3- A netwrok between the PC and the robot, the connector X66 54 | * of the robot shall be used 55 | * 56 | * Required software: 57 | * 1- MATLAB with Simulink, the code was written using MATLAB2018a, it 58 | * is recommended to use the same version. 59 | * 2- Sunrise.Workbench, this program will be used to synchronize the java 60 | * code to the robot. 61 | * 3- The Sunrise code was tested on (KUKA Sunrise.OS 1.11.0.7) 62 | * 63 | * Setup instructions: 64 | * 1- Add this class to a new project using kuka's Sunrsie.Workbench. 65 | * 2- Add reference to the Direct/SmartServo from inside the (StationSetup.cat) file. 66 | * 3- Change the following variables according to your tool and stiffness preference: 67 | * * externa_PC_IP: change this variable according to the IP of the PC used for control. 68 | * 69 | * 70 | * Utilization instructions: 71 | * 1- Synchronize the project to the controller of the robot. 72 | * 2- Run this application from the teach-pad of the robot, 73 | * This application has a time out of 10 seconds, if a connection 74 | * is not established during the 10 seconds interval the server will 75 | * turn off. 76 | * 3- Start the Simulink example (feedBackSmartDirectServoJoints.slx) 77 | * from the external PC. 78 | * 4- To change the timeout value, change the argument for the instruction 79 | * soc.setSoTimeout(10000), the argument is in milliseconds. 80 | * 81 | */ 82 | 83 | public class SimulinkFIIWADirectServoJoints extends RoboticsAPIApplication 84 | { 85 | 86 | private static String externa_PC_IP="172.31.69.55"; 87 | private LBR _lbr; 88 | 89 | 90 | public static double jpos[]; 91 | 92 | public static boolean loopFlag; 93 | 94 | public static double jDispMax[]; 95 | public static double updateCycleJointPos[]; 96 | private UDPServer udpServer; 97 | int _port; 98 | 99 | ServerSocket ss; 100 | Socket soc; 101 | 102 | 103 | 104 | 105 | 106 | public class UDPPublisher implements Runnable{ 107 | 108 | String ip; 109 | UDPPublisher(String ip) 110 | { 111 | this.ip=ip; 112 | Thread t=new Thread(this); 113 | t.setDaemon(true); 114 | t.start(); 115 | } 116 | public void run() 117 | { 118 | broadcastData(); 119 | } 120 | private void broadcastData() { 121 | // TODO Auto-generated method stub 122 | String message="Publishing robot state using UDP at port 30008 to destination: "+ ip; 123 | System.out.println(message); 124 | message="Streamed message: 7 actual joint angles, 7 measured torques, 7 external torques,"; 125 | System.out.println(message); 126 | message="State streaming frequency is approximatly at 25HZ"; 127 | System.out.println(message); 128 | int port=30008; 129 | InetAddress host; 130 | int numOfDoubles=7+7+7+6;// 7 actual joint angles, 7 measured torques, 7 external torques, 7 force at flange 131 | double[] doubleVals=new double[numOfDoubles]; 132 | 133 | byte[] buffer= new byte[8*numOfDoubles]; 134 | 135 | try { 136 | host = InetAddress.getByName(ip); 137 | DatagramSocket soc=new DatagramSocket(); 138 | while(loopFlag) 139 | { 140 | // Feedback of actual joints positions 141 | JointPosition jpos=_lbr.getCurrentJointPosition(); 142 | int valsCounter=0; 143 | for(int i=0;i<7;i++) 144 | { 145 | doubleVals[valsCounter]=jpos.get(i); 146 | valsCounter=valsCounter+1; 147 | } 148 | // Feedback of measured torques 149 | double[] m_tor=_lbr.getMeasuredTorque().getTorqueValues(); 150 | for(int i=0;i<7;i++) 151 | { 152 | doubleVals[valsCounter]=m_tor[i]; 153 | valsCounter=valsCounter+1; 154 | } 155 | // Feedback of external torques 156 | double[] ex_tor=_lbr.getExternalTorque().getTorqueValues(); 157 | for(int i=0;i<7;i++) 158 | { 159 | doubleVals[valsCounter]=ex_tor[i]; 160 | valsCounter=valsCounter+1; 161 | } 162 | // Feedback of forces at flange 163 | ForceSensorData f_at_flange=_lbr.getExternalForceTorque(_lbr.getFlange(),World.Current.getRootFrame()); 164 | doubleVals[valsCounter]=f_at_flange.getForce().getX(); 165 | valsCounter=valsCounter+1; 166 | doubleVals[valsCounter]=f_at_flange.getForce().getY(); 167 | valsCounter=valsCounter+1; 168 | doubleVals[valsCounter]=f_at_flange.getForce().getZ(); 169 | valsCounter=valsCounter+1; 170 | doubleVals[valsCounter]=f_at_flange.getTorque().getX(); 171 | valsCounter=valsCounter+1; 172 | doubleVals[valsCounter]=f_at_flange.getTorque().getY(); 173 | valsCounter=valsCounter+1; 174 | doubleVals[valsCounter]=f_at_flange.getTorque().getZ(); 175 | valsCounter=valsCounter+1; 176 | 177 | int count=0; 178 | for(int i=0;ijDispMax[k]) 351 | { 352 | jDispMax[k]=absDisp; 353 | } 354 | destination.set(k, temp); 355 | updateCycleJointPos[k]=temp; 356 | 357 | } 358 | 359 | /*terminateFlag=checkCollisionWithEEF(updateCycleJointPos); 360 | if(terminateFlag==true) 361 | { 362 | dabak.terminateBool=true; 363 | dabak.soc.close(); 364 | dabak.ss.close(); 365 | 366 | break; 367 | }*/ 368 | 369 | theDirectServoRuntime.setDestination(destination); 370 | 371 | } 372 | } 373 | catch (Exception e) 374 | { 375 | getLogger().info(e.getLocalizedMessage()); 376 | e.printStackTrace(); 377 | //Print statistics and parameters of the motion 378 | getLogger().info("Simple Cartesian Test \n" + theDirectServoRuntime.toString()); 379 | 380 | getLogger().info("Stop the DirectServo motion"); 381 | 382 | 383 | } 384 | theDirectServoRuntime.stopMotion(); 385 | 386 | } 387 | 388 | 389 | double getTheDisplacment(double dj) 390 | { 391 | 392 | return dj; 393 | /* 394 | double a=0.07; 395 | double b=a*0.75; 396 | double exponenet=-Math.pow(dj/b, 2); 397 | return Math.signum(dj)*a*(1-Math.exp(exponenet)); 398 | */ 399 | } 400 | 401 | 402 | /** 403 | * Main routine, which starts the application 404 | */ 405 | public static void main(String[] args) 406 | { 407 | SimulinkFIIWADirectServoJoints app = new SimulinkFIIWADirectServoJoints(); 408 | app.runApplication(); 409 | } 410 | 411 | public class UDPServer implements Runnable{ 412 | 413 | public double[] jpos=new double[7]; 414 | 415 | double stime=0; 416 | double endtime=0; 417 | 418 | int _port; 419 | int vectorSize=7; 420 | int packetCounter=0; 421 | 422 | byte[] buffer=new byte[8*vectorSize]; 423 | UDPServer(int port) 424 | { 425 | for(int i=0;i<7;i++) 426 | { 427 | jpos[i]=0; 428 | } 429 | Thread t=new Thread(this); 430 | t.setDaemon(true); 431 | t.start(); 432 | _port=port; 433 | } 434 | 435 | public void run() 436 | { 437 | System.out.println("Program will terminate if comuncation is not established in 10 seconds"); 438 | DatagramSocket soc=null; 439 | try { 440 | soc = new DatagramSocket(_port); 441 | soc.setSoTimeout(10000); // 5 seconds, if a time out occurred, turn off program 442 | DatagramPacket response=new DatagramPacket(buffer, buffer.length); 443 | while(true) 444 | { 445 | soc.receive(response); 446 | packetCounter=packetCounter+1; 447 | // String s= new String(buffer,0,response.getLength()) 448 | // System.out.println(response.getLength()); 449 | if(response.getLength()==8*vectorSize) 450 | { 451 | if(packetCounter==1) 452 | { 453 | stime=System.currentTimeMillis(); 454 | } 455 | else 456 | { 457 | endtime=System.currentTimeMillis(); 458 | } 459 | byte[] daB=new byte[8]; 460 | int counter=0; 461 | int jointNum=0; 462 | while(counter <8*vectorSize) 463 | { 464 | for(int i=0;i<8;i++) 465 | { 466 | daB[7-i]=buffer[counter]; 467 | counter=counter+1; 468 | } 469 | jpos[jointNum]=bytesToDouble(daB); 470 | jointNum=jointNum+1; 471 | //System.out.println(bytesToDouble(daB)); 472 | } 473 | } 474 | } 475 | } catch (SocketException e) { 476 | // TODO Auto-generated catch block 477 | e.printStackTrace(); 478 | //System.out.println(e.toString()); 479 | } catch (IOException e) { 480 | // TODO Auto-generated catch block 481 | e.printStackTrace(); 482 | System.out.println(e.toString()); 483 | } 484 | if(soc==null) 485 | {} 486 | else 487 | { 488 | if(soc.isClosed()) 489 | {} 490 | else 491 | { 492 | soc.close(); 493 | } 494 | } 495 | SimulinkFIIWADirectServoJoints.loopFlag=false; //Turn off main loop 496 | double time=(endtime-stime)/1000; 497 | double rate=packetCounter/time; 498 | System.out.println("update rate, packets/second"); 499 | System.out.println(rate); 500 | } 501 | } 502 | 503 | 504 | public double bytesToDouble(byte[] b) 505 | { 506 | return ByteBuffer.wrap(b).getDouble(); 507 | } 508 | 509 | } 510 | -------------------------------------------------------------------------------- /Joint space motion with feedback/iiwa/SimulinkFIIWASmartServoJoints.java: -------------------------------------------------------------------------------- 1 | package lbrExampleApplications; 2 | 3 | 4 | import static com.kuka.roboticsAPI.motionModel.BasicMotions.ptp; 5 | 6 | import java.io.IOException; 7 | import java.net.ServerSocket; 8 | import java.net.Socket; 9 | import java.util.Scanner; 10 | import java.util.StringTokenizer; 11 | import java.util.logging.Logger; 12 | 13 | import com.kuka.common.StatisticTimer; 14 | import com.kuka.common.ThreadUtil; 15 | import com.kuka.common.StatisticTimer.OneTimeStep; 16 | import com.kuka.connectivity.motionModel.directServo.DirectServo; 17 | import com.kuka.connectivity.motionModel.directServo.IDirectServoRuntime; 18 | import com.kuka.connectivity.motionModel.smartServo.ISmartServoRuntime; 19 | import com.kuka.connectivity.motionModel.smartServo.SmartServo; 20 | import com.kuka.generated.ioAccess.MediaFlangeIOGroup; 21 | import com.kuka.roboticsAPI.applicationModel.RoboticsAPIApplication; 22 | import com.kuka.roboticsAPI.controllerModel.Controller; 23 | import com.kuka.roboticsAPI.deviceModel.JointPosition; 24 | import com.kuka.roboticsAPI.deviceModel.LBR; 25 | import com.kuka.roboticsAPI.geometricModel.Frame; 26 | import com.kuka.roboticsAPI.geometricModel.LoadData; 27 | import com.kuka.roboticsAPI.geometricModel.ObjectFrame; 28 | import com.kuka.roboticsAPI.geometricModel.Tool; 29 | import com.kuka.roboticsAPI.geometricModel.World; 30 | import com.kuka.roboticsAPI.geometricModel.math.XyzAbcTransformation; 31 | import com.kuka.roboticsAPI.sensorModel.ForceSensorData; 32 | 33 | 34 | import java.io.IOException; 35 | import java.net.DatagramPacket; 36 | import java.net.DatagramSocket; 37 | import java.net.InetAddress; 38 | import java.net.SocketException; 39 | import java.net.UnknownHostException; 40 | import java.nio.ByteBuffer; 41 | 42 | import lbrExampleApplications.SimulinkFIIWADirectServoJoints.UDPPublisher; 43 | import lbrExampleApplications.SimulinkIIWADirectServoJoints.UDPServer; 44 | 45 | /* 46 | * SimulinkIIWA interface, soft real-time control in joint position mode 47 | * Copyright: Mohammad SAFEEA, 22nd/May/2018 48 | * License: MIT license 49 | * 50 | * This is a UDP server that receives motion commands from Simulink. 51 | * This server works together with the Simulink example: (feedBackSmartDirectServoJoints.slx) 52 | * 53 | * Also this script returns back a feedback about the (1)actual joints positions, 54 | * (2)the external torques and (3)the measured torques of all the joints to Simulink. 55 | * (4) EEF force moment. 56 | * 57 | * Below you can find quick start instructions, for more info, please refer to the youtube 58 | * video tutorials, a link for which is available in the repository main page. 59 | * 60 | * Hardware setup: 61 | * 1- KUKA iiwa 7R800 or 14R820 62 | * 2- External PC 63 | * 3- A network between the PC and the robot, the connector X66 64 | * of the robot shall be used 65 | * 66 | * Required software: 67 | * 1- MATLAB with Simulink, the code was written using MATLAB2018a, it 68 | * is recommended to use the same version. 69 | * 2- Sunrise.Workbench, this program will be used to synchronize the java 70 | * code to the robot. 71 | * 3- The Sunrise code was tested on (KUKA Sunrise.OS 1.11.0.7) 72 | * 73 | * Setup instructions: 74 | * 1- Add this class to a new project using kuka's Sunrsie.Workbench. 75 | * 2- Add reference to the Direct/SmartServo from inside the (StationSetup.cat) file. 76 | * 3- Change the following variables according to your preference: 77 | * * externa_PC_IP: change this variable according to the IP of the PC used for control. 78 | * * TRANSLATION_OF_TOOL: translation of tool center point with respect to flange frame. 79 | * * massOfTool: mass of the tool 80 | * * toolsCOMCoordiantes: coordinates of center of mass of tool with regards to the frame of the flange. 81 | * 82 | * 83 | * Utilization instructions: 84 | * 1- Synchronize the project to the controller of the robot. 85 | * 2- Run this application from the teach-pad of the robot, 86 | * This application has a time out of 10 seconds, if a connection 87 | * is not established during the 10 seconds interval the server will 88 | * turn off. 89 | * 3- Start the Simulink example (feedBackSmartDirectServoJoints.slx) 90 | * from the external PC. 91 | * 4- To change the timeout value, change the argument for the instruction 92 | * soc.setSoTimeout(10000), the argument is in milliseconds. 93 | * 94 | */ 95 | 96 | public class SimulinkFIIWASmartServoJoints extends RoboticsAPIApplication 97 | { 98 | private static String externa_PC_IP="172.31.69.55"; 99 | private LBR _lbr; 100 | private ISmartServoRuntime _theSmartServoRuntime = null; 101 | 102 | // Tool Data 103 | private Tool _toolAttachedToLBR; 104 | private LoadData _loadData; 105 | private static final String TOOL_FRAME = "toolFrame"; 106 | private static final double[] TRANSLATION_OF_TOOL = { 0, 0, 100 }; 107 | private static final double MASS = 0.4; 108 | private static final double[] CENTER_OF_MASS_IN_MILLIMETER = { 0, 0, 100 }; 109 | 110 | private int _count = 0; 111 | 112 | private static final int MILLI_SLEEP_TO_EMULATE_COMPUTATIONAL_EFFORT = 1; 113 | private int _steps = 0; 114 | 115 | public static boolean loopFlag; 116 | public static double[] jpos; 117 | private UDPServer udpServer; 118 | 119 | 120 | 121 | public class UDPPublisher implements Runnable{ 122 | 123 | String ip; 124 | UDPPublisher(String ip) 125 | { 126 | this.ip=ip; 127 | Thread t=new Thread(this); 128 | t.setDaemon(true); 129 | t.start(); 130 | } 131 | public void run() 132 | { 133 | broadcastData(); 134 | } 135 | private void broadcastData() { 136 | // TODO Auto-generated method stub 137 | String message="Publishing robot state using UDP at port 30008 to destination: "+ ip; 138 | System.out.println(message); 139 | message="Streamed message: 7 actual joint angles, 7 measured torques, 7 external torques,"; 140 | System.out.println(message); 141 | message="State streaming frequency is approximatly at 25HZ"; 142 | System.out.println(message); 143 | int port=30008; 144 | InetAddress host; 145 | int numOfDoubles=7+7+7+6;// 7 actual joint angles, 7 measured torques, 7 external torques, 7 force at flange 146 | double[] doubleVals=new double[numOfDoubles]; 147 | 148 | byte[] buffer= new byte[8*numOfDoubles]; 149 | 150 | try { 151 | host = InetAddress.getByName(ip); 152 | DatagramSocket soc=new DatagramSocket(); 153 | while(loopFlag) 154 | { 155 | // Feedback of actual joints positions 156 | JointPosition jpos=_lbr.getCurrentJointPosition(); 157 | int valsCounter=0; 158 | for(int i=0;i<7;i++) 159 | { 160 | doubleVals[valsCounter]=jpos.get(i); 161 | valsCounter=valsCounter+1; 162 | } 163 | // Feedback of measured torques 164 | double[] m_tor=_lbr.getMeasuredTorque().getTorqueValues(); 165 | for(int i=0;i<7;i++) 166 | { 167 | doubleVals[valsCounter]=m_tor[i]; 168 | valsCounter=valsCounter+1; 169 | } 170 | // Feedback of external torques 171 | double[] ex_tor=_lbr.getExternalTorque().getTorqueValues(); 172 | for(int i=0;i<7;i++) 173 | { 174 | doubleVals[valsCounter]=ex_tor[i]; 175 | valsCounter=valsCounter+1; 176 | } 177 | // Feedback of forces at flange 178 | ForceSensorData f_at_flange=_lbr.getExternalForceTorque(_lbr.getFlange(),World.Current.getRootFrame()); 179 | doubleVals[valsCounter]=f_at_flange.getForce().getX(); 180 | valsCounter=valsCounter+1; 181 | doubleVals[valsCounter]=f_at_flange.getForce().getY(); 182 | valsCounter=valsCounter+1; 183 | doubleVals[valsCounter]=f_at_flange.getForce().getZ(); 184 | valsCounter=valsCounter+1; 185 | doubleVals[valsCounter]=f_at_flange.getTorque().getX(); 186 | valsCounter=valsCounter+1; 187 | doubleVals[valsCounter]=f_at_flange.getTorque().getY(); 188 | valsCounter=valsCounter+1; 189 | doubleVals[valsCounter]=f_at_flange.getTorque().getZ(); 190 | valsCounter=valsCounter+1; 191 | 192 | int count=0; 193 | for(int i=0;i 150) 412 | { 413 | getLogger().info("Statistic Timing is unexpected slow, you should try to optimize TCP/IP Transfer"); 414 | getLogger().info("Under Windows, you should play with the registry, see the e.g. the SmartServo Class javaDoc for details"); 415 | } 416 | } 417 | 418 | 419 | double getTheDisplacment(double dj) 420 | { 421 | 422 | return dj; 423 | /* 424 | double a=0.07; 425 | double b=a*0.75; 426 | double exponenet=-Math.pow(dj/b, 2); 427 | return Math.signum(dj)*a*(1-Math.exp(exponenet)); 428 | */ 429 | } 430 | 431 | 432 | /** 433 | * Main routine, which starts the application 434 | */ 435 | public static void main(String[] args) 436 | { 437 | SimulinkFIIWASmartServoJoints app = new SimulinkFIIWASmartServoJoints(); 438 | app.runApplication(); 439 | } 440 | 441 | public class UDPServer implements Runnable{ 442 | 443 | public double[] jpos=new double[7]; 444 | 445 | double stime=0; 446 | double endtime=0; 447 | 448 | int _port; 449 | int vectorSize=7; 450 | int packetCounter=0; 451 | 452 | byte[] buffer=new byte[8*vectorSize]; 453 | UDPServer(int port) 454 | { 455 | for(int i=0;i<7;i++) 456 | { 457 | jpos[i]=0; 458 | } 459 | Thread t=new Thread(this); 460 | t.setDaemon(true); 461 | t.start(); 462 | _port=port; 463 | } 464 | 465 | public void run() 466 | { 467 | System.out.println("Program will terminate if comuncation is not established in 10 seconds"); 468 | DatagramSocket soc=null; 469 | try { 470 | soc = new DatagramSocket(_port); 471 | soc.setSoTimeout(10000); // 5 seconds, if a time out occurred, turn off program 472 | DatagramPacket response=new DatagramPacket(buffer, buffer.length); 473 | while(true) 474 | { 475 | soc.receive(response); 476 | packetCounter=packetCounter+1; 477 | // String s= new String(buffer,0,response.getLength()) 478 | // System.out.println(response.getLength()); 479 | if(response.getLength()==8*vectorSize) 480 | { 481 | if(packetCounter==1) 482 | { 483 | stime=System.currentTimeMillis(); 484 | } 485 | else 486 | { 487 | endtime=System.currentTimeMillis(); 488 | } 489 | byte[] daB=new byte[8]; 490 | int counter=0; 491 | int jointNum=0; 492 | while(counter <8*vectorSize) 493 | { 494 | for(int i=0;i<8;i++) 495 | { 496 | daB[7-i]=buffer[counter]; 497 | counter=counter+1; 498 | } 499 | jpos[jointNum]=bytesToDouble(daB); 500 | jointNum=jointNum+1; 501 | //System.out.println(bytesToDouble(daB)); 502 | } 503 | } 504 | } 505 | } catch (SocketException e) { 506 | // TODO Auto-generated catch block 507 | e.printStackTrace(); 508 | System.out.println(e.toString()); 509 | } catch (IOException e) { 510 | // TODO Auto-generated catch block 511 | e.printStackTrace(); 512 | System.out.println(e.toString()); 513 | } 514 | if(soc==null) 515 | {} 516 | else 517 | { 518 | if(soc.isClosed()) 519 | {} 520 | else 521 | { 522 | soc.close(); 523 | } 524 | } 525 | SimulinkFIIWASmartServoJoints.loopFlag=false; //Turn off main loop 526 | double time=(endtime-stime)/1000; 527 | double rate=packetCounter/time; 528 | System.out.println("update rate, packets/second"); 529 | System.out.println(rate); 530 | } 531 | } 532 | 533 | 534 | public double bytesToDouble(byte[] b) 535 | { 536 | return ByteBuffer.wrap(b).getDouble(); 537 | } 538 | 539 | } 540 | -------------------------------------------------------------------------------- /Joint space motion with feedback/simulink/feedBackSmartDirectServoJoints.slx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Modi1987/Simulink-iiwa-interface/5d0b6c1a84492946bbb26aa03efa3ab85d25d530/Joint space motion with feedback/simulink/feedBackSmartDirectServoJoints.slx -------------------------------------------------------------------------------- /Joint space motion with feedback/simulink/feedBackSmartDirectServoJoints.slxc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Modi1987/Simulink-iiwa-interface/5d0b6c1a84492946bbb26aa03efa3ab85d25d530/Joint space motion with feedback/simulink/feedBackSmartDirectServoJoints.slxc -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2019 Mohammad SAFEEA 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /Photos/SimulinkIIWAcover.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Modi1987/Simulink-iiwa-interface/5d0b6c1a84492946bbb26aa03efa3ab85d25d530/Photos/SimulinkIIWAcover.jpg -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Simulink-iiwa interface 2 | The SimulinkIIWA is an interface that allows the user to control KUKA iiwa manipulaotrs from inside Simulink. 3 | Unlike the ![KUKA Sunrise Toolbox or KST](https://github.com/Modi1987/KST-Kuka-Sunrise-Toolbox), the SimulinkIIWA interface is built upon UDP protocol instead of TCP/IP protocol. 4 | 5 | ![cover photo](https://github.com/Modi1987/Simulink-iiwa-interface/raw/master/Photos/SimulinkIIWAcover.jpg) 6 | -------------------------------------- 7 | 8 | # Requirments: 9 | 10 | The project in this repository was tested using: 11 | * Matlab 2018a, running under Windows 10. 12 | * KUKA iiwa 7R800 with Sunrise.OS 1.11.0.7 13 | 14 | -------------------------------------- 15 | 16 | # Video tutorials: 17 | 18 | Video tutorials on using the interface are [available in here](https://www.youtube.com/watch?v=at9xUItdidI&list=PLz558OYgHuZcK3ubmfA1rEm2UdLDDC37D). 19 | 20 | -------------------------------------- 21 | # Installation 22 | To setup the SimulinkIIWA interface the following steps, for Matlab and for the robot controller, shall be followed: 23 | 24 | ## On IIWA side: 25 | * You will need a computer connected to the robot through an ethernet cable on the X66 connector of the controller. 26 | * Open the Sunrise.Workbench on the computer side. 27 | * Download the [SimulinkIIWA](https://github.com/Modi1987/Simulink-iiwa-interface) repository into your computer, unzip it inside a folder named "SimulinkIIWA". 28 | * Then from inside the folder "SimulinkIIWA" pick up one of the operation modes available, for example, the [Cartesian motion open loop](https://github.com/Modi1987/Simulink-iiwa-interface/tree/master/Cartesian%20motion%20open%20loop). Each operation mode is provided in a separate folder, which contains the corresponding Sunrise.Workbench code along with the accompanying Simulink project. 29 | * Inside the [Cartesian motion open loop](https://github.com/Modi1987/Simulink-iiwa-interface/tree/master/Cartesian%20motion%20open%20loop) folder, open the [iiwa](https://github.com/Modi1987/Simulink-iiwa-interface/tree/master/Cartesian%20motion%20open%20loop/iiwa) sub-folder. 30 | * You shall copy the available JAVA files into a project inside the Sunrise.Workbench. Afterwards synchronise the project to the robot controller. 31 | * After synchronisation, the newly added application will show up in the applications-list on the smartPad of the robot under the name "SimulinkIIWADirectServoCartesian". 32 | 33 | ## On Matlab side: 34 | * First open Matlab 2018a 35 | * Then, you shall install the Simulink Desktop Real-time Kernel by [following the instructions in here](https://www.mathworks.com/help/sldrt/ug/real-time-windows-target-kernel.html). 36 | * Note: Mathworks provides the (Simulink Desktop Real-time) under Windows and Mac operating systems. Though the provided scripts were tested only under Windows. To check the installation status of the real-time kernel, type the command (rtwho) in the command window of Matlab 2018a. 37 | -------------------------------------- 38 | 39 | # Operation: 40 | First run the server on IIWA side using the smartPad, then you shall run the Simulink project in Matlab 2018a, as detailed in the following: 41 | 42 | ## On the IIWA side: 43 | * Run the server application from the smartPad, you find it in the applications-list under the name "SimulinkIIWADirectServoCartesian". 44 | * Then, you will have 10 seconds to connect to it from Simulink, if a connection was not initiated during the time limit the program will be terminated automatically. 45 | * If the connection is terminated, you have to run the server application again from the smartPad before initiating a connection. 46 | 47 | ## On Matlab side: 48 | * From inside the [Cartesian motion open loop](https://github.com/Modi1987/Simulink-iiwa-interface/tree/master/Cartesian%20motion%20open%20loop) folder, open the [simulink](https://github.com/Modi1987/Simulink-iiwa-interface/tree/master/Cartesian%20motion%20open%20loop/simulink) sub-folder. 49 | * You shall find the the Simulink project "smartDirectServoCartesian.slx", double click it. It shall open in Simulink. 50 | * After running the "SimulinkIIWADirectServoCartesian" on the robot side from the smartPad, you shall run the Simulink project "smartDirectServoCartesian.slx" from Simulink by clicking the run button. 51 | * The robot shall move according to the motion command from Simulink. 52 | 53 | 54 | 55 | ## Citation 56 | Please cite the following article in your publications if it helps your research 🙏 : 57 | 58 | ```javascript 59 | @article{doi:10.1080/0951192X.2023.2177744, 60 | author = {M. Safeea and P. Neto}, 61 | title = {Model-based hardware in the loop control of collaborative robots: Simulink and Python based interfaces}, 62 | journal = {International Journal of Computer Integrated Manufacturing}, 63 | volume = {0}, 64 | number = {0}, 65 | pages = {1-13}, 66 | year = {2023}, 67 | publisher = {Taylor & Francis}, 68 | doi = {10.1080/0951192X.2023.2177744}, 69 | } 70 | ``` 71 | -------------------------------------- 72 | # Update log: 73 | 74 | * Uploaded on 3rd-March-2019 75 | * Update on 11th-June-2019 (Bugfix for 14R820 in Cartesian motion mode). 76 | * Update on 20th-Jan-2020 (README updated) 77 | -------------------------------------- 78 | 79 | Copyright: Mohammad Safeea. 80 | 81 | Provided under MIT license ([check the license](https://github.com/Modi1987/Simulink-iiwa-interface/blob/master/LICENSE)). 82 | 83 | --------------------------------------------------------------------------------