diff --git a/Cartesian motion open loop/iiwa/SimulinkIIWADirectServoCartesian.java b/Cartesian motion open loop/iiwa/SimulinkIIWADirectServoCartesian.java
new file mode 100644
index 0000000..405d86b
--- /dev/null
+++ b/Cartesian motion open loop/iiwa/SimulinkIIWADirectServoCartesian.java	
@@ -0,0 +1,343 @@
+package lbrExampleApplications;
+
+
+import static com.kuka.roboticsAPI.motionModel.BasicMotions.ptp;
+
+import java.io.IOException;
+import java.net.ServerSocket;
+import java.net.Socket;
+import java.util.Scanner;
+import java.util.StringTokenizer;
+import java.util.logging.Logger;
+
+import com.kuka.connectivity.motionModel.directServo.DirectServo;
+import com.kuka.connectivity.motionModel.directServo.IDirectServoRuntime;
+import com.kuka.generated.ioAccess.MediaFlangeIOGroup;
+import com.kuka.roboticsAPI.applicationModel.RoboticsAPIApplication;
+import com.kuka.roboticsAPI.controllerModel.Controller;
+import com.kuka.roboticsAPI.deviceModel.JointPosition;
+import com.kuka.roboticsAPI.deviceModel.LBR;
+import com.kuka.roboticsAPI.geometricModel.Frame;
+
+ 
+import java.io.IOException;
+import java.net.DatagramPacket;
+import java.net.DatagramSocket;
+import java.net.SocketException;
+import java.nio.ByteBuffer;
+
+
+/*
+ * SimulinkIIWA interface, soft real-time control in Cartesian position mode
+ * Copyright: Mohammad SAFEEA, 22nd/May/2018
+ * License: MIT license
+ * 
+ * This is a UDP server that receives motion commands from Simulink.
+ * This server works together with the Simulink example: (smartDirectServoCartesian.slx)
+ * 
+ * 
+ * Below you can find quick start instructions, for more info, please refer to the youtube
+ * video tutorials, a link for which is available in the repository main page.
+ * 
+ * Hardware setup:
+ * 1- KUKA iiwa 7R800 only, modifications in Simulink schematic are required for 14R820
+ * 2- External PC
+ * 3- A network between the PC and the robot, the connector X66
+ *    of the robot shall be used
+ * 
+ * Required software:
+ * 1- MATLAB with Simulink, the code was written using MATLAB2018a, it 
+ *    is recommended to use the same version.
+ * 2- Sunrise.Workbench, this program will be used to synchronize the java
+ *    code to the robot.
+ * 3- The Sunrise code was tested on (KUKA Sunrise.OS 1.11.0.7)
+ * 
+ * Setup instructions:
+ * 1- Add this class to a new project  using kuka's Sunrsie.Workbench.
+ * 2- Add reference to the Direct/SmartServo from inside the (StationSetup.cat) file. 
+ * 3- Change the following variables according to preference:
+ * 	* externa_PC_IP: change this variable according to the IP of the PC used for control.
+ * 
+ * 
+ * Utilization instructions:
+ * 1- Synchronize the project to the controller of the robot.
+ * 2- Run this application from the teach-pad of the robot,
+ *    This application has a time out of 10 seconds, if a connection
+ *    is not established during the 10 seconds interval the server will
+ *    turn off.
+ * 3- Start the Simulink example (smartDirectServoCartesian.slx)
+ *    from the external PC.
+ * 4- To change the timeout value, change the argument for the instruction
+ *    soc.setSoTimeout(10000),  the argument is in milliseconds.
+ * 
+ */
+
+
+public class SimulinkIIWADirectServoCartesian extends RoboticsAPIApplication
+{
+    private LBR _lbr;
+  
+    
+    public static double EEfServoPos[];
+
+    public static boolean loopFlag;
+
+    public static double jDispMax[];
+    public static double updateCycleJointPos[];
+    private UDPServer udpServer;
+    int _port;
+    
+    ServerSocket ss;
+    Socket soc;
+    
+
+
+    
+    @Override
+    public void initialize()
+    {
+        _lbr = getContext().getDeviceFromType(LBR.class);
+        EEfServoPos=new double[6];
+        for(int i=0;i<6;i++)
+        {
+        	EEfServoPos[i]=0;
+        }
+        loopFlag=true;
+    }
+
+    /**
+     * Move to an initial Position WARNING: MAKE SURE, THAT the pose is collision free.
+     */
+    private void moveToInitialPosition()
+    {
+        _lbr.move(
+        		ptp(0., Math.PI / 180 * 30.,0.,-Math.PI / 180 * 80.,0.,Math.PI / 180 * 70., 0.).setJointVelocityRel(0.15));
+        /* Note: The Validation itself justifies, that in this very time instance, the load parameter setting was
+         * sufficient. This does not mean by far, that the parameter setting is valid in the sequel or lifetime of this
+         * program */
+        try
+        {
+        	
+        }
+        catch (IllegalStateException e)
+        {
+            getLogger().info("Omitting validation failure for this sample\n"
+                    + e.getMessage());
+        }
+    }
+
+
+    /**
+     * Main Application Routine
+     */
+    @Override
+    public void run()
+    {
+        moveToInitialPosition();
+        
+        System.out.print("You have ten seconds to establish a connection with iiwa");
+        int portTemp=30002;
+        udpServer=new UDPServer(portTemp);
+        directServoStartCartezian();
+        
+    }
+    
+
+	private void directServoStartCartezian() {
+		
+		boolean doDebugPrints = false;
+
+        DirectServo aDirectServoMotion = new DirectServo(
+                _lbr.getCurrentJointPosition());
+
+        aDirectServoMotion.setMinimumTrajectoryExecutionTime(40e-3);
+
+        getLogger().info("Starting DirectServo motion in position control mode");
+        _lbr.moveAsync(aDirectServoMotion);
+
+        getLogger().info("Get the runtime of the DirectServo motion");
+        IDirectServoRuntime theDirectServoRuntime = aDirectServoMotion
+                .getRuntime();
+
+        Frame aFrame = theDirectServoRuntime.getCurrentCartesianDestination(_lbr.getFlange());
+        Frame destFrame = aFrame.copyWithRedundancy();
+        // Initiate the initial position 
+        EEfServoPos[0]=aFrame.getX();
+        EEfServoPos[1]=aFrame.getY();
+        EEfServoPos[2]=aFrame.getZ();
+        EEfServoPos[3]=aFrame.getAlphaRad();
+        EEfServoPos[4]=aFrame.getBetaRad();
+        EEfServoPos[5]=aFrame.getGammaRad();
+        
+        try
+        {
+            // do a cyclic loop
+            // Do some timing...
+            // in nanosec
+
+			while(loopFlag==true)
+			{
+
+                // ///////////////////////////////////////////////////////
+                // Insert your code here
+                // e.g Visual Servoing or the like
+                // Synchronize with the realtime system
+                theDirectServoRuntime.updateWithRealtimeSystem();
+                Frame msrPose = theDirectServoRuntime
+                        .getCurrentCartesianDestination(_lbr.getFlange());
+
+                if (doDebugPrints)
+                {
+                	getLogger().info("Current cartesian goal " + aFrame);
+                    getLogger().info("Current joint destination "
+                            + theDirectServoRuntime.getCurrentJointDestination());
+                }
+
+                Thread.sleep(1);
+
+                for(int kk=0;kk<6;kk++)
+                {
+                	EEfServoPos[kk]=udpServer.EEFpos[kk];
+                }
+                // update Cartesian positions
+                destFrame.setX(EEfServoPos[0]);
+                destFrame.setY(EEfServoPos[1]);
+                destFrame.setZ(EEfServoPos[2]);
+                destFrame.setAlphaRad(EEfServoPos[3]);
+                destFrame.setBetaRad(EEfServoPos[4]);
+                destFrame.setGammaRad(EEfServoPos[5]);
+
+                if (doDebugPrints)
+                {
+                    getLogger().info("New cartesian goal " + destFrame);
+                    getLogger().info("LBR position "
+	                            + _lbr.getCurrentCartesianPosition(_lbr
+	                                    .getFlange()));
+                    getLogger().info("Measured cartesian pose from runtime "
+	                            + msrPose);
+                }
+
+	                theDirectServoRuntime.setDestination(destFrame);
+                
+            }
+        }
+        catch (Exception e)
+        {
+            getLogger().info(e.getLocalizedMessage());
+            e.printStackTrace();
+            //Print statistics and parameters of the motion
+            getLogger().info("Simple Cartesian Test \n" + theDirectServoRuntime.toString());
+
+            getLogger().info("Stop the DirectServo motion");
+            
+        }
+        theDirectServoRuntime.stopMotion();
+        getLogger().info("Stop the DirectServo motion for the stop instruction was sent");
+		
+	}
+
+
+    /**
+     * Main routine, which starts the application
+     */
+    public static void main(String[] args)
+    {
+        SimulinkIIWADirectServoCartesian app = new SimulinkIIWADirectServoCartesian();
+        app.runApplication();
+    }
+ 
+    public class UDPServer implements Runnable{
+    	
+    	public double[] EEFpos={575.87,0.05,397.56,Math.PI,0,Math.PI};
+    	
+    	double stime=0;
+		double endtime=0;
+		
+    	int _port;
+    	int vectorSize=6;
+    	int packetCounter=0;
+    	
+    	byte[] buffer=new byte[8*7];
+    	UDPServer(int port)
+    	{
+    		Thread t=new Thread(this);
+    		t.setDaemon(true);
+    		t.start();
+    		_port=port;
+    	}
+    	
+    	public void run()
+    	{
+    		System.out.println("Program will terminate if comuncation is not established in 10 seconds");
+    			DatagramSocket soc=null;
+    			try {
+    				soc = new DatagramSocket(_port);
+    				soc.setSoTimeout(10000); // 5 seconds, if a time out occurred, turn off program
+    				DatagramPacket response=new DatagramPacket(buffer, buffer.length);
+    				while(true)
+    				{
+    					soc.receive(response);
+    					packetCounter=packetCounter+1;
+    					// String s= new String(buffer,0,response.getLength())
+    					 // System.out.println(response.getLength());
+    					if(response.getLength()==8*vectorSize)
+    					{
+    						if(packetCounter==1)
+    						{
+    							stime=System.currentTimeMillis();
+    						}
+    						else
+    						{
+    							endtime=System.currentTimeMillis();
+    						}
+    						byte[] daB=new byte[8];
+    						int counter=0;
+    						int index=0;
+    						while(counter <8*vectorSize)
+    						{
+	    						for(int i=0;i<8;i++)
+	    						{
+	    							daB[7-i]=buffer[counter];
+	    							counter=counter+1;
+	    						}
+	    						EEFpos[index]=bytesToDouble(daB);
+	    						index=index+1;	    						
+	    						//System.out.println(bytesToDouble(daB));
+    						}
+    					}
+    				}
+    			} catch (SocketException e) {
+    				// TODO Auto-generated catch block
+    				e.printStackTrace();
+    				//System.out.println(e.toString());
+    			} catch (IOException e) {
+    				// TODO Auto-generated catch block
+    				e.printStackTrace();
+    				System.out.println(e.toString());
+    			}
+    			if(soc==null)
+    			{}
+    			else
+    			{
+    				if(soc.isClosed())
+    				{}
+    				else
+    				{
+    					soc.close();
+    				}
+    			}
+   				SimulinkIIWADirectServoCartesian.loopFlag=false; //Turn off main loop 	
+   				double time=(endtime-stime)/1000;
+    			double rate=packetCounter/time;
+    			System.out.println("update rate, packets/second");
+    			System.out.println(rate);
+    	}
+    }
+    	
+    	
+    	public double bytesToDouble(byte[] b)
+    	{
+    		return ByteBuffer.wrap(b).getDouble();
+    	}
+
+}
diff --git a/Cartesian motion open loop/iiwa/SimulinkIIWASmartServoCartesian.java b/Cartesian motion open loop/iiwa/SimulinkIIWASmartServoCartesian.java
new file mode 100644
index 0000000..7e0ffc6
--- /dev/null
+++ b/Cartesian motion open loop/iiwa/SimulinkIIWASmartServoCartesian.java	
@@ -0,0 +1,381 @@
+package lbrExampleApplications;
+
+
+import static com.kuka.roboticsAPI.motionModel.BasicMotions.ptp;
+
+import java.io.IOException;
+import java.net.ServerSocket;
+import java.net.Socket;
+import java.util.Scanner;
+import java.util.StringTokenizer;
+import java.util.logging.Logger;
+
+import com.kuka.common.ThreadUtil;
+import com.kuka.connectivity.motionModel.directServo.DirectServo;
+import com.kuka.connectivity.motionModel.directServo.IDirectServoRuntime;
+import com.kuka.connectivity.motionModel.smartServo.ISmartServoRuntime;
+import com.kuka.connectivity.motionModel.smartServo.SmartServo;
+import com.kuka.generated.ioAccess.MediaFlangeIOGroup;
+import com.kuka.roboticsAPI.applicationModel.RoboticsAPIApplication;
+import com.kuka.roboticsAPI.controllerModel.Controller;
+import com.kuka.roboticsAPI.deviceModel.JointPosition;
+import com.kuka.roboticsAPI.deviceModel.LBR;
+import com.kuka.roboticsAPI.geometricModel.Frame;
+import com.kuka.roboticsAPI.geometricModel.LoadData;
+import com.kuka.roboticsAPI.geometricModel.ObjectFrame;
+import com.kuka.roboticsAPI.geometricModel.Tool;
+import com.kuka.roboticsAPI.geometricModel.math.XyzAbcTransformation;
+
+ 
+import java.io.IOException;
+import java.net.DatagramPacket;
+import java.net.DatagramSocket;
+import java.net.SocketException;
+import java.nio.ByteBuffer;
+
+/*
+ * SimulinkIIWA interface, soft real-time control in Cartesian position mode
+ * Copyright: Mohammad SAFEEA, 22nd/May/2018
+ * License: MIT license
+ * 
+ * This is a UDP server that receives motion commands from Simulink.
+ * This server works together with the Simulink example: (smartDirectServoCartesian.slx)
+ * 
+ *  
+ * Below you can find quick start instructions, for more info, please refer to the youtube
+ * video tutorials, a link for which is available in the repository main page.
+ * 
+ * Hardware setup:
+ * 1- KUKA iiwa 7R800, modifications are required for the Simulink schematics for 14R820
+ * 2- External PC
+ * 3- A network between the PC and the robot, the connector X66
+ *    of the robot shall be used
+ * 
+ * Required software:
+ * 1- MATLAB with Simulink, the code was written using MATLAB2018a, it 
+ *    is recommended to use the same version.
+ * 2- Sunrise.Workbench, this program will be used to synchronize the java
+ *    code to the robot.
+ * 3- The Sunrise code was tested on (KUKA Sunrise.OS 1.11.0.7)
+ * 
+ * Setup instructions:
+ * 1- Add this class to a new project  using kuka's Sunrsie.Workbench.
+ * 2- Add reference to the Direct/SmartServo from inside the (StationSetup.cat) file. 
+ * 3- Change the following variables according to your preference:
+ * 	* externa_PC_IP: change this variable according to the IP of the PC used for control.
+ *  * TRANSLATION_OF_TOOL: translation of tool center point with respect to flange frame.
+ *  * massOfTool: mass of the tool
+ *  * toolsCOMCoordiantes: coordinates of center of mass of tool with regards to the frame of the flange.
+ *
+ * 
+ * Utilization instructions:
+ * 1- Synchronize the project to the controller of the robot.
+ * 2- Run this application from the teach-pad of the robot,
+ *    This application has a time out of 10 seconds, if a connection
+ *    is not established during the 10 seconds interval the server will
+ *    turn off.
+ * 3- Start the Simulink example (smartDirectServoCartesian.slx)
+ *    from the external PC.
+ * 4- To change the timeout value, change the argument for the instruction
+ *    soc.setSoTimeout(10000),  the argument is in milliseconds.
+ * 
+ */
+
+public class SimulinkIIWASmartServoCartesian extends RoboticsAPIApplication
+{
+    private LBR _lbr;
+  
+    
+    public static double EEfServoPos[];
+
+    public static boolean loopFlag;
+
+
+    public static double updateCycleJointPos[];
+    private UDPServer udpServer;
+    // Tool Data
+    private Tool _toolAttachedToLBR;
+    private LoadData _loadData;
+    private static final String TOOL_FRAME = "toolFrame";
+    private static final double[] TRANSLATION_OF_TOOL = { 0, 0, 0 };
+    private static final double MASS = 0.4;
+    private static final double[] CENTER_OF_MASS_IN_MILLIMETER = { 0, 0, 75 };
+
+    private static final int MILLI_SLEEP_TO_EMULATE_COMPUTATIONAL_EFFORT = 1;
+
+
+    
+    @Override
+    public void initialize()
+    {
+        _lbr = getContext().getDeviceFromType(LBR.class);
+        EEfServoPos=new double[6];
+        for(int i=0;i<6;i++)
+        {
+        	EEfServoPos[i]=0;
+        }
+        loopFlag=true;
+		addLoadData();
+    }
+
+    private void addLoadData()
+    {
+    	 // Create a Tool by Hand this is the tool we want to move with some mass
+        // properties and a TCP-Z-offset of 100.
+        _loadData = new LoadData();
+        _loadData.setMass(MASS);
+        _loadData.setCenterOfMass(
+                CENTER_OF_MASS_IN_MILLIMETER[0], CENTER_OF_MASS_IN_MILLIMETER[1],
+                CENTER_OF_MASS_IN_MILLIMETER[2]);
+        _toolAttachedToLBR = new Tool("Tool", _loadData);
+
+        XyzAbcTransformation trans = XyzAbcTransformation.ofTranslation(
+                TRANSLATION_OF_TOOL[0], TRANSLATION_OF_TOOL[1],
+                TRANSLATION_OF_TOOL[2]);
+        ObjectFrame aTransformation = _toolAttachedToLBR.addChildFrame(TOOL_FRAME
+                + "(TCP)", trans);
+        _toolAttachedToLBR.setDefaultMotionFrame(aTransformation);
+        // Attach tool to the robot
+        _toolAttachedToLBR.attachTo(_lbr.getFlange());
+    }
+
+    /**
+     * Move to an initial Position WARNING: MAKE SURE, THAT the pose is collision free.
+     */
+    private void moveToInitialPosition()
+    {
+        _lbr.move(
+        		ptp(0., Math.PI / 180 * 30.,0.,-Math.PI / 180 * 80.,0.,Math.PI / 180 * 70., 0.).setJointVelocityRel(0.15));
+        /* Note: The Validation itself justifies, that in this very time instance, the load parameter setting was
+         * sufficient. This does not mean by far, that the parameter setting is valid in the sequel or lifetime of this
+         * program */
+        try
+        {
+        	
+        }
+        catch (IllegalStateException e)
+        {
+            getLogger().info("Omitting validation failure for this sample\n"
+                    + e.getMessage());
+        }
+    }
+
+
+    /**
+     * Main Application Routine
+     */
+    @Override
+    public void run()
+    {
+        moveToInitialPosition();
+        
+        System.out.print("You have ten seconds to establish a connection with iiwa");
+        int portTemp=30002;
+        udpServer=new UDPServer(portTemp);
+        smartServoStartCartezian();
+        
+    }
+    
+
+	private void smartServoStartCartezian() {
+		
+        boolean doDebugPrints = false;
+
+        SmartServo aSmartServoMotion = new SmartServo(
+                _lbr.getCurrentJointPosition());
+
+        aSmartServoMotion.useTrace(true);
+
+        aSmartServoMotion.setMinimumTrajectoryExecutionTime(5e-3);
+
+        getLogger().info("Starting SmartServo motion in position control mode");
+        _toolAttachedToLBR.moveAsync(aSmartServoMotion);
+
+        getLogger().info("Get the runtime of the SmartServo motion");
+        ISmartServoRuntime theServoRuntime = aSmartServoMotion
+                .getRuntime();
+
+        Frame aFrame = theServoRuntime.getCurrentCartesianDestination(_toolAttachedToLBR.getDefaultMotionFrame());
+        Frame destFrame = aFrame.copyWithRedundancy();
+        // Initiate the initial position 
+        EEfServoPos[0]=aFrame.getX();
+        EEfServoPos[1]=aFrame.getY();
+        EEfServoPos[2]=aFrame.getZ();
+        EEfServoPos[3]=aFrame.getAlphaRad();
+        EEfServoPos[4]=aFrame.getBetaRad();
+        EEfServoPos[5]=aFrame.getGammaRad();
+        
+        
+        try
+        {
+            // do a cyclic loop
+            // Do some timing...
+            // in nanosec
+            while(loopFlag)
+            {
+                // Insert your code here
+                // e.g Visual Servoing or the like
+
+                // Synchronize with the realtime system
+                theServoRuntime.updateWithRealtimeSystem();
+
+                // Get the measured position 
+                Frame msrPose = theServoRuntime
+                        .getCurrentCartesianDestination(_lbr.getFlange());
+
+                if (doDebugPrints)
+                {
+                    getLogger().info("Current cartesian goal " + aFrame);
+                    getLogger().info("Current joint destination "
+                            + theServoRuntime.getCurrentJointDestination());
+                }
+
+                // Do some Computation
+                // emulate some computational effort - or waiting for external
+                // stuff
+                ThreadUtil.milliSleep(MILLI_SLEEP_TO_EMULATE_COMPUTATIONAL_EFFORT);
+
+                // compute a new commanded position
+                for(int kk=0;kk<6;kk++)
+                {
+                	EEfServoPos[kk]=udpServer.EEFpos[kk];
+                }
+                // update Cartesian positions
+                destFrame.setX(EEfServoPos[0]);
+                destFrame.setY(EEfServoPos[1]);
+                destFrame.setZ(EEfServoPos[2]);
+                destFrame.setAlphaRad(EEfServoPos[3]);
+                destFrame.setBetaRad(EEfServoPos[4]);
+                destFrame.setGammaRad(EEfServoPos[5]);
+                
+                if (doDebugPrints)
+                {
+                    getLogger().info("New cartesian goal " + destFrame);
+                    getLogger().info("LBR position "
+                            + _lbr.getCurrentCartesianPosition(_lbr
+                                    .getFlange()));
+                    getLogger().info("Measured cartesian pose from runtime "
+                            + msrPose);
+                }
+
+                theServoRuntime.setDestination(destFrame);
+            }
+        }
+        catch (Exception e)
+        {
+            getLogger().error(e.toString());
+            e.printStackTrace();
+        }
+
+        //Print statistics and parameters of the motion
+        getLogger().info("Simple cartesian test " + theServoRuntime.toString());
+
+        getLogger().info("Stop the SmartServo motion");
+        theServoRuntime.stopMotion();
+    }
+
+
+    /**
+     * Main routine, which starts the application
+     */
+    public static void main(String[] args)
+    {
+        SimulinkIIWASmartServoCartesian app = new SimulinkIIWASmartServoCartesian();
+        app.runApplication();
+    }
+ 
+    public class UDPServer implements Runnable{
+    	
+    	public double[] EEFpos={575.87,0.05,397.56,Math.PI,0,Math.PI};
+    	
+    	double stime=0;
+		double endtime=0;
+		
+    	int _port;
+    	int vectorSize=6;
+    	int packetCounter=0;
+    	
+    	byte[] buffer=new byte[8*7];
+    	UDPServer(int port)
+    	{
+    		Thread t=new Thread(this);
+    		t.setDaemon(true);
+    		t.start();
+    		_port=port;
+    	}
+    	
+    	public void run()
+    	{
+    		System.out.println("Program will terminate if comuncation is not established in 10 seconds");
+    			DatagramSocket soc=null;
+    			try {
+    				soc = new DatagramSocket(_port);
+    				soc.setSoTimeout(10000); // 5 seconds, if a time out occurred, turn off program
+    				DatagramPacket response=new DatagramPacket(buffer, buffer.length);
+    				while(true)
+    				{
+    					soc.receive(response);
+    					packetCounter=packetCounter+1;
+    					// String s= new String(buffer,0,response.getLength())
+    					 // System.out.println(response.getLength());
+    					if(response.getLength()==8*vectorSize)
+    					{
+    						if(packetCounter==1)
+    						{
+    							stime=System.currentTimeMillis();
+    						}
+    						else
+    						{
+    							endtime=System.currentTimeMillis();
+    						}
+    						byte[] daB=new byte[8];
+    						int counter=0;
+    						int index=0;
+    						while(counter <8*vectorSize)
+    						{
+	    						for(int i=0;i<8;i++)
+	    						{
+	    							daB[7-i]=buffer[counter];
+	    							counter=counter+1;
+	    						}
+	    						EEFpos[index]=bytesToDouble(daB);
+	    						index=index+1;	    						
+	    						//System.out.println(bytesToDouble(daB));
+    						}
+    					}
+    				}
+    			} catch (SocketException e) {
+    				// TODO Auto-generated catch block
+    				e.printStackTrace();
+    				//System.out.println(e.toString());
+    			} catch (IOException e) {
+    				// TODO Auto-generated catch block
+    				e.printStackTrace();
+    				System.out.println(e.toString());
+    			}
+    			if(soc==null)
+    			{}
+    			else
+    			{
+    				if(soc.isClosed())
+    				{}
+    				else
+    				{
+    					soc.close();
+    				}
+    			}
+   				SimulinkIIWASmartServoCartesian.loopFlag=false; //Turn off main loop 	
+   				double time=(endtime-stime)/1000;
+    			double rate=packetCounter/time;
+    			System.out.println("update rate, packets/second");
+    			System.out.println(rate);
+    	}
+    }
+    	
+    	
+    	public double bytesToDouble(byte[] b)
+    	{
+    		return ByteBuffer.wrap(b).getDouble();
+    	}
+
+}
diff --git a/Cartesian motion open loop/simulink/smartDirectServoCartesian.slx b/Cartesian motion open loop/simulink/smartDirectServoCartesian.slx
new file mode 100644
index 0000000..6cffc02
Binary files /dev/null and b/Cartesian motion open loop/simulink/smartDirectServoCartesian.slx differ
diff --git a/Cartesian motion open loop/simulink/smartDirectServoCartesian.slxc b/Cartesian motion open loop/simulink/smartDirectServoCartesian.slxc
new file mode 100644
index 0000000..ae21de5
Binary files /dev/null and b/Cartesian motion open loop/simulink/smartDirectServoCartesian.slxc differ
diff --git a/Cartesian motion with feedback/iiwa/SimulinkFIIWADirectServoCartesian.java b/Cartesian motion with feedback/iiwa/SimulinkFIIWADirectServoCartesian.java
new file mode 100644
index 0000000..aa7cd3e
--- /dev/null
+++ b/Cartesian motion with feedback/iiwa/SimulinkFIIWADirectServoCartesian.java	
@@ -0,0 +1,467 @@
+package lbrExampleApplications;
+
+
+import static com.kuka.roboticsAPI.motionModel.BasicMotions.ptp;
+
+import java.io.IOException;
+import java.net.ServerSocket;
+import java.net.Socket;
+import java.util.Scanner;
+import java.util.StringTokenizer;
+import java.util.logging.Logger;
+
+import com.kuka.connectivity.motionModel.directServo.DirectServo;
+import com.kuka.connectivity.motionModel.directServo.IDirectServoRuntime;
+import com.kuka.generated.ioAccess.MediaFlangeIOGroup;
+import com.kuka.roboticsAPI.applicationModel.RoboticsAPIApplication;
+import com.kuka.roboticsAPI.controllerModel.Controller;
+import com.kuka.roboticsAPI.deviceModel.JointPosition;
+import com.kuka.roboticsAPI.deviceModel.LBR;
+import com.kuka.roboticsAPI.geometricModel.Frame;
+import com.kuka.roboticsAPI.geometricModel.World;
+import com.kuka.roboticsAPI.sensorModel.ForceSensorData;
+import com.kuka.roboticsAPI.sensorModel.TorqueSensorData;
+
+ 
+import java.io.IOException;
+import java.net.DatagramPacket;
+import java.net.DatagramSocket;
+import java.net.InetAddress;
+import java.net.SocketException;
+import java.net.UnknownHostException;
+import java.nio.ByteBuffer;
+
+/*
+ * SimulinkIIWA interface, soft real-time control in Cartesian position mode
+ * Copyright: Mohammad SAFEEA, 22nd/May/2018
+ * License: MIT license
+ * 
+ * This is a UDP server that receives motion commands from Simulink.
+ * This server works together with the Simulink example: (feedBackSmartDirectServoCartesian1.slx)
+ * 
+ * Also this script returns back a feedback about the  (1)actual joints positions,
+ * (2)the external torques and (3)the measured torques of all the joints to Simulink.
+ * (4) EEF force moment.
+ * 
+ * Below you can find quick start instructions, for more info, please refer to the youtube
+ * video tutorials, a link for which is available in the repository main page.
+ * 
+ * Hardware setup:
+ * 1- KUKA iiwa 7R800 only, modifications in Simulink schematic are required for 14R820
+ * 2- External PC
+ * 3- A network between the PC and the robot, the connector X66
+ *    of the robot shall be used
+ * 
+ * Required software:
+ * 1- MATLAB with Simulink, the code was written using MATLAB2018a, it 
+ *    is recommended to use the same version.
+ * 2- Sunrise.Workbench, this program will be used to synchronize the java
+ *    code to the robot.
+ * 3- The Sunrise code was tested on (KUKA Sunrise.OS 1.11.0.7)
+ * 
+ * Setup instructions:
+ * 1- Add this class to a new project  using kuka's Sunrsie.Workbench.
+ * 2- Add reference to the Direct/SmartServo from inside the (StationSetup.cat) file. 
+ * 3- Change the following variables according to preference:
+ * 	* externa_PC_IP: change this variable according to the IP of the PC used for control.
+ * 
+ * 
+ * Utilization instructions:
+ * 1- Synchronize the project to the controller of the robot.
+ * 2- Run this application from the teach-pad of the robot,
+ *    This application has a time out of 10 seconds, if a connection
+ *    is not established during the 10 seconds interval the server will
+ *    turn off.
+ * 3- Start the Simulink example (feedBackSmartDirectServoCartesian1.slx)
+ *    from the external PC.
+ * 4- To change the timeout value, change the argument for the instruction
+ *    soc.setSoTimeout(10000),  the argument is in milliseconds.
+ * 
+ */
+
+
+public class SimulinkFIIWADirectServoCartesian extends RoboticsAPIApplication
+{
+	
+	private static String externa_PC_IP="172.31.69.55";
+    private LBR _lbr;   
+    public static double EEfServoPos[];
+    public static boolean loopFlag;
+    public static double jDispMax[];
+    public static double updateCycleJointPos[];
+    private UDPServer udpServer;
+    int _port;
+    
+    ServerSocket ss;
+    Socket soc;
+    
+
+
+    
+    @Override
+    public void initialize()
+    {
+        _lbr = getContext().getDeviceFromType(LBR.class);
+        EEfServoPos=new double[6];
+        for(int i=0;i<6;i++)
+        {
+        	EEfServoPos[i]=0;
+        }
+        loopFlag=true;
+    }
+
+    /**
+     * Move to an initial Position WARNING: MAKE SURE, THAT the pose is collision free.
+     */
+    private void moveToInitialPosition()
+    {
+        _lbr.move(
+        		ptp(0., Math.PI / 180 * 30.,0.,-Math.PI / 180 * 80.,0.,Math.PI / 180 * 70., 0.).setJointVelocityRel(0.15));
+        /* Note: The Validation itself justifies, that in this very time instance, the load parameter setting was
+         * sufficient. This does not mean by far, that the parameter setting is valid in the sequel or lifetime of this
+         * program */
+        try
+        {
+        	
+        }
+        catch (IllegalStateException e)
+        {
+            getLogger().info("Omitting validation failure for this sample\n"
+                    + e.getMessage());
+        }
+    }
+
+
+    /**
+     * Main Application Routine
+     */
+    @Override
+    public void run()
+    {
+        moveToInitialPosition();
+        
+        System.out.print("You have ten seconds to establish a connection with iiwa");
+        int portTemp=30002;
+        udpServer=new UDPServer(portTemp);
+        new UDPPublisher(externa_PC_IP);
+        directServoStartCartezian();
+        
+    }
+    
+
+	private void directServoStartCartezian() {
+		
+		boolean doDebugPrints = false;
+
+        DirectServo aDirectServoMotion = new DirectServo(
+                _lbr.getCurrentJointPosition());
+
+        aDirectServoMotion.setMinimumTrajectoryExecutionTime(40e-3);
+
+        getLogger().info("Starting DirectServo motion in position control mode");
+        _lbr.moveAsync(aDirectServoMotion);
+
+        getLogger().info("Get the runtime of the DirectServo motion");
+        IDirectServoRuntime theDirectServoRuntime = aDirectServoMotion
+                .getRuntime();
+
+        Frame aFrame = theDirectServoRuntime.getCurrentCartesianDestination(_lbr.getFlange());
+        Frame destFrame = aFrame.copyWithRedundancy();
+        // Initiate the initial position 
+        EEfServoPos[0]=aFrame.getX();
+        EEfServoPos[1]=aFrame.getY();
+        EEfServoPos[2]=aFrame.getZ();
+        EEfServoPos[3]=aFrame.getAlphaRad();
+        EEfServoPos[4]=aFrame.getBetaRad();
+        EEfServoPos[5]=aFrame.getGammaRad();
+        
+        try
+        {
+            // do a cyclic loop
+            // Do some timing...
+            // in nanosec
+
+			while(loopFlag==true)
+			{
+
+                // ///////////////////////////////////////////////////////
+                // Insert your code here
+                // e.g Visual Servoing or the like
+                // Synchronize with the realtime system
+                theDirectServoRuntime.updateWithRealtimeSystem();
+                Frame msrPose = theDirectServoRuntime
+                        .getCurrentCartesianDestination(_lbr.getFlange());
+
+                if (doDebugPrints)
+                {
+                	getLogger().info("Current cartesian goal " + aFrame);
+                    getLogger().info("Current joint destination "
+                            + theDirectServoRuntime.getCurrentJointDestination());
+                }
+
+                Thread.sleep(1);
+
+                for(int kk=0;kk<6;kk++)
+                {
+                	EEfServoPos[kk]=udpServer.EEFpos[kk];
+                }
+                // update Cartesian positions
+                destFrame.setX(EEfServoPos[0]);
+                destFrame.setY(EEfServoPos[1]);
+                destFrame.setZ(EEfServoPos[2]);
+                destFrame.setAlphaRad(EEfServoPos[3]);
+                destFrame.setBetaRad(EEfServoPos[4]);
+                destFrame.setGammaRad(EEfServoPos[5]);
+
+                if (doDebugPrints)
+                {
+                    getLogger().info("New cartesian goal " + destFrame);
+                    getLogger().info("LBR position "
+	                            + _lbr.getCurrentCartesianPosition(_lbr
+	                                    .getFlange()));
+                    getLogger().info("Measured cartesian pose from runtime "
+	                            + msrPose);
+                }
+
+	                theDirectServoRuntime.setDestination(destFrame);
+                
+            }
+        }
+        catch (Exception e)
+        {
+            getLogger().info(e.getLocalizedMessage());
+            e.printStackTrace();
+            //Print statistics and parameters of the motion
+            getLogger().info("Simple Cartesian Test \n" + theDirectServoRuntime.toString());
+
+            getLogger().info("Stop the DirectServo motion");
+            
+        }
+        theDirectServoRuntime.stopMotion();
+        getLogger().info("Stop the DirectServo motion for the stop instruction was sent");
+		
+	}
+
+
+    /**
+     * Main routine, which starts the application
+     */
+    public static void main(String[] args)
+    {
+        SimulinkFIIWADirectServoCartesian app = new SimulinkFIIWADirectServoCartesian();
+        app.runApplication();
+    }
+ 
+    public class UDPServer implements Runnable{
+    	
+    	public double[] EEFpos={575.87,0.05,397.56,Math.PI,0,Math.PI};
+    	
+    	double stime=0;
+		double endtime=0;
+		
+    	int _port;
+    	int vectorSize=6;
+    	int packetCounter=0;
+    	
+    	byte[] buffer=new byte[8*7];
+    	UDPServer(int port)
+    	{
+    		Thread t=new Thread(this);
+    		t.setDaemon(true);
+    		t.start();
+    		_port=port;
+    	}
+    	
+    	public void run()
+    	{
+    		System.out.println("Program will terminate if comuncation is not established in 10 seconds");
+    			DatagramSocket soc=null;
+    			try {
+    				soc = new DatagramSocket(_port);
+    				soc.setSoTimeout(10000); // 10 seconds, if a time out occurred, turn off program
+    				DatagramPacket response=new DatagramPacket(buffer, buffer.length);
+    				while(true)
+    				{
+    					soc.receive(response);
+    					packetCounter=packetCounter+1;
+    					// String s= new String(buffer,0,response.getLength())
+    					 // System.out.println(response.getLength());
+    					if(response.getLength()==8*vectorSize)
+    					{
+    						if(packetCounter==1)
+    						{
+    							stime=System.currentTimeMillis();
+    						}
+    						else
+    						{
+    							endtime=System.currentTimeMillis();
+    						}
+    						byte[] daB=new byte[8];
+    						int counter=0;
+    						int index=0;
+    						while(counter <8*vectorSize)
+    						{
+	    						for(int i=0;i<8;i++)
+	    						{
+	    							daB[7-i]=buffer[counter];
+	    							counter=counter+1;
+	    						}
+	    						EEFpos[index]=bytesToDouble(daB);
+	    						index=index+1;	    						
+	    						//System.out.println(bytesToDouble(daB));
+    						}
+    					}
+    				}
+    			} catch (SocketException e) {
+    				// TODO Auto-generated catch block
+    				e.printStackTrace();
+    				//System.out.println(e.toString());
+    			} catch (IOException e) {
+    				// TODO Auto-generated catch block
+    				e.printStackTrace();
+    				System.out.println(e.toString());
+    			}
+    			if(soc==null)
+    			{}
+    			else
+    			{
+    				if(soc.isClosed())
+    				{}
+    				else
+    				{
+    					soc.close();
+    				}
+    			}
+   				SimulinkFIIWADirectServoCartesian.loopFlag=false; //Turn off main loop 	
+   				double time=(endtime-stime)/1000;
+    			double rate=packetCounter/time;
+    			System.out.println("update rate, packets/second");
+    			System.out.println(rate);
+    	}
+    }
+
+    
+    public class UDPPublisher implements Runnable{
+    	
+    	String ip;
+    	UDPPublisher(String ip)
+    	{
+    		this.ip=ip;
+    		Thread t=new Thread(this);
+    		t.setDaemon(true);
+    		t.start();
+    	}
+    	public void run()
+    	{
+    		broadcastData();
+    	}
+    	private void broadcastData() {
+    		// TODO Auto-generated method stub
+    		String message="Publishing robot state using UDP at port 30008 to destination: "+ ip;
+    		System.out.println(message);
+    		message="Streamed message: 7 actual joint angles, 7 measured torques, 7 external torques,";
+    		System.out.println(message);
+    		message="State streaming frequency is approximatly at 25HZ";
+    		System.out.println(message);
+    		int port=30008;
+    		InetAddress host;
+    		int numOfDoubles=7+7+7+6;// 7 actual joint angles, 7 measured torques, 7 external torques, 7 force at flange
+    		double[] doubleVals=new double[numOfDoubles];
+    		
+    		byte[] buffer= new byte[8*numOfDoubles];
+    		
+    		try {
+    			host = InetAddress.getByName(ip);
+    			DatagramSocket soc=new DatagramSocket();
+    			while(loopFlag)
+    			{
+    				// Feedback of actual joints positions
+    				JointPosition jpos=_lbr.getCurrentJointPosition();
+    				int valsCounter=0;
+    				for(int i=0;i<7;i++)
+    				{
+    					doubleVals[valsCounter]=jpos.get(i);
+    					valsCounter=valsCounter+1;
+    				}
+    				// Feedback of measured torques
+    				double[] m_tor=_lbr.getMeasuredTorque().getTorqueValues();
+    				for(int i=0;i<7;i++)
+    				{
+    					doubleVals[valsCounter]=m_tor[i];
+    					valsCounter=valsCounter+1;
+    				}
+    				// Feedback of external torques
+    				double[] ex_tor=_lbr.getExternalTorque().getTorqueValues();
+    				for(int i=0;i<7;i++)
+    				{
+    					doubleVals[valsCounter]=ex_tor[i];
+    					valsCounter=valsCounter+1;
+    				}
+    				// Feedback of forces at flange    				
+    				ForceSensorData f_at_flange=_lbr.getExternalForceTorque(_lbr.getFlange(),World.Current.getRootFrame());				
+    				doubleVals[valsCounter]=f_at_flange.getForce().getX();
+    				valsCounter=valsCounter+1;
+    				doubleVals[valsCounter]=f_at_flange.getForce().getY();
+    				valsCounter=valsCounter+1;
+    				doubleVals[valsCounter]=f_at_flange.getForce().getZ();
+    				valsCounter=valsCounter+1;
+    				doubleVals[valsCounter]=f_at_flange.getTorque().getX();
+    				valsCounter=valsCounter+1;
+    				doubleVals[valsCounter]=f_at_flange.getTorque().getY();
+    				valsCounter=valsCounter+1;
+    				doubleVals[valsCounter]=f_at_flange.getTorque().getZ();
+    				valsCounter=valsCounter+1;
+    				
+    				int count=0;
+    				for(int i=0;i<numOfDoubles;i++)
+    				{
+    					byte[] bytes = new byte[8];
+    					double value=doubleVals[i];
+    				    ByteBuffer.wrap(bytes).putDouble(value);
+    				    for(int j=0;j<8;j++)
+    				    {
+    				    	buffer[count]=bytes[7-j];
+    				    	count=count+1;
+    				    }
+    				}
+    				
+    				DatagramPacket packet= new DatagramPacket(buffer, buffer.length,host, port);
+    				soc.send(packet);
+    				Thread.sleep(36);
+    			}
+    			message="Robot state publisher is terminated";
+        		System.out.println(message);
+    				
+    		} catch (UnknownHostException e1) {
+    			// TODO Auto-generated catch block
+    			//e1.printStackTrace();
+    			System.out.println("Error in UDP publisher, 1");
+    			System.out.println(e1.toString());
+    		} catch (SocketException e) {
+    			// TODO Auto-generated catch block
+    			//e.printStackTrace();
+    			System.out.println("Error in UDP publisher, 2");
+    			System.out.println(e.toString());
+    		} catch (IOException e) {
+    			// TODO Auto-generated catch block
+    			//e.printStackTrace();
+    			System.out.println("Error in UDP publisher, 3");
+    			System.out.println(e.toString());
+    		} catch (InterruptedException e) {
+    			// TODO Auto-generated catch block
+    			//e.printStackTrace();
+    			System.out.println("Error in UDP publisher, 4");
+    			System.out.println(e.toString());
+    		}
+    	}
+
+    }
+    	
+ 
+    
+    	public double bytesToDouble(byte[] b)
+    	{
+    		return ByteBuffer.wrap(b).getDouble();
+    	}
+
+}
diff --git a/Cartesian motion with feedback/iiwa/SimulinkFIIWASmartServoCartesian.java b/Cartesian motion with feedback/iiwa/SimulinkFIIWASmartServoCartesian.java
new file mode 100644
index 0000000..7ef6f65
--- /dev/null
+++ b/Cartesian motion with feedback/iiwa/SimulinkFIIWASmartServoCartesian.java	
@@ -0,0 +1,506 @@
+package lbrExampleApplications;
+
+
+import static com.kuka.roboticsAPI.motionModel.BasicMotions.ptp;
+
+import java.io.IOException;
+import java.net.ServerSocket;
+import java.net.Socket;
+import java.util.Scanner;
+import java.util.StringTokenizer;
+import java.util.logging.Logger;
+
+import com.kuka.common.ThreadUtil;
+import com.kuka.connectivity.motionModel.directServo.DirectServo;
+import com.kuka.connectivity.motionModel.directServo.IDirectServoRuntime;
+import com.kuka.connectivity.motionModel.smartServo.ISmartServoRuntime;
+import com.kuka.connectivity.motionModel.smartServo.SmartServo;
+import com.kuka.generated.ioAccess.MediaFlangeIOGroup;
+import com.kuka.roboticsAPI.applicationModel.RoboticsAPIApplication;
+import com.kuka.roboticsAPI.controllerModel.Controller;
+import com.kuka.roboticsAPI.deviceModel.JointPosition;
+import com.kuka.roboticsAPI.deviceModel.LBR;
+import com.kuka.roboticsAPI.geometricModel.Frame;
+import com.kuka.roboticsAPI.geometricModel.LoadData;
+import com.kuka.roboticsAPI.geometricModel.ObjectFrame;
+import com.kuka.roboticsAPI.geometricModel.Tool;
+import com.kuka.roboticsAPI.geometricModel.World;
+import com.kuka.roboticsAPI.geometricModel.math.XyzAbcTransformation;
+import com.kuka.roboticsAPI.sensorModel.ForceSensorData;
+
+ 
+import java.io.IOException;
+import java.net.DatagramPacket;
+import java.net.DatagramSocket;
+import java.net.InetAddress;
+import java.net.SocketException;
+import java.net.UnknownHostException;
+import java.nio.ByteBuffer;
+
+import lbrExampleApplications.SimulinkFIIWADirectServoCartesian.UDPPublisher;
+
+/*
+ * SimulinkIIWA interface, soft real-time control in Cartesian position mode
+ * Copyright: Mohammad SAFEEA, 22nd/May/2018
+ * License: MIT license
+ * 
+ * This is a UDP server that receives motion commands from Simulink.
+ * This server works together with the Simulink example: (feedBackSmartDirectServoCartesian.slx)
+ * 
+ * Also this script returns back a feedback about the  (1)actual joints positions,
+ * (2)the external torques and (3)the measured torques of all the joints to Simulink.
+ * (4) EEF force moment.
+ * 
+ * Below you can find quick start instructions, for more info, please refer to the youtube
+ * video tutorials, a link for which is available in the repository main page.
+ * 
+ * Hardware setup:
+ * 1- KUKA iiwa 7R800, modifications are required for the Simulink schematics for 14R820
+ * 2- External PC
+ * 3- A network between the PC and the robot, the connector X66
+ *    of the robot shall be used
+ * 
+ * Required software:
+ * 1- MATLAB with Simulink, the code was written using MATLAB2018a, it 
+ *    is recommended to use the same version.
+ * 2- Sunrise.Workbench, this program will be used to synchronize the java
+ *    code to the robot.
+ * 3- The Sunrise code was tested on (KUKA Sunrise.OS 1.11.0.7)
+ * 
+ * Setup instructions:
+ * 1- Add this class to a new project  using kuka's Sunrsie.Workbench.
+ * 2- Add reference to the Direct/SmartServo from inside the (StationSetup.cat) file. 
+ * 3- Change the following variables according to your preference:
+ * 	* externa_PC_IP: change this variable according to the IP of the PC used for control.
+ *  * TRANSLATION_OF_TOOL: translation of tool center point with respect to flange frame.
+ *  * massOfTool: mass of the tool
+ *  * toolsCOMCoordiantes: coordinates of center of mass of tool with regards to the frame of the flange.
+ *
+ * 
+ * Utilization instructions:
+ * 1- Synchronize the project to the controller of the robot.
+ * 2- Run this application from the teach-pad of the robot,
+ *    This application has a time out of 10 seconds, if a connection
+ *    is not established during the 10 seconds interval the server will
+ *    turn off.
+ * 3- Start the Simulink example (feedBackSmartDirectServoCartesian.slx)
+ *    from the external PC.
+ * 4- To change the timeout value, change the argument for the instruction
+ *    soc.setSoTimeout(10000),  the argument is in milliseconds.
+ * 
+ */
+
+public class SimulinkFIIWASmartServoCartesian extends RoboticsAPIApplication
+{
+	
+	private static String externa_PC_IP="172.31.69.55";
+    private LBR _lbr;     
+    public static double EEfServoPos[];
+    public static boolean loopFlag;
+    public static double updateCycleJointPos[];
+    private UDPServer udpServer;
+    // Tool Data
+    private Tool _toolAttachedToLBR;
+    private LoadData _loadData;
+    private static final String TOOL_FRAME = "toolFrame";
+    private static final double[] TRANSLATION_OF_TOOL = { 0, 0, 0 };
+    private static final double MASS = 0.4;
+    private static final double[] CENTER_OF_MASS_IN_MILLIMETER = { 0, 0, 75 };
+
+    private static final int MILLI_SLEEP_TO_EMULATE_COMPUTATIONAL_EFFORT = 1;
+
+
+
+    
+    public class UDPPublisher implements Runnable{
+    	
+    	String ip;
+    	UDPPublisher(String ip)
+    	{
+    		this.ip=ip;
+    		Thread t=new Thread(this);
+    		t.setDaemon(true);
+    		t.start();
+    	}
+    	public void run()
+    	{
+    		broadcastData();
+    	}
+    	private void broadcastData() {
+    		// TODO Auto-generated method stub
+    		String message="Publishing robot state using UDP at port 30008 to destination: "+ ip;
+    		System.out.println(message);
+    		message="Streamed message: 7 actual joint angles, 7 measured torques, 7 external torques,";
+    		System.out.println(message);
+    		message="State streaming frequency is approximatly at 25HZ";
+    		System.out.println(message);
+    		int port=30008;
+    		InetAddress host;
+    		int numOfDoubles=7+7+7+6;// 7 actual joint angles, 7 measured torques, 7 external torques, 7 force at flange
+    		double[] doubleVals=new double[numOfDoubles];
+    		
+    		byte[] buffer= new byte[8*numOfDoubles];
+    		
+    		try {
+    			host = InetAddress.getByName(ip);
+    			DatagramSocket soc=new DatagramSocket();
+    			while(loopFlag)
+    			{
+    				// Feedback of actual joints positions
+    				JointPosition jpos=_lbr.getCurrentJointPosition();
+    				int valsCounter=0;
+    				for(int i=0;i<7;i++)
+    				{
+    					doubleVals[valsCounter]=jpos.get(i);
+    					valsCounter=valsCounter+1;
+    				}
+    				// Feedback of measured torques
+    				double[] m_tor=_lbr.getMeasuredTorque().getTorqueValues();
+    				for(int i=0;i<7;i++)
+    				{
+    					doubleVals[valsCounter]=m_tor[i];
+    					valsCounter=valsCounter+1;
+    				}
+    				// Feedback of external torques
+    				double[] ex_tor=_lbr.getExternalTorque().getTorqueValues();
+    				for(int i=0;i<7;i++)
+    				{
+    					doubleVals[valsCounter]=ex_tor[i];
+    					valsCounter=valsCounter+1;
+    				}
+    				// Feedback of forces at flange    				
+    				ForceSensorData f_at_flange=_lbr.getExternalForceTorque(_lbr.getFlange(),World.Current.getRootFrame());				
+    				doubleVals[valsCounter]=f_at_flange.getForce().getX();
+    				valsCounter=valsCounter+1;
+    				doubleVals[valsCounter]=f_at_flange.getForce().getY();
+    				valsCounter=valsCounter+1;
+    				doubleVals[valsCounter]=f_at_flange.getForce().getZ();
+    				valsCounter=valsCounter+1;
+    				doubleVals[valsCounter]=f_at_flange.getTorque().getX();
+    				valsCounter=valsCounter+1;
+    				doubleVals[valsCounter]=f_at_flange.getTorque().getY();
+    				valsCounter=valsCounter+1;
+    				doubleVals[valsCounter]=f_at_flange.getTorque().getZ();
+    				valsCounter=valsCounter+1;
+    				
+    				int count=0;
+    				for(int i=0;i<numOfDoubles;i++)
+    				{
+    					byte[] bytes = new byte[8];
+    					double value=doubleVals[i];
+    				    ByteBuffer.wrap(bytes).putDouble(value);
+    				    for(int j=0;j<8;j++)
+    				    {
+    				    	buffer[count]=bytes[7-j];
+    				    	count=count+1;
+    				    }
+    				}
+    				
+    				DatagramPacket packet= new DatagramPacket(buffer, buffer.length,host, port);
+    				soc.send(packet);
+    				Thread.sleep(36);
+    			}
+    			message="Robot state publisher is terminated";
+        		System.out.println(message);
+    				
+    		} catch (UnknownHostException e1) {
+    			// TODO Auto-generated catch block
+    			//e1.printStackTrace();
+    			System.out.println("Error in UDP publisher, 1");
+    			System.out.println(e1.toString());
+    		} catch (SocketException e) {
+    			// TODO Auto-generated catch block
+    			//e.printStackTrace();
+    			System.out.println("Error in UDP publisher, 2");
+    			System.out.println(e.toString());
+    		} catch (IOException e) {
+    			// TODO Auto-generated catch block
+    			//e.printStackTrace();
+    			System.out.println("Error in UDP publisher, 3");
+    			System.out.println(e.toString());
+    		} catch (InterruptedException e) {
+    			// TODO Auto-generated catch block
+    			//e.printStackTrace();
+    			System.out.println("Error in UDP publisher, 4");
+    			System.out.println(e.toString());
+    		}
+    	}
+
+    }
+    	
+   
+    @Override
+    public void initialize()
+    {
+        _lbr = getContext().getDeviceFromType(LBR.class);
+        EEfServoPos=new double[6];
+        for(int i=0;i<6;i++)
+        {
+        	EEfServoPos[i]=0;
+        }
+        loopFlag=true;
+		addLoadData();
+    }
+
+    private void addLoadData()
+    {
+    	 // Create a Tool by Hand this is the tool we want to move with some mass
+        // properties and a TCP-Z-offset of 100.
+        _loadData = new LoadData();
+        _loadData.setMass(MASS);
+        _loadData.setCenterOfMass(
+                CENTER_OF_MASS_IN_MILLIMETER[0], CENTER_OF_MASS_IN_MILLIMETER[1],
+                CENTER_OF_MASS_IN_MILLIMETER[2]);
+        _toolAttachedToLBR = new Tool("Tool", _loadData);
+
+        XyzAbcTransformation trans = XyzAbcTransformation.ofTranslation(
+                TRANSLATION_OF_TOOL[0], TRANSLATION_OF_TOOL[1],
+                TRANSLATION_OF_TOOL[2]);
+        ObjectFrame aTransformation = _toolAttachedToLBR.addChildFrame(TOOL_FRAME
+                + "(TCP)", trans);
+        _toolAttachedToLBR.setDefaultMotionFrame(aTransformation);
+        // Attach tool to the robot
+        _toolAttachedToLBR.attachTo(_lbr.getFlange());
+    }
+
+    /**
+     * Move to an initial Position WARNING: MAKE SURE, THAT the pose is collision free.
+     */
+    private void moveToInitialPosition()
+    {
+        _lbr.move(
+        		ptp(0., Math.PI / 180 * 30.,0.,-Math.PI / 180 * 80.,0.,Math.PI / 180 * 70., 0.).setJointVelocityRel(0.15));
+        /* Note: The Validation itself justifies, that in this very time instance, the load parameter setting was
+         * sufficient. This does not mean by far, that the parameter setting is valid in the sequel or lifetime of this
+         * program */
+        try
+        {
+        	
+        }
+        catch (IllegalStateException e)
+        {
+            getLogger().info("Omitting validation failure for this sample\n"
+                    + e.getMessage());
+        }
+    }
+
+
+    /**
+     * Main Application Routine
+     */
+    @Override
+    public void run()
+    {
+        moveToInitialPosition();
+        
+        System.out.print("You have ten seconds to establish a connection with iiwa");
+        int portTemp=30002;
+        udpServer=new UDPServer(portTemp);
+        new UDPPublisher(externa_PC_IP);
+        smartServoStartCartezian();
+        
+    }
+    
+
+	private void smartServoStartCartezian() {
+		
+        boolean doDebugPrints = false;
+
+        SmartServo aSmartServoMotion = new SmartServo(
+                _lbr.getCurrentJointPosition());
+
+        aSmartServoMotion.useTrace(true);
+
+        aSmartServoMotion.setMinimumTrajectoryExecutionTime(5e-3);
+
+        getLogger().info("Starting SmartServo motion in position control mode");
+        _toolAttachedToLBR.moveAsync(aSmartServoMotion);
+
+        getLogger().info("Get the runtime of the SmartServo motion");
+        ISmartServoRuntime theServoRuntime = aSmartServoMotion
+                .getRuntime();
+
+        Frame aFrame = theServoRuntime.getCurrentCartesianDestination(_toolAttachedToLBR.getDefaultMotionFrame());
+        Frame destFrame = aFrame.copyWithRedundancy();
+        // Initiate the initial position 
+        EEfServoPos[0]=aFrame.getX();
+        EEfServoPos[1]=aFrame.getY();
+        EEfServoPos[2]=aFrame.getZ();
+        EEfServoPos[3]=aFrame.getAlphaRad();
+        EEfServoPos[4]=aFrame.getBetaRad();
+        EEfServoPos[5]=aFrame.getGammaRad();
+        
+        
+        try
+        {
+            // do a cyclic loop
+            // Do some timing...
+            // in nanosec
+            while(loopFlag)
+            {
+                // Insert your code here
+                // e.g Visual Servoing or the like
+
+                // Synchronize with the realtime system
+                theServoRuntime.updateWithRealtimeSystem();
+
+                // Get the measured position 
+                Frame msrPose = theServoRuntime
+                        .getCurrentCartesianDestination(_lbr.getFlange());
+
+                if (doDebugPrints)
+                {
+                    getLogger().info("Current cartesian goal " + aFrame);
+                    getLogger().info("Current joint destination "
+                            + theServoRuntime.getCurrentJointDestination());
+                }
+
+                // Do some Computation
+                // emulate some computational effort - or waiting for external
+                // stuff
+                ThreadUtil.milliSleep(MILLI_SLEEP_TO_EMULATE_COMPUTATIONAL_EFFORT);
+
+                // compute a new commanded position
+                for(int kk=0;kk<6;kk++)
+                {
+                	EEfServoPos[kk]=udpServer.EEFpos[kk];
+                }
+                // update Cartesian positions
+                destFrame.setX(EEfServoPos[0]);
+                destFrame.setY(EEfServoPos[1]);
+                destFrame.setZ(EEfServoPos[2]);
+                destFrame.setAlphaRad(EEfServoPos[3]);
+                destFrame.setBetaRad(EEfServoPos[4]);
+                destFrame.setGammaRad(EEfServoPos[5]);
+                
+                if (doDebugPrints)
+                {
+                    getLogger().info("New cartesian goal " + destFrame);
+                    getLogger().info("LBR position "
+                            + _lbr.getCurrentCartesianPosition(_lbr
+                                    .getFlange()));
+                    getLogger().info("Measured cartesian pose from runtime "
+                            + msrPose);
+                }
+
+                theServoRuntime.setDestination(destFrame);
+            }
+        }
+        catch (Exception e)
+        {
+            getLogger().error(e.toString());
+            e.printStackTrace();
+        }
+
+        //Print statistics and parameters of the motion
+        getLogger().info("Simple cartesian test " + theServoRuntime.toString());
+
+        getLogger().info("Stop the SmartServo motion");
+        theServoRuntime.stopMotion();
+    }
+
+
+    /**
+     * Main routine, which starts the application
+     */
+    public static void main(String[] args)
+    {
+        SimulinkFIIWASmartServoCartesian app = new SimulinkFIIWASmartServoCartesian();
+        app.runApplication();
+    }
+ 
+    public class UDPServer implements Runnable{
+    	
+    	public double[] EEFpos={575.87,0.05,397.56,Math.PI,0,Math.PI};
+    	
+    	double stime=0;
+		double endtime=0;
+		
+    	int _port;
+    	int vectorSize=6;
+    	int packetCounter=0;
+    	
+    	byte[] buffer=new byte[8*7];
+    	UDPServer(int port)
+    	{
+    		Thread t=new Thread(this);
+    		t.setDaemon(true);
+    		t.start();
+    		_port=port;
+    	}
+    	
+    	public void run()
+    	{
+    		System.out.println("Program will terminate if comuncation is not established in 10 seconds");
+    			DatagramSocket soc=null;
+    			try {
+    				soc = new DatagramSocket(_port);
+    				soc.setSoTimeout(10000); // 5 seconds, if a time out occurred, turn off program
+    				DatagramPacket response=new DatagramPacket(buffer, buffer.length);
+    				while(true)
+    				{
+    					soc.receive(response);
+    					packetCounter=packetCounter+1;
+    					// String s= new String(buffer,0,response.getLength())
+    					 // System.out.println(response.getLength());
+    					if(response.getLength()==8*vectorSize)
+    					{
+    						if(packetCounter==1)
+    						{
+    							stime=System.currentTimeMillis();
+    						}
+    						else
+    						{
+    							endtime=System.currentTimeMillis();
+    						}
+    						byte[] daB=new byte[8];
+    						int counter=0;
+    						int index=0;
+    						while(counter <8*vectorSize)
+    						{
+	    						for(int i=0;i<8;i++)
+	    						{
+	    							daB[7-i]=buffer[counter];
+	    							counter=counter+1;
+	    						}
+	    						EEFpos[index]=bytesToDouble(daB);
+	    						index=index+1;	    						
+	    						//System.out.println(bytesToDouble(daB));
+    						}
+    					}
+    				}
+    			} catch (SocketException e) {
+    				// TODO Auto-generated catch block
+    				e.printStackTrace();
+    				//System.out.println(e.toString());
+    			} catch (IOException e) {
+    				// TODO Auto-generated catch block
+    				e.printStackTrace();
+    				System.out.println(e.toString());
+    			}
+    			if(soc==null)
+    			{}
+    			else
+    			{
+    				if(soc.isClosed())
+    				{}
+    				else
+    				{
+    					soc.close();
+    				}
+    			}
+   				SimulinkFIIWASmartServoCartesian.loopFlag=false; //Turn off main loop 	
+   				double time=(endtime-stime)/1000;
+    			double rate=packetCounter/time;
+    			System.out.println("update rate, packets/second");
+    			System.out.println(rate);
+    	}
+    }
+    	
+    	
+    	public double bytesToDouble(byte[] b)
+    	{
+    		return ByteBuffer.wrap(b).getDouble();
+    	}
+
+}
diff --git a/Cartesian motion with feedback/simulink/feedBackSmartDirectServoCartesian1.slx b/Cartesian motion with feedback/simulink/feedBackSmartDirectServoCartesian1.slx
new file mode 100644
index 0000000..c676a80
Binary files /dev/null and b/Cartesian motion with feedback/simulink/feedBackSmartDirectServoCartesian1.slx differ
diff --git a/Cartesian motion with feedback/simulink/feedBackSmartDirectServoCartesian1.slxc b/Cartesian motion with feedback/simulink/feedBackSmartDirectServoCartesian1.slxc
new file mode 100644
index 0000000..b7eba1e
Binary files /dev/null and b/Cartesian motion with feedback/simulink/feedBackSmartDirectServoCartesian1.slxc differ
diff --git a/Cartesian motion with feedback/simulink/freeHandGuiding_01.slx b/Cartesian motion with feedback/simulink/freeHandGuiding_01.slx
new file mode 100644
index 0000000..87923ef
Binary files /dev/null and b/Cartesian motion with feedback/simulink/freeHandGuiding_01.slx differ
diff --git a/Cartesian motion with feedback/simulink/freeHandGuiding_01.slxc b/Cartesian motion with feedback/simulink/freeHandGuiding_01.slxc
new file mode 100644
index 0000000..6853660
Binary files /dev/null and b/Cartesian motion with feedback/simulink/freeHandGuiding_01.slxc differ
diff --git a/Impedance control open loop/iiwa/SimulinkIIWA_impedanceJoints.java b/Impedance control open loop/iiwa/SimulinkIIWA_impedanceJoints.java
new file mode 100644
index 0000000..63bf16b
--- /dev/null
+++ b/Impedance control open loop/iiwa/SimulinkIIWA_impedanceJoints.java	
@@ -0,0 +1,266 @@
+package lbrExampleApplications;
+
+
+import static com.kuka.roboticsAPI.motionModel.BasicMotions.ptp;
+
+import java.io.IOException;
+import java.net.ServerSocket;
+import java.net.Socket;
+import java.util.Scanner;
+import java.util.StringTokenizer;
+import java.util.logging.Logger;
+
+import com.kuka.common.StatisticTimer;
+import com.kuka.common.ThreadUtil;
+import com.kuka.common.StatisticTimer.OneTimeStep;
+import com.kuka.connectivity.motionModel.directServo.DirectServo;
+import com.kuka.connectivity.motionModel.directServo.IDirectServoRuntime;
+import com.kuka.connectivity.motionModel.smartServo.ISmartServoRuntime;
+import com.kuka.connectivity.motionModel.smartServo.SmartServo;
+import com.kuka.generated.ioAccess.MediaFlangeIOGroup;
+import com.kuka.roboticsAPI.applicationModel.RoboticsAPIApplication;
+import com.kuka.roboticsAPI.controllerModel.Controller;
+import com.kuka.roboticsAPI.deviceModel.JointPosition;
+import com.kuka.roboticsAPI.deviceModel.LBR;
+import com.kuka.roboticsAPI.geometricModel.Frame;
+import com.kuka.roboticsAPI.geometricModel.LoadData;
+import com.kuka.roboticsAPI.geometricModel.ObjectFrame;
+import com.kuka.roboticsAPI.geometricModel.Tool;
+import com.kuka.roboticsAPI.geometricModel.math.XyzAbcTransformation;
+
+ 
+import java.io.IOException;
+import java.net.DatagramPacket;
+import java.net.DatagramSocket;
+import java.net.SocketException;
+import java.nio.ByteBuffer;
+
+import lbrExampleApplications.SimulinkIIWADirectServoJoints.UDPServer;
+
+
+/*
+ * SimulinkIIWA interface, soft real-time control with impedence
+ * Copyright: Mohammad SAFEEA, 22nd/May/2018
+ * License: MIT license
+ * 
+ * 
+ * This is a UDP server that receives motion commands from Simulink.
+ * This server works together with the Simulink example: (impedanceJoints.slx)
+ * 
+ * 
+ * Below you can find quick start instructions, for more info, please refer to the youtube
+ * video tutorials, a link for which is available in the repository main page.
+ * 
+ * Hardware setup:
+ * 1- KUKA iiwa 7R800 or 14R820
+ * 2- External PC
+ * 3- A network between the PC and the robot, the connector X66
+ *    of the robot shall be used
+ * 
+ * Required software:
+ * 1- MATLAB with Simulink, the code was written using MATLAB2018a, it 
+ *    is recommended to use the same version.
+ * 2- Sunrise.Workbench, this program will be used to synchronize the java
+ *    code to the robot.
+ * 3- The Sunrise code was tested on (KUKA Sunrise.OS 1.11.0.7)
+ * 
+ * Setup instructions:
+ * 1- Add this class and the class (SmartServoWithImpedenceSimulinkIIWAinterface.java) 
+ *    to new project  using kuka's Sunrsie.Workbench.
+ * 2- Add reference to the SmartServo from inside the (StationSetup.cat) file. 
+ * 3- Change the following variables according to your tool and stiffness preference:
+ * 	* externa_PC_IP: change this variable according to the IP of the PC used for control.
+ *  * TRANSLATION_OF_TOOL: translation of tool center point with respect to flange frame.
+ *  * massOfTool: mass of the tool
+ *  * toolsCOMCoordiantes: coordinates of center of mass of tool with regards to the frame of the flange.
+ *  * cStiffness: Cartesian stiffness
+ *  * rStiffness: rotational stiffness
+ *  * nStiffness: null space stiffness
+ * 
+ * 
+ * 
+ * Utilization instructions:
+ * 1- Synchronize the project to the controller of the robot.
+ * 2- Run this application from the teach-pad of the robot,
+ *    This application has a time out of 10 seconds, if a connection
+ *    is not established during the 10 seconds interval the server will
+ *    turn off.
+ * 3- Start the Simulink example (impedanceJoints.slx)
+ *    from the external PC.
+ * 4- To change the timeout value, change the argument for the instruction
+ *    soc.setSoTimeout(10000),  the argument is in milliseconds.
+ * 
+ */
+
+public class SimulinkIIWA_impedanceJoints extends RoboticsAPIApplication
+{
+    private LBR _lbr;
+    private Controller kuka_Sunrise_Cabinet_1;
+    private int _count = 0;
+
+    private int _steps = 0;
+
+    private UDPServer udpServer;
+    
+    // Tool data
+    private static double[] TRANSLATION_OF_TOOL = { 0, 0, 100 };
+    private static double massOfTool = 1.1;
+    private static double[] toolsCOMCoordiantes = { 0, 0, 45 };
+    
+    // stiffness data
+    private static double cStiffness=450; // cartesian stiffness
+    private static double rStiffness=40; // rotational stiffness
+    private static double nStiffness=20; // null space stiffness
+
+    
+    @Override
+    public void initialize()
+    {
+        _lbr = getContext().getDeviceFromType(LBR.class);
+        kuka_Sunrise_Cabinet_1=getController("KUKA_Sunrise_Cabinet_1");
+        SmartServoWithImpedenceSimulinkIIWAinterface.smartServoMotionFlag=true;
+    }
+ /**
+     * Move to an initial Position WARNING: MAKE SURE, THAT the pose is collision free.
+     */
+    private void moveToInitialPosition()
+    {
+    	_lbr.move(
+        		ptp(0., Math.PI / 180 * 30.,0.,-Math.PI / 180 * 80.,0.,Math.PI / 180 * 70., 0.).setJointVelocityRel(0.15));        /* Note: The Validation itself justifies, that in this very time instance, the load parameter setting was
+         * sufficient. This does not mean by far, that the parameter setting is valid in the sequel or lifetime of this
+         * program */
+        try
+        {
+        	
+        }
+        catch (IllegalStateException e)
+        {
+            getLogger().info("Omitting validation failure for this sample\n"
+                    + e.getMessage());
+        }
+    }
+
+    /**
+     * Main Application Routine
+     */
+    @Override
+    public void run()
+    {
+        moveToInitialPosition();
+        System.out.print("You have ten seconds to establish a connection with iiwa");
+        int portTemp=30002;
+        udpServer=new UDPServer(portTemp);
+        try {
+			SmartServoWithImpedenceSimulinkIIWAinterface.startRealTimeWithImpedence(_lbr, kuka_Sunrise_Cabinet_1, massOfTool,
+					toolsCOMCoordiantes, cStiffness,
+					rStiffness, nStiffness);
+		} catch (Exception e) {
+			// TODO Bloco catch gerado automaticamente
+			e.printStackTrace();
+		}
+    }
+   
+ 
+    /**
+     * Main routine, which starts the application
+     */
+    public static void main(String[] args)
+    {
+        SimulinkIIWA_impedanceJoints app = new SimulinkIIWA_impedanceJoints();
+        app.runApplication();
+    }
+ 
+    public class UDPServer implements Runnable{
+    	
+    	
+    	double stime=0;
+		double endtime=0;
+		
+    	int _port;
+    	int vectorSize=7;
+    	int packetCounter=0;
+    	
+    	byte[] buffer=new byte[8*vectorSize];
+    	UDPServer(int port)
+    	{
+    		Thread t=new Thread(this);
+    		t.setDaemon(true);
+    		t.start();
+    		_port=port;
+    	}
+    	
+    	public void run()
+    	{
+    			System.out.println("Program will terminate if comuncation is not established in 10 seconds");
+    			DatagramSocket soc=null;
+    			try {
+    				soc = new DatagramSocket(_port);
+    				soc.setSoTimeout(10000); // 5 seconds, if a time out occurred, turn off program
+    				DatagramPacket response=new DatagramPacket(buffer, buffer.length);
+    				while(true)
+    				{
+    					soc.receive(response);
+    					packetCounter=packetCounter+1;
+    					// String s= new String(buffer,0,response.getLength())
+    					 // System.out.println(response.getLength());
+    					if(response.getLength()==8*vectorSize)
+    					{
+    						if(packetCounter==1)
+    						{
+    							stime=System.currentTimeMillis();
+    						}
+    						else
+    						{
+    							endtime=System.currentTimeMillis();
+    						}
+    						byte[] daB=new byte[8];
+    						int counter=0;
+    						int jointNum=0;
+    						while(counter <8*vectorSize)
+    						{
+	    						for(int i=0;i<8;i++)
+	    						{
+	    							daB[7-i]=buffer[counter];
+	    							counter=counter+1;
+	    						}
+	    						SmartServoWithImpedenceSimulinkIIWAinterface.smartServoJpos[jointNum]=bytesToDouble(daB);
+	    						jointNum=jointNum+1;	    						
+	    						//System.out.println(bytesToDouble(daB));
+    						}
+    					}
+    				}
+    			} catch (SocketException e) {
+    				// TODO Auto-generated catch block
+    				e.printStackTrace();
+    				System.out.println(e.toString());
+    			} catch (IOException e) {
+    				// TODO Auto-generated catch block
+    				e.printStackTrace();
+    				System.out.println(e.toString());
+    			}
+    			if(soc==null)
+    			{}
+    			else
+    			{
+    				if(soc.isClosed())
+    				{}
+    				else
+    				{
+    					soc.close();
+    				}
+    			}
+    			SmartServoWithImpedenceSimulinkIIWAinterface.smartServoMotionFlag=false; //Turn off main loop 	
+   				double time=(endtime-stime)/1000;
+    			double rate=packetCounter/time;
+    			System.out.println("update rate, packets/second");
+    			System.out.println(rate);
+    	}
+    }
+    	
+    	
+    	public double bytesToDouble(byte[] b)
+    	{
+    		return ByteBuffer.wrap(b).getDouble();
+    	}
+
+}
diff --git a/Impedance control open loop/simulink/impedanceJoints.slx b/Impedance control open loop/simulink/impedanceJoints.slx
new file mode 100644
index 0000000..b04c8d5
Binary files /dev/null and b/Impedance control open loop/simulink/impedanceJoints.slx differ
diff --git a/Impedance control open loop/simulink/impedanceJoints.slxc b/Impedance control open loop/simulink/impedanceJoints.slxc
new file mode 100644
index 0000000..5f82fc5
Binary files /dev/null and b/Impedance control open loop/simulink/impedanceJoints.slxc differ
diff --git a/Impedance control with feedback/iiwa/SimulinkFIIWA_impedanceJoints.java b/Impedance control with feedback/iiwa/SimulinkFIIWA_impedanceJoints.java
new file mode 100644
index 0000000..f0b4fc8
--- /dev/null
+++ b/Impedance control with feedback/iiwa/SimulinkFIIWA_impedanceJoints.java	
@@ -0,0 +1,395 @@
+package lbrExampleApplications;
+
+
+import static com.kuka.roboticsAPI.motionModel.BasicMotions.ptp;
+
+import java.io.IOException;
+import java.net.ServerSocket;
+import java.net.Socket;
+import java.util.Scanner;
+import java.util.StringTokenizer;
+import java.util.logging.Logger;
+
+import com.kuka.common.StatisticTimer;
+import com.kuka.common.ThreadUtil;
+import com.kuka.common.StatisticTimer.OneTimeStep;
+import com.kuka.connectivity.motionModel.directServo.DirectServo;
+import com.kuka.connectivity.motionModel.directServo.IDirectServoRuntime;
+import com.kuka.connectivity.motionModel.smartServo.ISmartServoRuntime;
+import com.kuka.connectivity.motionModel.smartServo.SmartServo;
+import com.kuka.generated.ioAccess.MediaFlangeIOGroup;
+import com.kuka.roboticsAPI.applicationModel.RoboticsAPIApplication;
+import com.kuka.roboticsAPI.controllerModel.Controller;
+import com.kuka.roboticsAPI.deviceModel.JointPosition;
+import com.kuka.roboticsAPI.deviceModel.LBR;
+import com.kuka.roboticsAPI.geometricModel.Frame;
+import com.kuka.roboticsAPI.geometricModel.LoadData;
+import com.kuka.roboticsAPI.geometricModel.ObjectFrame;
+import com.kuka.roboticsAPI.geometricModel.Tool;
+import com.kuka.roboticsAPI.geometricModel.World;
+import com.kuka.roboticsAPI.geometricModel.math.XyzAbcTransformation;
+import com.kuka.roboticsAPI.sensorModel.ForceSensorData;
+
+ 
+import java.io.IOException;
+import java.net.DatagramPacket;
+import java.net.DatagramSocket;
+import java.net.InetAddress;
+import java.net.SocketException;
+import java.net.UnknownHostException;
+import java.nio.ByteBuffer;
+
+import lbrExampleApplications.SimulinkFIIWADirectServoCartesian.UDPPublisher;
+import lbrExampleApplications.SimulinkIIWADirectServoJoints.UDPServer;
+
+/*
+ * SimulinkIIWA interface, soft real-time control with impedence
+ * Copyright: Mohammad SAFEEA, 22nd/May/2018
+ * License: MIT license
+ * 
+ * 
+ * This is a UDP server that receives motion commands from Simulink.
+ * This server works together with the Simulink example: (feedbackImpedanceJoints.slx)
+ * 
+ * Also this script returns back a feedback about the  (1)actual joints positions,
+ * (2)the external torques and (3)the measured torques of all the joints to Simulink.
+ * (4) EEF force moment.
+ * 
+ * Below you can find quick start instructions, for more info, please refer to the youtube
+ * video tutorials, a link for which is available in the repository main page.
+ * 
+ * Hardware setup:
+ * 1- KUKA iiwa 7R800 or 14R820
+ * 2- External PC
+ * 3- A network between the PC and the robot, the connector X66
+ *    of the robot shall be used
+ * 
+ * Required software:
+ * 1- MATLAB with Simulink, the code was written using MATLAB2018a, it 
+ *    is recommended to use the same version.
+ * 2- Sunrise.Workbench, this program will be used to synchronize the java
+ *    code to the robot.
+ * 3- The Sunrise code was tested on (KUKA Sunrise.OS 1.11.0.7)
+ * 
+ * Setup instructions:
+ * 1- Add this class and the class (SmartServoWithImpedenceSimulinkIIWAinterface.java) 
+ *    to new project  using kuka's Sunrsie.Workbench.
+ * 2- Add reference to the SmartServo from inside the (StationSetup.cat) file. 
+ * 3- Change the following variables according to your tool and stiffness preference:
+ * 	* externa_PC_IP: change this variable according to the IP of the PC used for control.
+ *  * TRANSLATION_OF_TOOL: translation of tool center point with respect to flange frame.
+ *  * massOfTool: mass of the tool
+ *  * toolsCOMCoordiantes: coordinates of center of mass of tool with regards to the frame of the flange.
+ *  * cStiffness: Cartesian stiffness
+ *  * rStiffness: rotational stiffness
+ *  * nStiffness: null space stiffness
+ * 
+ * 
+ * 
+ * Utilization instructions:
+ * 1- Synchronize the project to the controller of the robot.
+ * 2- Run this application from the teach-pad of the robot,
+ *    This application has a time out of 10 seconds, if a connection
+ *    is not established during the 10 seconds interval the server will
+ *    turn off.
+ * 3- Start the Simulink example (feedbackImpedanceJoints.slx)
+ *    from the external PC.
+ * 4- To change the timeout value, change the argument for the instruction
+ *    soc.setSoTimeout(10000),  the argument is in milliseconds.
+ * 
+ */
+
+public class SimulinkFIIWA_impedanceJoints extends RoboticsAPIApplication
+{
+	
+	private static String externa_PC_IP="172.31.69.55";
+    private LBR _lbr;
+    private Controller kuka_Sunrise_Cabinet_1;
+    private int _count = 0;
+
+    private int _steps = 0;
+
+    private UDPServer udpServer;
+    
+    // Tool data
+    private static double[] TRANSLATION_OF_TOOL = { 0, 0, 100 };
+    private static double massOfTool = 1.1;
+    private static double[] toolsCOMCoordiantes = { 0, 0, 45 };
+    
+    // stiffness data
+    private static double cStiffness=450; // cartesian stiffness
+    private static double rStiffness=40; // rotational stiffness
+    private static double nStiffness=20; // null space stiffness
+
+    
+    @Override
+    public void initialize()
+    {
+        _lbr = getContext().getDeviceFromType(LBR.class);
+        kuka_Sunrise_Cabinet_1=getController("KUKA_Sunrise_Cabinet_1");
+        SmartServoWithImpedenceSimulinkIIWAinterface.smartServoMotionFlag=true;
+    }
+ /**
+     * Move to an initial Position WARNING: MAKE SURE, THAT the pose is collision free.
+     */
+    private void moveToInitialPosition()
+    {
+    	_lbr.move(
+        		ptp(0., Math.PI / 180 * 30.,0.,-Math.PI / 180 * 80.,0.,Math.PI / 180 * 70., 0.).setJointVelocityRel(0.15));        /* Note: The Validation itself justifies, that in this very time instance, the load parameter setting was
+         * sufficient. This does not mean by far, that the parameter setting is valid in the sequel or lifetime of this
+         * program */
+        try
+        {
+        	
+        }
+        catch (IllegalStateException e)
+        {
+            getLogger().info("Omitting validation failure for this sample\n"
+                    + e.getMessage());
+        }
+    }
+
+    /**
+     * Main Application Routine
+     */
+    @Override
+    public void run()
+    {
+        moveToInitialPosition();
+        System.out.print("You have ten seconds to establish a connection with iiwa");
+        int portTemp=30002;
+        udpServer=new UDPServer(portTemp);
+        new UDPPublisher(externa_PC_IP);
+        try {
+			SmartServoWithImpedenceSimulinkIIWAinterface.startRealTimeWithImpedence(_lbr, kuka_Sunrise_Cabinet_1, massOfTool,
+					toolsCOMCoordiantes, cStiffness,
+					rStiffness, nStiffness);
+		} catch (Exception e) {
+			// TODO Bloco catch gerado automaticamente
+			e.printStackTrace();
+		}
+    }
+   
+ 
+    /**
+     * Main routine, which starts the application
+     */
+    public static void main(String[] args)
+    {
+        SimulinkFIIWA_impedanceJoints app = new SimulinkFIIWA_impedanceJoints();
+        app.runApplication();
+    }
+ 
+    public class UDPServer implements Runnable{
+    	
+    	
+    	double stime=0;
+		double endtime=0;
+		
+    	int _port;
+    	int vectorSize=7;
+    	int packetCounter=0;
+    	
+    	byte[] buffer=new byte[8*vectorSize];
+    	UDPServer(int port)
+    	{
+    		Thread t=new Thread(this);
+    		t.setDaemon(true);
+    		t.start();
+    		_port=port;
+    	}
+    	
+    	public void run()
+    	{
+    			System.out.println("Program will terminate if comuncation is not established in 10 seconds");
+    			DatagramSocket soc=null;
+    			try {
+    				soc = new DatagramSocket(_port);
+    				soc.setSoTimeout(10000); // 10 seconds, if a time out occurred, turn off program
+    				DatagramPacket response=new DatagramPacket(buffer, buffer.length);
+    				while(true)
+    				{
+    					soc.receive(response);
+    					packetCounter=packetCounter+1;
+    					// String s= new String(buffer,0,response.getLength())
+    					 // System.out.println(response.getLength());
+    					if(response.getLength()==8*vectorSize)
+    					{
+    						if(packetCounter==1)
+    						{
+    							stime=System.currentTimeMillis();
+    						}
+    						else
+    						{
+    							endtime=System.currentTimeMillis();
+    						}
+    						byte[] daB=new byte[8];
+    						int counter=0;
+    						int jointNum=0;
+    						while(counter <8*vectorSize)
+    						{
+	    						for(int i=0;i<8;i++)
+	    						{
+	    							daB[7-i]=buffer[counter];
+	    							counter=counter+1;
+	    						}
+	    						SmartServoWithImpedenceSimulinkIIWAinterface.smartServoJpos[jointNum]=bytesToDouble(daB);
+	    						jointNum=jointNum+1;	    						
+	    						//System.out.println(bytesToDouble(daB));
+    						}
+    					}
+    				}
+    			} catch (SocketException e) {
+    				// TODO Auto-generated catch block
+    				e.printStackTrace();
+    				System.out.println(e.toString());
+    			} catch (IOException e) {
+    				// TODO Auto-generated catch block
+    				e.printStackTrace();
+    				System.out.println(e.toString());
+    			}
+    			if(soc==null)
+    			{}
+    			else
+    			{
+    				if(soc.isClosed())
+    				{}
+    				else
+    				{
+    					soc.close();
+    				}
+    			}
+    			SmartServoWithImpedenceSimulinkIIWAinterface.smartServoMotionFlag=false; //Turn off main loop 	
+   				double time=(endtime-stime)/1000;
+    			double rate=packetCounter/time;
+    			System.out.println("update rate, packets/second");
+    			System.out.println(rate);
+    	}
+    }
+    	
+
+    
+    public class UDPPublisher implements Runnable{
+    	
+    	String ip;
+    	UDPPublisher(String ip)
+    	{
+    		this.ip=ip;
+    		Thread t=new Thread(this);
+    		t.setDaemon(true);
+    		t.start();
+    	}
+    	public void run()
+    	{
+    		broadcastData();
+    	}
+    	private void broadcastData() {
+    		// TODO Auto-generated method stub
+    		String message="Publishing robot state using UDP at port 30008 to destination: "+ ip;
+    		System.out.println(message);
+    		message="Streamed message: 7 actual joint angles, 7 measured torques, 7 external torques,";
+    		System.out.println(message);
+    		message="State streaming frequency is approximatly at 25HZ";
+    		System.out.println(message);
+    		int port=30008;
+    		InetAddress host;
+    		int numOfDoubles=7+7+7+6;// 7 actual joint angles, 7 measured torques, 7 external torques, 7 force at flange
+    		double[] doubleVals=new double[numOfDoubles];
+    		
+    		byte[] buffer= new byte[8*numOfDoubles];
+    		
+    		try {
+    			host = InetAddress.getByName(ip);
+    			DatagramSocket soc=new DatagramSocket();
+    			while(SmartServoWithImpedenceSimulinkIIWAinterface.smartServoMotionFlag)
+    			{
+    				// Feedback of actual joints positions
+    				JointPosition jpos=_lbr.getCurrentJointPosition();
+    				int valsCounter=0;
+    				for(int i=0;i<7;i++)
+    				{
+    					doubleVals[valsCounter]=jpos.get(i);
+    					valsCounter=valsCounter+1;
+    				}
+    				// Feedback of measured torques
+    				double[] m_tor=_lbr.getMeasuredTorque().getTorqueValues();
+    				for(int i=0;i<7;i++)
+    				{
+    					doubleVals[valsCounter]=m_tor[i];
+    					valsCounter=valsCounter+1;
+    				}
+    				// Feedback of external torques
+    				double[] ex_tor=_lbr.getExternalTorque().getTorqueValues();
+    				for(int i=0;i<7;i++)
+    				{
+    					doubleVals[valsCounter]=ex_tor[i];
+    					valsCounter=valsCounter+1;
+    				}
+    				// Feedback of forces at flange    				
+    				ForceSensorData f_at_flange=_lbr.getExternalForceTorque(_lbr.getFlange(),World.Current.getRootFrame());				
+    				doubleVals[valsCounter]=f_at_flange.getForce().getX();
+    				valsCounter=valsCounter+1;
+    				doubleVals[valsCounter]=f_at_flange.getForce().getY();
+    				valsCounter=valsCounter+1;
+    				doubleVals[valsCounter]=f_at_flange.getForce().getZ();
+    				valsCounter=valsCounter+1;
+    				doubleVals[valsCounter]=f_at_flange.getTorque().getX();
+    				valsCounter=valsCounter+1;
+    				doubleVals[valsCounter]=f_at_flange.getTorque().getY();
+    				valsCounter=valsCounter+1;
+    				doubleVals[valsCounter]=f_at_flange.getTorque().getZ();
+    				valsCounter=valsCounter+1;
+    				
+    				int count=0;
+    				for(int i=0;i<numOfDoubles;i++)
+    				{
+    					byte[] bytes = new byte[8];
+    					double value=doubleVals[i];
+    				    ByteBuffer.wrap(bytes).putDouble(value);
+    				    for(int j=0;j<8;j++)
+    				    {
+    				    	buffer[count]=bytes[7-j];
+    				    	count=count+1;
+    				    }
+    				}
+    				
+    				DatagramPacket packet= new DatagramPacket(buffer, buffer.length,host, port);
+    				soc.send(packet);
+    				Thread.sleep(36);
+    			}
+    			message="Robot state publisher is terminated";
+        		System.out.println(message);
+    				
+    		} catch (UnknownHostException e1) {
+    			// TODO Auto-generated catch block
+    			//e1.printStackTrace();
+    			System.out.println("Error in UDP publisher, 1");
+    			System.out.println(e1.toString());
+    		} catch (SocketException e) {
+    			// TODO Auto-generated catch block
+    			//e.printStackTrace();
+    			System.out.println("Error in UDP publisher, 2");
+    			System.out.println(e.toString());
+    		} catch (IOException e) {
+    			// TODO Auto-generated catch block
+    			//e.printStackTrace();
+    			System.out.println("Error in UDP publisher, 3");
+    			System.out.println(e.toString());
+    		} catch (InterruptedException e) {
+    			// TODO Auto-generated catch block
+    			//e.printStackTrace();
+    			System.out.println("Error in UDP publisher, 4");
+    			System.out.println(e.toString());
+    		}
+    	}
+
+    }
+    	
+ 
+    
+    	public double bytesToDouble(byte[] b)
+    	{
+    		return ByteBuffer.wrap(b).getDouble();
+    	}
+
+}
diff --git a/Impedance control with feedback/simulink/feedbackImpedanceJoints.slx b/Impedance control with feedback/simulink/feedbackImpedanceJoints.slx
new file mode 100644
index 0000000..df006d9
Binary files /dev/null and b/Impedance control with feedback/simulink/feedbackImpedanceJoints.slx differ
diff --git a/Impedance control with feedback/simulink/feedbackImpedanceJoints.slxc b/Impedance control with feedback/simulink/feedbackImpedanceJoints.slxc
new file mode 100644
index 0000000..904de2a
Binary files /dev/null and b/Impedance control with feedback/simulink/feedbackImpedanceJoints.slxc differ
diff --git a/Joint space motion open loop/iiwa/SimulinkIIWADirectServoJoints.java b/Joint space motion open loop/iiwa/SimulinkIIWADirectServoJoints.java
new file mode 100644
index 0000000..bf79c46
--- /dev/null
+++ b/Joint space motion open loop/iiwa/SimulinkIIWADirectServoJoints.java	
@@ -0,0 +1,382 @@
+package lbrExampleApplications;
+
+
+import static com.kuka.roboticsAPI.motionModel.BasicMotions.ptp;
+
+import java.io.IOException;
+import java.net.ServerSocket;
+import java.net.Socket;
+import java.util.Scanner;
+import java.util.StringTokenizer;
+import java.util.logging.Logger;
+
+import com.kuka.connectivity.motionModel.directServo.DirectServo;
+import com.kuka.connectivity.motionModel.directServo.IDirectServoRuntime;
+import com.kuka.generated.ioAccess.MediaFlangeIOGroup;
+import com.kuka.roboticsAPI.applicationModel.RoboticsAPIApplication;
+import com.kuka.roboticsAPI.controllerModel.Controller;
+import com.kuka.roboticsAPI.deviceModel.JointPosition;
+import com.kuka.roboticsAPI.deviceModel.LBR;
+import com.kuka.roboticsAPI.geometricModel.Frame;
+
+ 
+import java.io.IOException;
+import java.net.DatagramPacket;
+import java.net.DatagramSocket;
+import java.net.SocketException;
+import java.nio.ByteBuffer;
+
+/*
+ * SimulinkIIWA interface, soft real-time control in joints position mode
+ * Copyright: Mohammad SAFEEA, 22nd/May/2018
+ * License: MIT license
+ * 
+ * This is a UDP server that receives motion commands from Simulink.
+ * This server works together with the Simulink example: (smartDirectServoJoints.slx)
+ * 
+ * Also this script returns back a feedback about the  (1)actual joints positions,
+ * (2)the external torques and (3)the measured torques of all the joints to Simulink.
+ * 
+ * Below you can find quick start instructions, for more info, please refer to the youtube
+ * video tutorials, a link for which is available in the repository main page.
+ * 
+ * Hardware setup:
+ * 1- KUKA iiwa 7R800 or 14R820
+ * 2- External PC
+ * 3- A netwrok between the PC and the robot, the connector X66
+ *    of the robot shall be used
+ * 
+ * Required software:
+ * 1- MATLAB with Simulink, the code was written using MATLAB2018a, it 
+ *    is recommended to use the same version.
+ * 2- Sunrise.Workbench, this program will be used to synchronize the java
+ *    code to the robot.
+ * 3- The Sunrise code was tested on (KUKA Sunrise.OS 1.11.0.7)
+ * 
+ * Setup instructions:
+ * 1- Add this class to a new project  using kuka's Sunrsie.Workbench.
+ * 2- Add reference to the Direct/SmartServo from inside the (StationSetup.cat) file. 
+ * 3- Change the following variables according to your tool and stiffness preference:
+ * 	* externa_PC_IP: change this variable according to the IP of the PC used for control.
+ * 
+ * 
+ * Utilization instructions:
+ * 1- Synchronize the project to the controller of the robot.
+ * 2- Run this application from the teach-pad of the robot,
+ *    This application has a time out of 10 seconds, if a connection
+ *    is not established during the 10 seconds interval the server will
+ *    turn off.
+ * 3- Start the Simulink example (smartDirectServoJoints.slx)
+ *    from the external PC.
+ * 4- To change the timeout value, change the argument for the instruction
+ *    soc.setSoTimeout(10000),  the argument is in milliseconds.
+ * 
+ */
+
+public class SimulinkIIWADirectServoJoints extends RoboticsAPIApplication
+{
+    private LBR _lbr;
+  
+    
+    public static double jpos[];
+
+    public static boolean loopFlag;
+
+    public static double jDispMax[];
+    public static double updateCycleJointPos[];
+    private UDPServer udpServer;
+    int _port;
+    
+    ServerSocket ss;
+    Socket soc;
+    
+
+
+    
+    @Override
+    public void initialize()
+    {
+        _lbr = getContext().getDeviceFromType(LBR.class);
+        jpos=new double[7];
+        jDispMax=new double[7];
+        updateCycleJointPos=new double[7];
+        for(int i=0;i<7;i++)
+        {
+        	jpos[i]=0;
+        	jDispMax[i]=0;
+        	updateCycleJointPos[i]=0;
+        }
+        loopFlag=true;
+        _port=30003;
+    }
+
+    /**
+     * Move to an initial Position WARNING: MAKE SURE, THAT the pose is collision free.
+     */
+    private void moveToInitialPosition()
+    {
+        _lbr.move(
+        		ptp(0., 0.,0.,0.,0.,0., 0.).setJointVelocityRel(0.15));
+        /* Note: The Validation itself justifies, that in this very time instance, the load parameter setting was
+         * sufficient. This does not mean by far, that the parameter setting is valid in the sequel or lifetime of this
+         * program */
+        try
+        {
+        	
+        }
+        catch (IllegalStateException e)
+        {
+            getLogger().info("Omitting validation failure for this sample\n"
+                    + e.getMessage());
+        }
+    }
+
+    public void moveToSomePosition()
+    {
+        _lbr.move(
+                ptp(0., Math.PI / 180 * 20., 0., -Math.PI / 180 * 60., 0.,
+                        Math.PI / 180 * 90., 0.));
+    }
+
+    /**
+     * Main Application Routine
+     */
+    @Override
+    public void run()
+    {
+        moveToInitialPosition();
+        System.out.print("You have ten seconds to establish a connection with iiwa");
+        int portTemp=30002;
+        udpServer=new UDPServer(portTemp);
+        
+        startDirectServo();
+    }
+    
+    
+    private void startDirectServo()
+    {
+    	boolean doDebugPrints = false;      
+        
+        JointPosition initialPosition = new JointPosition(
+                _lbr.getCurrentJointPosition());
+        
+        
+        for(int i=0;i<7;i++)
+        {
+        	jpos[i]=initialPosition.get(i);
+        }
+        DirectServo aDirectServoMotion = new DirectServo(initialPosition);
+
+        aDirectServoMotion.setMinimumTrajectoryExecutionTime(40e-3);
+
+        getLogger().info("Starting DirectServo motion in position control mode");
+        _lbr.moveAsync(aDirectServoMotion);
+
+        getLogger().info("Get the runtime of the DirectServo motion");
+        IDirectServoRuntime theDirectServoRuntime = aDirectServoMotion
+                .getRuntime();
+        
+        JointPosition destination = new JointPosition(
+                _lbr.getJointCount());
+        double disp;
+        double temp;
+        double absDisp;
+        
+        try
+        {
+            // do a cyclic loop
+            // Do some timing...
+            // in nanosec
+
+			while(loopFlag==true)
+			{
+
+                // ///////////////////////////////////////////////////////
+                // Insert your code here
+                // e.g Visual Servoing or the like
+                // Synchronize with the realtime system
+                //theDirectServoRuntime.updateWithRealtimeSystem();
+
+                if (doDebugPrints)
+                {
+                	getLogger().info("Current fifth joint position " + jpos[5]);
+                    getLogger().info("Current joint destination "
+                            + theDirectServoRuntime.getCurrentJointDestination());
+                }
+
+
+                Thread.sleep(1);
+                
+                JointPosition currentPos = new JointPosition(
+                        _lbr.getCurrentJointPosition());
+                
+                
+                
+                for (int k = 0; k < destination.getAxisCount(); ++k)
+                {
+                		
+                		jpos[k]=udpServer.jpos[k];
+                		double dj=jpos[k]-currentPos.get(k);
+                		disp= getTheDisplacment( dj);
+                		temp=currentPos.get(k)+disp;
+                		absDisp=Math.abs(disp);
+                		if(absDisp>jDispMax[k])
+                		{
+                			jDispMax[k]=absDisp;
+                		}
+                        destination.set(k, temp);
+                        updateCycleJointPos[k]=temp;
+                        
+                }
+                	
+                /*terminateFlag=checkCollisionWithEEF(updateCycleJointPos);
+                if(terminateFlag==true)
+                {
+                	dabak.terminateBool=true;
+                	dabak.soc.close();
+                	dabak.ss.close();
+                	
+                	break;
+                }*/
+                
+	            theDirectServoRuntime.setDestination(destination);
+	                
+            }
+        }
+        catch (Exception e)
+        {
+            getLogger().info(e.getLocalizedMessage());
+            e.printStackTrace();
+            //Print statistics and parameters of the motion
+            getLogger().info("Simple Cartesian Test \n" + theDirectServoRuntime.toString());
+
+            getLogger().info("Stop the DirectServo motion");
+
+            
+        }
+        theDirectServoRuntime.stopMotion();
+
+    }
+    
+    
+    double getTheDisplacment(double dj)
+    {
+    	
+    	return dj;
+    	/*
+    	double   a=0.07; 
+    	double b=a*0.75; 
+		double exponenet=-Math.pow(dj/b, 2);
+		return Math.signum(dj)*a*(1-Math.exp(exponenet));
+		*/
+    }
+
+
+    /**
+     * Main routine, which starts the application
+     */
+    public static void main(String[] args)
+    {
+        SimulinkIIWADirectServoJoints app = new SimulinkIIWADirectServoJoints();
+        app.runApplication();
+    }
+ 
+    public class UDPServer implements Runnable{
+    	
+    	public double[] jpos=new double[7];
+    	
+    	double stime=0;
+		double endtime=0;
+		
+    	int _port;
+    	int vectorSize=7;
+    	int packetCounter=0;
+    	
+    	byte[] buffer=new byte[8*vectorSize];
+    	UDPServer(int port)
+    	{
+    		for(int i=0;i<7;i++)
+    		{
+    			jpos[i]=0;
+    		}
+    		Thread t=new Thread(this);
+    		t.setDaemon(true);
+    		t.start();
+    		_port=port;
+    	}
+    	
+    	public void run()
+    	{
+    		System.out.println("Program will terminate if comuncation is not established in 10 seconds");
+    			DatagramSocket soc=null;
+    			try {
+    				soc = new DatagramSocket(_port);
+    				soc.setSoTimeout(10000); // 5 seconds, if a time out occurred, turn off program
+    				DatagramPacket response=new DatagramPacket(buffer, buffer.length);
+    				while(true)
+    				{
+    					soc.receive(response);
+    					packetCounter=packetCounter+1;
+    					// String s= new String(buffer,0,response.getLength())
+    					 // System.out.println(response.getLength());
+    					if(response.getLength()==8*vectorSize)
+    					{
+    						if(packetCounter==1)
+    						{
+    							stime=System.currentTimeMillis();
+    						}
+    						else
+    						{
+    							endtime=System.currentTimeMillis();
+    						}
+    						byte[] daB=new byte[8];
+    						int counter=0;
+    						int jointNum=0;
+    						while(counter <8*vectorSize)
+    						{
+	    						for(int i=0;i<8;i++)
+	    						{
+	    							daB[7-i]=buffer[counter];
+	    							counter=counter+1;
+	    						}
+	    						jpos[jointNum]=bytesToDouble(daB);
+	    						jointNum=jointNum+1;	    						
+	    						//System.out.println(bytesToDouble(daB));
+    						}
+    					}
+    				}
+    			} catch (SocketException e) {
+    				// TODO Auto-generated catch block
+    				e.printStackTrace();
+    				//System.out.println(e.toString());
+    			} catch (IOException e) {
+    				// TODO Auto-generated catch block
+    				e.printStackTrace();
+    				System.out.println(e.toString());
+    			}
+    			if(soc==null)
+    			{}
+    			else
+    			{
+    				if(soc.isClosed())
+    				{}
+    				else
+    				{
+    					soc.close();
+    				}
+    			}
+   				SimulinkIIWADirectServoJoints.loopFlag=false; //Turn off main loop 	
+   				double time=(endtime-stime)/1000;
+    			double rate=packetCounter/time;
+    			System.out.println("update rate, packets/second");
+    			System.out.println(rate);
+    	}
+    }
+    	
+    	
+    	public double bytesToDouble(byte[] b)
+    	{
+    		return ByteBuffer.wrap(b).getDouble();
+    	}
+
+}
diff --git a/Joint space motion open loop/iiwa/SimulinkIIWASmartServoJoints.java b/Joint space motion open loop/iiwa/SimulinkIIWASmartServoJoints.java
new file mode 100644
index 0000000..2b6d11d
--- /dev/null
+++ b/Joint space motion open loop/iiwa/SimulinkIIWASmartServoJoints.java	
@@ -0,0 +1,413 @@
+package lbrExampleApplications;
+
+
+import static com.kuka.roboticsAPI.motionModel.BasicMotions.ptp;
+
+import java.io.IOException;
+import java.net.ServerSocket;
+import java.net.Socket;
+import java.util.Scanner;
+import java.util.StringTokenizer;
+import java.util.logging.Logger;
+
+import com.kuka.common.StatisticTimer;
+import com.kuka.common.ThreadUtil;
+import com.kuka.common.StatisticTimer.OneTimeStep;
+import com.kuka.connectivity.motionModel.directServo.DirectServo;
+import com.kuka.connectivity.motionModel.directServo.IDirectServoRuntime;
+import com.kuka.connectivity.motionModel.smartServo.ISmartServoRuntime;
+import com.kuka.connectivity.motionModel.smartServo.SmartServo;
+import com.kuka.generated.ioAccess.MediaFlangeIOGroup;
+import com.kuka.roboticsAPI.applicationModel.RoboticsAPIApplication;
+import com.kuka.roboticsAPI.controllerModel.Controller;
+import com.kuka.roboticsAPI.deviceModel.JointPosition;
+import com.kuka.roboticsAPI.deviceModel.LBR;
+import com.kuka.roboticsAPI.geometricModel.Frame;
+import com.kuka.roboticsAPI.geometricModel.LoadData;
+import com.kuka.roboticsAPI.geometricModel.ObjectFrame;
+import com.kuka.roboticsAPI.geometricModel.Tool;
+import com.kuka.roboticsAPI.geometricModel.math.XyzAbcTransformation;
+
+ 
+import java.io.IOException;
+import java.net.DatagramPacket;
+import java.net.DatagramSocket;
+import java.net.SocketException;
+import java.nio.ByteBuffer;
+
+import lbrExampleApplications.SimulinkIIWADirectServoJoints.UDPServer;
+
+/*
+ * SimulinkIIWA interface, soft real-time control in joint position mode
+ * Copyright: Mohammad SAFEEA, 22nd/May/2018
+ * License: MIT license
+ * 
+ * This is a UDP server that receives motion commands from Simulink.
+ * This server works together with the Simulink example: (smartDirectServoJoints.slx)
+ * 
+ * Below you can find quick start instructions, for more info, please refer to the youtube
+ * video tutorials, a link for which is available in the repository main page.
+ * 
+ * Hardware setup:
+ * 1- KUKA iiwa 7R800 or 14R820
+ * 2- External PC
+ * 3- A network between the PC and the robot, the connector X66
+ *    of the robot shall be used
+ * 
+ * Required software:
+ * 1- MATLAB with Simulink, the code was written using MATLAB2018a, it 
+ *    is recommended to use the same version.
+ * 2- Sunrise.Workbench, this program will be used to synchronize the java
+ *    code to the robot.
+ * 3- The Sunrise code was tested on (KUKA Sunrise.OS 1.11.0.7)
+ * 
+ * Setup instructions:
+ * 1- Add this class to a new project  using kuka's Sunrsie.Workbench.
+ * 2- Add reference to the Direct/SmartServo from inside the (StationSetup.cat) file. 
+ * 3- Change the following variables according to your preference:
+ * 	* externa_PC_IP: change this variable according to the IP of the PC used for control.
+ *  * TRANSLATION_OF_TOOL: translation of tool center point with respect to flange frame.
+ *  * massOfTool: mass of the tool
+ *  * toolsCOMCoordiantes: coordinates of center of mass of tool with regards to the frame of the flange.
+ *
+ * 
+ * Utilization instructions:
+ * 1- Synchronize the project to the controller of the robot.
+ * 2- Run this application from the teach-pad of the robot,
+ *    This application has a time out of 10 seconds, if a connection
+ *    is not established during the 10 seconds interval the server will
+ *    turn off.
+ * 3- Start the Simulink example (smartDirectServoJoints.slx)
+ *    from the external PC.
+ * 4- To change the timeout value, change the argument for the instruction
+ *    soc.setSoTimeout(10000),  the argument is in milliseconds.
+ * 
+ */
+
+
+public class SimulinkIIWASmartServoJoints extends RoboticsAPIApplication
+{
+    private LBR _lbr;
+    private ISmartServoRuntime _theSmartServoRuntime = null;
+
+    // Tool Data
+    private Tool _toolAttachedToLBR;
+    private LoadData _loadData;
+    private static final String TOOL_FRAME = "toolFrame";
+    private static final double[] TRANSLATION_OF_TOOL = { 0, 0, 100 };
+    private static final double MASS = 0.4;
+    private static final double[] CENTER_OF_MASS_IN_MILLIMETER = { 0, 0, 100 };
+
+    private int _count = 0;
+
+    private static final int MILLI_SLEEP_TO_EMULATE_COMPUTATIONAL_EFFORT = 1;
+    private int _steps = 0;
+
+    public static boolean loopFlag;
+    public static double[] jpos;
+    private UDPServer udpServer;
+    
+      
+
+
+    
+    @Override
+    public void initialize()
+    {
+        _lbr = getContext().getDeviceFromType(LBR.class);
+        jpos=new double[7];
+        for(int i=0;i<7;i++)
+        {
+        	jpos[i]=0;
+        }
+        loopFlag=true;
+		addLoadData();
+    }
+
+    private void addLoadData()
+    {
+    	 // Create a Tool by Hand this is the tool we want to move with some mass
+        // properties and a TCP-Z-offset of 100.
+        _loadData = new LoadData();
+        _loadData.setMass(MASS);
+        _loadData.setCenterOfMass(
+                CENTER_OF_MASS_IN_MILLIMETER[0], CENTER_OF_MASS_IN_MILLIMETER[1],
+                CENTER_OF_MASS_IN_MILLIMETER[2]);
+        _toolAttachedToLBR = new Tool("Tool", _loadData);
+
+        XyzAbcTransformation trans = XyzAbcTransformation.ofTranslation(
+                TRANSLATION_OF_TOOL[0], TRANSLATION_OF_TOOL[1],
+                TRANSLATION_OF_TOOL[2]);
+        ObjectFrame aTransformation = _toolAttachedToLBR.addChildFrame(TOOL_FRAME
+                + "(TCP)", trans);
+        _toolAttachedToLBR.setDefaultMotionFrame(aTransformation);
+        // Attach tool to the robot
+        _toolAttachedToLBR.attachTo(_lbr.getFlange());
+    }
+
+    /**
+     * Move to an initial Position WARNING: MAKE SURE, THAT the pose is collision free.
+     */
+    private void moveToInitialPosition()
+    {
+        _lbr.move(
+        		ptp(0., 0.,0.,0.,0.,0., 0.).setJointVelocityRel(0.15));
+        /* Note: The Validation itself justifies, that in this very time instance, the load parameter setting was
+         * sufficient. This does not mean by far, that the parameter setting is valid in the sequel or lifetime of this
+         * program */
+        try
+        {
+        	
+        }
+        catch (IllegalStateException e)
+        {
+            getLogger().info("Omitting validation failure for this sample\n"
+                    + e.getMessage());
+        }
+    }
+
+    /**
+     * Main Application Routine
+     */
+    @Override
+    public void run()
+    {
+        moveToInitialPosition();
+        System.out.print("You have ten seconds to establish a connection with iiwa");
+        int portTemp=30002;
+        udpServer=new UDPServer(portTemp);
+    	startSmartServo();
+    }
+    
+    private void startSmartServo(){
+        
+        boolean doDebugPrints = false;
+
+        JointPosition initialPosition = new JointPosition(
+                _lbr.getCurrentJointPosition());
+        SmartServo aSmartServoMotion = new SmartServo(initialPosition);
+
+        // Set the motion properties to 20% of systems abilities
+        aSmartServoMotion.setJointAccelerationRel(0.2);
+        aSmartServoMotion.setJointVelocityRel(0.2);
+
+        aSmartServoMotion.setMinimumTrajectoryExecutionTime(20e-3);
+
+        getLogger().info("Starting SmartServo motion in position control mode");
+        _toolAttachedToLBR.getDefaultMotionFrame().moveAsync(aSmartServoMotion);
+
+        getLogger().info("Get the runtime of the SmartServo motion");
+        _theSmartServoRuntime = aSmartServoMotion.getRuntime();
+
+        // create an JointPosition Instance, to play with
+        JointPosition destination = new JointPosition(
+                _lbr.getJointCount());
+        for(int i=0;i<7;i++){
+        	jpos[i]=destination.get(i);
+        }
+        getLogger().info("Start loop");
+        // For Roundtrip time measurement...
+        StatisticTimer timing = new StatisticTimer();
+        try
+        {
+            // do a cyclic loop
+            // Refer to some timing...
+            // in nanosec
+            
+            long startTimeStamp = System.nanoTime();
+            while(loopFlag==true)
+            {
+            	_steps=_steps+1;
+                // Timing - draw one step
+                OneTimeStep aStep = timing.newTimeStep();
+                // ///////////////////////////////////////////////////////
+                // Insert your code here
+                // e.g Visual Servoing or the like
+                // emulate some computational effort - or waiting for external
+                // stuff
+                ThreadUtil.milliSleep(MILLI_SLEEP_TO_EMULATE_COMPUTATIONAL_EFFORT);
+                _theSmartServoRuntime.updateWithRealtimeSystem();
+                // Get the measured position
+                JointPosition curMsrJntPose = _theSmartServoRuntime
+                        .getAxisQMsrOnController();
+
+                for (int k = 0; k < destination.getAxisCount(); ++k)
+                {
+                	jpos[k]=udpServer.jpos[k];
+                    destination.set(k, jpos[k]);
+                }
+                _theSmartServoRuntime.setDestination(destination);
+
+                // ////////////////////////////////////////////////////////////
+                if (doDebugPrints)
+                {
+                    getLogger().info("Step " + _steps + " New Goal "
+                            + destination);
+                    getLogger().info("Fine ipo finished " + _theSmartServoRuntime.isDestinationReached());
+                    if (_theSmartServoRuntime.isDestinationReached())
+                    {
+                        _count++;
+                    }
+                    getLogger().info("Ipo state " + _theSmartServoRuntime.getFineIpoState());
+                    getLogger().info("Remaining time " + _theSmartServoRuntime.getRemainingTime());
+                    getLogger().info("LBR Position "
+                            + _lbr.getCurrentJointPosition());
+                    getLogger().info(" Measured LBR Position "
+                            + curMsrJntPose);
+                    if (_steps % 100 == 0)
+                    {
+                        // Some internal values, which can be displayed
+                        getLogger().info("Simple Joint Test - step " + _steps + _theSmartServoRuntime.toString());
+
+                    }
+                }
+                aStep.end();
+
+            }
+        }
+        catch (Exception e)
+        {
+            getLogger().info(e.getLocalizedMessage());
+            e.printStackTrace();
+        }
+        ThreadUtil.milliSleep(1000);
+
+        //Print statistics and parameters of the motion
+        getLogger().info("Displaying final states after loop ");
+        getLogger().info(getClass().getName() + _theSmartServoRuntime.toString());
+        // Stop the motion
+
+        getLogger().info("Stop the SmartServo motion");
+        _theSmartServoRuntime.stopMotion();
+
+        getLogger().info(_count + " times was the destination reached.");
+        getLogger().info("Statistic Timing of Overall Loop " + timing);
+        if (timing.getMeanTimeMillis() > 150)
+        {
+            getLogger().info("Statistic Timing is unexpected slow, you should try to optimize TCP/IP Transfer");
+            getLogger().info("Under Windows, you should play with the registry, see the e.g. the SmartServo Class javaDoc for details");
+        }
+    }
+
+    
+    double getTheDisplacment(double dj)
+    {
+    	
+    	return dj;
+    	/*
+    	double   a=0.07; 
+    	double b=a*0.75; 
+		double exponenet=-Math.pow(dj/b, 2);
+		return Math.signum(dj)*a*(1-Math.exp(exponenet));
+		*/
+    }
+
+
+    /**
+     * Main routine, which starts the application
+     */
+    public static void main(String[] args)
+    {
+        SimulinkIIWASmartServoJoints app = new SimulinkIIWASmartServoJoints();
+        app.runApplication();
+    }
+ 
+    public class UDPServer implements Runnable{
+    	
+    	public double[] jpos=new double[7];
+    	
+    	double stime=0;
+		double endtime=0;
+		
+    	int _port;
+    	int vectorSize=7;
+    	int packetCounter=0;
+    	
+    	byte[] buffer=new byte[8*vectorSize];
+    	UDPServer(int port)
+    	{
+    		for(int i=0;i<7;i++)
+    		{
+    			jpos[i]=0;
+    		}
+    		Thread t=new Thread(this);
+    		t.setDaemon(true);
+    		t.start();
+    		_port=port;
+    	}
+    	
+    	public void run()
+    	{
+    			System.out.println("Program will terminate if comuncation is not established in 10 seconds");
+    			DatagramSocket soc=null;
+    			try {
+    				soc = new DatagramSocket(_port);
+    				soc.setSoTimeout(10000); // 5 seconds, if a time out occurred, turn off program
+    				DatagramPacket response=new DatagramPacket(buffer, buffer.length);
+    				while(true)
+    				{
+    					soc.receive(response);
+    					packetCounter=packetCounter+1;
+    					// String s= new String(buffer,0,response.getLength())
+    					 // System.out.println(response.getLength());
+    					if(response.getLength()==8*vectorSize)
+    					{
+    						if(packetCounter==1)
+    						{
+    							stime=System.currentTimeMillis();
+    						}
+    						else
+    						{
+    							endtime=System.currentTimeMillis();
+    						}
+    						byte[] daB=new byte[8];
+    						int counter=0;
+    						int jointNum=0;
+    						while(counter <8*vectorSize)
+    						{
+	    						for(int i=0;i<8;i++)
+	    						{
+	    							daB[7-i]=buffer[counter];
+	    							counter=counter+1;
+	    						}
+	    						jpos[jointNum]=bytesToDouble(daB);
+	    						jointNum=jointNum+1;	    						
+	    						//System.out.println(bytesToDouble(daB));
+    						}
+    					}
+    				}
+    			} catch (SocketException e) {
+    				// TODO Auto-generated catch block
+    				e.printStackTrace();
+    				System.out.println(e.toString());
+    			} catch (IOException e) {
+    				// TODO Auto-generated catch block
+    				e.printStackTrace();
+    				System.out.println(e.toString());
+    			}
+    			if(soc==null)
+    			{}
+    			else
+    			{
+    				if(soc.isClosed())
+    				{}
+    				else
+    				{
+    					soc.close();
+    				}
+    			}
+   				SimulinkIIWASmartServoJoints.loopFlag=false; //Turn off main loop 	
+   				double time=(endtime-stime)/1000;
+    			double rate=packetCounter/time;
+    			System.out.println("update rate, packets/second");
+    			System.out.println(rate);
+    	}
+    }
+    	
+    	
+    	public double bytesToDouble(byte[] b)
+    	{
+    		return ByteBuffer.wrap(b).getDouble();
+    	}
+
+}
diff --git a/Joint space motion open loop/simulink/smartDirectServoJoints.slx b/Joint space motion open loop/simulink/smartDirectServoJoints.slx
new file mode 100644
index 0000000..9e69a1a
Binary files /dev/null and b/Joint space motion open loop/simulink/smartDirectServoJoints.slx differ
diff --git a/Joint space motion open loop/simulink/smartDirectServoJoints.slxc b/Joint space motion open loop/simulink/smartDirectServoJoints.slxc
new file mode 100644
index 0000000..4eabd5a
Binary files /dev/null and b/Joint space motion open loop/simulink/smartDirectServoJoints.slxc differ
diff --git a/Joint space motion with feedback/iiwa/SimulinkFIIWADirectServoJoints.java b/Joint space motion with feedback/iiwa/SimulinkFIIWADirectServoJoints.java
new file mode 100644
index 0000000..d2f7b79
--- /dev/null
+++ b/Joint space motion with feedback/iiwa/SimulinkFIIWADirectServoJoints.java	
@@ -0,0 +1,509 @@
+package lbrExampleApplications;
+
+
+import static com.kuka.roboticsAPI.motionModel.BasicMotions.ptp;
+
+import java.io.IOException;
+import java.net.ServerSocket;
+import java.net.Socket;
+import java.util.Scanner;
+import java.util.StringTokenizer;
+import java.util.logging.Logger;
+
+import com.kuka.connectivity.motionModel.directServo.DirectServo;
+import com.kuka.connectivity.motionModel.directServo.IDirectServoRuntime;
+import com.kuka.generated.ioAccess.MediaFlangeIOGroup;
+import com.kuka.roboticsAPI.applicationModel.RoboticsAPIApplication;
+import com.kuka.roboticsAPI.controllerModel.Controller;
+import com.kuka.roboticsAPI.deviceModel.JointPosition;
+import com.kuka.roboticsAPI.deviceModel.LBR;
+import com.kuka.roboticsAPI.geometricModel.Frame;
+import com.kuka.roboticsAPI.geometricModel.World;
+import com.kuka.roboticsAPI.sensorModel.ForceSensorData;
+
+ 
+import java.io.IOException;
+import java.net.DatagramPacket;
+import java.net.DatagramSocket;
+import java.net.InetAddress;
+import java.net.SocketException;
+import java.net.UnknownHostException;
+import java.nio.ByteBuffer;
+
+import lbrExampleApplications.SimulinkFIIWADirectServoCartesian.UDPPublisher;
+
+/*
+ * SimulinkIIWA interface, soft real-time control in joints position mode
+ * Copyright: Mohammad SAFEEA, 22nd/May/2018
+ * License: MIT license
+ * 
+ * This is a UDP server that receives motion commands from Simulink.
+ * This server works together with the Simulink example: (feedBackSmartDirectServoJoints.slx)
+ * 
+ * Also this script returns back a feedback about the  (1)actual joints positions,
+ * (2)the external torques and (3)the measured torques of all the joints to Simulink.
+ * (4) EEF force moment.
+ * 
+ * Below you can find quick start instructions, for more info, please refer to the youtube
+ * video tutorials, a link for which is available in the repository main page.
+ * 
+ * Hardware setup:
+ * 1- KUKA iiwa 7R800 or 14R820
+ * 2- External PC
+ * 3- A netwrok between the PC and the robot, the connector X66
+ *    of the robot shall be used
+ * 
+ * Required software:
+ * 1- MATLAB with Simulink, the code was written using MATLAB2018a, it 
+ *    is recommended to use the same version.
+ * 2- Sunrise.Workbench, this program will be used to synchronize the java
+ *    code to the robot.
+ * 3- The Sunrise code was tested on (KUKA Sunrise.OS 1.11.0.7)
+ * 
+ * Setup instructions:
+ * 1- Add this class to a new project  using kuka's Sunrsie.Workbench.
+ * 2- Add reference to the Direct/SmartServo from inside the (StationSetup.cat) file. 
+ * 3- Change the following variables according to your tool and stiffness preference:
+ * 	* externa_PC_IP: change this variable according to the IP of the PC used for control.
+ * 
+ * 
+ * Utilization instructions:
+ * 1- Synchronize the project to the controller of the robot.
+ * 2- Run this application from the teach-pad of the robot,
+ *    This application has a time out of 10 seconds, if a connection
+ *    is not established during the 10 seconds interval the server will
+ *    turn off.
+ * 3- Start the Simulink example (feedBackSmartDirectServoJoints.slx)
+ *    from the external PC.
+ * 4- To change the timeout value, change the argument for the instruction
+ *    soc.setSoTimeout(10000),  the argument is in milliseconds.
+ * 
+ */
+
+public class SimulinkFIIWADirectServoJoints extends RoboticsAPIApplication
+{
+	
+	private static String externa_PC_IP="172.31.69.55";
+    private LBR _lbr;
+  
+    
+    public static double jpos[];
+
+    public static boolean loopFlag;
+
+    public static double jDispMax[];
+    public static double updateCycleJointPos[];
+    private UDPServer udpServer;
+    int _port;
+    
+    ServerSocket ss;
+    Socket soc;
+    
+
+
+
+    
+    public class UDPPublisher implements Runnable{
+    	
+    	String ip;
+    	UDPPublisher(String ip)
+    	{
+    		this.ip=ip;
+    		Thread t=new Thread(this);
+    		t.setDaemon(true);
+    		t.start();
+    	}
+    	public void run()
+    	{
+    		broadcastData();
+    	}
+    	private void broadcastData() {
+    		// TODO Auto-generated method stub
+    		String message="Publishing robot state using UDP at port 30008 to destination: "+ ip;
+    		System.out.println(message);
+    		message="Streamed message: 7 actual joint angles, 7 measured torques, 7 external torques,";
+    		System.out.println(message);
+    		message="State streaming frequency is approximatly at 25HZ";
+    		System.out.println(message);
+    		int port=30008;
+    		InetAddress host;
+    		int numOfDoubles=7+7+7+6;// 7 actual joint angles, 7 measured torques, 7 external torques, 7 force at flange
+    		double[] doubleVals=new double[numOfDoubles];
+    		
+    		byte[] buffer= new byte[8*numOfDoubles];
+    		
+    		try {
+    			host = InetAddress.getByName(ip);
+    			DatagramSocket soc=new DatagramSocket();
+    			while(loopFlag)
+    			{
+    				// Feedback of actual joints positions
+    				JointPosition jpos=_lbr.getCurrentJointPosition();
+    				int valsCounter=0;
+    				for(int i=0;i<7;i++)
+    				{
+    					doubleVals[valsCounter]=jpos.get(i);
+    					valsCounter=valsCounter+1;
+    				}
+    				// Feedback of measured torques
+    				double[] m_tor=_lbr.getMeasuredTorque().getTorqueValues();
+    				for(int i=0;i<7;i++)
+    				{
+    					doubleVals[valsCounter]=m_tor[i];
+    					valsCounter=valsCounter+1;
+    				}
+    				// Feedback of external torques
+    				double[] ex_tor=_lbr.getExternalTorque().getTorqueValues();
+    				for(int i=0;i<7;i++)
+    				{
+    					doubleVals[valsCounter]=ex_tor[i];
+    					valsCounter=valsCounter+1;
+    				}
+    				// Feedback of forces at flange    				
+    				ForceSensorData f_at_flange=_lbr.getExternalForceTorque(_lbr.getFlange(),World.Current.getRootFrame());				
+    				doubleVals[valsCounter]=f_at_flange.getForce().getX();
+    				valsCounter=valsCounter+1;
+    				doubleVals[valsCounter]=f_at_flange.getForce().getY();
+    				valsCounter=valsCounter+1;
+    				doubleVals[valsCounter]=f_at_flange.getForce().getZ();
+    				valsCounter=valsCounter+1;
+    				doubleVals[valsCounter]=f_at_flange.getTorque().getX();
+    				valsCounter=valsCounter+1;
+    				doubleVals[valsCounter]=f_at_flange.getTorque().getY();
+    				valsCounter=valsCounter+1;
+    				doubleVals[valsCounter]=f_at_flange.getTorque().getZ();
+    				valsCounter=valsCounter+1;
+    				
+    				int count=0;
+    				for(int i=0;i<numOfDoubles;i++)
+    				{
+    					byte[] bytes = new byte[8];
+    					double value=doubleVals[i];
+    				    ByteBuffer.wrap(bytes).putDouble(value);
+    				    for(int j=0;j<8;j++)
+    				    {
+    				    	buffer[count]=bytes[7-j];
+    				    	count=count+1;
+    				    }
+    				}
+    				
+    				DatagramPacket packet= new DatagramPacket(buffer, buffer.length,host, port);
+    				soc.send(packet);
+    				Thread.sleep(36);
+    			}
+    			message="Robot state publisher is terminated";
+        		System.out.println(message);
+    				
+    		} catch (UnknownHostException e1) {
+    			// TODO Auto-generated catch block
+    			//e1.printStackTrace();
+    			System.out.println("Error in UDP publisher, 1");
+    			System.out.println(e1.toString());
+    		} catch (SocketException e) {
+    			// TODO Auto-generated catch block
+    			//e.printStackTrace();
+    			System.out.println("Error in UDP publisher, 2");
+    			System.out.println(e.toString());
+    		} catch (IOException e) {
+    			// TODO Auto-generated catch block
+    			//e.printStackTrace();
+    			System.out.println("Error in UDP publisher, 3");
+    			System.out.println(e.toString());
+    		} catch (InterruptedException e) {
+    			// TODO Auto-generated catch block
+    			//e.printStackTrace();
+    			System.out.println("Error in UDP publisher, 4");
+    			System.out.println(e.toString());
+    		}
+    	}
+
+    }
+    	
+   
+    @Override
+    public void initialize()
+    {
+        _lbr = getContext().getDeviceFromType(LBR.class);
+        jpos=new double[7];
+        jDispMax=new double[7];
+        updateCycleJointPos=new double[7];
+        for(int i=0;i<7;i++)
+        {
+        	jpos[i]=0;
+        	jDispMax[i]=0;
+        	updateCycleJointPos[i]=0;
+        }
+        loopFlag=true;
+        _port=30003;
+    }
+
+    /**
+     * Move to an initial Position WARNING: MAKE SURE, THAT the pose is collision free.
+     */
+    private void moveToInitialPosition()
+    {
+        _lbr.move(
+        		ptp(0., 0.,0.,0.,0.,0., 0.).setJointVelocityRel(0.15));
+        /* Note: The Validation itself justifies, that in this very time instance, the load parameter setting was
+         * sufficient. This does not mean by far, that the parameter setting is valid in the sequel or lifetime of this
+         * program */
+        try
+        {
+        	
+        }
+        catch (IllegalStateException e)
+        {
+            getLogger().info("Omitting validation failure for this sample\n"
+                    + e.getMessage());
+        }
+    }
+
+    public void moveToSomePosition()
+    {
+        _lbr.move(
+                ptp(0., Math.PI / 180 * 20., 0., -Math.PI / 180 * 60., 0.,
+                        Math.PI / 180 * 90., 0.));
+    }
+
+    /**
+     * Main Application Routine
+     */
+    @Override
+    public void run()
+    {
+        moveToInitialPosition();
+        System.out.print("You have ten seconds to establish a connection with iiwa");
+        int portTemp=30002;
+        udpServer=new UDPServer(portTemp);
+        new UDPPublisher(externa_PC_IP);
+        startDirectServo();
+    }
+    
+    
+    private void startDirectServo()
+    {
+    	boolean doDebugPrints = false;      
+        
+        JointPosition initialPosition = new JointPosition(
+                _lbr.getCurrentJointPosition());
+        
+        
+        for(int i=0;i<7;i++)
+        {
+        	jpos[i]=initialPosition.get(i);
+        }
+        DirectServo aDirectServoMotion = new DirectServo(initialPosition);
+
+        aDirectServoMotion.setMinimumTrajectoryExecutionTime(40e-3);
+
+        getLogger().info("Starting DirectServo motion in position control mode");
+        _lbr.moveAsync(aDirectServoMotion);
+
+        getLogger().info("Get the runtime of the DirectServo motion");
+        IDirectServoRuntime theDirectServoRuntime = aDirectServoMotion
+                .getRuntime();
+        
+        JointPosition destination = new JointPosition(
+                _lbr.getJointCount());
+        double disp;
+        double temp;
+        double absDisp;
+        
+        try
+        {
+            // do a cyclic loop
+            // Do some timing...
+            // in nanosec
+
+			while(loopFlag==true)
+			{
+
+                // ///////////////////////////////////////////////////////
+                // Insert your code here
+                // e.g Visual Servoing or the like
+                // Synchronize with the realtime system
+                //theDirectServoRuntime.updateWithRealtimeSystem();
+
+                if (doDebugPrints)
+                {
+                	getLogger().info("Current fifth joint position " + jpos[5]);
+                    getLogger().info("Current joint destination "
+                            + theDirectServoRuntime.getCurrentJointDestination());
+                }
+
+
+                Thread.sleep(1);
+                
+                JointPosition currentPos = new JointPosition(
+                        _lbr.getCurrentJointPosition());
+                
+                
+                
+                for (int k = 0; k < destination.getAxisCount(); ++k)
+                {
+                		
+                		jpos[k]=udpServer.jpos[k];
+                		double dj=jpos[k]-currentPos.get(k);
+                		disp= getTheDisplacment( dj);
+                		temp=currentPos.get(k)+disp;
+                		absDisp=Math.abs(disp);
+                		if(absDisp>jDispMax[k])
+                		{
+                			jDispMax[k]=absDisp;
+                		}
+                        destination.set(k, temp);
+                        updateCycleJointPos[k]=temp;
+                        
+                }
+                	
+                /*terminateFlag=checkCollisionWithEEF(updateCycleJointPos);
+                if(terminateFlag==true)
+                {
+                	dabak.terminateBool=true;
+                	dabak.soc.close();
+                	dabak.ss.close();
+                	
+                	break;
+                }*/
+                
+	            theDirectServoRuntime.setDestination(destination);
+	                
+            }
+        }
+        catch (Exception e)
+        {
+            getLogger().info(e.getLocalizedMessage());
+            e.printStackTrace();
+            //Print statistics and parameters of the motion
+            getLogger().info("Simple Cartesian Test \n" + theDirectServoRuntime.toString());
+
+            getLogger().info("Stop the DirectServo motion");
+
+            
+        }
+        theDirectServoRuntime.stopMotion();
+
+    }
+    
+    
+    double getTheDisplacment(double dj)
+    {
+    	
+    	return dj;
+    	/*
+    	double   a=0.07; 
+    	double b=a*0.75; 
+		double exponenet=-Math.pow(dj/b, 2);
+		return Math.signum(dj)*a*(1-Math.exp(exponenet));
+		*/
+    }
+
+
+    /**
+     * Main routine, which starts the application
+     */
+    public static void main(String[] args)
+    {
+        SimulinkFIIWADirectServoJoints app = new SimulinkFIIWADirectServoJoints();
+        app.runApplication();
+    }
+ 
+    public class UDPServer implements Runnable{
+    	
+    	public double[] jpos=new double[7];
+    	
+    	double stime=0;
+		double endtime=0;
+		
+    	int _port;
+    	int vectorSize=7;
+    	int packetCounter=0;
+    	
+    	byte[] buffer=new byte[8*vectorSize];
+    	UDPServer(int port)
+    	{
+    		for(int i=0;i<7;i++)
+    		{
+    			jpos[i]=0;
+    		}
+    		Thread t=new Thread(this);
+    		t.setDaemon(true);
+    		t.start();
+    		_port=port;
+    	}
+    	
+    	public void run()
+    	{
+    		System.out.println("Program will terminate if comuncation is not established in 10 seconds");
+    			DatagramSocket soc=null;
+    			try {
+    				soc = new DatagramSocket(_port);
+    				soc.setSoTimeout(10000); // 5 seconds, if a time out occurred, turn off program
+    				DatagramPacket response=new DatagramPacket(buffer, buffer.length);
+    				while(true)
+    				{
+    					soc.receive(response);
+    					packetCounter=packetCounter+1;
+    					// String s= new String(buffer,0,response.getLength())
+    					 // System.out.println(response.getLength());
+    					if(response.getLength()==8*vectorSize)
+    					{
+    						if(packetCounter==1)
+    						{
+    							stime=System.currentTimeMillis();
+    						}
+    						else
+    						{
+    							endtime=System.currentTimeMillis();
+    						}
+    						byte[] daB=new byte[8];
+    						int counter=0;
+    						int jointNum=0;
+    						while(counter <8*vectorSize)
+    						{
+	    						for(int i=0;i<8;i++)
+	    						{
+	    							daB[7-i]=buffer[counter];
+	    							counter=counter+1;
+	    						}
+	    						jpos[jointNum]=bytesToDouble(daB);
+	    						jointNum=jointNum+1;	    						
+	    						//System.out.println(bytesToDouble(daB));
+    						}
+    					}
+    				}
+    			} catch (SocketException e) {
+    				// TODO Auto-generated catch block
+    				e.printStackTrace();
+    				//System.out.println(e.toString());
+    			} catch (IOException e) {
+    				// TODO Auto-generated catch block
+    				e.printStackTrace();
+    				System.out.println(e.toString());
+    			}
+    			if(soc==null)
+    			{}
+    			else
+    			{
+    				if(soc.isClosed())
+    				{}
+    				else
+    				{
+    					soc.close();
+    				}
+    			}
+   				SimulinkFIIWADirectServoJoints.loopFlag=false; //Turn off main loop 	
+   				double time=(endtime-stime)/1000;
+    			double rate=packetCounter/time;
+    			System.out.println("update rate, packets/second");
+    			System.out.println(rate);
+    	}
+    }
+    	
+    	
+    	public double bytesToDouble(byte[] b)
+    	{
+    		return ByteBuffer.wrap(b).getDouble();
+    	}
+
+}
diff --git a/Joint space motion with feedback/iiwa/SimulinkFIIWASmartServoJoints.java b/Joint space motion with feedback/iiwa/SimulinkFIIWASmartServoJoints.java
new file mode 100644
index 0000000..b74856a
--- /dev/null
+++ b/Joint space motion with feedback/iiwa/SimulinkFIIWASmartServoJoints.java	
@@ -0,0 +1,539 @@
+package lbrExampleApplications;
+
+
+import static com.kuka.roboticsAPI.motionModel.BasicMotions.ptp;
+
+import java.io.IOException;
+import java.net.ServerSocket;
+import java.net.Socket;
+import java.util.Scanner;
+import java.util.StringTokenizer;
+import java.util.logging.Logger;
+
+import com.kuka.common.StatisticTimer;
+import com.kuka.common.ThreadUtil;
+import com.kuka.common.StatisticTimer.OneTimeStep;
+import com.kuka.connectivity.motionModel.directServo.DirectServo;
+import com.kuka.connectivity.motionModel.directServo.IDirectServoRuntime;
+import com.kuka.connectivity.motionModel.smartServo.ISmartServoRuntime;
+import com.kuka.connectivity.motionModel.smartServo.SmartServo;
+import com.kuka.generated.ioAccess.MediaFlangeIOGroup;
+import com.kuka.roboticsAPI.applicationModel.RoboticsAPIApplication;
+import com.kuka.roboticsAPI.controllerModel.Controller;
+import com.kuka.roboticsAPI.deviceModel.JointPosition;
+import com.kuka.roboticsAPI.deviceModel.LBR;
+import com.kuka.roboticsAPI.geometricModel.Frame;
+import com.kuka.roboticsAPI.geometricModel.LoadData;
+import com.kuka.roboticsAPI.geometricModel.ObjectFrame;
+import com.kuka.roboticsAPI.geometricModel.Tool;
+import com.kuka.roboticsAPI.geometricModel.World;
+import com.kuka.roboticsAPI.geometricModel.math.XyzAbcTransformation;
+import com.kuka.roboticsAPI.sensorModel.ForceSensorData;
+
+ 
+import java.io.IOException;
+import java.net.DatagramPacket;
+import java.net.DatagramSocket;
+import java.net.InetAddress;
+import java.net.SocketException;
+import java.net.UnknownHostException;
+import java.nio.ByteBuffer;
+
+import lbrExampleApplications.SimulinkFIIWADirectServoJoints.UDPPublisher;
+import lbrExampleApplications.SimulinkIIWADirectServoJoints.UDPServer;
+
+/*
+ * SimulinkIIWA interface, soft real-time control in joint position mode
+ * Copyright: Mohammad SAFEEA, 22nd/May/2018
+ * License: MIT license
+ * 
+ * This is a UDP server that receives motion commands from Simulink.
+ * This server works together with the Simulink example: (feedBackSmartDirectServoJoints.slx)
+ * 
+ * Also this script returns back a feedback about the  (1)actual joints positions,
+ * (2)the external torques and (3)the measured torques of all the joints to Simulink.
+ * (4) EEF force moment.
+ * 
+ * Below you can find quick start instructions, for more info, please refer to the youtube
+ * video tutorials, a link for which is available in the repository main page.
+ * 
+ * Hardware setup:
+ * 1- KUKA iiwa 7R800 or 14R820
+ * 2- External PC
+ * 3- A network between the PC and the robot, the connector X66
+ *    of the robot shall be used
+ * 
+ * Required software:
+ * 1- MATLAB with Simulink, the code was written using MATLAB2018a, it 
+ *    is recommended to use the same version.
+ * 2- Sunrise.Workbench, this program will be used to synchronize the java
+ *    code to the robot.
+ * 3- The Sunrise code was tested on (KUKA Sunrise.OS 1.11.0.7)
+ * 
+ * Setup instructions:
+ * 1- Add this class to a new project  using kuka's Sunrsie.Workbench.
+ * 2- Add reference to the Direct/SmartServo from inside the (StationSetup.cat) file. 
+ * 3- Change the following variables according to your preference:
+ * 	* externa_PC_IP: change this variable according to the IP of the PC used for control.
+ *  * TRANSLATION_OF_TOOL: translation of tool center point with respect to flange frame.
+ *  * massOfTool: mass of the tool
+ *  * toolsCOMCoordiantes: coordinates of center of mass of tool with regards to the frame of the flange.
+ *
+ * 
+ * Utilization instructions:
+ * 1- Synchronize the project to the controller of the robot.
+ * 2- Run this application from the teach-pad of the robot,
+ *    This application has a time out of 10 seconds, if a connection
+ *    is not established during the 10 seconds interval the server will
+ *    turn off.
+ * 3- Start the Simulink example (feedBackSmartDirectServoJoints.slx)
+ *    from the external PC.
+ * 4- To change the timeout value, change the argument for the instruction
+ *    soc.setSoTimeout(10000),  the argument is in milliseconds.
+ * 
+ */
+
+public class SimulinkFIIWASmartServoJoints extends RoboticsAPIApplication
+{
+	private static String externa_PC_IP="172.31.69.55";
+    private LBR _lbr;
+    private ISmartServoRuntime _theSmartServoRuntime = null;
+
+    // Tool Data
+    private Tool _toolAttachedToLBR;
+    private LoadData _loadData;
+    private static final String TOOL_FRAME = "toolFrame";
+    private static final double[] TRANSLATION_OF_TOOL = { 0, 0, 100 };
+    private static final double MASS = 0.4;
+    private static final double[] CENTER_OF_MASS_IN_MILLIMETER = { 0, 0, 100 };
+
+    private int _count = 0;
+
+    private static final int MILLI_SLEEP_TO_EMULATE_COMPUTATIONAL_EFFORT = 1;
+    private int _steps = 0;
+
+    public static boolean loopFlag;
+    public static double[] jpos;
+    private UDPServer udpServer;
+    
+ 
+    
+    public class UDPPublisher implements Runnable{
+    	
+    	String ip;
+    	UDPPublisher(String ip)
+    	{
+    		this.ip=ip;
+    		Thread t=new Thread(this);
+    		t.setDaemon(true);
+    		t.start();
+    	}
+    	public void run()
+    	{
+    		broadcastData();
+    	}
+    	private void broadcastData() {
+    		// TODO Auto-generated method stub
+    		String message="Publishing robot state using UDP at port 30008 to destination: "+ ip;
+    		System.out.println(message);
+    		message="Streamed message: 7 actual joint angles, 7 measured torques, 7 external torques,";
+    		System.out.println(message);
+    		message="State streaming frequency is approximatly at 25HZ";
+    		System.out.println(message);
+    		int port=30008;
+    		InetAddress host;
+    		int numOfDoubles=7+7+7+6;// 7 actual joint angles, 7 measured torques, 7 external torques, 7 force at flange
+    		double[] doubleVals=new double[numOfDoubles];
+    		
+    		byte[] buffer= new byte[8*numOfDoubles];
+    		
+    		try {
+    			host = InetAddress.getByName(ip);
+    			DatagramSocket soc=new DatagramSocket();
+    			while(loopFlag)
+    			{
+    				// Feedback of actual joints positions
+    				JointPosition jpos=_lbr.getCurrentJointPosition();
+    				int valsCounter=0;
+    				for(int i=0;i<7;i++)
+    				{
+    					doubleVals[valsCounter]=jpos.get(i);
+    					valsCounter=valsCounter+1;
+    				}
+    				// Feedback of measured torques
+    				double[] m_tor=_lbr.getMeasuredTorque().getTorqueValues();
+    				for(int i=0;i<7;i++)
+    				{
+    					doubleVals[valsCounter]=m_tor[i];
+    					valsCounter=valsCounter+1;
+    				}
+    				// Feedback of external torques
+    				double[] ex_tor=_lbr.getExternalTorque().getTorqueValues();
+    				for(int i=0;i<7;i++)
+    				{
+    					doubleVals[valsCounter]=ex_tor[i];
+    					valsCounter=valsCounter+1;
+    				}
+    				// Feedback of forces at flange    				
+    				ForceSensorData f_at_flange=_lbr.getExternalForceTorque(_lbr.getFlange(),World.Current.getRootFrame());				
+    				doubleVals[valsCounter]=f_at_flange.getForce().getX();
+    				valsCounter=valsCounter+1;
+    				doubleVals[valsCounter]=f_at_flange.getForce().getY();
+    				valsCounter=valsCounter+1;
+    				doubleVals[valsCounter]=f_at_flange.getForce().getZ();
+    				valsCounter=valsCounter+1;
+    				doubleVals[valsCounter]=f_at_flange.getTorque().getX();
+    				valsCounter=valsCounter+1;
+    				doubleVals[valsCounter]=f_at_flange.getTorque().getY();
+    				valsCounter=valsCounter+1;
+    				doubleVals[valsCounter]=f_at_flange.getTorque().getZ();
+    				valsCounter=valsCounter+1;
+    				
+    				int count=0;
+    				for(int i=0;i<numOfDoubles;i++)
+    				{
+    					byte[] bytes = new byte[8];
+    					double value=doubleVals[i];
+    				    ByteBuffer.wrap(bytes).putDouble(value);
+    				    for(int j=0;j<8;j++)
+    				    {
+    				    	buffer[count]=bytes[7-j];
+    				    	count=count+1;
+    				    }
+    				}
+    				
+    				DatagramPacket packet= new DatagramPacket(buffer, buffer.length,host, port);
+    				soc.send(packet);
+    				Thread.sleep(36);
+    			}
+    			message="Robot state publisher is terminated";
+        		System.out.println(message);
+    				
+    		} catch (UnknownHostException e1) {
+    			// TODO Auto-generated catch block
+    			//e1.printStackTrace();
+    			System.out.println("Error in UDP publisher, 1");
+    			System.out.println(e1.toString());
+    		} catch (SocketException e) {
+    			// TODO Auto-generated catch block
+    			//e.printStackTrace();
+    			System.out.println("Error in UDP publisher, 2");
+    			System.out.println(e.toString());
+    		} catch (IOException e) {
+    			// TODO Auto-generated catch block
+    			//e.printStackTrace();
+    			System.out.println("Error in UDP publisher, 3");
+    			System.out.println(e.toString());
+    		} catch (InterruptedException e) {
+    			// TODO Auto-generated catch block
+    			//e.printStackTrace();
+    			System.out.println("Error in UDP publisher, 4");
+    			System.out.println(e.toString());
+    		}
+    	}
+
+    }
+    	
+ 
+    
+    @Override
+    public void initialize()
+    {
+        _lbr = getContext().getDeviceFromType(LBR.class);
+        jpos=new double[7];
+        for(int i=0;i<7;i++)
+        {
+        	jpos[i]=0;
+        }
+        loopFlag=true;
+		addLoadData();
+    }
+
+    private void addLoadData()
+    {
+    	 // Create a Tool by Hand this is the tool we want to move with some mass
+        // properties and a TCP-Z-offset of 100.
+        _loadData = new LoadData();
+        _loadData.setMass(MASS);
+        _loadData.setCenterOfMass(
+                CENTER_OF_MASS_IN_MILLIMETER[0], CENTER_OF_MASS_IN_MILLIMETER[1],
+                CENTER_OF_MASS_IN_MILLIMETER[2]);
+        _toolAttachedToLBR = new Tool("Tool", _loadData);
+
+        XyzAbcTransformation trans = XyzAbcTransformation.ofTranslation(
+                TRANSLATION_OF_TOOL[0], TRANSLATION_OF_TOOL[1],
+                TRANSLATION_OF_TOOL[2]);
+        ObjectFrame aTransformation = _toolAttachedToLBR.addChildFrame(TOOL_FRAME
+                + "(TCP)", trans);
+        _toolAttachedToLBR.setDefaultMotionFrame(aTransformation);
+        // Attach tool to the robot
+        _toolAttachedToLBR.attachTo(_lbr.getFlange());
+    }
+
+    /**
+     * Move to an initial Position WARNING: MAKE SURE, THAT the pose is collision free.
+     */
+    private void moveToInitialPosition()
+    {
+        _lbr.move(
+        		ptp(0., 0.,0.,0.,0.,0., 0.).setJointVelocityRel(0.15));
+        /* Note: The Validation itself justifies, that in this very time instance, the load parameter setting was
+         * sufficient. This does not mean by far, that the parameter setting is valid in the sequel or lifetime of this
+         * program */
+        try
+        {
+        	
+        }
+        catch (IllegalStateException e)
+        {
+            getLogger().info("Omitting validation failure for this sample\n"
+                    + e.getMessage());
+        }
+    }
+
+    /**
+     * Main Application Routine
+     */
+    @Override
+    public void run()
+    {
+        moveToInitialPosition();
+        System.out.print("You have ten seconds to establish a connection with iiwa");
+        int portTemp=30002;
+        udpServer=new UDPServer(portTemp);
+        new UDPPublisher(externa_PC_IP);
+    	startSmartServo();
+    }
+    
+    private void startSmartServo(){
+        
+        boolean doDebugPrints = false;
+
+        JointPosition initialPosition = new JointPosition(
+                _lbr.getCurrentJointPosition());
+        SmartServo aSmartServoMotion = new SmartServo(initialPosition);
+
+        // Set the motion properties to 20% of systems abilities
+        aSmartServoMotion.setJointAccelerationRel(0.2);
+        aSmartServoMotion.setJointVelocityRel(0.2);
+
+        aSmartServoMotion.setMinimumTrajectoryExecutionTime(20e-3);
+
+        getLogger().info("Starting SmartServo motion in position control mode");
+        _toolAttachedToLBR.getDefaultMotionFrame().moveAsync(aSmartServoMotion);
+
+        getLogger().info("Get the runtime of the SmartServo motion");
+        _theSmartServoRuntime = aSmartServoMotion.getRuntime();
+
+        // create an JointPosition Instance, to play with
+        JointPosition destination = new JointPosition(
+                _lbr.getJointCount());
+        for(int i=0;i<7;i++){
+        	jpos[i]=destination.get(i);
+        }
+        getLogger().info("Start loop");
+        // For Roundtrip time measurement...
+        StatisticTimer timing = new StatisticTimer();
+        try
+        {
+            // do a cyclic loop
+            // Refer to some timing...
+            // in nanosec
+            
+            long startTimeStamp = System.nanoTime();
+            while(loopFlag==true)
+            {
+            	_steps=_steps+1;
+                // Timing - draw one step
+                OneTimeStep aStep = timing.newTimeStep();
+                // ///////////////////////////////////////////////////////
+                // Insert your code here
+                // e.g Visual Servoing or the like
+                // emulate some computational effort - or waiting for external
+                // stuff
+                ThreadUtil.milliSleep(MILLI_SLEEP_TO_EMULATE_COMPUTATIONAL_EFFORT);
+                _theSmartServoRuntime.updateWithRealtimeSystem();
+                // Get the measured position
+                JointPosition curMsrJntPose = _theSmartServoRuntime
+                        .getAxisQMsrOnController();
+
+                for (int k = 0; k < destination.getAxisCount(); ++k)
+                {
+                	jpos[k]=udpServer.jpos[k];
+                    destination.set(k, jpos[k]);
+                }
+                _theSmartServoRuntime.setDestination(destination);
+
+                // ////////////////////////////////////////////////////////////
+                if (doDebugPrints)
+                {
+                    getLogger().info("Step " + _steps + " New Goal "
+                            + destination);
+                    getLogger().info("Fine ipo finished " + _theSmartServoRuntime.isDestinationReached());
+                    if (_theSmartServoRuntime.isDestinationReached())
+                    {
+                        _count++;
+                    }
+                    getLogger().info("Ipo state " + _theSmartServoRuntime.getFineIpoState());
+                    getLogger().info("Remaining time " + _theSmartServoRuntime.getRemainingTime());
+                    getLogger().info("LBR Position "
+                            + _lbr.getCurrentJointPosition());
+                    getLogger().info(" Measured LBR Position "
+                            + curMsrJntPose);
+                    if (_steps % 100 == 0)
+                    {
+                        // Some internal values, which can be displayed
+                        getLogger().info("Simple Joint Test - step " + _steps + _theSmartServoRuntime.toString());
+
+                    }
+                }
+                aStep.end();
+
+            }
+        }
+        catch (Exception e)
+        {
+            getLogger().info(e.getLocalizedMessage());
+            e.printStackTrace();
+        }
+        ThreadUtil.milliSleep(1000);
+
+        //Print statistics and parameters of the motion
+        getLogger().info("Displaying final states after loop ");
+        getLogger().info(getClass().getName() + _theSmartServoRuntime.toString());
+        // Stop the motion
+
+        getLogger().info("Stop the SmartServo motion");
+        _theSmartServoRuntime.stopMotion();
+
+        getLogger().info(_count + " times was the destination reached.");
+        getLogger().info("Statistic Timing of Overall Loop " + timing);
+        if (timing.getMeanTimeMillis() > 150)
+        {
+            getLogger().info("Statistic Timing is unexpected slow, you should try to optimize TCP/IP Transfer");
+            getLogger().info("Under Windows, you should play with the registry, see the e.g. the SmartServo Class javaDoc for details");
+        }
+    }
+
+    
+    double getTheDisplacment(double dj)
+    {
+    	
+    	return dj;
+    	/*
+    	double   a=0.07; 
+    	double b=a*0.75; 
+		double exponenet=-Math.pow(dj/b, 2);
+		return Math.signum(dj)*a*(1-Math.exp(exponenet));
+		*/
+    }
+
+
+    /**
+     * Main routine, which starts the application
+     */
+    public static void main(String[] args)
+    {
+        SimulinkFIIWASmartServoJoints app = new SimulinkFIIWASmartServoJoints();
+        app.runApplication();
+    }
+ 
+    public class UDPServer implements Runnable{
+    	
+    	public double[] jpos=new double[7];
+    	
+    	double stime=0;
+		double endtime=0;
+		
+    	int _port;
+    	int vectorSize=7;
+    	int packetCounter=0;
+    	
+    	byte[] buffer=new byte[8*vectorSize];
+    	UDPServer(int port)
+    	{
+    		for(int i=0;i<7;i++)
+    		{
+    			jpos[i]=0;
+    		}
+    		Thread t=new Thread(this);
+    		t.setDaemon(true);
+    		t.start();
+    		_port=port;
+    	}
+    	
+    	public void run()
+    	{
+    			System.out.println("Program will terminate if comuncation is not established in 10 seconds");
+    			DatagramSocket soc=null;
+    			try {
+    				soc = new DatagramSocket(_port);
+    				soc.setSoTimeout(10000); // 5 seconds, if a time out occurred, turn off program
+    				DatagramPacket response=new DatagramPacket(buffer, buffer.length);
+    				while(true)
+    				{
+    					soc.receive(response);
+    					packetCounter=packetCounter+1;
+    					// String s= new String(buffer,0,response.getLength())
+    					 // System.out.println(response.getLength());
+    					if(response.getLength()==8*vectorSize)
+    					{
+    						if(packetCounter==1)
+    						{
+    							stime=System.currentTimeMillis();
+    						}
+    						else
+    						{
+    							endtime=System.currentTimeMillis();
+    						}
+    						byte[] daB=new byte[8];
+    						int counter=0;
+    						int jointNum=0;
+    						while(counter <8*vectorSize)
+    						{
+	    						for(int i=0;i<8;i++)
+	    						{
+	    							daB[7-i]=buffer[counter];
+	    							counter=counter+1;
+	    						}
+	    						jpos[jointNum]=bytesToDouble(daB);
+	    						jointNum=jointNum+1;	    						
+	    						//System.out.println(bytesToDouble(daB));
+    						}
+    					}
+    				}
+    			} catch (SocketException e) {
+    				// TODO Auto-generated catch block
+    				e.printStackTrace();
+    				System.out.println(e.toString());
+    			} catch (IOException e) {
+    				// TODO Auto-generated catch block
+    				e.printStackTrace();
+    				System.out.println(e.toString());
+    			}
+    			if(soc==null)
+    			{}
+    			else
+    			{
+    				if(soc.isClosed())
+    				{}
+    				else
+    				{
+    					soc.close();
+    				}
+    			}
+   				SimulinkFIIWASmartServoJoints.loopFlag=false; //Turn off main loop 	
+   				double time=(endtime-stime)/1000;
+    			double rate=packetCounter/time;
+    			System.out.println("update rate, packets/second");
+    			System.out.println(rate);
+    	}
+    }
+    	
+    	
+    	public double bytesToDouble(byte[] b)
+    	{
+    		return ByteBuffer.wrap(b).getDouble();
+    	}
+
+}
diff --git a/Joint space motion with feedback/simulink/feedBackSmartDirectServoJoints.slx b/Joint space motion with feedback/simulink/feedBackSmartDirectServoJoints.slx
new file mode 100644
index 0000000..645ad1b
Binary files /dev/null and b/Joint space motion with feedback/simulink/feedBackSmartDirectServoJoints.slx differ
diff --git a/Joint space motion with feedback/simulink/feedBackSmartDirectServoJoints.slxc b/Joint space motion with feedback/simulink/feedBackSmartDirectServoJoints.slxc
new file mode 100644
index 0000000..3f8c072
Binary files /dev/null and b/Joint space motion with feedback/simulink/feedBackSmartDirectServoJoints.slxc differ
diff --git a/README.md b/README.md
index 6841117..103fd00 100644
--- a/README.md
+++ b/README.md
@@ -2,9 +2,50 @@
 The Simulink-iiwa is an interface that allows the user to control KUKA iiwa manipulaotrs from Simulink.
 Unlike the ![KUKA Sunrise Toolbox or KST](https://github.com/Modi1987/KST-Kuka-Sunrise-Toolbox), the Simulink interface is built upon UDP protocol instead of TCP/IP protocol.
 
+--------------------------------------
+
+# Requirments:
+
 The project in this rpository was tested using:
-Matlab 2018a
-KUKA iiwa 7R800 with Sunrise.OS 1.11.0.7
+* Matlab 2018a, running under Windows 10.
+* KUKA iiwa 7R800 with Sunrise.OS 1.11.0.7
+
+--------------------------------------
+# Installation
+There are various steps that you shall dow on the Matlab side and on the iiwa side:
+
+## Matlab side:
+* First open Matlab 2018a 
+* 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).
+* 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 kernel type (rtwho) in the command window of Matlab 2018a. 
+
+## IIWA side:
+* You will need a computer connected to the robot through an ethernet cable on X66 connector.
+* Open the Sunrise.Workbench on the computer side.
+* Download the repository into your computer, unzip it inside a folder (SimulinkIIWA) for example.
+* Then from inside the (SimulinkIIWA) pick up one of the operation modes available, for example (Cartesian motion open loop). Each operation mode is provided in separate folder, which contains the corresponding Sunrise.Workbench code along with the Simulink interface.
+* Inside the (Cartesian motion open loop) folder, open the (iiwa) sub-folder. 
+* You shall copy the available (java) file into a project inside the Sunrise.Workbench then synchronise into the robot controller.
+* The newly added application shall show up in the application list on the smart pad of the robot.
+--------------------------------------
+
+# Operation:
+First run the server on IIWA side using the smart pad, then you shall run the Simulink project in Matlab 2018a.
+
+## IIWA side:
+* Run the server application from the smart pad, you find it in the applications list under the name (SimulinkIIWADirectServoCartesian).
+* You will have 10 seconds to connect, if a connection was not initiated during the time limit the program will be terminated automatically.
+* If the connection is terminated, you have to run the server again before initiating a connection.
+
+## Matlab side:
+* Inside the (Cartesian motion open loop) folder, open the (iiwa) sub-folder. 
+* You shall find the simulink application, run it using Matlab 2018a.
+* After runnng the (SimulinkIIWADirectServoCartesian) on the robot side, you shall run the simulink project from matlab.
+* The robot shall move according to the motion command from simulink.
+
+--------------------------------------
+
+Copyright: Mohammad Safeea.
 
-Copyright: Mohammad Safeea
+The project is a work in progress, use it under your own responsibility, check the license.