KUKA iiwa — VREP顯示機械臂位置

 　　機械臂控制器中Background task通過UDP向外部程序每隔50ms循環發送當前各軸位置：

package sampleBackgroundTask;

import javax.inject.Inject;
import java.util.concurrent.TimeUnit;
import com.kuka.roboticsAPI.applicationModel.tasks.CycleBehavior;
import com.kuka.roboticsAPI.applicationModel.tasks.RoboticsAPICyclicBackgroundTask;
import com.kuka.roboticsAPI.deviceModel.JointPosition;
import com.kuka.roboticsAPI.deviceModel.LBR;
import java.net.*;
import java.io.IOException; 


public class BackgroundTask extends RoboticsAPICyclicBackgroundTask {
    @Inject
    private LBR robot; 
    
    byte[] posArr;    
    int port = 30003;
    InetAddress IP = null; 
    DatagramSocket datagramSocket = null; 
    DatagramPacket packet; 

    @Override
    public void initialize() {
        // initialize your task here
        initializeCyclic(2000, 50, TimeUnit.MILLISECONDS, CycleBehavior.BestEffort);
        
        try {
            datagramSocket = new DatagramSocket();   
            IP =  InetAddress.getByName("192.168.1.50"); // get PC's address
        } 
        catch (SocketException e)         { e.printStackTrace(); } 
        catch (UnknownHostException e)  { e.printStackTrace(); } 
    }

    @Override
    public void runCyclic() {
        // your task execution starts here
         JointPosition jPos = robot.getCurrentJointPosition();  
        posArr = jPos.toString().getBytes();  
        packet = new DatagramPacket(posArr, posArr.length, IP, port); 
        try {
            datagramSocket.send(packet);
        } 
        catch (IOException e)
        {  e.printStackTrace(); } 
    }
}

 　　虛擬模型關節設為Passive模式，VREP中的Lua代碼獲取KUKA iiwa各軸位置並設置關節角：

function sysCall_init()
    -- do some initialization here:
    local socket = require 'socket'
    local host = "192.168.1.50"
    local port = 30003
    udp = assert(socket.udp())
    udp:settimeout(-1)
    assert(udp:setsockname(host, port))
    
    -- Get joint handles
    jointHandles = {-1,-1,-1,-1,-1,-1,-1}
    for i=1,7,1 do
        jointHandles[i] = sim.getObjectHandle('LBR_iiwa_7_R800_joint'..i)
    end
end


function sysCall_actuation()
    local data,receip,receport = udp:receivefrom()
    if data then
        --print("udp:receivefrom: " .. data .. receip, receport)
        local jointPositions = getJointPositions(data)
        for i=1,7,1 do
            sim.setJointPosition(jointHandles[i], jointPositions[i])
        end
    end
end


function getJointPositions(s)
    local result = {}
    local s = string.sub(s, 2, -2) -- Return a substring of the string
    for match in string.gmatch(s, "[^,]+") do
        table.insert(result, tonumber(match))  
    end
    return result
end

　　手動拖拽實際的機械臂，VREP中的虛擬模型按照軸的反饋位置實時更新：

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

參考：

Tutorial:Networking with UDP