Skip to content

Mit KI weiterarbeiten

Mit KI weiterarbeiten

Component Kit Schunk 2D Grasping-Kit (inkl. Vision Controller) ​

Dieser Artikel beschreibt die Kommunikation zwischen dem Schunk 2D Grasping-Kit und dem HORST Robotersystem.

Dieser Artikel ist sowohl für alle horstOS Versionen, wie auch für horstFX gültig.

1. Versionshinweise ​

Der Artikel sowie die Beispielprogramme und Funktionen beziehen sich auf folgende Versionen:

  • Robotersteuerungssoftware Version 2023.11.01
  • Schunk Grasping-Kit Native Protocol V3
  • Schunk Grasping-Kit Software Version 2.1.0
  • Schunk SVC Kit Inbetriebnahmeanleitung Auflage 03.00 | 24.04.2023 | de

2. Einleitung ​

Das Schunk 2D Grasping-Kit ist eine Anwendungslösung zum Greifen von nicht-lageorientierten Werkstücken bestehend aus einem Kamerasystem, einem applikationsspezifischen Greifsystem und einem Industrie-PC (SVC) auf dem die von SCHUNK entwickelten KI-Software installiert ist. Diese gewährleistet ein zuverlässiges Erkennen der Objekte, berechnet die besten Griffpunkte und setzt keinerlei Vorkenntnisse in der Programmierung oder Bildverarbeitung voraus.

Dieser Artikel beschreibt die Kommunikation zwischen dem Robotersystem HORST und dem Schunk 2D Grasping-Kit.

3. Einrichtung des Systems ​

Der mechanische Aufbau sowie die elektrische Verkabelung ist gut in der SVC Kit Inbetriebnahmeanleitung von Schunk beschrieben. Informationen zum Anschluss der Spannungsversorgung sowie der Netzwerkkabel findet man im Abschnitt 4.2 Spannungsversorgung und SVC installieren in der SVC Kit Inbetriebnahmeanleitung.

Um auf die Weboberfläche des Schunk Vision Controller SVC zugreifen zu können, muss sich die IP-Adresse des SVC in dem selben Bereich befinden wie die des PCs. Standardmäßig wird der SVC mit der IP-Adresse 192.168.0.101 ausgeliefert. Im Abschnitt 4.6 IP-Adresse einstellen und Software starten in der SVC Kit Inbetriebnahmeanleitung findet man dazu weitere Informationen.

Die Einrichtung des Grasping-Kits ist sehr gut über diese Weboberfläche des Systems, mittels Videos und Hilfeartikeln, dokumentiert. Über den Reiter "Docs" findet man ein Video, welches die erforderlichen Schritte zur Einstellung der Kamera, der Kalibrierung sowie der Einrichtung eines Projekts beschreibt. Weiterhin kann man bei jedem Einrichtungsschritt durch einen Klick auf das "? Help" - Symbol weitere Unterstützung in Video- oder Textform erhalten.

GraspingKitDocumentation

4. Kommunikation ​

Der Roboter und das Kamerasystem kommunizieren mittels TCP/IP miteinander. Informationen zur Kommunikation sowie der Befehlssatz befinden sich in der Dokumentation zu dem Schunk Grasping-Kit Native Protocol V3. Dieses kann auch über die Weboberfläche unter dem Bereich Robot -> PLUGIN heruntergeladen werden.

Eine Nachricht an das Kamerasystem bzw. von dem Kamerasystem besteht aus einem Prefix, einem Header sowie einem Body. Der Prefix beschreibt die genutzte Protokoll Version sowie die Länge der folgenden Zeichen innerhalb Nachricht (74 Bytes in Protokoll V3). Der Header beinhaltet den Typ der Nachricht sowie die Absicht des Befehls. Der Body liefert weitere Informationen zu dem Befehl. Die Nachrichten werden in hexadezimaler Darstellung an das Kamerasystem gesendet. Innerhalb der Nachricht muss zwingend auf die passenden Datentypen und deren Position geachtet werden. Je nach verwendetem Protokoll können diese sich unterscheiden. Daher muss im Prefix der Nachricht das passende Protokoll mitgeschickt werden (Hier immer V3 bzw. 0x00 0x03).

Innerhalb der Weboberfläche können unter dem Bereich Robot -> Communication die empfangenen und gesendeten Befehle überprüft werden. Mithilfe der Sandbox können Antworten von dem Kamerasystem an den Roboter erstellt sowie gesendet werden, ohne dass reale Objekte eingelernt und vom System detektiert werden.

4.1. Einstellen der IP-Adresse ​

Damit der Roboter und das Kamerasystem miteinander kommunizieren können, müssen sich die IP-Adressen im gleichen Bereich befinden. Die IP-Adresse des Roboters kann wie im folgenden Artikel beschrieben geändert werden: IP-Adresse des Roboters ändern

Die IP-Adresse des Kamerasystems kann über die Weboberfläche im Bereich Settings -> Port to robot angepasst werden.

GraspingKitIPAdresse

4.2. Herstellen der Verbindung ​

Mittels folgender Funktion kann in horstOS eine Verbindung zu dem Kamerasystem hergestellt werden:

// Connection
var IPGraspingKit = "192.168.0.76";
var PortGraspingKit = 42001;

// connect to GraspingKit
var Socket = connect();

// connect to GraspingKit
function connect()
{
    var Socket = new java.net.Socket();
    try
    {
        Socket.connect(new java.net.InetSocketAddress(IPGraspingKit, PortGraspingKit), 10000); //10s timeout
        showHint("Connection to GraspingKit established.");
        return Socket;
    }
    catch(e)
    {
        showError("A connection to the GraspingKit could not be established!");
        exitRobotScript();
    }
}

4.3. Senden von Informationen an das Kamerasystem ​

Beim Senden von Nachrichten an das Kamerasystem muss der Syntax des Schunk Native Protocol V3 beachtet werden. Die Nachrichten werden hexadezimal codiert gesendet. Zum Beispiel sieht der Befehl um den Status des Kamerasystems abzufragen folgendermaßen aus:

// build hex request
var Request = [0x00, 0x03, 0x00, 0x00, 0x00, 0x4A, 0x01, 0x00, 0x00, 0x02];

// fill request message to specified size with 0x00
for (var i = 10; i<80; i++)
{
      Request[i] = 0x00;
}

Gesendet wird der Befehl mit folgender Funktion:

// send it to GraspingKit
writeMessage(Socket, Request);

// send raw data via socket
function writeMessage(socket, Message)
{
        var jMessage = Java.to(Message,"byte[]");
        var dout = new java.io.DataOutputStream(socket.getOutputStream());
        dout.write(jMessage);
        dout.flush();
}

4.4. Einlesen von Informationen von dem Kamerasystem ​

Nachrichten von dem Kamerasystem können mittels folgender Funktionen eingelesen werden:

// read the response
var Response = readMessage(Socket);

// read the raw message
function readMessage(socket)
{
    // set timeout
    socket.setSoTimeout(20000);

    try
    {
        var ByteArray = Java.type("byte[]");
        var dIn = new java.io.DataInputStream(socket.getInputStream());

        // read the Prefix to get the length of the remaining data message
        var Prefix = new ByteArray(6);
        dIn.readFully(Prefix, 0, Prefix.length); // read the Prefix
        var msgID =  Prefix[0];
           var dataLength = (Prefix[5] & 0xFF) + (Prefix[4]  & 0xFF) + (Prefix[3] & 0xFF) + (Prefix[2]  & 0xFF);
        //showInfo(dataLength);

        // read the remaining data message
        var Message = new ByteArray(dataLength);
        dIn.readFully(Message, 0, Message.length);
        var Data =  Java.from(Message);
        
        return {Prefix: Prefix, Data: Data}
    }
    catch(e)
    {
        showError("Timeout while reading the message from the camera. The program is aborted due to safety reasons.");
        // close communication sockets and exit robot script
        closeRobotScript();
    }
}

Hierbei wird zunächst der Prefix über die bekannte Länge eingelesen. Anschließend wird mithilfe der Längenangabe der noch verbliebenen Nachricht, welche man durch den Prefix erhält, die verbleibende Nachricht vollends eingelesen.

Mittels folgender Funktion kann die Nachricht als hexadezimal codierte Nachricht angezeigt werden:

showInfo(toHexString(Response.Data));

// convert message to hex string
function toHexString(byteArray) 
{
        var s = '';
        byteArray.forEach(function(byte) 
        {
            s += '0x' + ('0' + (byte & 0xFF).toString(16)).slice(-2) + " ";
        });
        return s;
}

4.5. Trennen der Verbindung ​

Bei einem Abbruch des Programms oder im Fehlerfall ist darauf zu achten, dass die Socket-Verbindung zu dem Kamerasystem richtig getrennt wird. Das kann über folgenden Funktionsaufruf realisiert werden:

// close sockets
Socket.close();

4.6. Komplette Beispielfunktion ​

Ein an das Kamerasystem gesendeter Befehl enthält als Antwort immer den Hinweis, ob der Befehl von dem System richtig verarbeitet werden konnte. D.h. nach dem Senden eines Befehls kann direkt die Antwort eingelesen und interpretiert werden.

// read the response
var Response = readMessage(Socket);
    
// wrap byte array into buffer
var bb = java.nio.ByteBuffer.wrap(Response.Data).order(java.nio.ByteOrder.BIG_ENDIAN);

// break down telegram for separating the GraspingKit response
var Index = breakDownTelegram();

// get byte at given index
var Success = bb.get(Index.ReplyCode);    // reply code
var State = bb.get(6);                    // state
    
if (Success != 1.0)
{ 
    showError("Receiver encountured a failure processing the getState message.");
}

// break down telegram for separating the GraspingKit response
function breakDownTelegram()
{
    // index via protocol documentation
    var CommType = 0;        // uint8 = 1 Byte
    var ReplyCode = 1;       // uint8 = 1 Byte
    var ReplyCounter = 2;    // uint8 = 1 Byte
    var MsgType = 3;         // uint8 = 1 Byte
    var Body = 4;            // size depends on "Message Type"

    return {CommType:CommType, ReplyCode:ReplyCode, MsgType:MsgType, Body:Body}
}

Folgende Funktion ist ein komplettes Beispiel, wie der Status des Kamerasystems ausgelesen und interpretiert werden kann:

// get the state of GraspingKit
function getState()
{
    // build hex request
    var Request = [0x00, 0x03, 0x00, 0x00, 0x00, 0x4A, 0x01, 0x00, 0x00, 0x02];
    // fill request message to specified size with 0x00
    for (var i = 10; i<80; i++)
    {
        Request[i] = 0x00;
    }
    // send it to GraspingKit
    writeMessage(Socket, Request);

    // read the response
    var Response = readMessage(Socket);
    //showInfo(toHexString(Response.Data));
    
    // wrap byte array into buffer
    var bb = java.nio.ByteBuffer.wrap(Response.Data).order(java.nio.ByteOrder.BIG_ENDIAN);

    // break down telegram for separating the GraspingKit response
    var Index = breakDownTelegram();

    // get byte at given index
    var Success = bb.get(Index.ReplyCode);    // reply code
    var State = bb.get(6);                    // state
    
    if (Success != 1.0)
    { 
        showError("Receiver encountured a failure processing the getState message.");
    }
    else
    {
        switch (State)
        {
            case 1.0:
                showInfo("State GraspingKit: The server is initializing.");
                break;
            case 2.0:
                showHint("State GraspingKit: The server is operational an ready to serve requests.");
                break;
            case 3.0:
                showWarning("State GraspingKit: The server has stopped operating.");
                break;
            case 4.0:
                showError("State GraspingKit: The server has encountered a critical error!");
        }    
    }    
}

5. Funktionen ​

Zur Kommunikation mit dem Kamerasystem stehen folgende Funktionen zur Verfügung:

  • getProtocolVersion() : Gibt die neustmögliche Protokollversion des Kamerasystems zurück
  • getState() : Liefert den Status des Kamerasystems zurück
  • registerRobot() : Registriert den Roboter
  • setProject() : Aktiviert das gewünschte Projekt
  • takeSnap() : Triggert eine neue Bildaufnahme
  • getGrasp() : Liefert Informationen zur Objektposition
  • provideGraspfeedback(Feedback) : Gibt Rückmeldung über den Erfolg des Greifprozesses an das Kamerasystem
  • getObjectCount() : Man erhält die Anzahl der gefundenen Objekte in einer Szene
  • sendCurrentRobotPose() : Die aktuelle Roboterposition wird an das Kamerasystem gesendet

Bei dem Senden eines Trigger-Befehls (takeSnap()) zur neuen Bildaufnahme muss darauf geachtet werden, dass danach nur ein getGrasp() Befehl an das Kamerasystem geschickt werden darf. Ebenso muss vor einem getGrasp() Befehl ein Trigger-Befehl gesendet werden. Werden dazwischen andere Befehle gesendet, können diese nicht richtig interpretiert werden. Zwischen den beiden Befehlen können natürlich andere Roboter-Aktionen oder Befehle an andere Peripherie erfolgen.

Das Beispielprogramm, welches in Kapitel 7 heruntergeladen werden kann, beinhaltet die oben aufgelisteten Funktionen.

5.1.1 Beschreibung der Funktionen takeSnap() und getGrasp() ​

Als Beispiel wird im Folgenden die Funktion takeSnap() sowie getGrasp() beschrieben. Die übrigen Funktionen können analog dazu betrachtet werden. Weitere Informationen zu den Funktionen sowie Parametern findet man in der Schnittstellendokumentation Native Protocol V3 von Schunk.

Bei einem Trigger-Befehl werden folgende Parameter benötigt und mit an das Kamerasystem übergeben:

  • GraspMode : 3 für automatischer Griff, 2 für einen der eingelernten Griffe, 1 für nur der aktive Griff darf verwendet werden
  • ClassID : Klassen ID des Zielobjekts. 0 für zufälliges bzw. jedes Objekt
  • Tool : 1 für Parallelgreifer von Außen greifend, 2 für Parallelgreifer von Innen greifend, 3 für Kontaktgreifer wie z. B. Saug- oder Magnetgreifer
// Camera Data (described in simple protocol v3 description)
var GraspMode = 2;       // 3 = auto grasp: any_grasp model - free grasp-planning, 1 = aktive grasp: grasp must match target object's active grasp definition, 2 = any grasp: grasp must match any of target object's grasp definition 
var ClassID = 0;         // class id of target object, 0 = random object
var Tool = 1;            // 1 = exterior: Exterior two-finger parallel gripper, 2 = interior: Interior two-finger parallel gripper, 3 = contact: suction, magnet or adhesion with a contact area

// trigger camera and request a grasp
takeSnap();

// trigger camera and request a grasp
function takeSnap()
{
    showHint("Taking snapshot and getting grasp");

    // convert integer to needed format
    var ConvGraspMode = java.nio.ByteBuffer.allocate(1).put(GraspMode).array();
    var ConvClassID = java.nio.ByteBuffer.allocate(2).putShort(ClassID).array();
    var ConvTool = java.nio.ByteBuffer.allocate(1).put(Tool).array();

    // build hex request
    var Request = [0x00, 0x03, 0x00, 0x00, 0x00, 0x4A, 0x01, 0x00, 0x00, 0x10, 0x00, ConvGraspMode[0], ConvClassID[0], ConvClassID[1], ConvTool[0], 0x01];
    // fill request message to specified size with 0x00
    for (var i = 16; i<80; i++)
    {
        Request[i] = 0x00;
    }

    // send it to GraspingKit
    writeMessage(Socket, Request);
}

Mittels der getGrasp() Funktion erhält man nach dem Triggern die für das Greifen des Objekts relevanten Informationen.

// get grasp data
function getGrasp()
{
    showHint("Getting grasp data");

    // read the response
    var Response = readMessage(Socket);
    //showInfo(toHexString(Response.Data));
    LOG.info(toHexString(Response.Data));
    
    // wrap byte array into buffer
    var bb = java.nio.ByteBuffer.wrap(Response.Data).order(java.nio.ByteOrder.BIG_ENDIAN);

    // break down telegram for separating the GraspingKit response
    var Index = breakDownTelegram();

    // get byte at given index - Index without prefix -> index 6 lower than in protocol v3 doc
    var ReplyCode = bb.get(Index.ReplyCode);    // reply code
    var ReplyCounter = bb.get(2);               // reply counter
    var MsgType = bb.get(3);                    // message type
    var RespGraspMode = bb.get(7);              // GraspMode
    var ObjectClass = bb.getShort(8);           // object class
    var ObjectInstance = bb.getShort(10);       // object instance
    var Stroke = bb.getInt(12)/1000;            // distance between both fingers to set before approaching the object
    var AngleOffset = bb.getInt(16)/1000;       // angular offset in microedges beteween object model and robot flange at position zero
    var CenterOffsetX = bb.getInt(20)/1000;     // translation X in micrometers from object center to grasp point in robots base coordinate frame
    var CenterOffsetY = bb.getInt(24)/1000;     // translation Y in micrometers from object center to grasp point in robots base coordinate frame
    var CenterOffsetZ = bb.getInt(28)/1000;     // translation Z in micrometers from object center to grasp point in robots base coordinate frame        
    var RespTool = bb.get(32);                  // type of tool used in the request
    var RespPoseFormat = bb.get(33);            // pose format
    var GraspPoseX = bb.getInt(34)/1000;        // position X
    var GraspPoseY = bb.getInt(38)/1000;        // position Y
    var GraspPoseZ = bb.getInt(42)/1000;        // position Z
    var GraspPoseQx = bb.getInt(46)/1000;       // quaternion qx
    var GraspPoseQy = bb.getInt(50)/1000;       // quaternion qy
    var GraspPoseQz = bb.getInt(54)/1000;       // quaternion qz
    var GraspPoseQw = bb.getInt(58)/1000;       // quaternion qw
    var ObjectCount = bb.getShort(62);          // number of all detected objects
    var CandidateCount = bb.getShort(64);       // number of detected objects of the requested class id

    // If Error
    if (ReplyCode == 2.0)
    { 
        showError("Receiver encountured a failure processing the getGrasp message!");
        var NoObj = 1;

        return {NoObj:NoObj};
    }
    // If no success
    else if (ReplyCode != 1.0)
    {    
        switch (ReplyCode)
        {
            case 3.0:
                //showInfo("The server could not find the requested Object!");
                showHint("The server could not find the requested Object!");
                break;
            case 4.0:
                //showInfo("The server could not find a valid grasp!");
                showHint("The server could not find a valid grasp!");
                break;
            case 5.0:
                showError("The object class is either invalid or does not exist!");
        }
        
        var NoObj = 1;
        
        return {NoObj:NoObj, ObjectCount:ObjectCount, CandidateCount:CandidateCount};
    }
    // If success
    else
    {
        // convert values
        GraspPoseX = GraspPoseX/1000;
        GraspPoseY = GraspPoseY/1000;
        GraspPoseZ = GraspPoseZ/1000;
        GraspPoseQz = -GraspPoseQx/1000;
        GraspPoseQy = GraspPoseQy/1000;
        GraspPoseQx = GraspPoseQz/1000;
        GraspPoseQw = GraspPoseQw/1000;

        //showInfo("GraspPoses in m:\n" + "X: "  + GraspPoseX + " Y: "  + GraspPoseY + " Z: "  + GraspPoseZ
        //    + "\nQx: "  + GraspPoseQx + " Qy: "  + GraspPoseQy  + " Qz: "  + GraspPoseQz + " Qw: "  + GraspPoseQw); 

        // get normal surface
        var Normal = getSurfaceNormalByQuaternion(GraspPoseQx, GraspPoseQy, GraspPoseQz, GraspPoseQw);
        var NoObj = 0;    
    
        return {Normal:Normal, NoObj:NoObj, 
                GraspPoseX:GraspPoseX, GraspPoseY:GraspPoseY, GraspPoseZ:GraspPoseZ,
                GraspPoseQx:GraspPoseQx, GraspPoseQy:GraspPoseQy, GraspPoseQz:GraspPoseQz, GraspPoseQw:GraspPoseQw,
                Stroke:Stroke, ObjectCount:ObjectCount, CandidateCount:CandidateCount,
                CenterOffsetX: CenterOffsetX, CenterOffsetY: CenterOffsetY, CenterOffsetZ: CenterOffsetZ}
    }
}

Man muss beachten, dass das Kamerasystem bei den Variablen ObjectCount sowie CandidateCount Objekte mitzählt, auch wenn diese nicht gegriffen werden können (Z.B. durch Kollisionen der Greiferbacken mit anderen Objekten oder dem ROI). Findet das Kamerasystem jedoch keinen gültigen Griff bei allen Objekten in der Szene, übermittelt das Kamerasystem für die Variablen ObjectCount sowie CandidateCount eine 0. Um dennoch die Anzahl der erkannten Objekte in der Szene zu erhalten, kann die Funktion getObjectCount() genutzt werden*.*

Bei den Rotationskonventionen ist darauf zu achten, dass das Koordinatensystem der Kamera zu dem des Roboters passt. Bei den Quaternionen ergibt sich damit:

GraspPoseQz = -GraspPoseQx; GraspPoseQy = GraspPoseQy; GraspPoseQx = GraspPoseQz; GraspPoseQw = GraspPoseQw;

Das negative Vorzeichen der GraspPoseQz ergibt sich aus dem eingestellten TCP des Greifers und kann daher variieren.

Bei den Rückgabewerten werden nur die notwendigsten Variablen zurückgegeben. Sollen weitere Variablen aus der Funktion übergeben werden, können diese analog zu den bestehenden in den return Befehl hinzugefügt werden.

Folgendermaßen kann die getGrasp() Funktion aufgerufen und verwendet werden:

// get grasp data
GraspData = getGrasp();

// grasping position
move({
       'Coord': 'CARTESIAN_BASIS', 
        'MoveType': 'LINEAR', 
        'PoseRelation': 'ABSOLUTE', 
        'anyconfiguration': false, 
        'speed.ratio': 0.7, 
        'target': {'xyz+quat': [GraspData.GraspPoseX, GraspData.GraspPoseY, GraspData.GraspPoseZ, GraspData.GraspPoseQx, GraspData.GraspPoseQy, GraspData.GraspPoseQz, GraspData.GraspPoseQw]}, 
         'tool': 'Schunk EGK25'
}, "ApproachPos");

6. Beispielprogramm ​

Das in Abschnitt 7 herunterzuladende Beispielprogramm beinhaltet alle in Abschnitt 5 aufgeführten Funktionen. Jedoch werden nicht alle Funktionen verwendet bzw. aufgerufen.

In dem Programm wird eine Verbindung zu dem Kamerasystem aufgebaut und der Roboter wird registriert. Im Anschluss daran wird das Projekt mit der Nummer 1 aktiviert und der Status des Systems abgefragt. In einer Endlosschleife wird anschließend die Kamera getriggert und das Objekt an einer bestimmten Position abgelegt. Nach der Ablage wird das Kamerasystem erneut getriggert und an der Lokalisierungsposition werden die neuen Objektkoordinaten eingelesen. Kann das System kein Objekt bzw. keinen gültigen Griff finden, wird nach einer Sekunde Pause die Kamera erneut getriggert.

7. Downloadbereich ​

7.1. SVC Kit Inbetriebnahmeanleitung ​

SVC Kit Inbetriebnahmeanleitung

7.2. SVC Native Protocol V3 ​

SVC Native Protocol V3

7.3. horstOS Beispielprogramm ​

horstOS Beispielprogramm