Dieser Artikel beschreibt die Kommunikation zwischen dem Schunk 2D Grasping-Kit und dem HORST Robotersystem.
1. Versionshinweise
Der Artikel sowie die Beispielprogramme und Funktionen beziehen sich auf folgende Versionen:
- horstFX 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 horstFX 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