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.
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.
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

