﻿' 25/09/2017 (Frederic De Oliveira - www.rpc3d.com) ' Converted from C# to VB.Net ' Wrapped some methods from python API (RoboDK v3.2.6) // python code on non-wrapped method is commented for later conversion ' The RoboDK class allows interacting with RoboDK ' This library includes a robotics toolbox for C#, inspired from Peter Corke's Robotics Toolbox: ' http://petercorke.com/Robotics_Toolbox.html ' ' In this library: pose = transformation matrix = homogeneous matrix = 4x4 matrix ' Visit: http://www.j3d.org/matrix_faq/matrfaq_latest.html ' to better understand homogeneous matrix operations ' ' This library includes the mathematics to operate with homogeneous matrices for robotics. Imports System.Collections.Generic Imports System.Linq Imports System.Text Imports System.Threading.Tasks Imports System.Net.Sockets ' for Socket Imports Microsoft.Win32 ' for registry key ''' ''' Matrix class for robotics. ''' Public Class Mat ' simple matrix class for homogeneous operations Public rows As Integer Public cols As Integer Private _mat As Double(,) ' Class used for Matrix exceptions Public Class MatException Inherits Exception Public Sub New(Message As String) MyBase.New(Message) End Sub End Class ''' ''' Matrix class constructor for any size matrix ''' ''' dimension 1 size (rows) ''' dimension 2 size (columns) Public Sub New(Rows__1 As Integer, Cols__2 As Integer) ' Matrix Class constructor rows = Rows__1 cols = Cols__2 _mat = New Double(rows - 1, cols - 1) {} End Sub ''' ''' Matrix class constructor for a 4x4 homogeneous matrix ''' ''' Position [0,0] ''' Position [0,1] ''' Position [0,2] ''' Position [0,3] ''' Position [1,0] ''' Position [1,1] ''' Position [1,2] ''' Position [1,3] ''' Position [2,0] ''' Position [2,1] ''' Position [2,2] ''' Position [2,3] Public Sub New(nx As Double, ox As Double, ax As Double, tx As Double, ny As Double, oy As Double, _ ay As Double, ty As Double, nz As Double, oz As Double, az As Double, tz As Double) ' Matrix Class constructor rows = 4 cols = 4 _mat = New Double(rows - 1, cols - 1) {} _mat(0, 0) = nx _mat(1, 0) = ny _mat(2, 0) = nz _mat(0, 1) = ox _mat(1, 1) = oy _mat(2, 1) = oz _mat(0, 2) = ax _mat(1, 2) = ay _mat(2, 2) = az _mat(0, 3) = tx _mat(1, 3) = ty _mat(2, 3) = tz _mat(3, 0) = 0.0 _mat(3, 1) = 0.0 _mat(3, 2) = 0.0 _mat(3, 3) = 1.0 End Sub ''' ''' Matrix class constructor for a 4x4 homogeneous matrix as a copy from another matrix ''' Public Sub New(pose As Mat) rows = pose.rows cols = pose.cols _mat = New Double(rows - 1, cols - 1) {} For i As Integer = 0 To rows - 1 For j As Integer = 0 To cols - 1 _mat(i, j) = pose(i, j) Next Next End Sub ''' ''' Matrix class constructor for a 4x1 vector [x,y,z,1] ''' ''' x coordinate ''' y coordinate ''' z coordinate Public Sub New(x As Double, y As Double, z As Double) rows = 4 cols = 1 _mat = New Double(rows - 1, cols - 1) {} _mat(0, 0) = x _mat(1, 0) = y _mat(2, 0) = z _mat(3, 0) = 1.0 End Sub '---------------------------------------------------- '-------- Generic matrix usage --------------- ''' ''' Return a translation matrix ''' | 1 0 0 X | ''' transl(X,Y,Z) = | 0 1 0 Y | ''' | 0 0 1 Z | ''' | 0 0 0 1 | ''' ''' translation along X (mm) ''' translation along Y (mm) ''' translation along Z (mm) ''' Public Shared Function transl(x As Double, y As Double, z As Double) As Mat Dim mat__1 As Mat = Mat.IdentityMatrix(4, 4) mat__1.setPos(x, y, z) Return mat__1 End Function ''' ''' Return a X-axis rotation matrix ''' | 1 0 0 0 | ''' rotx(rx) = | 0 cos(rx) -sin(rx) 0 | ''' | 0 sin(rx) cos(rx) 0 | ''' | 0 0 0 1 | ''' ''' rotation around X axis (in radians) ''' Public Shared Function rotx(rx As Double) As Mat Dim cx As Double = Math.Cos(rx) Dim sx As Double = Math.Sin(rx) Return New Mat(1, 0, 0, 0, 0, cx, _ -sx, 0, 0, sx, cx, 0) End Function ''' ''' Return a Y-axis rotation matrix ''' | cos(ry) 0 sin(ry) 0 | ''' roty(ry) = | 0 1 0 0 | ''' | -sin(ry) 0 cos(ry) 0 | ''' | 0 0 0 1 | ''' ''' rotation around Y axis (in radians) ''' Public Shared Function roty(ry As Double) As Mat Dim cy As Double = Math.Cos(ry) Dim sy As Double = Math.Sin(ry) Return New Mat(cy, 0, sy, 0, 0, 1, _ 0, 0, -sy, 0, cy, 0) End Function ''' ''' Return a Z-axis rotation matrix ''' | cos(rz) -sin(rz) 0 0 | ''' rotz(rx) = | sin(rz) cos(rz) 0 0 | ''' | 0 0 1 0 | ''' | 0 0 0 1 | ''' ''' rotation around Z axis (in radians) ''' Public Shared Function rotz(rz As Double) As Mat Dim cz As Double = Math.Cos(rz) Dim sz As Double = Math.Sin(rz) Return New Mat(cz, -sz, 0, 0, sz, cz, _ 0, 0, 0, 0, 1, 0) End Function '---------------------------------------------------- '------ Pose to xyzrpw and xyzrpw to pose------------ ''' ''' Calculates the equivalent position and euler angles ([x,y,z,r,p,w] vector) of the given pose ''' Note: transl(x,y,z)*rotz(w*pi/180)*roty(p*pi/180)*rotx(r*pi/180) ''' See also: FromXYZRPW() ''' ''' XYZWPR translation and rotation in mm and degrees Public Function ToXYZRPW() As Double() Dim xyzwpr As Double() = New Double(5) {} Dim x As Double = _mat(0, 3) Dim y As Double = _mat(1, 3) Dim z As Double = _mat(2, 3) Dim w As Double, p As Double, r As Double If _mat(2, 0) > (1.0 - 0.000001) Then p = -Math.PI * 0.5 r = 0 w = Math.Atan2(-_mat(1, 2), _mat(1, 1)) ElseIf _mat(2, 0) < -1.0 + 0.000001 Then p = 0.5 * Math.PI r = 0 w = Math.Atan2(_mat(1, 2), _mat(1, 1)) Else p = Math.Atan2(-_mat(2, 0), Math.Sqrt(_mat(0, 0) * _mat(0, 0) + _mat(1, 0) * _mat(1, 0))) w = Math.Atan2(_mat(1, 0), _mat(0, 0)) r = Math.Atan2(_mat(2, 1), _mat(2, 2)) End If xyzwpr(0) = x xyzwpr(1) = y xyzwpr(2) = z xyzwpr(3) = r * 180.0 / Math.PI xyzwpr(4) = p * 180.0 / Math.PI xyzwpr(5) = w * 180.0 / Math.PI Return xyzwpr End Function ''' ''' Calculates the pose from the position and euler angles ([x,y,z,r,p,w] vector) ''' The result is the same as calling: H = transl(x,y,z)*rotz(w*pi/180)*roty(p*pi/180)*rotx(r*pi/180) ''' ''' ''' ''' ''' ''' ''' ''' Homogeneous matrix (4x4) Public Shared Function FromXYZRPW(x As Double, y As Double, z As Double, w As Double, p As Double, r As Double) As Mat Dim a As Double = r * Math.PI / 180.0 Dim b As Double = p * Math.PI / 180.0 Dim c As Double = w * Math.PI / 180.0 Dim ca As Double = Math.Cos(a) Dim sa As Double = Math.Sin(a) Dim cb As Double = Math.Cos(b) Dim sb As Double = Math.Sin(b) Dim cc As Double = Math.Cos(c) Dim sc As Double = Math.Sin(c) Return New Mat(cb * cc, cc * sa * sb - ca * sc, sa * sc + ca * cc * sb, x, cb * sc, ca * cc + sa * sb * sc, _ ca * sb * sc - cc * sa, y, -sb, cb * sa, ca * cb, z) End Function ''' ''' Calculates the pose from the position and euler angles ([x,y,z,r,p,w] vector) ''' The result is the same as calling: H = transl(x,y,z)*rotz(w*pi/180)*roty(p*pi/180)*rotx(r*pi/180) ''' ''' ''' Homogeneous matrix (4x4) Public Shared Function FromXYZRPW(xyzwpr As Double()) As Mat If xyzwpr.Length < 6 Then Return Nothing End If Return FromXYZRPW(xyzwpr(0), xyzwpr(1), xyzwpr(2), xyzwpr(3), xyzwpr(4), xyzwpr(5)) End Function ''' ''' Calculates the pose from the position and euler angles ([x,y,z,rx,ry,rz] array) ''' The result is the same as calling: H = transl(x,y,z)*rotx(rx*pi/180)*roty(ry*pi/180)*rotz(rz*pi/180) ''' ''' ''' ''' ''' ''' ''' ''' Homogeneous matrix (4x4) Public Shared Function FromTxyzRxyz(x As Double, y As Double, z As Double, rx As Double, ry As Double, rz As Double) As Mat Dim a As Double = rx * Math.PI / 180.0 Dim b As Double = ry * Math.PI / 180.0 Dim c As Double = rz * Math.PI / 180.0 Dim crx As Double = Math.Cos(a) Dim srx As Double = Math.Sin(a) Dim cry As Double = Math.Cos(b) Dim sry As Double = Math.Sin(b) Dim crz As Double = Math.Cos(c) Dim srz As Double = Math.Sin(c) Return New Mat(cry * crz, -cry * srz, sry, x, crx * srz + crz * srx * sry, crx * crz - srx * sry * srz, _ -cry * srx, y, srx * srz - crx * crz * sry, crz * srx + crx * sry * srz, crx * cry, z) End Function ''' ''' Calculates the pose from the position and euler angles ([x,y,z,rx,ry,rz] array) ''' The result is the same as calling: H = transl(x,y,z)*rotx(rx*pi/180)*roty(ry*pi/180)*rotz(rz*pi/180) ''' ''' Homogeneous matrix (4x4) Public Shared Function FromTxyzRxyz(xyzwpr As Double()) As Mat If xyzwpr.Length < 6 Then Return Nothing End If Return FromTxyzRxyz(xyzwpr(0), xyzwpr(1), xyzwpr(2), xyzwpr(3), xyzwpr(4), xyzwpr(5)) End Function ''' ''' Calculates the equivalent position and euler angles ([x,y,z,rx,ry,rz] array) of a pose ''' Note: Pose = transl(x,y,z)*rotx(rx*pi/180)*roty(ry*pi/180)*rotz(rz*pi/180) ''' See also: FromTxyzRxyz() ''' ''' XYZWPR translation and rotation in mm and degrees Public Function ToTxyzRxyz() As Double() Dim xyzwpr As Double() = New Double(5) {} Dim x As Double = _mat(0, 3) Dim y As Double = _mat(1, 3) Dim z As Double = _mat(2, 3) Dim rx1 As Double = 0 Dim ry1 As Double = 0 Dim rz1 As Double = 0 Dim a As Double = _mat(0, 0) Dim b As Double = _mat(0, 1) Dim c As Double = _mat(0, 2) Dim d As Double = _mat(1, 2) Dim e As Double = _mat(2, 2) If c = 1 Then ry1 = 0.5 * Math.PI rx1 = 0 rz1 = Math.Atan2(_mat(1, 0), _mat(1, 1)) ElseIf c = -1 Then ry1 = -Math.PI / 2 rx1 = 0 rz1 = Math.Atan2(_mat(1, 0), _mat(1, 1)) Else Dim sy As Double = c Dim cy1 As Double = +Math.Sqrt(1 - sy * sy) Dim sx1 As Double = -d / cy1 Dim cx1 As Double = e / cy1 Dim sz1 As Double = -b / cy1 Dim cz1 As Double = a / cy1 rx1 = Math.Atan2(sx1, cx1) ry1 = Math.Atan2(sy, cy1) rz1 = Math.Atan2(sz1, cz1) End If xyzwpr(0) = x xyzwpr(1) = y xyzwpr(2) = z xyzwpr(3) = rx1 * 180.0 / Math.PI xyzwpr(4) = ry1 * 180.0 / Math.PI xyzwpr(5) = rz1 * 180.0 / Math.PI Return xyzwpr End Function ''' ''' Converts a pose (4x4 matrix) to a Staubli XYZWPR target ''' ''' XYZWPR translation and rotation in mm and degrees Public Function ToStaubli() As Double() 'Converts a pose (4x4 matrix) to a Staubli XYZWPR target Dim xyzwpr As Double() = ToTxyzRxyz() 'already in degrees and mm 'xyzwpr(3) *= 180.0 / Math.PI 'xyzwpr(4) *= 180.0 / Math.PI 'xyzwpr(5) *= 180.0 / Math.PI Return xyzwpr End Function 'def ToMotoman(H): ' """Converts a pose (4x4 matrix) to a Motoman XYZWPR target ' :param H: pose ' :type H: :class:`.Mat`""" ' xyzwpr = pose_2_xyzrpw(H) ' return xyzwpr 'def Pose_2_Fanuc(H): ' """Converts a pose (4x4 matrix) to a Fanuc XYZWPR target ' :param H: pose ' :type H: :class:`.Mat`""" ' xyzwpr = pose_2_xyzrpw(H) ' return xyzwpr 'def Motoman_2_Pose(xyzwpr): ' """Converts a Motoman target to a pose (4x4 matrix)""" ' return xyzrpw_2_pose(xyzwpr) 'def Pose_2_KUKA(H): ' """Converts a pose (4x4 matrix) to a Kuka target ' :param H: pose ' :type H: :class:`.Mat`""" ' x = H[0,3] ' y = H[1,3] ' z = H[2,3] ' if (H[2,0]) > (1.0 - 1e-6): ' p = -pi/2 ' r = 0 ' w = atan2(-H[1,2],H[1,1]) ' elif (H[2,0]) < (-1.0 + 1e-6): ' p = pi/2 ' r = 0 ' w = atan2(H[1,2],H[1,1]) ' else: ' p = atan2(-H[2,0],sqrt(H[0,0]*H[0,0]+H[1,0]*H[1,0])) ' w = atan2(H[1,0],H[0,0]) ' r = atan2(H[2,1],H[2,2]) ' return [x, y, z, w*180/pi, p*180/pi, r*180/pi] 'def KUKA_2_Pose(xyzrpw): ' """Converts a KUKA XYZABC target to a pose (4x4 matrix)""" ' [x,y,z,r,p,w] = xyzrpw ' a = r*math.pi/180.0 ' b = p*math.pi/180.0 ' c = w*math.pi/180.0 ' ca = math.cos(a) ' sa = math.sin(a) ' cb = math.cos(b) ' sb = math.sin(b) ' cc = math.cos(c) ' sc = math.sin(c) ' return Mat([[cb*ca, ca*sc*sb - cc*sa, sc*sa + cc*ca*sb, x],[cb*sa, cc*ca + sc*sb*sa, cc*sb*sa - ca*sc, y],[-sb, cb*sc, cc*cb, z],[0.0,0.0,0.0,1.0]]) 'def Adept_2_Pose(xyzrpw): ' """Converts an Adept XYZRPW target to a pose (4x4 matrix)""" ' [x,y,z,r,p,w] = xyzrpw ' a = r*math.pi/180.0 ' b = p*math.pi/180.0 ' c = w*math.pi/180.0 ' ca = math.cos(a) ' sa = math.sin(a) ' cb = math.cos(b) ' sb = math.sin(b) ' cc = math.cos(c) ' sc = math.sin(c) ' return Mat([[ca*cb*cc - sa*sc, - cc*sa - ca*cb*sc, ca*sb, x],[ca*sc + cb*cc*sa, ca*cc - cb*sa*sc, sa*sb, y],[-cc*sb, sb*sc, cb, z],[0.0,0.0,0.0,1.0]]) 'def Pose_2_Adept(H): ' """Converts a pose to an Adept target ' :param H: pose ' :type H: :class:`.Mat`""" ' x = H[0,3] ' y = H[1,3] ' z = H[2,3] ' if H[2,2] > (1.0 - 1e-6): ' r = 0 ' p = 0 ' w = atan2(H[1,0],H[0,0]) ' elif H[2,2] < (-1.0 + 1e-6): ' r = 0 ' p = pi ' w = atan2(H[1,0],H[1,1]) ' else: ' cb=H[2,2] ' sb=+sqrt(1-cb*cb) ' sc=H[2,1]/sb ' cc=-H[2,0]/sb ' sa=H[1,2]/sb ' ca=H[0,2]/sb ' r = atan2(sa,ca) ' p = atan2(sb,cb) ' w = atan2(sc,cc) ' return [x, y, z, r*180/pi, p*180/pi, w*180/pi] 'def Comau_2_Pose(xyzrpw): ' """Converts a Comau XYZRPW target to a pose (4x4 matrix)""" ' return Adept_2_Pose(xyzrpw) 'def Pose_2_Comau(H): ' """Converts a pose to a Comau target ' :param H: pose ' :type H: :class:`.Mat`""" ' return Pose_2_Adept(H) 'def Pose_2_Nachi(pose): ' """Converts a pose to a Nachi XYZRPW target ' :param pose: pose ' :type pose: :class:`.Mat`""" ' [x,y,z,r,p,w] = pose_2_xyzrpw(pose) ' return [x,y,z,w,p,r] 'def Nachi_2_Pose(xyzwpr): ' """Converts a Nachi XYZRPW target to a pose (4x4 matrix)""" ' return Fanuc_2_Pose(xyzwpr) ''' ''' Returns the quaternion of a pose (4x4 matrix) ''' ''' ''' Private Shared Function PoseToQuaternion(Ti As Mat) As Double() Dim q As Double() = New Double(3) {} Dim a As Double = (Ti(0, 0)) Dim b As Double = (Ti(1, 1)) Dim c As Double = (Ti(2, 2)) Dim sign2 As Double = 1.0 Dim sign3 As Double = 1.0 Dim sign4 As Double = 1.0 If (Ti(2, 1) - Ti(1, 2)) < 0 Then sign2 = -1 End If If (Ti(0, 2) - Ti(2, 0)) < 0 Then sign3 = -1 End If If (Ti(1, 0) - Ti(0, 1)) < 0 Then sign4 = -1 End If q(0) = 0.5 * Math.Sqrt(Math.Max(a + b + c + 1, 0)) q(1) = 0.5 * sign2 * Math.Sqrt(Math.Max(a - b - c + 1, 0)) q(2) = 0.5 * sign3 * Math.Sqrt(Math.Max(-a + b - c + 1, 0)) q(3) = 0.5 * sign4 * Math.Sqrt(Math.Max(-a - b + c + 1, 0)) Return q End Function ''' ''' Returns the pose (4x4 matrix) from quaternion data ''' ''' ''' Private Shared Function QuaternionToPose(qin As Double()) As Mat Dim qnorm As Double = Math.Sqrt(qin(0) * qin(0) + qin(1) * qin(1) + qin(2) * qin(2) + qin(3) * qin(3)) Dim q As Double() = New Double(3) {} q(0) = qin(0) / qnorm q(1) = qin(1) / qnorm q(2) = qin(2) / qnorm q(3) = qin(3) / qnorm Dim pose As New Mat(1 - 2 * q(2) * q(2) - 2 * q(3) * q(3), 2 * q(1) * q(2) - 2 * q(3) * q(0), 2 * q(1) * q(3) + 2 * q(2) * q(0), 0, 2 * q(1) * q(2) + 2 * q(3) * q(0), 1 - 2 * q(1) * q(1) - 2 * q(3) * q(3), _ 2 * q(2) * q(3) - 2 * q(1) * q(0), 0, 2 * q(1) * q(3) - 2 * q(2) * q(0), 2 * q(2) * q(3) + 2 * q(1) * q(0), 1 - 2 * q(1) * q(1) - 2 * q(2) * q(2), 0) Return pose End Function ''' ''' Converts a pose to an ABB target ''' ''' ''' Private Shared Function PoseToABB(H As Mat) As Double() Dim q As Double() = PoseToQuaternion(H) Dim xyzq1234 As Double() = {H(0, 3), H(1, 3), H(2, 3), q(0), q(1), q(2), _ q(3)} Return xyzq1234 End Function ''' ''' Calculates the equivalent position and euler angles ([x,y,z,r,p,w] vector) of the given pose in Universal Robots format ''' Note: The difference between ToUR and ToXYZWPR is that the first one uses radians for the orientation and the second one uses degres ''' Note: transl(x,y,z)*rotx(rx*pi/180)*roty(ry*pi/180)*rotz(rz*pi/180) ''' See also: FromXYZRPW() ''' ''' XYZWPR translation and rotation in mm and radians Public Function PoseToUR() As Double() Dim xyzwpr As Double() = New Double(5) {} Dim x As Double = _mat(0, 3) Dim y As Double = _mat(1, 3) Dim z As Double = _mat(2, 3) Dim angle As Double = Math.Acos(Math.Min(Math.Max((_mat(0, 0) + _mat(1, 1) + _mat(2, 2) - 1) / 2, -1), 1)) Dim rx As Double = _mat(2, 1) - _mat(1, 2) Dim ry As Double = _mat(0, 2) - _mat(2, 0) Dim rz As Double = _mat(1, 0) - _mat(0, 1) If angle = 0 Then rx = 0 ry = 0 rz = 0 Else rx = rx * angle / (2 * Math.Sin(angle)) ry = ry * angle / (2 * Math.Sin(angle)) rz = rz * angle / (2 * Math.Sin(angle)) End If xyzwpr(0) = x xyzwpr(1) = y xyzwpr(2) = z xyzwpr(3) = rx xyzwpr(4) = ry xyzwpr(5) = rz Return xyzwpr End Function ''' ''' Calculates the pose from the position and euler angles ([x,y,z,r,p,w] vector) ''' Note: The difference between FromUR and FromXYZWPR is that the first one uses radians for the orientation and the second one uses degres ''' The result is the same as calling: H = transl(x,y,z)*rotx(rx)*roty(ry)*rotz(rz) ''' ''' The position and euler angles array ''' Homogeneous matrix (4x4) Public Shared Function URToPose(xyzwpr As Double()) As Mat Dim x As Double = xyzwpr(0) Dim y As Double = xyzwpr(1) Dim z As Double = xyzwpr(2) Dim w As Double = xyzwpr(3) Dim p As Double = xyzwpr(4) Dim r As Double = xyzwpr(5) Dim angle As Double = Math.Sqrt(w * w + p * p + r * r) If angle < 0.000001 Then Return Identity4x4() End If Dim c As Double = Math.Cos(angle) Dim s As Double = Math.Sin(angle) Dim ux As Double = w / angle Dim uy As Double = p / angle Dim uz As Double = r / angle Return New Mat(ux * ux + c * (1 - ux * ux), ux * uy * (1 - c) - uz * s, ux * uz * (1 - c) + uy * s, x, ux * uy * (1 - c) + uz * s, uy * uy + (1 - uy * uy) * c, _ uy * uz * (1 - c) - ux * s, y, ux * uz * (1 - c) - uy * s, uy * uz * (1 - c) + ux * s, uz * uz + (1 - uz * uz) * c, z) End Function ''' ''' Converts a matrix into a one-dimensional array of doubles ''' ''' one-dimensional array Public Function ToDoubles() As Double() Dim cnt As Integer = 0 Dim array As Double() = New Double(rows * cols - 1) {} For j As Integer = 0 To cols - 1 For i As Integer = 0 To rows - 1 array(cnt) = _mat(i, j) cnt = cnt + 1 Next Next Return array End Function ''' ''' Check if the matrix is square ''' Public Function IsSquare() As [Boolean] Return (rows = cols) End Function Public Function Is4x4() As [Boolean] If cols <> 4 OrElse rows <> 4 Then Return False End If Return True End Function ''' ''' Check if the matrix is homogeneous (4x4) ''' Public Function IsHomogeneous() As [Boolean] If Not Is4x4() Then Return False End If Return True ' ' test = self[0:3,0:3]; ' test = test*test.tr() ' test[0,0] = test[0,0] - 1.0 ' test[1,1] = test[1,1] - 1.0 ' test[2,2] = test[2,2] - 1.0 ' zero = 0.0 ' for x in range(3): ' for y in range(3): ' zero = zero + abs(test[x,y]) ' if zero > 1e-4: ' return False ' return True ' End Function ''' ''' Returns the inverse of a homogeneous matrix (4x4 matrix) ''' ''' Homogeneous matrix (4x4) Public Function inv() As Mat If Not IsHomogeneous() Then Throw New MatException("Can't invert a non-homogeneous matrix") End If Dim xyz As Double() = Me.Pos() Dim mat_xyz As New Mat(xyz(0), xyz(1), xyz(2)) Dim hinv As Mat = Me.Duplicate() hinv.setPos(0, 0, 0) hinv = hinv.Transpose() Dim new_pos As Mat = rotate(hinv, mat_xyz) hinv(0, 3) = -new_pos(0, 0) hinv(1, 3) = -new_pos(1, 0) hinv(2, 3) = -new_pos(2, 0) Return hinv End Function ''' ''' Rotate a vector given a matrix (rotation matrix or homogeneous matrix) ''' ''' 4x4 homogeneous matrix or 3x3 rotation matrix ''' 4x1 or 3x1 vector ''' Public Shared Function rotate(pose As Mat, vector As Mat) As Mat If pose.cols < 3 OrElse pose.rows < 3 OrElse vector.rows < 3 Then Throw New MatException("Invalid matrix size") End If Dim pose3x3 As Mat = pose.Duplicate() Dim vector3 As Mat = vector.Duplicate() pose3x3.rows = 3 pose3x3.cols = 3 vector3.rows = 3 Return pose3x3 * vector3 End Function ''' ''' Returns the XYZ position of the Homogeneous matrix ''' ''' XYZ position Public Function Pos() As Double() If Not Is4x4() Then Return Nothing End If Dim xyz As Double() = New Double(2) {} xyz(0) = _mat(0, 3) xyz(1) = _mat(1, 3) xyz(2) = _mat(2, 3) Return xyz End Function ''' ''' Sets the 4x4 position of the Homogeneous matrix ''' ''' XYZ position Public Sub setPos(xyz As Double()) If Not Is4x4() OrElse xyz.Length < 3 Then Return End If _mat(0, 3) = xyz(0) _mat(1, 3) = xyz(1) _mat(2, 3) = xyz(2) End Sub ''' ''' Sets the 4x4 position of the Homogeneous matrix ''' ''' X position ''' Y position ''' Z position Public Sub setPos(x As Double, y As Double, z As Double) If Not Is4x4() Then Return End If _mat(0, 3) = x _mat(1, 3) = y _mat(2, 3) = z End Sub Default Public Property Item(iRow As Integer, iCol As Integer) As Double ' Access this matrix as a 2D array Get Return _mat(iRow, iCol) End Get Set(value As Double) _mat(iRow, iCol) = value End Set End Property Public Function GetCol(k As Integer) As Mat Dim m As New Mat(rows, 1) For i As Integer = 0 To rows - 1 m(i, 0) = _mat(i, k) Next Return m End Function Public Sub SetCol(v As Mat, k As Integer) For i As Integer = 0 To rows - 1 _mat(i, k) = v(i, 0) Next End Sub Public Function Duplicate() As Mat ' Function returns the copy of this matrix Dim matrix As New Mat(rows, cols) For i As Integer = 0 To rows - 1 For j As Integer = 0 To cols - 1 matrix(i, j) = matrix(i, j) Next Next Return matrix End Function Public Shared Function ZeroMatrix(iRows As Integer, iCols As Integer) As Mat ' Function generates the zero matrix Dim matrix As New Mat(iRows, iCols) For i As Integer = 0 To iRows - 1 For j As Integer = 0 To iCols - 1 matrix(i, j) = 0 Next Next Return matrix End Function Public Shared Function IdentityMatrix(iRows As Integer, iCols As Integer) As Mat ' Function generates the identity matrix Dim matrix As Mat = ZeroMatrix(iRows, iCols) For i As Integer = 0 To Math.Min(iRows, iCols) - 1 matrix(i, i) = 1 Next Return matrix End Function ''' ''' Returns an identity 4x4 matrix (homogeneous matrix) ''' ''' Public Shared Function Identity4x4() As Mat Return Mat.IdentityMatrix(4, 4) End Function ' ' public static Mat Parse(string ps) // Function parses the matrix from string ' { ' string s = NormalizeMatrixString(ps); ' string[] rows = Regex.Split(s, "\r\n"); ' string[] nums = rows.Split(' '); ' Mat matrix = new Mat(rows.Length, nums.Length); ' try ' { ' for (int i = 0; i < rows.Length; i++) ' { ' nums = rows[i].Split(' '); ' for (int j = 0; j < nums.Length; j++) matrix[i, j] = double.Parse(nums[j]); ' } ' } ' catch (FormatException exc) { throw new MatException("Wrong input format!"); } ' return matrix; ' } Public Overrides Function ToString() As String ' Function returns matrix as a string Dim s As String = "" For i As Integer = 0 To rows - 1 For j As Integer = 0 To cols - 1 s += [String].Format("{0,5:0.00}", _mat(i, j)) + " " Next s += vbCr & vbLf Next Return s End Function ''' ''' Transpose a matrix ''' ''' Public Function Transpose() As Mat Return Transpose(Me) End Function Public Shared Function Transpose(m As Mat) As Mat ' Matrix transpose, for any rectangular matrix Dim t As New Mat(m.cols, m.rows) For i As Integer = 0 To m.rows - 1 For j As Integer = 0 To m.cols - 1 t(j, i) = m(i, j) Next Next Return t End Function Private Shared Sub SafeAplusBintoC(A As Mat, xa As Integer, ya As Integer, B As Mat, xb As Integer, yb As Integer, _ C As Mat, size As Integer) For i As Integer = 0 To size - 1 ' rows For j As Integer = 0 To size - 1 ' cols C(i, j) = 0 If xa + j < A.cols AndAlso ya + i < A.rows Then C(i, j) += A(ya + i, xa + j) End If If xb + j < B.cols AndAlso yb + i < B.rows Then C(i, j) += B(yb + i, xb + j) End If Next Next End Sub Private Shared Sub SafeAminusBintoC(A As Mat, xa As Integer, ya As Integer, B As Mat, xb As Integer, yb As Integer, _ C As Mat, size As Integer) For i As Integer = 0 To size - 1 ' rows For j As Integer = 0 To size - 1 ' cols C(i, j) = 0 If xa + j < A.cols AndAlso ya + i < A.rows Then C(i, j) += A(ya + i, xa + j) End If If xb + j < B.cols AndAlso yb + i < B.rows Then C(i, j) -= B(yb + i, xb + j) End If Next Next End Sub Private Shared Sub SafeACopytoC(A As Mat, xa As Integer, ya As Integer, C As Mat, size As Integer) For i As Integer = 0 To size - 1 ' rows For j As Integer = 0 To size - 1 ' cols C(i, j) = 0 If xa + j < A.cols AndAlso ya + i < A.rows Then C(i, j) += A(ya + i, xa + j) End If Next Next End Sub Private Shared Sub AplusBintoC(A As Mat, xa As Integer, ya As Integer, B As Mat, xb As Integer, yb As Integer, _ C As Mat, size As Integer) For i As Integer = 0 To size - 1 ' rows For j As Integer = 0 To size - 1 C(i, j) = A(ya + i, xa + j) + B(yb + i, xb + j) Next Next End Sub Private Shared Sub AminusBintoC(A As Mat, xa As Integer, ya As Integer, B As Mat, xb As Integer, yb As Integer, _ C As Mat, size As Integer) For i As Integer = 0 To size - 1 ' rows For j As Integer = 0 To size - 1 C(i, j) = A(ya + i, xa + j) - B(yb + i, xb + j) Next Next End Sub Private Shared Sub ACopytoC(A As Mat, xa As Integer, ya As Integer, C As Mat, size As Integer) For i As Integer = 0 To size - 1 ' rows For j As Integer = 0 To size - 1 C(i, j) = A(ya + i, xa + j) Next Next End Sub Private Shared Function StrassenMultiply(A As Mat, B As Mat) As Mat ' Smart matrix multiplication If A.cols <> B.rows Then Throw New MatException("Wrong dimension of matrix!") End If Dim R As Mat Dim msize As Integer = Math.Max(Math.Max(A.rows, A.cols), Math.Max(B.rows, B.cols)) If msize < 32 Then R = ZeroMatrix(A.rows, B.cols) For i As Integer = 0 To R.rows - 1 For j As Integer = 0 To R.cols - 1 For k As Integer = 0 To A.cols - 1 R(i, j) += A(i, k) * B(k, j) Next Next Next Return R End If Dim size As Integer = 1 Dim n As Integer = 0 While msize > size size *= 2 n += 1 End While Dim h As Integer = size / 2 Dim mField As Mat(,) = New Mat(n - 1, 8) {} ' ' * 8x8, 8x8, 8x8, ... ' * 4x4, 4x4, 4x4, ... ' * 2x2, 2x2, 2x2, ... ' * . . . ' Dim z As Integer For i As Integer = 0 To n - 5 ' rows z = CInt(Math.Pow(2, n - i - 1)) For j As Integer = 0 To 8 mField(i, j) = New Mat(z, z) Next Next SafeAplusBintoC(A, 0, 0, A, h, h, _ mField(0, 0), h) SafeAplusBintoC(B, 0, 0, B, h, h, _ mField(0, 1), h) StrassenMultiplyRun(mField(0, 0), mField(0, 1), mField(0, 1 + 1), 1, mField) ' (A11 + A22) * (B11 + B22); SafeAplusBintoC(A, 0, h, A, h, h, _ mField(0, 0), h) SafeACopytoC(B, 0, 0, mField(0, 1), h) StrassenMultiplyRun(mField(0, 0), mField(0, 1), mField(0, 1 + 2), 1, mField) ' (A21 + A22) * B11; SafeACopytoC(A, 0, 0, mField(0, 0), h) SafeAminusBintoC(B, h, 0, B, h, h, _ mField(0, 1), h) StrassenMultiplyRun(mField(0, 0), mField(0, 1), mField(0, 1 + 3), 1, mField) 'A11 * (B12 - B22); SafeACopytoC(A, h, h, mField(0, 0), h) SafeAminusBintoC(B, 0, h, B, 0, 0, _ mField(0, 1), h) StrassenMultiplyRun(mField(0, 0), mField(0, 1), mField(0, 1 + 4), 1, mField) 'A22 * (B21 - B11); SafeAplusBintoC(A, 0, 0, A, h, 0, _ mField(0, 0), h) SafeACopytoC(B, h, h, mField(0, 1), h) StrassenMultiplyRun(mField(0, 0), mField(0, 1), mField(0, 1 + 5), 1, mField) '(A11 + A12) * B22; SafeAminusBintoC(A, 0, h, A, 0, 0, _ mField(0, 0), h) SafeAplusBintoC(B, 0, 0, B, h, 0, _ mField(0, 1), h) StrassenMultiplyRun(mField(0, 0), mField(0, 1), mField(0, 1 + 6), 1, mField) '(A21 - A11) * (B11 + B12); SafeAminusBintoC(A, h, 0, A, h, h, _ mField(0, 0), h) SafeAplusBintoC(B, 0, h, B, h, h, _ mField(0, 1), h) StrassenMultiplyRun(mField(0, 0), mField(0, 1), mField(0, 1 + 7), 1, mField) ' (A12 - A22) * (B21 + B22); R = New Mat(A.rows, B.cols) ' result ' C11 For i As Integer = 0 To Math.Min(h, R.rows) - 1 ' rows For j As Integer = 0 To Math.Min(h, R.cols) - 1 ' cols R(i, j) = mField(0, 1 + 1)(i, j) + mField(0, 1 + 4)(i, j) - mField(0, 1 + 5)(i, j) + mField(0, 1 + 7)(i, j) Next Next ' C12 For i As Integer = 0 To Math.Min(h, R.rows) - 1 ' rows For j As Integer = h To Math.Min(2 * h, R.cols) - 1 ' cols R(i, j) = mField(0, 1 + 3)(i, j - h) + mField(0, 1 + 5)(i, j - h) Next Next ' C21 For i As Integer = h To Math.Min(2 * h, R.rows) - 1 ' rows For j As Integer = 0 To Math.Min(h, R.cols) - 1 ' cols R(i, j) = mField(0, 1 + 2)(i - h, j) + mField(0, 1 + 4)(i - h, j) Next Next ' C22 For i As Integer = h To Math.Min(2 * h, R.rows) - 1 ' rows For j As Integer = h To Math.Min(2 * h, R.cols) - 1 ' cols R(i, j) = mField(0, 1 + 1)(i - h, j - h) - mField(0, 1 + 2)(i - h, j - h) + mField(0, 1 + 3)(i - h, j - h) + mField(0, 1 + 6)(i - h, j - h) Next Next Return R End Function ' function for square matrix 2^N x 2^N Private Shared Sub StrassenMultiplyRun(A As Mat, B As Mat, C As Mat, l As Integer, f As Mat(,)) ' A * B into C, level of recursion, matrix field Dim size As Integer = A.rows Dim h As Integer = size / 2 If size < 32 Then For i As Integer = 0 To C.rows - 1 For j As Integer = 0 To C.cols - 1 C(i, j) = 0 For k As Integer = 0 To A.cols - 1 C(i, j) += A(i, k) * B(k, j) Next Next Next Return End If AplusBintoC(A, 0, 0, A, h, h, _ f(l, 0), h) AplusBintoC(B, 0, 0, B, h, h, _ f(l, 1), h) StrassenMultiplyRun(f(l, 0), f(l, 1), f(l, 1 + 1), l + 1, f) ' (A11 + A22) * (B11 + B22); AplusBintoC(A, 0, h, A, h, h, _ f(l, 0), h) ACopytoC(B, 0, 0, f(l, 1), h) StrassenMultiplyRun(f(l, 0), f(l, 1), f(l, 1 + 2), l + 1, f) ' (A21 + A22) * B11; ACopytoC(A, 0, 0, f(l, 0), h) AminusBintoC(B, h, 0, B, h, h, _ f(l, 1), h) StrassenMultiplyRun(f(l, 0), f(l, 1), f(l, 1 + 3), l + 1, f) 'A11 * (B12 - B22); ACopytoC(A, h, h, f(l, 0), h) AminusBintoC(B, 0, h, B, 0, 0, _ f(l, 1), h) StrassenMultiplyRun(f(l, 0), f(l, 1), f(l, 1 + 4), l + 1, f) 'A22 * (B21 - B11); AplusBintoC(A, 0, 0, A, h, 0, _ f(l, 0), h) ACopytoC(B, h, h, f(l, 1), h) StrassenMultiplyRun(f(l, 0), f(l, 1), f(l, 1 + 5), l + 1, f) '(A11 + A12) * B22; AminusBintoC(A, 0, h, A, 0, 0, _ f(l, 0), h) AplusBintoC(B, 0, 0, B, h, 0, _ f(l, 1), h) StrassenMultiplyRun(f(l, 0), f(l, 1), f(l, 1 + 6), l + 1, f) '(A21 - A11) * (B11 + B12); AminusBintoC(A, h, 0, A, h, h, _ f(l, 0), h) AplusBintoC(B, 0, h, B, h, h, _ f(l, 1), h) StrassenMultiplyRun(f(l, 0), f(l, 1), f(l, 1 + 7), l + 1, f) ' (A12 - A22) * (B21 + B22); ' C11 For i As Integer = 0 To h - 1 ' rows For j As Integer = 0 To h - 1 ' cols C(i, j) = f(l, 1 + 1)(i, j) + f(l, 1 + 4)(i, j) - f(l, 1 + 5)(i, j) + f(l, 1 + 7)(i, j) Next Next ' C12 For i As Integer = 0 To h - 1 ' rows For j As Integer = h To size - 1 ' cols C(i, j) = f(l, 1 + 3)(i, j - h) + f(l, 1 + 5)(i, j - h) Next Next ' C21 For i As Integer = h To size - 1 ' rows For j As Integer = 0 To h - 1 ' cols C(i, j) = f(l, 1 + 2)(i - h, j) + f(l, 1 + 4)(i - h, j) Next Next ' C22 For i As Integer = h To size - 1 ' rows For j As Integer = h To size - 1 ' cols C(i, j) = f(l, 1 + 1)(i - h, j - h) - f(l, 1 + 2)(i - h, j - h) + f(l, 1 + 3)(i - h, j - h) + f(l, 1 + 6)(i - h, j - h) Next Next End Sub Public Shared Function StupidMultiply(m1 As Mat, m2 As Mat) As Mat ' Stupid matrix multiplication If m1.cols <> m2.rows Then Throw New MatException("Wrong dimensions of matrix!") End If Dim result As Mat = ZeroMatrix(m1.rows, m2.cols) For i As Integer = 0 To result.rows - 1 For j As Integer = 0 To result.cols - 1 For k As Integer = 0 To m1.cols - 1 result(i, j) += m1(i, k) * m2(k, j) Next Next Next Return result End Function Private Shared Function Multiply(n As Double, m As Mat) As Mat ' Multiplication by constant n Dim r As New Mat(m.rows, m.cols) For i As Integer = 0 To m.rows - 1 For j As Integer = 0 To m.cols - 1 r(i, j) = m(i, j) * n Next Next Return r End Function Private Shared Function Add(m1 As Mat, m2 As Mat) As Mat ' Add matrix If m1.rows <> m2.rows OrElse m1.cols <> m2.cols Then Throw New MatException("Matrices must have the same dimensions!") End If Dim r As New Mat(m1.rows, m1.cols) For i As Integer = 0 To r.rows - 1 For j As Integer = 0 To r.cols - 1 r(i, j) = m1(i, j) + m2(i, j) Next Next Return r End Function Public Shared Function NormalizeMatrixString(matStr As String) As String ' From Andy - thank you! :) ' Remove any multiple spaces While matStr.IndexOf(" ") <> -1 matStr = matStr.Replace(" ", " ") End While ' Remove any spaces before or after newlines matStr = matStr.Replace(" " & vbCr & vbLf, vbCr & vbLf) matStr = matStr.Replace(vbCr & vbLf & " ", vbCr & vbLf) ' If the data ends in a newline, remove the trailing newline. ' Make it easier by first replacing \r\n’s with |’s then ' restore the |’s with \r\n’s matStr = matStr.Replace(vbCr & vbLf, "|") While matStr.LastIndexOf("|") = (matStr.Length - 1) matStr = matStr.Substring(0, matStr.Length - 1) End While matStr = matStr.Replace("|", vbCr & vbLf) Return matStr.Trim() End Function ' Operators Public Shared Operator -(m As Mat) As Mat Return Mat.Multiply(-1, m) End Operator Public Shared Operator +(m1 As Mat, m2 As Mat) As Mat Return Mat.Add(m1, m2) End Operator Public Shared Operator -(m1 As Mat, m2 As Mat) As Mat Return Mat.Add(m1, -m2) End Operator Public Shared Operator *(m1 As Mat, m2 As Mat) As Mat Return Mat.StrassenMultiply(m1, m2) End Operator Public Shared Operator *(n As Double, m As Mat) As Mat Return Mat.Multiply(n, m) End Operator End Class ''' ''' This class is the link to allows to create macros and automate Robodk. ''' Any interaction is made through \"items\" (Item() objects). An item is an object in the ''' robodk tree (it can be either a robot, an object, a tool, a frame, a program, ...). ''' Public Class RoboDK ''' ''' Class used for RoboDK exceptions ''' Public Class RDKException Inherits Exception Public Sub New(Message As String) MyBase.New(Message) End Sub End Class ' Tree item types Public Const ITEM_TYPE_ANY As Integer = -1 Public Const ITEM_TYPE_STATION As Integer = 1 Public Const ITEM_TYPE_ROBOT As Integer = 2 Public Const ITEM_TYPE_FRAME As Integer = 3 Public Const ITEM_TYPE_TOOL As Integer = 4 Public Const ITEM_TYPE_OBJECT As Integer = 5 Public Const ITEM_TYPE_TARGET As Integer = 6 Public Const ITEM_TYPE_PROGRAM As Integer = 8 Public Const ITEM_TYPE_INSTRUCTION As Integer = 9 Public Const ITEM_TYPE_PROGRAM_PYTHON As Integer = 10 Public Const ITEM_TYPE_MACHINING As Integer = 11 Public Const ITEM_TYPE_BALLBARVALIDATION As Integer = 12 Public Const ITEM_TYPE_CALIBPROJECT As Integer = 13 Public Const ITEM_TYPE_VALID_ISO9283 As Integer = 14 ' Instruction types Public Const INS_TYPE_INVALID As Integer = -1 Public Const INS_TYPE_MOVE As Integer = 0 Public Const INS_TYPE_MOVEC As Integer = 1 Public Const INS_TYPE_CHANGESPEED As Integer = 2 Public Const INS_TYPE_CHANGEFRAME As Integer = 3 Public Const INS_TYPE_CHANGETOOL As Integer = 4 Public Const INS_TYPE_CHANGEROBOT As Integer = 5 Public Const INS_TYPE_PAUSE As Integer = 6 Public Const INS_TYPE_EVENT As Integer = 7 Public Const INS_TYPE_CODE As Integer = 8 Public Const INS_TYPE_PRINT As Integer = 9 ' Move types Public Const MOVE_TYPE_INVALID As Integer = -1 Public Const MOVE_TYPE_JOINT As Integer = 1 Public Const MOVE_TYPE_LINEAR As Integer = 2 Public Const MOVE_TYPE_CIRCULAR As Integer = 3 ' Station parameters request Public Const PATH_OPENSTATION As String = "PATH_OPENSTATION" Public Const FILE_OPENSTATION As String = "FILE_OPENSTATION" Public Const PATH_DESKTOP As String = "PATH_DESKTOP" ' Script execution types Public Const RUNMODE_SIMULATE As Integer = 1 ' performs the simulation moving the robot (default) Public Const RUNMODE_QUICKVALIDATE As Integer = 2 ' performs a quick check to validate the robot movements Public Const RUNMODE_MAKE_ROBOTPROG As Integer = 3 ' makes the robot program Public Const RUNMODE_MAKE_ROBOTPROG_AND_UPLOAD As Integer = 4 ' makes the robot program and updates it to the robot Public Const RUNMODE_MAKE_ROBOTPROG_AND_START As Integer = 5 ' makes the robot program and starts it on the robot (independently from the PC) Public Const RUNMODE_RUN_ROBOT As Integer = 6 ' moves the real robot from the PC (PC is the client, the robot behaves like a server) ' Program execution type Public Const PROGRAM_RUN_ON_SIMULATOR As Integer = 1 ' Set the program to run on the simulator Public Const PROGRAM_RUN_ON_ROBOT As Integer = 2 ' Set the program to run on the robot ' Robot connection status Public Const ROBOTCOM_PROBLEMS As Integer = -3 Public Const ROBOTCOM_DISCONNECTED As Integer = -2 Public Const ROBOTCOM_NOT_CONNECTED As Integer = -1 Public Const ROBOTCOM_READY As Integer = 0 Public Const ROBOTCOM_WORKING As Integer = 1 Public Const ROBOTCOM_WAITING As Integer = 2 Public Const ROBOTCOM_UNKNOWN As Integer = -1000 ' TCP calibration types Public Const CALIBRATE_TCP_BY_POINT As Integer = 0 Public Const CALIBRATE_TCP_BY_PLANE As Integer = 1 ' Reference frame calibration types Public Const CALIBRATE_FRAME_3P_P1_ON_X As Integer = 0 'Calibrate by 3 points: [X, X+, Y+] (p1 on X axis) Public Const CALIBRATE_FRAME_3P_P1_ORIGIN As Integer = 1 'Calibrate by 3 points: [Origin, X+, XY+] (p1 is origin) Public Const CALIBRATE_FRAME_6P As Integer = 2 'Calibrate by 6 points Public Const CALIBRATE_TURNTABLE As Integer = 3 'Calibrate turntable ' projection types (for AddCurve) Public Const PROJECTION_NONE As Integer = 0 ' No curve projection Public Const PROJECTION_CLOSEST As Integer = 1 ' The projection will the closest point on the surface Public Const PROJECTION_ALONG_NORMAL As Integer = 2 ' The projection will be done along the normal. Public Const PROJECTION_ALONG_NORMAL_RECALC As Integer = 3 ' The projection will be done along the normal. Furthermore, the normal will be recalculated according to the surface normal. ' Euler type Public Const JOINT_FORMAT = -1 ' Using joints (not poses) Public Const EULER_RX_RYp_RZpp As Integer = 0 ' generic Public Const EULER_RZ_RYp_RXpp As Integer = 1 ' ABB RobotStudio Public Const EULER_RZ_RYp_RZpp As Integer = 2 ' Kawasaki, Adept, Staubli Public Const EULER_RZ_RXp_RZpp As Integer = 3 ' CATIA, SolidWorks Public Const EULER_RX_RY_RZ As Integer = 4 ' Fanuc, Kuka, Motoman, Nachi Public Const EULER_RZ_RY_RX As Integer = 5 ' CRS Public Const EULER_QUEATERNION As Integer = 6 ' ABB Rapid ' State of the RoboDK window Public Const WINDOWSTATE_HIDDEN As Integer = -1 Public Const WINDOWSTATE_SHOW As Integer = 0 Public Const WINDOWSTATE_MINIMIZED As Integer = 1 Public Const WINDOWSTATE_NORMAL As Integer = 2 Public Const WINDOWSTATE_MAXIMIZED As Integer = 3 Public Const WINDOWSTATE_FULLSCREEN As Integer = 4 Public Const WINDOWSTATE_CINEMA As Integer = 5 Public Const WINDOWSTATE_FULLSCREEN_CINEMA As Integer = 6 ' Instruction program call type: Public Const INSTRUCTION_CALL_PROGRAM As Integer = 0 Public Const INSTRUCTION_INSERT_CODE As Integer = 1 Public Const INSTRUCTION_START_THREAD As Integer = 2 Public Const INSTRUCTION_COMMENT As Integer = 3 ' Object selection features: Public Const FEATURE_NONE As Integer = 0 Public Const FEATURE_SURFACE As Integer = 1 Public Const FEATURE_CURVE As Integer = 2 Public Const FEATURE_POINT As Integer = 3 ' Spray gun simulation: Public Const SPRAY_OFF As Integer = 0 Public Const SPRAY_ON As Integer = 1 ' Collision checking state Public Const COLLISION_OFF As Integer = 0 Public Const COLLISION_ON As Integer = 1 ' NC program result Public Const NC_POSITIONS_NOT_REACHABLE As Integer = -2 Public Const NC_OK As Integer = 1 Public PROCESS As System.Diagnostics.Process = Nothing ' pointer to the process Private APPLICATION_DIR As String = "" ' file path to the robodk program (executable), typically C:/RoboDK/bin/RoboDK.exe. Leave empty to use the registry key: HKEY_LOCAL_MACHINE\SOFTWARE\RoboDK Private ARGUMENTS As String = "" ' arguments to provide to RoboDK on startup Private SAFE_MODE As Integer = 1 ' checks that provided items exist in memory Private AUTO_UPDATE As Integer = 0 ' if AUTO_UPDATE is zero, the scene is rendered after every function call Private TIMEOUT As Integer = 10 * 1000 ' timeout for communication, in seconds Private COM As Socket ' tcpip com Private IP As String = "localhost" ' IP address of the simulator (localhost if it is the same computer), otherwise, use RL = Robolink('yourip') to set to a different IP Private PORT_START As Integer = 20500 ' port to start looking for app connection Private PORT_END As Integer = 20500 ' port to stop looking for app connection Private START_HIDDEN As Boolean = False ' forces to start hidden. ShowRoboDK must be used to show the window Private PORT As Integer = -1 ' port where connection succeeded Private PORT_FORCED As Integer = -1 ' port to force RoboDK to start listening 'Returns 1 if connection is valid, returns 0 if connection is invalid Private Function _is_connected() As Boolean Return COM.Connected End Function ''' ''' Checks if the object is currently linked to RoboDK ''' ''' Public Function Connected() As Boolean 'return COM.Connected;//does not work well Dim part1 As Boolean = COM.Poll(1000, SelectMode.SelectRead) Dim part2 As Boolean = COM.Available = 0 If part1 AndAlso part2 Then Return False Else Return True End If End Function 'If we are not connected it will attempt a connection, if it fails, it will throw an error Private Sub _check_connection() If Not _is_connected() AndAlso Not Connect() Then Throw New RDKException("Can't connect to RoboDK library") End If End Sub ' checks the status of the connection Private Function _check_status() As Integer Dim status As Integer = _rec_int() If status > 0 AndAlso status < 10 Then Dim strproblems As String strproblems = "Unknown error" If status = 1 Then strproblems = "Invalid item provided: The item identifier provided is not valid or it does not exist." ElseIf status = 2 Then 'output warning strproblems = _rec_line() 'print("WARNING: " + strproblems); '#warn(strproblems)# does not show where is the problem... Return 0 ElseIf status = 3 Then ' output error strproblems = _rec_line() Throw New RDKException(strproblems) ElseIf status = 9 Then strproblems = "Invalid license. Contact us at: info@robodk.com" End If 'print(strproblems); 'raise Exception(strproblems) Throw New RDKException(strproblems) ' everything is OK 'status = status ElseIf status = 0 Then Else 'raise Exception('Problems running function'); Throw New RDKException("Problems running function") End If Return status End Function 'Formats the color in a vector of size 4x1 and ranges [0,1] Private Function _check_color(color As Double()) As Boolean If color.Length < 4 Then Throw New RDKException("Invalid color. A color must be a 4-size double array [r,g,b,a]") 'raise Exception('Problems running function'); Return False End If Return True End Function 'Sends a string of characters with a \\n Private Sub _send_line(line As String) line.Replace(ControlChars.Lf, " "c) ' one new line at the end only! Dim data As Byte() = System.Text.Encoding.UTF8.GetBytes(line & Convert.ToString(vbLf)) COM.Send(data) End Sub Private Function _rec_line() As String 'Receives a string. It reads until if finds LF (\\n) Dim buffer As Byte() = New Byte(0) {} Dim bytesread As Integer = COM.Receive(buffer, 1, SocketFlags.None) Dim line As String = "" While bytesread > 0 AndAlso buffer(0) <> Convert.ToByte(ControlChars.Lf) line = line & System.Text.Encoding.UTF8.GetString(buffer) bytesread = COM.Receive(buffer, 1, SocketFlags.None) End While Return line End Function 'Sends an item pointer Private Sub _send_item(item As Item) Dim bytes As Byte() If item Is Nothing Then Dim zero As UInt64 = 0 bytes = BitConverter.GetBytes(zero) Else bytes = BitConverter.GetBytes(item.ID) End If If bytes.Length <> 8 Then Throw New RDKException("API error") End If Array.Reverse(bytes) COM.Send(bytes) End Sub 'Receives an item pointer Private Function _rec_item() As Item Dim buffer1 As Byte() = New Byte(7) {} Dim buffer2 As Byte() = New Byte(3) {} Dim read1 As Integer = COM.Receive(buffer1, 8, SocketFlags.None) Dim read2 As Integer = COM.Receive(buffer2, 4, SocketFlags.None) If read1 <> 8 OrElse read2 <> 4 Then Return Nothing End If Array.Reverse(buffer1) Array.Reverse(buffer2) Dim item As UInt64 = BitConverter.ToUInt64(buffer1, 0) 'Console.WriteLine("Received item: " + item.ToString()); Dim type As Int32 = BitConverter.ToInt32(buffer2, 0) Return New Item(Me, item, type) End Function Private Sub _send_pose(pose As Mat) If Not pose.IsHomogeneous() Then ' warning!! Return End If Const nvalues As Integer = 16 Dim bytesarray As Byte() = New Byte(8 * nvalues - 1) {} Dim cnt As Integer = 0 For j As Integer = 0 To pose.cols - 1 For i As Integer = 0 To pose.rows - 1 Dim onedouble As Byte() = BitConverter.GetBytes(CDbl(pose(i, j))) Array.Reverse(onedouble) Array.Copy(onedouble, 0, bytesarray, cnt * 8, 8) cnt = cnt + 1 Next Next COM.Send(bytesarray, 8 * nvalues, SocketFlags.None) End Sub Private Function _rec_pose() As Mat Dim pose As New Mat(4, 4) Dim bytes As Byte() = New Byte(16 * 8 - 1) {} Dim nbytes As Integer = COM.Receive(bytes, 16 * 8, SocketFlags.None) If nbytes <> 16 * 8 Then 'raise Exception('Problems running function'); Throw New RDKException("Invalid pose sent") End If Dim cnt As Integer = 0 For j As Integer = 0 To pose.cols - 1 For i As Integer = 0 To pose.rows - 1 Dim onedouble As Byte() = New Byte(7) {} Array.Copy(bytes, cnt, onedouble, 0, 8) Array.Reverse(onedouble) pose(i, j) = BitConverter.ToDouble(onedouble, 0) cnt = cnt + 8 Next Next Return pose End Function Private Sub _send_xyz(xyzpos As Double()) For i As Integer = 0 To 2 Dim bytes As Byte() = BitConverter.GetBytes(CDbl(xyzpos(i))) Array.Reverse(bytes) COM.Send(bytes, 8, SocketFlags.None) Next End Sub Private Sub _rec_xyz(xyzpos As Double()) Dim bytes As Byte() = New Byte(3 * 8 - 1) {} Dim nbytes As Integer = COM.Receive(bytes, 3 * 8, SocketFlags.None) If nbytes <> 3 * 8 Then 'raise Exception('Problems running function'); Throw New RDKException("Invalid pose sent") End If For i As Integer = 0 To 2 Dim onedouble As Byte() = New Byte(7) {} Array.Copy(bytes, i * 8, onedouble, 0, 8) Array.Reverse(onedouble) xyzpos(i) = BitConverter.ToDouble(onedouble, 0) Next End Sub Private Sub _send_int(number As Int32) Dim bytes As Byte() = BitConverter.GetBytes(number) Array.Reverse(bytes) ' convert from big endian to little endian COM.Send(bytes) End Sub Private Function _rec_int() As Int32 Dim bytes As Byte() = New Byte(3) {} Dim read As Integer = COM.Receive(bytes, 4, SocketFlags.None) If read < 4 Then Return 0 End If Array.Reverse(bytes) ' convert from little endian to big endian Return BitConverter.ToInt32(bytes, 0) End Function ' Sends an array of doubles Private Sub _send_array(values As Double()) If values Is Nothing Then _send_int(0) Return End If Dim nvalues As Integer = values.Length _send_int(nvalues) Dim bytesarray As Byte() = New Byte(8 * nvalues - 1) {} For i As Integer = 0 To nvalues - 1 Dim onedouble As Byte() = BitConverter.GetBytes(values(i)) Array.Reverse(onedouble) Array.Copy(onedouble, 0, bytesarray, i * 8, 8) Next COM.Send(bytesarray, 8 * nvalues, SocketFlags.None) End Sub ' Receives an array of doubles Private Function _rec_array() As Double() Dim nvalues As Integer = _rec_int() If nvalues > 0 Then Dim values As Double() = New Double(nvalues - 1) {} Dim bytes As Byte() = New Byte(nvalues * 8 - 1) {} Dim read As Integer = COM.Receive(bytes, nvalues * 8, SocketFlags.None) For i As Integer = 0 To nvalues - 1 Dim onedouble As Byte() = New Byte(7) {} Array.Copy(bytes, i * 8, onedouble, 0, 8) Array.Reverse(onedouble) values(i) = BitConverter.ToDouble(onedouble, 0) Next Return values End If Return Nothing End Function ' sends a 2 dimensional matrix Private Sub _send_matrix(mat As Mat) _send_int(mat.rows) _send_int(mat.cols) For j As Integer = 0 To mat.cols - 1 For i As Integer = 0 To mat.rows - 1 Dim bytes As Byte() = BitConverter.GetBytes(CDbl(mat(i, j))) Array.Reverse(bytes) COM.Send(bytes, 8, SocketFlags.None) Next Next End Sub ' receives a 2 dimensional matrix (nxm) Private Function _rec_matrix() As Mat Dim size1 As Integer = _rec_int() Dim size2 As Integer = _rec_int() Dim recvsize As Integer = size1 * size2 * 8 Dim bytes As Byte() = New Byte(recvsize - 1) {} Dim mat As New Mat(size1, size2) Dim BUFFER_SIZE As Integer = 256 Dim received As Integer = 0 If recvsize > 0 Then Dim to_receive As Integer = Math.Min(recvsize, BUFFER_SIZE) While to_receive > 0 Dim nbytesok As Integer = COM.Receive(bytes, received, to_receive, SocketFlags.None) If nbytesok <= 0 Then 'raise Exception('Problems running function'); Throw New RDKException("Can't receive matrix properly") End If received = received + nbytesok to_receive = Math.Min(recvsize - received, BUFFER_SIZE) End While End If Dim cnt As Integer = 0 For j As Integer = 0 To mat.cols - 1 For i As Integer = 0 To mat.rows - 1 Dim onedouble As Byte() = New Byte(7) {} Array.Copy(bytes, cnt, onedouble, 0, 8) Array.Reverse(onedouble) mat(i, j) = BitConverter.ToDouble(onedouble, 0) cnt = cnt + 8 Next Next Return mat End Function ' private move type, to be used by public methods (MoveJ and MoveL) Private Sub _moveX(target As Item, joints As Double(), mat_target As Mat, itemrobot As Item, movetype As Integer, Optional blocking As Boolean = True) itemrobot.WaitMove() Dim command As String = "MoveX" _send_line(command) _send_int(movetype) If target IsNot Nothing Then _send_int(3) _send_array(Nothing) _send_item(target) ElseIf joints IsNot Nothing Then _send_int(1) _send_array(joints) _send_item(Nothing) ElseIf mat_target IsNot Nothing AndAlso mat_target.IsHomogeneous() Then _send_int(2) _send_array(mat_target.ToDoubles()) _send_item(Nothing) Else 'raise Exception('Problems running function'); Throw New RDKException("Invalid target type") End If _send_item(itemrobot) _check_status() If blocking Then itemrobot.WaitMove() End If End Sub ' private move type, to be used by public methods (MoveJ and MoveL) Private Sub _moveC_private(target1 As Item, joints1 As Double(), mat_target1 As Mat, target2 As Item, joints2 As Double(), mat_target2 As Mat, _ itemrobot As Item, Optional blocking As Boolean = True) itemrobot.WaitMove() Dim command As String = "MoveC" _send_line(command) _send_int(3) If target1 IsNot Nothing Then _send_int(3) _send_array(Nothing) _send_item(target1) ElseIf joints1 IsNot Nothing Then _send_int(1) _send_array(joints1) _send_item(Nothing) ElseIf mat_target1 IsNot Nothing AndAlso mat_target1.IsHomogeneous() Then _send_int(2) _send_array(mat_target1.ToDoubles()) _send_item(Nothing) Else Throw New RDKException("Invalid type of target 1") End If '////////////////////////////////// If target2 IsNot Nothing Then _send_int(3) _send_array(Nothing) _send_item(target2) ElseIf joints2 IsNot Nothing Then _send_int(1) _send_array(joints2) _send_item(Nothing) ElseIf mat_target2 IsNot Nothing AndAlso mat_target2.IsHomogeneous() Then _send_int(2) _send_array(mat_target2.ToDoubles()) _send_item(Nothing) Else Throw New RDKException("Invalid type of target 2") End If '////////////////////////////////// _send_item(itemrobot) _check_status() If blocking Then itemrobot.WaitMove() End If End Sub '%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% ''' ''' Creates a link with RoboDK ''' ''' ''' ''' Public Sub New(Optional robodk_ip As String = "localhost", Optional start_hidden__1 As Boolean = False, Optional com_port As Integer = -1, Optional args As String = "", Optional path As String = "") 'A connection is attempted upon creation of the object""" If robodk_ip <> "" Then IP = robodk_ip End If START_HIDDEN = start_hidden__1 If com_port > 0 Then PORT_FORCED = com_port PORT_START = com_port PORT_END = com_port End If If path <> "" Then APPLICATION_DIR = path End If If args <> "" Then ARGUMENTS = args End If Connect() End Sub ''' ''' Disconnect from the RoboDK API. This flushes any pending program generation. ''' ''' Public Function Disconnect() As Boolean If COM.Connected Then COM.Disconnect(False) End If Return True End Function ''' ''' Disconnect from the RoboDK API. This flushes any pending program generation. ''' ''' Public Function Finish() As Boolean Return Disconnect() End Function Private Function _Set_connection_params(Optional safe_mode__1 As Integer = 1, Optional auto_update__2 As Integer = 0, Optional timeout__3 As Integer = -1) As Boolean 'Sets some behavior parameters: SAFE_MODE, AUTO_UPDATE and TIMEOUT. SAFE_MODE = safe_mode__1 AUTO_UPDATE = auto_update__2 If timeout__3 >= 0 Then TIMEOUT = timeout__3 End If _send_line("CMD_START") _send_line(Convert.ToString(SAFE_MODE) + " " + Convert.ToString(AUTO_UPDATE)) Dim response As String = _rec_line() If response = "READY" Then Return True End If Return False End Function ''' ''' Starts the link with RoboDK (automatic upon creation of the object) ''' ''' Public Function Connect() As Boolean 'Establishes a connection with robodk. robodk must be running, otherwise, the variable APPLICATION_DIR must be set properly. Dim connected As Boolean = False Dim _port As Integer For i As Integer = 0 To 1 For _port = PORT_START To PORT_END COM = New Socket(AddressFamily.InterNetwork, SocketType.Stream, ProtocolType.IP) 'COM = new Socket(SocketType.Stream, ProtocolType.IPv4); COM.SendTimeout = 1000 COM.ReceiveTimeout = 1000 Try COM.Connect(IP, _port) connected = _is_connected() If connected Then COM.SendTimeout = TIMEOUT COM.ReceiveTimeout = TIMEOUT Exit Try End If Catch e As Exception 'connected = false; End Try Next If connected Then PORT = _port Exit For Else If IP <> "localhost" Then Exit For End If Dim _arguments As String = "" If PORT_FORCED > 0 Then _arguments = (_arguments & Convert.ToString("/PORT=")) + PORT_FORCED.ToString() + " " End If If START_HIDDEN Then _arguments = _arguments & Convert.ToString("/NOSPLASH /NOSHOW /HIDDEN ") End If If ARGUMENTS <> "" Then _arguments = _arguments & ARGUMENTS End If If APPLICATION_DIR = "" Then Dim install_path As String = Nothing ' retrieve install path from the registry: Dim localKey As RegistryKey = RegistryKey.OpenBaseKey(Microsoft.Win32.RegistryHive.LocalMachine, RegistryView.Registry64) localKey = localKey.OpenSubKey("SOFTWARE\RoboDK") If localKey IsNot Nothing Then install_path = localKey.GetValue("INSTDIR").ToString() If install_path IsNot Nothing Then APPLICATION_DIR = install_path & "\bin\RoboDK.exe" End If End If End If If APPLICATION_DIR = "" Then APPLICATION_DIR = "C:/RoboDK/bin/RoboDK.exe" End If PROCESS = System.Diagnostics.Process.Start(APPLICATION_DIR, _arguments) System.Threading.Thread.Sleep(2000) End If Next If connected AndAlso Not _Set_connection_params() Then connected = False PROCESS = Nothing End If Return connected End Function ' %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% ' public methods ''' ''' Returns an item by its name. If there is no exact match it will return the last closest match. ''' ''' Item name ''' Filter by item type RoboDK.ITEM_TYPE_... ''' Public Function getItem(name As String, Optional itemtype As Integer = -1) As Item _check_connection() Dim command As String If itemtype < 0 Then command = "G_Item" _send_line(command) _send_line(name) Else command = "G_Item2" _send_line(command) _send_line(name) _send_int(itemtype) End If Dim item As Item = _rec_item() _check_status() Return item End Function ''' ''' Returns a list of items (list of name or pointers) of all available items in the currently open station in robodk. ''' Optionally, use a filter to return specific items (example: getItemListNames(filter = ITEM_CASE_ROBOT)) ''' ''' ITEM_TYPE ''' Public Function getItemListNames(Optional filter As Integer = -1) As String() _check_connection() Dim command As String If filter < 0 Then command = "G_List_Items" _send_line(command) Else command = "G_List_Items_Type" _send_line(command) _send_int(filter) End If Dim numitems As Integer = _rec_int() Dim listnames As String() = New String(numitems - 1) {} For i As Integer = 0 To numitems - 1 listnames(i) = _rec_line() Next _check_status() Return listnames End Function ''' ''' Returns a list of items (list of name or pointers) of all available items in the currently open station in robodk. ''' Optionally, use a filter to return specific items (example: getItemListNames(filter = ITEM_CASE_ROBOT)) ''' ''' ITEM_TYPE ''' Public Function getItemList(Optional filter As Integer = -1) As Item() _check_connection() Dim command As String If filter < 0 Then command = "G_List_Items_ptr" _send_line(command) Else command = "G_List_Items_Type_ptr" _send_line(command) _send_int(filter) End If Dim numitems As Integer = _rec_int() Dim listitems As Item() = New Item(numitems - 1) {} For i As Integer = 0 To numitems - 1 listitems(i) = _rec_item() Next _check_status() Return listitems End Function '//// add more methods ''' ''' Shows a RoboDK popup to select one object from the open station. ''' An item type can be specified to filter desired items. If no type is specified, all items are selectable. ''' ''' Message to pop up ''' optionally filter by RoboDK.ITEM_TYPE_* ''' Public Function ItemUserPick(Optional message As String = "Pick one item", Optional itemtype As Integer = -1) As Item _check_connection() Dim command As String = "PickItem" _send_line(command) _send_line(message) _send_int(itemtype) COM.ReceiveTimeout = 3600 * 1000 Dim item As Item = _rec_item() COM.ReceiveTimeout = TIMEOUT _check_status() Return item End Function ''' ''' Shows or raises the RoboDK window ''' Public Sub ShowRoboDK() _check_connection() Dim command As String = "RAISE" _send_line(command) _check_status() End Sub ''' ''' Hides the RoboDK window ''' Public Sub HideRoboDK() _check_connection() Dim command As String = "HIDE" _send_line(command) _check_status() End Sub ''' ''' Closes RoboDK window and finishes RoboDK execution ''' Public Sub CloseRoboDK() _check_connection() Dim command As String = "QUIT" _send_line(command) _check_status() COM.Disconnect(False) PROCESS = Nothing End Sub ''' ''' Set the state of the RoboDK window ''' ''' Public Sub setWindowState(Optional windowstate As Integer = WINDOWSTATE_NORMAL) _check_connection() Dim command As String = "S_WindowState" _send_line(command) _send_int(windowstate) _check_status() End Sub Public Sub ShowMessage(message As String) _check_connection() Dim command As String = "ShowMessage" _send_line(command) _send_line(message) COM.ReceiveTimeout = 3600 * 1000 _check_status() COM.ReceiveTimeout = TIMEOUT End Sub '''//////////// Add More methods Public Sub Copy(item As Item) 'Makes a copy of an item (same as Ctrl+C), which can be pasted (Ctrl+V) using Paste_Item(). 'In 1 : item 'Example: ' RL = Robolink() ' object = RL.Item('My Object'); ' object.Copy() #RL.Copy(object); also works ' newobject = RL.Paste(); ' newobject.setName('My Object (copy 1)'); ' newobject = RL.Paste(); ' newobject.setName('My Object (copy 2)');""" _check_connection() Dim command As String = "Copy" _send_line(command) _send_item(item) _check_status() End Sub Public Function Paste(Optional toparent As Item = Nothing) As Item 'Pastes the copied item (same as Ctrl+V). Needs to be used after Copy_Item(). See Copy_Item() for an example. 'In 1 (optional): item -> parent to paste to""" _check_connection() Dim command As String = "Paste" _send_line(command) _send_item(toparent) Dim newitem As Item = _rec_item() _check_status() Return newitem End Function ''' ''' Loads a file and attaches it to parent. It can be any file supported by robodk. ''' ''' absolute path of the file ''' parent to attach. Leave empty for new stations or to load an object at the station root ''' Newly added object. Check with item.Valid() for a successful load Public Function AddFile(filename As String, Optional parent As Item = Nothing) As Item 'Example: 'RL = Robolink() 'item = Add_File(r'C:\\Users\\Name\\Desktop\\object.step') 'RL.Set_Pose(item, transl(100,50,500))""" _check_connection() Dim command As String = "Add" _send_line(command) _send_line(filename) _send_item(parent) Dim newitem As Item = _rec_item() _check_status() Return newitem End Function '''//////////// Add More methods ''' ''' Adds a shape provided triangle coordinates. Triangles must be provided as a list of vertices. A vertex normal can be provided optionally. ''' ''' matrix 3xN or 6xN -> N must be multiple of 3 because vertices must be stacked by groups of 3. Each group is a triangle. ''' If True, the curve will be added as part of the object in the RoboDK item tree (a reference object must be provided) ''' added object/shape (0 if failed) Public Function AddShape(triangle_points As Mat, Optional add_to As Item = Nothing) As Item ' if isinstance(triangle_points,list): ' triangle_points = tr(Mat(triangle_points)) ' elif not isinstance(triangle_points, Mat): ' raise Exception("triangle_points must be a 3xN or 6xN list or matrix") _check_connection() Dim command As String = "AddShape" _send_line(command) _send_matrix(triangle_points) _send_item(add_to) Dim newitem As Item = _rec_item() _check_status() Return newitem End Function ''' ''' Adds a curve provided point coordinates. The provided points must be a list of vertices. A vertex normal can be provided optionally. ''' ''' matrix 3xN or 6xN -> N must be multiple of 3 ''' object to add the curve and/or project the curve to the surface ''' If True, the curve will be added as part of the object in the RoboDK item tree (a reference object must be provided) ''' Type of projection. For example: PROJECTION_ALONG_NORMAL_RECALC will project along the point normal and recalculate the normal vector on the surface projected. ''' added object/curve (null if failed) Public Function AddCurve(curve_points As Mat, Optional reference_object As Item = Nothing, Optional add_to_ref As Boolean = False, Optional projection_type As Integer = PROJECTION_ALONG_NORMAL_RECALC) As Item _check_connection() Dim command As String = "AddWire" _send_line(command) _send_matrix(curve_points) _send_item(reference_object) _send_int(If(add_to_ref, 1, 0)) _send_int(projection_type) Dim newitem As Item = _rec_item() _check_status() Return newitem End Function ''' ''' Adds a list of points to an object. The provided points must be a list of vertices. A vertex normal can be provided optionally. ''' ''' matrix 3xN or 6xN -> N must be multiple of 3 ''' object to add the curve and/or project the curve to the surface ''' If True, the points will be added as part of the object in the RoboDK item tree (a reference object must be provided) ''' Type of projection. For example: PROJECTION_ALONG_NORMAL_RECALC will project along the point normal and recalculate the normal vector on the surface projected. ''' added object/curve (0 if failed) Public Function AddPoints(points As Mat, Optional reference_object As Item = Nothing, Optional add_to_ref As Boolean = False, Optional projection_type As Integer = PROJECTION_ALONG_NORMAL_RECALC) As Item ' if isinstance(points,list): ' points = Mat(points).tr() ' elif not isinstance(points, Mat): ' raise Exception("points must be a 3xN or 6xN list or matrix") _check_connection() Dim command As String = "AddPoints" _send_line(command) _send_matrix(points) _send_item(reference_object) If add_to_ref Then _send_int(1) Else _send_int(0) End If _send_int(projection_type) Dim newitem As Item = _rec_item() _check_status() Return newitem End Function ''' ''' Projects a point given its coordinates. The provided points must be a list of [XYZ] coordinates. Optionally, a vertex normal can be provided [XYZijk]. ''' ''' matrix 3xN or 6xN -> list of points to project ''' object to project ''' Type of projection. For example: PROJECTION_ALONG_NORMAL_RECALC will project along the point normal and recalculate the normal vector on the surface projected. ''' Public Function ProjectPoints(points As Mat, object_project As Item, Optional projection_type As Integer = PROJECTION_ALONG_NORMAL_RECALC) As Mat _check_connection() Dim command As String = "ProjectPoints" _send_line(command) _send_matrix(points) _send_item(object_project) _send_int(projection_type) Dim projected_points As Mat = _rec_matrix() _check_status() Return projected_points End Function ''' ''' Save an item to a file. If no item is provided, the open station is saved. ''' ''' absolute path to save the file ''' object or station to save. Leave empty to automatically save the current station. Public Sub Save(filename As String, Optional itemsave As Item = Nothing) _check_connection() Dim command As String = "Save" _send_line(command) _send_line(filename) _send_item(itemsave) _check_status() End Sub ''' ''' Adds a new empty station. ''' ''' name of the station ''' the new station created Public Function AddStation(Optional name As String = "New Station") As Item _check_connection() Dim command As String = "NewStation" _send_line(command) _send_line(name) Dim newitem As Item = _rec_item() _check_status() Return newitem End Function ''' ''' Closes the current station without suggesting to save ''' Public Sub CloseStation() _check_connection() Dim command As String = "Remove" _send_line(command) _send_item(New Item(Me)) _check_status() End Sub ''' ''' Adds a new target that can be reached with a robot. ''' ''' name of the target ''' parent to attach to (such as a frame) ''' main robot that will be used to go to self target ''' the new target created Public Function AddTarget(name As String, Optional itemparent As Item = Nothing, Optional itemrobot As Item = Nothing) As Item _check_connection() Dim command As String = "Add_TARGET" _send_line(command) _send_line(name) _send_item(itemparent) _send_item(itemrobot) Dim newitem As Item = _rec_item() _check_status() Return newitem End Function ''' ''' Adds a new Frame that can be referenced by a robot. ''' ''' name of the reference frame ''' parent to attach to (such as the robot base frame) ''' the new reference frame created Public Function AddFrame(name As String, Optional itemparent As Item = Nothing) As Item _check_connection() Dim command As String = "Add_FRAME" _send_line(command) _send_line(name) _send_item(itemparent) Dim newitem As Item = _rec_item() _check_status() Return newitem End Function ''' ''' Adds a new Program. ''' ''' name of the program ''' robot that will be used ''' the new program created Public Function AddProgram(name As String, Optional itemrobot As Item = Nothing) As Item _check_connection() Dim command As String = "Add_PROG" _send_line(command) _send_line(name) _send_item(itemrobot) Dim newitem As Item = _rec_item() _check_status() Return newitem End Function ''' ''' Adds a new machining project. Machining projects can also be used for 3D printing, curve following and point following. ''' ''' name of the machining project ''' robot that will be used ''' the new program created Public Function AddMillingProject(name As String, Optional itemrobot As Item = Nothing) As Item _check_connection() Dim command As String = "Add_MACHINING" _send_line(command) _send_line(name) _send_item(itemrobot) Dim newitem As Item = _rec_item() _check_status() Return newitem End Function '%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% ''' ''' Adds a function call in the program output. RoboDK will handle the syntax when the code is generated for a specific robot. If the program exists it will also run the program in simulate mode. ''' ''' Function name with parameters (if any) ''' Public Function RunProgram(function_w_params As String) As Integer Return RunCode(function_w_params, True) End Function ''' ''' Adds code to run in the program output. If the program exists it will also run the program in simulate mode. ''' ''' ''' ''' Public Function RunCode(code As String, Optional code_is_fcn_call As Boolean = False) As Integer _check_connection() Dim command As String = "RunCode" _send_line(command) _send_int(If(code_is_fcn_call, 1, 0)) _send_line(code) Dim prog_status As Integer = _rec_int() _check_status() Return prog_status End Function ''' ''' Shows a message or a comment in the output robot program. ''' ''' ''' Public Sub RunMessage(message As String, Optional message_is_comment As Boolean = False) _check_connection() Dim command As String = "RunMessage" _send_line(command) _send_int(If(message_is_comment, 1, 0)) _send_line(message) _check_status() End Sub ''' ''' Renders the scene. This function turns off rendering unless always_render is set to true. ''' ''' Public Sub Render(Optional always_render As Boolean = False) Dim auto_render As Boolean = Not always_render _check_connection() Dim command As String = "Render" _send_line(command) _send_int(If(auto_render, 1, 0)) _check_status() End Sub ''' ''' Returns (1/True) if object_inside is inside the object_parent ''' ''' ''' ''' Public Function IsInside(object_inside As Item, object_parent As Item) As Boolean _check_connection() Dim command As String = "IsInside" _send_line(command) _send_item(object_inside) _send_item(object_parent) Dim inside As Integer = _rec_int() _check_status() Return inside > 0 End Function ''' ''' Set collision checking ON or OFF (COLLISION_OFF/COLLISION_OFF) according to the collision map. If collision check is activated it returns the number of pairs of objects that are currently in a collision state. ''' ''' ''' Number of pairs of objects in a collision state Public Function setCollisionActive(Optional check_state As Integer = COLLISION_ON) As Integer _check_connection() _send_line("Collision_SetState") _send_int(check_state) Dim ncollisions As Integer = _rec_int() _check_status() Return ncollisions End Function ''' ''' Set collision checking ON or OFF (COLLISION_ON/COLLISION_OFF) for a specific pair of objects. This allows altering the collision map for Collision checking. ''' Specify the link id for robots or moving mechanisms (id 0 is the base). ''' ''' Set to COLLISION_ON or COLLISION_OFF ''' Item 1 ''' Item 2 ''' Joint id for Item 1 (if Item 1 is a robot or a mechanism) ''' Joint id for Item 2 (if Item 2 is a robot or a mechanism) ''' Returns true if succeeded. Returns false if setting the pair failed (wrong id was provided) Public Function setCollisionActivePair(check_state As Integer, item1 As Item, item2 As Item, Optional id1 As Integer = 0, Optional id2 As Integer = 0) As Boolean _check_connection() Dim command As String = "Collision_SetPair" _send_line(command) _send_item(item1) _send_item(item2) _send_int(id1) _send_int(id2) _send_int(check_state) Dim success As Integer = _rec_int() _check_status() Return success > 0 End Function ''' ''' Returns the number of pairs of objects that are currently in a collision state. ''' ''' Public Function Collisions() As Integer _check_connection() Dim command As String = "Collisions" _send_line(command) Dim ncollisions As Integer = _rec_int() _check_status() Return ncollisions End Function ''' ''' Returns 1 if item1 and item2 collided. Otherwise returns 0. ''' ''' ''' ''' ncollisions Public Function Collision(item1 As Item, item2 As Item) As Boolean _check_connection() Dim command As String = "Collided" _send_line(command) _send_item(item1) _send_item(item2) Dim ncollisions As Integer = _rec_int() _check_status() Return ncollisions > 0 End Function ''' ''' Sets the current simulation speed. Set the speed to 1 for a real-time simulation. The slowest speed allowed is 0.001 times the real speed. Set to a high value (>100) for fast simulation results. ''' ''' Public Sub setSimulationSpeed(speed As Double) _check_connection() Dim command As String = "SimulateSpeed" _send_line(command) _send_int(CInt(speed * 1000.0)) _check_status() End Sub ''' ''' Gets the current simulation speed. Set the speed to 1 for a real-time simulation. ''' ''' Public Function SimulationSpeed() As Double _check_connection() Dim command As String = "GetSimulateSpeed" _send_line(command) Dim speed As Double = CDbl(_rec_int()) / 1000.0 _check_status() Return speed End Function ''' ''' Sets the behavior of the RoboDK API. By default, robodk shows the path simulation for movement instructions (run_mode=1=RUNMODE_SIMULATE). ''' Setting the run_mode to RUNMODE_QUICKVALIDATE allows performing a quick check to see if the path is feasible. ''' if robot.Connect() is used, RUNMODE_RUN_FROM_PC is selected automatically. ''' ''' int = RUNMODE ''' RUNMODE_SIMULATE=1 performs the simulation moving the robot (default) ''' RUNMODE_QUICKVALIDATE=2 performs a quick check to validate the robot movements ''' RUNMODE_MAKE_ROBOTPROG=3 makes the robot program ''' RUNMODE_RUN_REAL=4 moves the real robot is it is connected Public Sub setRunMode(Optional run_mode As Integer = 1) _check_connection() Dim command As String = "S_RunMode" _send_line(command) _send_int(run_mode) _check_status() End Sub ''' ''' Returns the behavior of the RoboDK API. By default, robodk shows the path simulation for movement instructions (run_mode=1) ''' ''' int = RUNMODE ''' RUNMODE_SIMULATE=1 performs the simulation moving the robot (default) ''' RUNMODE_QUICKVALIDATE=2 performs a quick check to validate the robot movements ''' RUNMODE_MAKE_ROBOTPROG=3 makes the robot program ''' RUNMODE_RUN_REAL=4 moves the real robot is it is connected Public Function RunMode() As Integer _check_connection() Dim command As String = "G_RunMode" _send_line(command) Dim runmode__1 As Integer = _rec_int() _check_status() Return runmode__1 End Function ''' ''' Gets all the user parameters from the open RoboDK station. ''' The parameters can also be modified by right clicking the station and selecting "shared parameters" ''' User parameters can be added or modified by the user ''' ''' list of pairs of strings as parameter-value (list of a list) Public Function getParams() As List(Of List(Of String)) _check_connection() Dim command As String = "G_Params" _send_line(command) Dim paramlist As New List(Of List(Of String))() Dim nparam As Integer = _rec_int() For i As Integer = 0 To nparam - 1 Dim param As String = _rec_line() Dim value As String = _rec_line() Dim param_value As New List(Of String)() param_value.Add(param) param_value.Add(value) paramlist.Add(param_value) Next _check_status() Return paramlist End Function ''' ''' Gets a global or a user parameter from the open RoboDK station. ''' The parameters can also be modified by right clicking the station and selecting "shared parameters" ''' Some available parameters: ''' PATH_OPENSTATION = folder path of the current .stn file ''' FILE_OPENSTATION = file path of the current .stn file ''' PATH_DESKTOP = folder path of the user's folder ''' Other parameters can be added or modified by the user ''' ''' RoboDK parameter ''' value Public Function getParam(param As String) As String _check_connection() Dim command As String = "G_Param" _send_line(command) _send_line(param) Dim value As String = _rec_line() If value.StartsWith("UNKNOWN ") Then value = Nothing End If _check_status() Return value End Function ''' ''' Sets a global parameter from the RoboDK station. If the parameters exists, it will be modified. If not, it will be added to the station. ''' The parameters can also be modified by right clicking the station and selecting "shared parameters" ''' ''' RoboDK parameter ''' value Public Sub setParam(param As String, value As String) _check_connection() Dim command As String = "S_Param" _send_line(command) _send_line(param) _send_line(value) _check_status() End Sub ''' ''' Displays a sequence of joints ''' ''' joint sequence as a 6xN matrix or instruction sequence as a 7xN matrix Public Sub ShowSequence(matrix As Mat) ' """Displays a sequence of joints ' In 1 : joint sequence as a 6xN matrix or instruction sequence as a 7xN matrix""" getItem(0).ShowSequence(matrix) End Sub 'def LaserTracker_Measure(self, estimate=[0,0,0], search=False): ' """Takes a laser tracker measurement with respect to the reference frame. If an estimate point is provided, the laser tracker will first move to those coordinates. If search is True, the tracker will search for a target. ' Returns the XYZ coordinates of target if it was found. Othewise it retuns None.""" ' self._check_connection() ' command = 'MeasLT' ' self._send_line(command) ' self._send_xyz(estimate) ' self._send_int(1 if search else 0) ' xyz = self._rec_xyz() ' self._check_status() ' if xyz*xyz + xyz*xyz + xyz*xyz < 0.0001: ' return None ' return xyz 'def StereoCamera_Measure(self): ' """Takes a measurement with the C-Track stereocamera. Returns two poses, the base reference frame and the measured object reference frame. Status is 0 if measurement succeeded.""" ' self._check_connection() ' command = 'MeasPose' ' self._send_line(command) ' pose1 = self._rec_pose() ' pose2 = self._rec_pose() ' npoints1 = self._rec_int() ' npoints2 = self._rec_int() ' time = self._rec_int() ' status = self._rec_int() ' self._check_status() ' return pose1, pose2, npoints1, npoints2, time, status 'def Collision_Line(self, p1, p2, ref=eye(4)): ' """Checks the collision between a line and the station. The line is composed by 2 points. ' In 1 : p1 -> start point of the line ' In 2 : p2 -> end point of the line ' In 3 : pose (optional) -> reference of the 2 points ' Out 1 : collision -> True if there is a collision, False otherwise ' Out 2 : item -> Item collided ' Out 3 : point -> collision point (station reference)""" ' p1abs = ref*p1; ' p2abs = ref*p2; ' self._check_connection() ' command = 'CollisionLine' ' self._send_line(command) ' self._send_xyz(p1abs) ' self._send_xyz(p2abs) ' itempicked = self._rec_item() ' xyz = self._rec_xyz() ' collision = itempicked.Valid() ' self._check_status() ' return collision, itempicked, xyz 'def setPoses(self, items, poses): ' """Sets the relative positions (poses) of a list of items with respect to their parent. For example, the position of an object/frame/target with respect to its parent. ' Use this function instead of setPose() for faster rendering. ' In 1 : List of items ' In 2 : List of poses""" ' if len(items) != len(poses): ' raise Exception('The number of items must match the number of poses') ' if len(items) == 0: ' return ' self._check_connection() ' command = 'S_Hlocals' ' self._send_line(command) ' self._send_int(len(items)) ' for i in range(len(items)): ' self._send_item(items[i]) ' self._send_pose(poses[i]) ' self._check_status() 'def setPosesAbs(self, items, poses): ' """Sets the absolute positions (poses) of a list of items with respect to the station reference. For example, the position of an object/frame/target with respect to its parent. ' Use this function instead of setPose() for faster rendering. ' In 1 : List of items ' In 2 : List of poses""" ' if len(items) != len(poses): ' raise Exception('The number of items must match the number of poses') ' if len(items) == 0: ' return ' self._check_connection() ' command = 'S_Hlocal_AbsS' ' self._send_line(command) ' self._send_int(len(items)) ' for i in range(len(items)): ' self._send_item(items[i]) ' self._send_pose(poses[i]) ' self._check_status() ''' ''' Returns the current joints of a list of robots. ''' ''' list of robot items ''' list of robot joints (double x nDOF) Public Function Joints(robot_item_list As Item()) As Double()() _check_connection() Dim command As String = "G_ThetasList" _send_line(command) Dim nrobs As Integer = robot_item_list.Length _send_int(nrobs) Dim joints_list As Double()() = New Double(nrobs - 1)() {} For i As Integer = 0 To nrobs - 1 _send_item(robot_item_list(i)) joints_list(i) = _rec_array() Next _check_status() Return joints_list End Function ''' ''' Sets the current robot joints for a list of robot items and a list of a set of joints. ''' ''' list of robot items ''' list of robot joints (double x nDOF) Public Sub setJoints(robot_item_list As Item(), joints_list As Double()()) Dim nrobs As Integer = Math.Min(robot_item_list.Length, joints_list.Length) _check_connection() Dim command As String = "S_ThetasList" _send_line(command) _send_int(nrobs) For i As Integer = 0 To nrobs - 1 _send_item(robot_item_list(i)) _send_array(joints_list(i)) Next _check_status() End Sub ''' ''' Calibrate a tool (TCP) given a number of points or calibration joints. Important: If the robot is calibrated, provide joint values to maximize accuracy. ''' ''' matrix of poses in a given format or a list of joints ''' stats[mean, standard deviation, max] - Output error stats summary ''' Euler format. Optionally, use JOINT_FORMAT and provide the robot. ''' type of algorithm (by point, plane, ...) ''' Robot used for calibration (if using joint values) ''' TCP as [x, y, z] - calculated TCP ''' Public Function CalibrateTool(poses_joints As Mat, ByRef error_stats As Double(), Optional format As Integer = EULER_RX_RY_RZ, Optional algorithm As Integer = CALIBRATE_TCP_BY_POINT, Optional robot As Item = Nothing) As Double() _check_connection() Dim command As String = "CalibTCP2" _send_line(command) _send_matrix(poses_joints) _send_int(format) _send_int(algorithm) _send_item(robot) Dim tcp As Double() = _rec_array() error_stats = _rec_array() Dim error_graph As Mat = _rec_matrix() _check_status() Return tcp 'errors = errors[:, 1].tolist() End Function ''' ''' Calibrate a Reference Frame given a list of points or joint values. Important: If the robot is calibrated, provide joint values to maximize accuracy. ''' ''' points as a 3xN matrix or nDOFsxN) - List of points or a list of robot joints ''' type of algorithm(by point, plane, ...) CALIBRATE_FRAME_... ''' use points or joint values. The robot item must be provided if joint values is used. ''' ''' Public Function CalibrateReference(joints As Mat, Optional method As Integer = CALIBRATE_FRAME_3P_P1_ON_X, Optional use_joints As Boolean = False, Optional robot As Item = Nothing) As Mat _check_connection() Dim command As String = "CalibFrame" _send_line(command) _send_matrix(joints) _send_int(If(use_joints, -1, 0)) _send_int(method) _send_item(robot) Dim reference_pose As Mat = _rec_pose() Dim error_stats As Double() = _rec_array() _check_status() 'errors = errors[:, 1].tolist() Return reference_pose End Function ''' ''' Defines the name of the program when the program is generated. It is also possible to specify the name of the post processor as well as the folder to save the program. ''' This method must be called before any program output is generated (before any robot movement or other instruction). ''' ''' name of the program ''' folder to save the program, leave empty to use the default program folder ''' name of the post processor (for a post processor in C:/RoboDK/Posts/Fanuc_post.py it is possible to provide "Fanuc_post.py" or simply "Fanuc_post") ''' Robot to link ''' Public Function ProgramStart(progname As String, Optional defaultfolder As String = "", Optional postprocessor As String = "", Optional robot As Item = Nothing) As Integer _check_connection() Dim command As String = "ProgramStart" _send_line(command) _send_line(progname) _send_line(defaultfolder) _send_line(postprocessor) _send_item(robot) Dim errors As Integer = _rec_int() _check_status() Return errors End Function ''' ''' Set the pose of the wold reference frame with respect to the view (camera/screen) ''' ''' Public Sub setViewPose(pose As Mat) _check_connection() Dim command As String = "S_ViewPose" _send_line(command) _send_pose(pose) _check_status() End Sub ''' ''' Get the pose of the wold reference frame with respect to the view (camera/screen) ''' Public Function ViewPose() As Mat _check_connection() Dim command As String = "G_ViewPose" _send_line(command) Dim pose As Mat = _rec_pose() _check_status() Return pose End Function '#------------------------------------------------------------------ '#----------------------- CAMERA VIEWS ---------------------------- 'def Cam2D_Add(self, item_object, cam_params=""): ' """Adds a 2D camera view. ' In 1 : Parameters of the camera ' Out 1 : camera handle pointer""" ' self._check_connection() ' command = 'Cam2D_Add' ' self._send_line(command) ' self._send_item(item_object) ' self._send_line(cam_params) ' cam_handle = self._rec_ptr() ' self._check_status() ' return cam_handle 'def Cam2D_Snapshot(self, file_save_img, cam_handle=0): ' """Returns the current joints of a list of robots. ' In 1 : Parameters of the camera ' Out 1 : 1 if success, 0 otherwise""" ' self._check_connection() ' command = 'Cam2D_Snapshot' ' self._send_line(command) ' self._send_ptr(int(cam_handle)) ' self._send_line(file_save_img) ' success = self._rec_int() ' self._check_status() ' return success 'def Cam2D_Close(self, cam_handle=0): ' """Closes all camera windows or one specific camera if the camera handle is provided. ' In 1 : Camera handle (optional) ' Out 1 : 1 if success, 0 otherwise""" ' self._check_connection() ' if cam_handle == 0: ' command = 'Cam2D_CloseAll' ' self._send_line(command) ' else: ' command = 'Cam2D_Close' ' self._send_line(command) ' self._send_ptr(cam_handle) ' success = self._rec_int() ' self._check_status() ' return success 'def Cam2D_SetParams(self, params, cam_handle=0): ' """Set the parameters of the camera. ' In 1 : Parameters of the camera ' Out 1 : 1 if success, 0 otherwise""" ' self._check_connection() ' command = 'Cam2D_SetParams' ' self._send_line(command) ' self._send_ptr(int(cam_handle)) ' self._send_line(params) ' success = self._rec_int() ' self._check_status() ' return success '#------------------------------------------------------------------ '#----------------------- SPRAY GUN SIMULATION ---------------------------- 'def Spray_Add(self, item_tool=0, item_object=0, params="", points=None, geometry=None): ' """Adds spray gun""" ' self._check_connection() ' command = 'Gun_Add' ' self._send_line(command) ' self._send_item(item_tool) ' self._send_item(item_object) ' self._send_line(params) ' self._send_matrix(points) ' self._send_matrix(geometry) ' id_spray = self._rec_int() ' self._check_status() ' return id_spray 'def Spray_SetState(self, state=SPRAY_ON, id_spray=-1): ' """Sets the state of a spray gun""" ' self._check_connection() ' command = 'Gun_SetState' ' self._send_line(command) ' self._send_int(id_spray) ' self._send_int(state) ' success = self._rec_int() ' self._check_status() ' return success 'def Spray_GetStats(self, id_spray=-1): ' """Gets statistics about spray guns""" ' self._check_connection() ' command = 'Gun_Stats' ' self._send_line(command) ' self._send_int(id_spray) ' info = self._rec_line() ' info.replace('
','\t') ' print(info) ' data = self._rec_matrix() ' self._check_status() ' return info, data 'def Spray_Clear(self, id_spray=-1): ' """Stops simulating a spray gun""" ' self._check_connection() ' command = 'Gun_Clear' ' self._send_line(command) ' self._send_int(id_spray) ' success = self._rec_int() ' self._check_status() ' return success 'def SignatureRobot(self, robot=0): ' """Gets the signature of the robot after calibration.""" ' self._check_connection() ' command = 'G_AbsAccParam' ' self._send_line(command) ' self._send_item(robot) ' r2b = self._rec_pose() ' # get nominal kinematics ' nominal_base = self._rec_pose() ' nominal_tool = self._rec_pose() ' ndofs = self._rec_int() ' nominal_kin = [] ' for i in range(ndofs): ' nominal_kin.append(self._rec_array()) ' accurate_base = self._rec_pose() ' accurate_tool = self._rec_pose() ' ndofs = self._rec_int() ' accurate_kin = [] ' for i in range(ndofs): ' accurate_kin.append(self._rec_array()) ' coupling = self._rec_array() ' stiffness = self._rec_array() ' self._check_status() ' return r2b, nominal_base, nominal_too ''' ''' The Item class represents an item in RoboDK station. An item can be a robot, a frame, a tool, an object, a target, ... any item visible in the station tree. ''' An item can also be seen as a node where other items can be attached to (child items). ''' Every item has one parent item/node and can have one or more child items/nodes ''' RoboLinkItem is a "friend" class of RoboLink. ''' Public Class Item Public ID As UInt64 = 0 Private link As RoboDK ' pointer to the RoboLink connection Private _type As Integer = -1 Public Sub New(ByRef connection_link As RoboDK, Optional item_ptr As UInt64 = 0, Optional itemtype As Integer = -1) ID = item_ptr link = connection_link _type = itemtype End Sub Public Function ToString2() As String If Valid() Then Return [String].Format("RoboDK item {0} of type {1}", ID, _type) Else Return "RoboDK item (INVALID)" End If End Function ''' ''' Returns an integer that represents the type of the item (robot, object, tool, frame, ...) ''' Compare the returned value to ITEM_CASE_* variables ''' ''' ''' Public Shadows Function Equals(item_other As Item) As Boolean Return Me.ID = item_other.ID End Function ''' ''' Returns the RoboDK link Robolink(). ''' ''' Public Function RDK() As RoboDK Return link End Function ''' ''' Returns the RoboDK link Robolink (old version)(). ''' ''' Public Function RL() As RoboDK Return link End Function '''///// GENERIC ITEM CALLS ''' ''' Returns the type of an item (robot, object, target, reference frame, ...) ''' ''' Public Function Type() As Integer link._check_connection() Dim command As String = "G_Item_Type" link._send_line(command) link._send_item(Me) Dim itemtype As Integer = link._rec_int() link._check_status() Return itemtype End Function '/// add more methods ''' ''' Copy the item to the clipboard (same as Ctrl+C). Use together with Paste() to duplicate items. ''' Public Sub Copy() link.Copy(Me) End Sub ''' ''' Paste the item from the clipboard as a child of this item (same as Ctrl+V) ''' Public Function Paste() As Item Return link.Paste(Me) End Function ''' ''' Adds an object as a child of this object ''' ''' Public Function AddFile(filename As String) As Item Return link.AddFile(filename) End Function ''' ''' Save a station or object to a file ''' ''' Public Sub Save(filename As String) link.Save(filename, Me) End Sub ''' ''' Checks if this item is in a collision state with another item ''' ''' Public Function Collision(item_check As Item) As Boolean Return link.Collision(Me, item_check) End Function ''' ''' Returns True if the object is inside the provided object ''' ''' Public Function IsInside(item_check As Item) As Boolean Return link.IsInside(Me, item_check) End Function ''' ''' Makes a copy of the geometry fromitem adding it at a given position (pose) relative to this item. ''' Public Sub AddGeometry(fromitem As Item, pose As Mat) link._check_connection() Dim command As String = "CopyFaces" link._send_line(command) link._send_item(fromitem) link._send_item(Me) link._send_pose(pose) link._check_status() End Sub ''' ''' Deletes an item and its childs from the station. ''' Public Sub Delete() link._check_connection() Dim command As String = "Remove" link._send_line(command) link._send_item(Me) link._check_status() ID = 0 End Sub ''' ''' Checks if the item is valid. An invalid item will be returned by an unsuccessful function call. ''' ''' true if valid, false if invalid Public Function Valid() As Boolean If ID = 0 Then Return False End If Return True End Function '/// add more methods ''' ''' Moves the item to another location (parent) without changing the current position in the station ''' ''' parent to attach the item Public Sub setParentStatic(parent As Item) link._check_connection() Dim command As String = "S_Parent_Static" link._send_line(command) link._send_item(Me) link._send_item(parent) link._check_status() End Sub ''' ''' Attaches the closest object to the provided tool (see also: Set_Parent_Static). ''' ''' the item that was attached (item.Valid() is False if none found) Public Function AttachClosest() As Item link._check_connection() Dim command As String = "Attach_Closest" link._send_line(command) link._send_item(Me) Dim item_attached As Item = link._rec_item() link._check_status() Return item_attached End Function ''' ''' Detaches the closest object attached to the tool (see also: setParentStatic). ''' ''' ''' the item that was detached (item.Valid() is False if none found) Public Function DetachClosest(Optional parent As Item = Nothing) link._check_connection() Dim command As String = "Detach_Closest" link._send_line(command) link._send_item(Me) link._send_item(parent) Dim item_detached As Item = link._rec_item() link._check_status() Return item_detached End Function ''' ''' Detaches any object attached to a tool (see also: setParentStatic). ''' ''' Public Sub DetachAll(Optional parent As Item = Nothing) link._check_connection() Dim command As String = "Detach_All" link._send_line(command) link._send_item(Me) link._send_item(parent) link._check_status() End Sub ''' ''' Returns the parent item of the item. ''' ''' the parent item of the item. Public Function Parent() As Item link._check_connection() Dim command As String = "G_Parent" link._send_line(command) link._send_item(Me) Dim parentItem As Item = link._rec_item() link._check_status() Return parentItem End Function ''' ''' Returns a list of the item childs that are attached to the provided item. ''' ''' item x n -> list of child items Public Function Childs() As Item() link._check_connection() Dim command As String = "G_Childs" link._send_line(command) link._send_item(Me) Dim nitems As Integer = link._rec_int() Dim itemlist As Item() = New Item(nitems - 1) {} For i As Integer = 0 To nitems - 1 itemlist(i) = link._rec_item() Next link._check_status() Return itemlist End Function ''' ''' Returns 1 if the item is visible, otherwise, returns 0. ''' ''' true if visible, false if not visible Public Function Visible() As Boolean link._check_connection() Dim command As String = "G_Visible" link._send_line(command) link._send_item(Me) Dim visible__1 As Integer = link._rec_int() link._check_status() Return (visible__1 <> 0) End Function ''' ''' Sets the item visiblity status ''' ''' ''' srt the visible reference frame (1) or not visible (0) Public Sub setVisible(visible As Boolean, Optional visible_frame As Integer = -1) If visible_frame < 0 Then visible_frame = If(visible, 1, 0) End If link._check_connection() Dim command As String = "S_Visible" link._send_line(command) link._send_item(Me) link._send_int(If(visible, 1, 0)) link._send_int(visible_frame) link._check_status() End Sub ''' ''' Returns the name of an item. The name of the item is always displayed in the RoboDK station tree ''' ''' name of the item Public Function Name() As String link._check_connection() Dim command As String = "G_Name" link._send_line(command) link._send_item(Me) Dim _name As String = link._rec_line() link._check_status() Return _name End Function ''' ''' Set the name of a RoboDK item. ''' ''' Public Sub setName(name As String) link._check_connection() Dim command As String = "S_Name" link._send_line(command) link._send_item(Me) link._send_line(name) link._check_status() End Sub ' add more methods ''' ''' Sets a any property value to an item. ''' ''' string ''' matrix Public Sub setValue(varname As String, value As Mat) link._check_connection() Dim command As String = "S_Gen_Mat" link._send_line(command) link._send_item(Me) link._send_line(varname) link._send_matrix(value) link._check_status() End Sub ''' ''' Sets a any property value to an item. ''' ''' string ''' string Public Sub setValue(varname As String, value As String) link._check_connection() Dim command As String = "S_Gen_Str" link._send_line(command) link._send_item(Me) link._send_line(varname) link._send_line(value) link._check_status() End Sub ''' ''' Sets the local position (pose) of an object, target or reference frame. For example, the position of an object/frame/target with respect to its parent. ''' If a robot is provided, it will set the pose of the end efector. ''' ''' 4x4 homogeneous matrix Public Sub setPose(pose As Mat) link._check_connection() Dim command As String = "S_Hlocal" link._send_line(command) link._send_item(Me) link._send_pose(pose) link._check_status() End Sub ''' ''' Returns the local position (pose) of an object, target or reference frame. For example, the position of an object/frame/target with respect to its parent. ''' If a robot is provided, it will get the pose of the end efector ''' ''' 4x4 homogeneous matrix (pose) Public Function Pose() As Mat link._check_connection() Dim command As String = "G_Hlocal" link._send_line(command) link._send_item(Me) Dim pose__1 As Mat = link._rec_pose() link._check_status() Return pose__1 End Function ''' ''' Sets the position (pose) the object geometry with respect to its own reference frame. This procedure works for tools and objects. ''' ''' 4x4 homogeneous matrix Public Sub setGeometryPose(pose As Mat) link._check_connection() Dim command As String = "S_Hgeom" link._send_line(command) link._send_item(Me) link._send_pose(pose) link._check_status() End Sub ''' ''' Returns the position (pose) the object geometry with respect to its own reference frame. This procedure works for tools and objects. ''' ''' 4x4 homogeneous matrix (pose) Public Function GeometryPose() As Mat link._check_connection() Dim command As String = "G_Hgeom" link._send_line(command) link._send_item(Me) Dim pose As Mat = link._rec_pose() link._check_status() Return pose End Function ''' ''' Sets the global position (pose) of an item. For example, the position of an object/frame/target with respect to the station origin. ''' ''' 4x4 homogeneous matrix (pose) Public Sub setPoseAbs(pose As Mat) link._check_connection() Dim command As String = "S_Hlocal_Abs" link._send_line(command) link._send_item(Me) link._send_pose(pose) link._check_status() End Sub ''' ''' Returns the global position (pose) of an item. For example, the position of an object/frame/target with respect to the station origin. ''' ''' 4x4 homogeneous matrix (pose) Public Function PoseAbs() As Mat link._check_connection() Dim command As String = "G_Hlocal_Abs" link._send_line(command) link._send_item(Me) Dim pose As Mat = link._rec_pose() link._check_status() Return pose End Function ''' ''' Changes the color of a robot/object/tool. A color must must in the format COLOR=[R,G,B,(A=1)] where all values range from 0 to 1. ''' Alpha (A) defaults to 1 (100% opaque). Set A to 0 to make an object transparent. ''' ''' color to change to ''' filter by this color ''' optional tolerance to use if a color filter is used (defaults to 0.1) Public Sub Recolor(tocolor As Double(), Optional fromcolor As Double() = Nothing, Optional tolerance As Double = 0.1) link._check_connection() If fromcolor Is Nothing Then fromcolor = New Double() {0, 0, 0, 0} tolerance = 2 End If link._check_color(tocolor) link._check_color(fromcolor) Dim command As String = "Recolor" link._send_line(command) link._send_item(Me) Dim combined As Double() = New Double(8) {} combined(0) = tolerance Array.Copy(fromcolor, 0, combined, 1, 4) Array.Copy(tocolor, 0, combined, 5, 4) link._send_array(combined) link._check_status() End Sub ''' ''' Set the color of an object, tool or robot. A color must must in the format COLOR=[R,G,B,(A=1)] where all values range from 0 to 1. ''' Alpha (A) defaults to 1 (100% opaque). Set A to 0 to make an object transparent. ''' ''' color to change to Public Sub setColor(tocolor As Double()) link._check_connection() If link._check_color(tocolor) Then Dim command As String = "S_Color" link._send_line(command) link._send_item(Me) link._send_array(tocolor) link._check_status() End If End Sub ''' ''' Returns the color of an object, tool or robot (first color found). A color is in the format COLOR=[R,G,B,(A=1)] where all values range from 0 to 1. ''' ''' the color as a list of Double of an object, tool or robot (first color found). A color is in the format COLOR=[R,G,B,(A=1)] where all values range from 0 to 1. Public Function Color() As List(Of Double) link._check_connection() Dim command As String = "G_Color" link._send_line(command) link._send_item(Me) Dim ColorArray As Double() = link._rec_array() link._check_status() Return ColorArray.ToList() End Function ''' ''' Apply a scale to an object to make it bigger or smaller. ''' The scale can be uniform (if scale is a float value) or per axis (if scale is a vector). ''' ''' scale to apply as [scale_x, scale_y, scale_z] Public Sub Scale(scale__1 As Double()) link._check_connection() If scale__1.Length <> 3 Then Throw New RDKException("scale must be a single value or a 3-vector value") End If Dim command As String = "Scale" link._send_line(command) link._send_item(Me) link._send_array(scale__1) link._check_status() End Sub '#"""Object specific calls""" ''' ''' Adds a shape to the object provided some triangle coordinates. Triangles must be provided as a list of vertices. A vertex normal can be provided optionally ''' ''' matrix 3xN or 6xN -> N must be multiple of 3 because vertices must be stacked by groups of 3. Each group is a triangle. ''' returns the object where the curve was added or null if failed Public Function AddShape(triangle_points As Mat) As Item '"""Adds a shape to the object provided some triangle coordinates. Triangles must be provided as a list of vertices. A vertex normal can be provided optionally. 'In 1 : matrix 3xN or 6xN -> N must be multiple of 3 because vertices must be stacked by groups of 3. Each group is a triangle.""" Return link.AddShape(triangle_points, Me) End Function ''' ''' Adds a curve provided point coordinates. The provided points must be a list of vertices. A vertex normal can be provided optionally. ''' ''' matrix 3xN or 6xN -> N must be multiple of 3 ''' add_to_ref -> If True, the curve will be added as part of the object in the RoboDK item tree ''' Type of projection. For example: PROJECTION_ALONG_NORMAL_RECALC will project along the point normal and recalculate the normal vector on the surface projected. ''' returns the object where the curve was added or null if failed Public Function AddCurve(curve_points As Mat, Optional add_to_ref As Boolean = False, Optional projection_type As Integer = PROJECTION_ALONG_NORMAL_RECALC) As Item Return link.AddCurve(curve_points, Me, add_to_ref, projection_type) End Function ''' ''' Adds a list of points to an object. The provided points must be a list of vertices. A vertex normal can be provided optionally. ''' ''' matrix 3xN or 6xN -> N must be multiple of 3 ''' add_to_ref -> If True, the curve will be added as part of the object in the RoboDK item tree ''' Type of projection. For example: PROJECTION_ALONG_NORMAL_RECALC will project along the point normal and recalculate the normal vector on the surface projected. ''' returns the object where the curve was added or null if failed Public Function AddPoints(points As Mat, Optional add_to_ref As Boolean = False, Optional projection_type As Integer = PROJECTION_ALONG_NORMAL_RECALC) As Item Return link.AddPoints(points, Me, add_to_ref, projection_type) End Function ''' ''' Projects a point to the object given its coordinates. The provided points must be a list of [XYZ] coordinates. Optionally, a vertex normal can be provided [XYZijk]. ''' ''' matrix 3xN or 6xN -> list of points to project ''' projection_type -> Type of projection. For example: PROJECTION_ALONG_NORMAL_RECALC will project along the point normal and recalculate the normal vector on the surface projected. ''' projected points (empty matrix if failed) Public Function ProjectPoints(points As Mat, Optional projection_type As Integer = PROJECTION_ALONG_NORMAL_RECALC) As Mat Return link.ProjectPoints(points, Me, projection_type) End Function ''' ''' Is the item selected. ''' ''' FEATURE_CURVE if the last point selected was a wire, FEATURE_POINT if the last feature selected was a point or FEATURE_SURFACE otherwise. ''' index of the curve or point in the object ''' is_selected as boolean Public Function SelectedFeature(ByRef feature_type As Integer, feature_id As Integer) As Boolean link._check_connection() Dim command As String = "G_ObjSelection" link._send_line(command) link._send_item(Me) Dim is_selected As Integer = link._rec_int() feature_type = link._rec_int() feature_id = link._rec_int() link._check_status() Return is_selected > 0 End Function 'def GetPoints(self, feature_type=FEATURE_SURFACE, feature_id=0): ' """Retrieves the point under the mouse cursor, a curve or the 3D points of an object. The points are provided in [XYZijk] format, where the XYZ is the point coordinate and ijk is the surface normal. ' In 1 : feature_type (int) -> set to FEATURE_SURFACE to retrieve the point under the mouse cursor, FEATURE_CURVE to retrieve the list of points for that wire, or FEATURE_POINT to retrieve the list of points. ' In 2 : feature_id -> used only if FEATURE_CURVE is specified, it allows retrieving the appropriate curve id of an object ' Out 1 : List of points""" ' self.link._check_connection() ' command = 'G_ObjPoint' ' self.link._send_line(command) ' self.link._send_item(self) ' self.link._send_int(feature_type) ' self.link._send_int(feature_id) ' points = self.link._rec_matrix() ' feature_name = self.link._rec_line() ' self.link._check_status() ' return points.tr().rows, feature_name ''' ''' Adds a new machining project. Machining projects can also be used for 3D printing, curve following and point following. ''' ''' name of the project ''' (optional): item -> curve or point object to automatically set up a curve/point follow project ''' ''' newprog created Public Function setMillingParameters(ncfile As String, ByRef status As Integer, Optional part As Item = Nothing, Optional params As String = "") As Item link._check_connection() Dim command As String = "S_MachiningParams" link._send_line(command) link._send_item(Me) link._send_line(ncfile) link._send_item(part) link._send_line(params) link.COM.ReceiveTimeout = 3600 * 1000 Dim newprog As Item = link._rec_item() link.COM.ReceiveTimeout = link.TIMEOUT status = link._rec_int() / 1000.0 link._check_status() Return newprog End Function '"""Target item calls""" ''' ''' Sets a target as a cartesian target. A cartesian target moves to cartesian coordinates. ''' Public Sub setAsCartesianTarget() link._check_connection() Dim command As String = "S_Target_As_RT" link._send_line(command) link._send_item(Me) link._check_status() End Sub ''' ''' Sets a target as a joint target. A joint target moves to a joints position without regarding the cartesian coordinates. ''' Public Sub setAsJointTarget() link._check_connection() Dim command As String = "S_Target_As_JT" link._send_line(command) link._send_item(Me) link._check_status() End Sub '#####Robot item calls#### ''' ''' Returns the current joints of a robot or the joints of a target. If the item is a cartesian target, it returns the preferred joints (configuration) to go to that cartesian position. ''' ''' double x n -> joints matrix Public Function Joints() As Double() link._check_connection() Dim command As String = "G_Thetas" link._send_line(command) link._send_item(Me) Dim joints__1 As Double() = link._rec_array() link._check_status() Return joints__1 End Function ' add more methods ' def JointPoses(self, joints = None): '"""Returns the positions of the joint links for a provided robot configuration (joints). If no joints are provided it will return the poses for the current robot position. 'Out 1 : 4x4 x n -> array of 4x4 homogeneous matrices. Index 0 is the base frame reference (it never moves when the joints move). '""" 'self.link._check_connection() 'command = 'G_LinkPoses' 'self.link._send_line(command) 'self.link._send_item(self) 'if joints is None: ' self.link._send_array([]) 'else: ' self.link._send_array(joints) 'nlinks = self.link._rec_int() 'poses = [] 'for i in range(nlinks): ' poses.append(self.link._rec_pose()) 'self.link._check_status() 'return poses ''' ''' Returns the home joints of a robot. These joints can be manually set in the robot "Parameters" menu, then select "Set home position" ''' ''' double x n -> joints array Public Function JointsHome() As Double() link._check_connection() Dim command As String = "G_Home" link._send_line(command) link._send_item(Me) Dim joints As Double() = link._rec_array() link._check_status() Return joints End Function ''' ''' Returns an item pointer to the link id (0 for the robot base, 1 for the first link, ...). ''' This is useful if we want to show/hide certain links or change their geometry. ''' ''' item object (pointer) to the robot link Public Function ObjectLink(Optional link_id As Integer = 0) As Item link._check_connection() Dim command As String = "G_LinkObjId" link._send_line(command) link._send_item(Me) link._send_int(link_id) Dim linkedItem = link._rec_item() link._check_status() Return linkedItem End Function ''' ''' Sets the current joints of a robot or the joints of a target. It the item is a cartesian target, it returns the preferred joints (configuration) to go to that cartesian position. ''' ''' Public Sub setJoints(joints As Double()) link._check_connection() Dim command As String = "S_Thetas" link._send_line(command) link._send_array(joints) link._send_item(Me) link._check_status() End Sub ''' ''' Returns the joint limits of a robot ''' ''' ''' Public Sub JointLimits(lower_limits As Double(), upper_limits As Double()) link._check_connection() Dim command As String = "G_RobLimits" link._send_line(command) link._send_item(Me) lower_limits = link._rec_array() upper_limits = link._rec_array() Dim joints_type As Double = link._rec_int() / 1000.0 link._check_status() End Sub ''' ''' Set the robot joint limits ''' ''' ''' Public Sub setJointLimits(lower_limit As Double(), upper_limit As Double()) link._check_connection() Dim command As String = "S_RobLimits" link._send_line(command) link._send_item(Me) link._send_array(lower_limit) link._send_array(upper_limit) link._check_status() End Sub ''' ''' Sets the robot of a program or a target. You must set the robot linked to a program or a target every time you copy paste these objects. ''' If the robot is not provided, the first available robot will be chosen automatically. ''' ''' Robot item Public Sub setRobot(Optional robot As Item = Nothing) link._check_connection() Dim command As String = "S_Robot" link._send_line(command) link._send_item(Me) link._send_item(robot) link._check_status() End Sub ''' ''' Obsolete: Use setPoseFrame instead. ''' Sets the frame of a robot (user frame). The frame can be either an item or a 4x4 Matrix. ''' If "frame" is an item, it links the robot to the frame item. If frame is a 4x4 Matrix, it updates the linked pose of the robot frame. ''' ''' item/pose -> frame item or 4x4 Matrix (pose of the reference frame) Public Sub setFrame(frame As Item) setPoseFrame(frame) End Sub ''' ''' Obsolete: Use setPoseFrame instead. ''' Sets the frame of a robot (user frame). The frame can be either an item or a 4x4 Matrix. ''' If "frame" is an item, it links the robot to the frame item. If frame is a 4x4 Matrix, it updates the linked pose of the robot frame. ''' ''' item/pose -> frame item or 4x4 Matrix (pose of the reference frame) Public Sub setFrame(frame As Mat) setPoseFrame(frame) End Sub ''' ''' Obsolete: Use setPoseTool instead. ''' Sets the tool pose of a robot. The tool pose can be either an item or a 4x4 Matrix. ''' If "tool" is an item, it links the robot to the tool item. If tool is a 4x4 Matrix, it updates the linked pose of the robot tool. ''' ''' item/pose -> tool item or 4x4 Matrix (pose of the tool frame) Public Sub setTool(tool As Item) setPoseTool(tool) End Sub ''' ''' Obsolete: Use setPoseTool instead. ''' Sets the tool pose of a robot. The tool pose can be either an item or a 4x4 Matrix. ''' If "tool" is an item, it links the robot to the tool item. If tool is a 4x4 Matrix, it updates the linked pose of the robot tool. ''' ''' item/pose -> tool item or 4x4 Matrix (pose of the tool frame) Public Sub setTool(tool As Mat) setPoseTool(tool) End Sub ''' ''' Sets the reference frame of a robot(user frame). The frame can be either an item or a pose. ''' If "frame" is an item, it links the robot to the frame item. If frame is a pose, it updates the linked pose of the robot frame (with respect to the robot reference frame). ''' ''' 4x4 homogeneous matrix (pose) Public Sub setPoseFrame(frame_pose As Mat) link._check_connection() Dim command As String = "S_Frame" link._send_line(command) link._send_pose(frame_pose) link._send_item(Me) link._check_status() End Sub ''' ''' Sets the tool of a robot or a tool object (Tool Center Point, or TCP). The tool pose can be either an item or a 4x4 Matrix. ''' If the item is a tool, it links the robot to the tool item.If tool is a pose, it updates the current robot TCP. ''' ''' 4x4 homogeneous matrix (pose) Public Sub setPoseFrame(frame_item As RoboDK.Item) link._check_connection() Dim command As String = "S_Frame_ptr" link._send_line(command) link._send_item(frame_item) link._send_item(Me) link._check_status() End Sub ''' ''' Sets the tool of a robot or a tool object (Tool Center Point, or TCP). The tool pose can be either an item or a 4x4 Matrix. ''' If the item is a tool, it links the robot to the tool item.If tool is a pose, it updates the current robot TCP. ''' ''' 4x4 homogeneous matrix (pose) Public Sub setPoseTool(tool_pose As Mat) link._check_connection() Dim command As String = "S_Tool" link._send_line(command) link._send_pose(tool_pose) link._send_item(Me) link._check_status() End Sub ''' ''' Sets the tool of a robot or a tool object (Tool Center Point, or TCP). The tool pose can be either an item or a 4x4 Matrix. ''' If the item is a tool, it links the robot to the tool item.If tool is a pose, it updates the current robot TCP. ''' ''' Tool item Public Sub setPoseTool(tool_item As RoboDK.Item) link._check_connection() Dim command As String = "S_Tool_ptr" link._send_line(command) link._send_item(tool_item) link._send_item(Me) link._check_status() End Sub ''' ''' Returns the tool pose of an item. If a robot is provided it will get the tool pose of the active tool held by the robot. ''' ''' 4x4 homogeneous matrix (pose) Public Function PoseTool() As Mat link._check_connection() Dim command As String = "G_Tool" link._send_line(command) link._send_item(Me) Dim pose As Mat = link._rec_pose() link._check_status() Return pose End Function ''' ''' Returns the reference frame pose of an item. If a robot is provided it will get the tool pose of the active reference frame used by the robot. ''' ''' 4x4 homogeneous matrix (pose) Public Function PoseFrame() As Mat link._check_connection() Dim command As String = "G_Frame" link._send_line(command) link._send_item(Me) Dim pose As Mat = link._rec_pose() link._check_status() Return pose End Function ''' ''' Obsolete: Use setPoseTool(pose) instead. Sets the tool pose of a tool item. If a robot is provided it will set the tool pose of the active tool held by the robot. ''' ''' 4x4 homogeneous matrix (pose) Public Sub setHtool(pose As Mat) link._check_connection() Dim command As String = "S_Htool" link._send_line(command) link._send_item(Me) link._send_pose(pose) link._check_status() End Sub ''' ''' Obsolete: Use PoseTool() instead. ''' Returns the tool pose of an item. If a robot is provided it will get the tool pose of the active tool held by the robot. ''' ''' 4x4 homogeneous matrix (pose) Public Function Htool() As Mat link._check_connection() Dim command As String = "G_Htool" link._send_line(command) link._send_item(Me) Dim pose As Mat = link._rec_pose() link._check_status() Return pose End Function ''' ''' Adds an empty tool to the robot provided the tool pose (4x4 Matrix) and the tool name. ''' ''' pose -> TCP as a 4x4 Matrix (pose of the tool frame) ''' New tool name ''' new item created Public Function AddTool(tool_pose As Mat, Optional tool_name As String = "New TCP") As Item link._check_connection() Dim command As String = "AddToolEmpty" link._send_line(command) link._send_item(Me) link._send_pose(tool_pose) link._send_line(tool_name) Dim newtool As Item = link._rec_item() link._check_status() Return newtool End Function ''' ''' Computes the forward kinematics of the robot for the provided joints. The tool and the reference frame are not taken into account. ''' ''' ''' 4x4 homogeneous matrix: pose of the robot flange with respect to the robot base Public Function SolveFK(joints As Double()) As Mat link._check_connection() Dim command As String = "G_FK" link._send_line(command) link._send_array(joints) link._send_item(Me) Dim pose As Mat = link._rec_pose() link._check_status() Return pose End Function ''' ''' Returns the robot configuration state for a set of robot joints. ''' ''' array of joints ''' 3-array -> configuration status as [REAR, LOWERARM, FLIP] Public Function JointsConfig(joints As Double()) As Double() link._check_connection() Dim command As String = "G_Thetas_Config" link._send_line(command) link._send_array(joints) link._send_item(Me) Dim config As Double() = link._rec_array() link._check_status() Return config End Function ''' ''' Computes the inverse kinematics for the specified robot and pose. The joints returned are the closest to the current robot configuration (see SolveIK_All()) ''' ''' 4x4 matrix -> pose of the robot flange with respect to the robot base frame ''' array of joints Public Function SolveIK(pose As Mat) As Double() link._check_connection() Dim command As String = "G_IK" link._send_line(command) link._send_pose(pose) link._send_item(Me) Dim joints As Double() = link._rec_array() link._check_status() Return joints End Function ''' ''' Computes the inverse kinematics for the specified robot and pose. The function returns all available joint solutions as a 2D matrix. ''' ''' 4x4 matrix -> pose of the robot tool with respect to the robot frame ''' double x n x m -> joint list (2D matrix) Public Function SolveIK_All(pose As Mat) As Mat link._check_connection() Dim command As String = "G_IK_cmpl" link._send_line(command) link._send_pose(pose) link._send_item(Me) Dim joints_list As Mat = link._rec_matrix() link._check_status() Return joints_list End Function 'def FilterTarget(self, pose, joints_approx=None): ' """Filters a target to improve accuracy. ' In 1 : 4x4 matrix -> pose of the robot TCP with respect to the robot reference frame ' In 2 : double x n -> approximate joints to define preferred configuration ' Out 1 : double x n x m -> joint list (2D matrix)""" ' self.link._check_connection() ' command = 'FilterTarget' ' self.link._send_line(command) ' self.link._send_pose(pose) ' if joints_approx is None: ' joints_approx = [0,0,0,0,0,0] ' self.link._send_array(joints_approx) ' self.link._send_item(self) ' pose_filtered = self.link._rec_pose() ' joints_filtered = self.link._rec_array() ' self.link._check_status() ' return pose_filtered, joints_filtered ''' ''' Connect to a real robot using the robot driver. ''' ''' IP of the robot to connect. Leave empty to use the one defined in RoboDK ''' status -> true if connected successfully, false if connection failed Public Function Connect(Optional robot_ip As String = "") As Boolean link._check_connection() Dim command As String = "Connect" link._send_line(command) link._send_item(Me) link._send_line(robot_ip) Dim status As Integer = link._rec_int() link._check_status() Return status <> 0 End Function 'def ConnectSafe(self, robot_ip = '', max_attempts=5, wait_connection=4): ' """Connect to a real robot and wait for a connection to succeed. ' In 1 : robot_ip (string) -> IP of the robot to connect. Leave blank to use the IP selected in the connection panel of the robot. ' Out 1 : status -> 1 if connection succeeded 0 if it failed""" ' trycount = 0 ' refresh_rate = 0.5 ' self.Connect() ' tic() ' timer1 = toc() ' pause(refresh_rate) ' while True: ' con_status, status_msg = self.ConnectedState() ' print(status_msg) ' if con_status == ROBOTCOM_READY: ' print(status_msg) ' break ' elif con_status == ROBOTCOM_DISCONNECTED: ' print('Trying to reconnect...') ' self.Connect() ' if toc() - timer1 > wait_connection: ' timer1 = toc() ' self.Disconnect() ' trycount = trycount + 1 ' if trycount >= max_attempts: ' print('Failed to connect: Timed out') ' break ' print('Retrying connection...') ' pause(refresh_rate) ' return con_status 'def ConnectionParams(self): ' """Retrieve robot connection parameters ' Out 1 : string -> robot IP ' Out 2 : int -> port connection ' Out 3 : string -> remote path ' Out 4 : string -> FTP user name ' Out 5 : string -> FTP password""" ' self.link._check_connection() ' command = 'ConnectParams' ' self.link._send_line(command) ' self.link._send_item(self) ' robot_ip = self.link._rec_line() ' port = self.link._rec_int() ' remote_path = self.link._rec_line() ' ftp_user = self.link._rec_line() ' ftp_pass = self.link._rec_line() ' self.link._check_status() ' return robot_ip, port, remote_path, ftp_user, ftp_pass 'def setConnectionParams(self, robot_ip, port, remote_path, ftp_user, ftp_pass): ' """Retrieve robot connection parameters ' In 1 : string -> robot IP ' In 2 : int -> port connection ' In 3 : string -> remote path ' In 4 : string -> FTP user name ' In 5 : string -> FTP password""" ' self.link._check_connection() ' command = 'setConnectParams' ' self.link._send_line(command) ' self.link._send_item(self) ' self.link._send_line(robot_ip) ' self.link._send_int(port) ' self.link._send_line(remote_path) ' self.link._send_line(ftp_user) ' self.link._send_line(ftp_pass) ' self.link._check_status() 'def ConnectedState(self): ' """Check connection status with a real robobt ' Out 1 : status code -> (int) ROBOTCOM_READY if the robot is ready to move, otherwise, status message will provide more information about the issue ' Out 2 : status message -> Message description of the robot status""" ' self.link._check_connection() ' command = 'ConnectedState' ' self.link._send_line(command) ' self.link._send_item(self) ' robotcom_status = self.link._rec_int() ' status_msg = self.link._rec_line() ' self.link._check_status() ' return robotcom_status, status_msg ''' ''' Disconnect from a real robot (when the robot driver is used) ''' ''' status -> true if disconnected successfully, false if it failed. It can fail if it was previously disconnected manually for example. Public Function Disconnect() As Boolean link._check_connection() Dim command As String = "Disconnect" link._send_line(command) link._send_item(Me) Dim status As Integer = link._rec_int() link._check_status() Return status <> 0 End Function ''' ''' Moves a robot to a specific target ("Move Joint" mode). By default, this function blocks until the robot finishes its movements. ''' ''' target -> target to move to as a target item (RoboDK target item) ''' blocking -> True if we want the instruction to block until the robot finished the movement (default=true) Public Sub MoveJ(itemtarget As Item, Optional blocking As Boolean = True) link._moveX(itemtarget, Nothing, Nothing, Me, 1, blocking) End Sub ''' ''' Moves a robot to a specific target ("Move Joint" mode). By default, this function blocks until the robot finishes its movements. ''' ''' joints -> joint target to move to. ''' blocking -> True if we want the instruction to block until the robot finished the movement (default=true) Public Sub MoveJ(joints As Double(), Optional blocking As Boolean = True) link._moveX(Nothing, joints, Nothing, Me, 1, blocking) End Sub ''' ''' Moves a robot to a specific target ("Move Joint" mode). By default, this function blocks until the robot finishes its movements. ''' ''' pose -> pose target to move to. It must be a 4x4 Homogeneous matrix ''' blocking -> True if we want the instruction to block until the robot finished the movement (default=true) Public Sub MoveJ(target As Mat, Optional blocking As Boolean = True) link._moveX(Nothing, Nothing, target, Me, 1, blocking) End Sub ''' ''' Moves a robot to a specific target ("Move Linear" mode). By default, this function blocks until the robot finishes its movements. ''' ''' target -> target to move to as a target item (RoboDK target item) ''' blocking -> True if we want the instruction to block until the robot finished the movement (default=true) Public Sub MoveL(itemtarget As Item, Optional blocking As Boolean = True) link._moveX(itemtarget, Nothing, Nothing, Me, 2, blocking) End Sub ''' ''' Moves a robot to a specific target ("Move Linear" mode). By default, this function blocks until the robot finishes its movements. ''' ''' joints -> joint target to move to. ''' blocking -> True if we want the instruction to block until the robot finished the movement (default=true) Public Sub MoveL(joints As Double(), Optional blocking As Boolean = True) link._moveX(Nothing, joints, Nothing, Me, 2, blocking) End Sub ''' ''' Moves a robot to a specific target ("Move Linear" mode). By default, this function blocks until the robot finishes its movements. ''' ''' pose -> pose target to move to. It must be a 4x4 Homogeneous matrix ''' blocking -> True if we want the instruction to block until the robot finished the movement (default=true) Public Sub MoveL(target As Mat, Optional blocking As Boolean = True) link._moveX(Nothing, Nothing, target, Me, 2, blocking) End Sub 'def SearchL(self, target, blocking=True): ' """Moves a robot to a specific target looking for a specific signal to be triggered ("Search Linear"). This function waits (blocks) until the robot finishes its movements. ' In 1 : joints/pose/target -> target to move to. It can be the robot joints (Nx1 or 1xN), the pose (4x4) or a target (item pointer) ' In 2 (optional): blocking -> True if we want the instruction to wait until the robot finished the movement (default=True)""" ' self.link._moveX(target, self, 5, blocking) ''' ''' Moves a robot to a specific target ("Move Circular" mode). By default, this function blocks until the robot finishes its movements. ''' ''' target -> intermediate target to move to as a target item (RoboDK target item) ''' target -> final target to move to as a target item (RoboDK target item) ''' blocking -> True if we want the instruction to block until the robot finished the movement (default=true) Public Sub MoveC(itemtarget1 As Item, itemtarget2 As Item, Optional blocking As Boolean = True) link._moveC_private(itemtarget1, Nothing, Nothing, itemtarget2, Nothing, Nothing, _ Me, blocking) End Sub ''' ''' Moves a robot to a specific target ("Move Circular" mode). By default, this function blocks until the robot finishes its movements. ''' ''' joints -> intermediate joint target to move to. ''' joints -> final joint target to move to. ''' blocking -> True if we want the instruction to block until the robot finished the movement (default=true) Public Sub MoveC(joints1 As Double(), joints2 As Double(), Optional blocking As Boolean = True) link._moveC_private(Nothing, joints1, Nothing, Nothing, joints2, Nothing, _ Me, blocking) End Sub ''' ''' Moves a robot to a specific target ("Move Circular" mode). By default, this function blocks until the robot finishes its movements. ''' ''' pose -> intermediate pose target to move to. It must be a 4x4 Homogeneous matrix ''' pose -> final pose target to move to. It must be a 4x4 Homogeneous matrix ''' blocking -> True if we want the instruction to block until the robot finished the movement (default=true) Public Sub MoveC(target1 As Mat, target2 As Mat, Optional blocking As Boolean = True) link._moveC_private(Nothing, Nothing, target1, Nothing, Nothing, target2, _ Me, blocking) End Sub ''' ''' Checks if a joint movement is free of collision. ''' ''' joints -> start joints ''' joints -> destination joints ''' (optional): maximum joint step in degrees ''' collision : returns 0 if the movement is free of collision. Otherwise it returns the number of pairs of objects that collided if there was a collision. Public Function MoveJ_Collision(j1 As Double(), j2 As Double(), Optional minstep_deg As Double = -1) As Integer link._check_connection() Dim command As String = "CollisionMove" link._send_line(command) link._send_item(Me) link._send_array(j1) link._send_array(j2) link._send_int(CInt(minstep_deg * 1000.0)) Dim collision As Integer = link._rec_int() link._check_status() Return collision End Function ''' ''' Checks if a linear movement is free of collision. ''' ''' joints -> start joints ''' joints -> destination joints ''' (optional): maximum joint step in degrees ''' collision : returns 0 if the movement is free of collision. Otherwise it returns the number of pairs of objects that collided if there was a collision. Public Function MoveL_Collision(j1 As Double(), j2 As Double(), Optional minstep_deg As Double = -1) As Integer link._check_connection() Dim command As String = "CollisionMoveL" link._send_line(command) link._send_item(Me) link._send_array(j1) link._send_array(j2) link._send_int(CInt(minstep_deg * 1000.0)) Dim collision As Integer = link._rec_int() link._check_status() Return collision End Function ' ''' ' ''' Sets the speed and/or the acceleration of a robot. ' ''' ' ''' speed -> speed in mm/s (-1 = no change) ' ''' acceleration (optional) -> acceleration in mm/s2 (-1 = no change) '' '' public void setSpeed(double speed, double accel = -1) '' { '' link.check_connection(); '' string command = "S_Speed"; '' link.send_line(command); '' link.send_int((int)(speed * 1000.0)); '' link.send_int((int)(accel * 1000.0)); '' link.send_item(this); '' link.check_status(); '' '' } ''' ''' Sets the speed and/or the acceleration of a robot. ''' ''' linear speed in mm/s (-1 = no change) ''' linear acceleration in mm/s2 (-1 = no change) ''' joint speed in deg/s (-1 = no change) ''' joint acceleration in deg/s2 (-1 = no change) Public Sub setSpeed(speed_linear As Double, Optional accel_linear As Double = -1, Optional speed_joints As Double = -1, Optional accel_joints As Double = -1) link._check_connection() Dim command As String = "S_Speed4" link._send_line(command) link._send_item(Me) Dim speed_accel As Double() = New Double(3) {} speed_accel(0) = speed_linear speed_accel(1) = accel_linear speed_accel(2) = speed_joints speed_accel(3) = accel_joints link._send_array(speed_accel) link._check_status() End Sub 'def setAcceleration(self, accel_linear): ' """Sets the linear acceleration of a robot in mm/s2 ' In 1 : angular speed -> acceleration in mm/s2""" ' self.setSpeed(-1,accel_linear,-1,-1) 'def setSpeedJoints(self, speed_joints): ' """Sets the joint speed of a robot in deg/s for rotary joints and mm/s for linear joints ' In 1 : joint speed -> speed in deg/s for rotary joints and mm/s for linear joints""" ' self.setSpeed(-1,-1,speed_joints,-1) 'def setAccelerationJoints(self, accel_joints): ' """Sets the joint acceleration of a robot ' In 1 : joint acceleration -> acceleration in deg/s2 for rotary joints and mm/s2 for linear joints""" ' self.setSpeed(-1,-1,-1,accel_joints) ''' ''' Sets the robot movement smoothing accuracy (also known as zone data value). ''' ''' zonedata value (int) (robot dependent, set to -1 for fine movements) Public Sub setZoneData(zonedata As Double) link._check_connection() Dim command As String = "S_ZoneData" link._send_line(command) link._send_int(CInt(zonedata * 1000.0)) link._send_item(Me) link._check_status() End Sub ''' ''' Displays a sequence of joints ''' ''' joint sequence as a 6xN matrix or instruction sequence as a 7xN matrix Public Sub ShowSequence(sequence As Mat) link._check_connection() Dim command As String = "Show_Seq" link._send_line(command) link._send_matrix(sequence) link._send_item(Me) link._check_status() End Sub ''' ''' Checks if a robot or program is currently running (busy or moving) ''' ''' busy status (true=moving, false=stopped) Public Function Busy() As Boolean link._check_connection() Dim command As String = "IsBusy" link._send_line(command) link._send_item(Me) Dim busy__1 As Integer = link._rec_int() link._check_status() Return (busy__1 > 0) End Function ''' ''' Stops a program or a robot ''' Public Sub [Stop]() link._check_connection() Dim command As String = "Stop" link._send_line(command) link._send_item(Me) link._check_status() End Sub ''' ''' Waits (blocks) until the robot finishes its movement. ''' ''' timeout -> Max time to wait for robot to finish its movement (in seconds) Public Sub WaitMove(Optional timeout_sec As Double = 300) link._check_connection() Dim command As String = "WaitMove" link._send_line(command) link._send_item(Me) link._check_status() link.COM.ReceiveTimeout = CInt(timeout_sec * 1000.0) link._check_status() 'will wait here; link.COM.ReceiveTimeout = link.TIMEOUT 'int isbusy = link.Busy(this); 'while (isbusy) '{ ' busy = link.Busy(item); '} End Sub ''' ''' Wait until the program finishes. ''' Public Sub WaitFinished() While Me.Busy() Threading.Thread.Sleep(0.05 * 1000) End While End Sub '////// ADD MORE METHODS ' ---- Program item calls ----- ''' ''' Defines the name of the program when the program is generated. It is also possible to specify the name of the post processor as well as the folder to save the program. ''' This method must be called before any program output is generated (before any robot movement or other instruction). ''' ''' name of the program ''' folder to save the program, leave empty to use the default program folder ''' name of the post processor (for a post processor in C:/RoboDK/Posts/Fanuc_post.py it is possible to provide "Fanuc_post.py" or simply "Fanuc_post") Public Function ProgramStart(programname As String, Optional folder As String = "", Optional postprocessor As String = "") As Integer Return link.ProgramStart(programname, folder, postprocessor, Me) End Function ''' ''' Sets the accuracy of the robot active or inactive. ''' ''' Public Sub setAccuracyActive(Optional accurate As Boolean = True) link._check_connection() Dim command As String = "S_AbsAccOn" link._send_line(command) link._send_item(Me) If accurate Then link._send_int(1) Else link._send_int(0) End If link._check_status() End Sub ''' ''' Filters a program file to improve accuracy for a specific robot. The robot must have been previously calibrated. ''' ''' File path of the program. ''' Summary of the filter process.. ''' status : 0 if the filter succeeded, below 0 if there are conversion problems. Public Function FilterProgram(filestr As String, ByRef filter_msg As String) As Integer link._check_connection() Dim command As String = "FilterProg2" link._send_line(command) link._send_item(Me) link._send_line(filestr) Dim filter_status As Integer = link._rec_int() filter_msg = link._rec_line() link._check_status() Return filter_status End Function ''' ''' Saves a program to a file. ''' ''' folder path of the program ''' success Public Function MakeProgram(folder As String) As Boolean link._check_connection() Dim command As String = "MakeProg" link._send_line(command) link._send_item(Me) link._send_line(folder) link.COM.ReceiveTimeout = 3600 * 1000 Dim success As Boolean = False Dim prog_status As Integer = link._rec_int() Dim prog_log_str As String = link._rec_line() success = (prog_status > 0) link.COM.ReceiveTimeout = link.TIMEOUT link._check_status() Return success End Function ''' ''' Sets if the program will be run in simulation mode or on the real robot. ''' Use: "PROGRAM_RUN_ON_SIMULATOR" to set the program to run on the simulator only or "PROGRAM_RUN_ON_ROBOT" to force the program to run on the robot. ''' Public Sub setRunType(program_run_type As Integer) link._check_connection() Dim command As String = "S_ProgRunType" link._send_line(command) link._send_item(Me) link._send_int(program_run_type) link._check_status() End Sub ''' ''' Runs a program. It returns the number of instructions that can be executed successfully (a quick program check is performed before the program starts) ''' This is a non-blocking call. Use IsBusy() to check if the program execution finished. ''' Notes: ''' if setRunMode(RUNMODE_SIMULATE) is used -> the program will be simulated (default run mode) ''' if setRunMode(RUNMODE_RUN_ROBOT) is used -> the program will run on the robot (default when RUNMODE_RUN_ROBOT is used) ''' if setRunMode(RUNMODE_RUN_ROBOT) is used together with program.setRunType(PROGRAM_RUN_ON_ROBOT) -> the program will run sequentially on the robot the same way as if we right clicked the program and selected "Run on robot" in the RoboDK GUI ''' ''' number of instructions that can be executed Public Function RunProgram() As Integer link._check_connection() Dim command As String = "RunProg" link._send_line(command) link._send_item(Me) Dim prog_status As Integer = link._rec_int() link._check_status() Return prog_status End Function ''' ''' Runs a program. It returns the number of instructions that can be executed successfully (a quick program check is performed before the program starts) ''' Program parameters can be provided for Python calls. ''' This is a non-blocking call.Use IsBusy() to check if the program execution finished. ''' Notes: if setRunMode(RUNMODE_SIMULATE) is used -> the program will be simulated (default run mode) ''' if setRunMode(RUNMODE_RUN_ROBOT) is used ->the program will run on the robot(default when RUNMODE_RUN_ROBOT is used) ''' if setRunMode(RUNMODE_RUN_ROBOT) is used together with program.setRunType(PROGRAM_RUN_ON_ROBOT) -> the program will run sequentially on the robot the same way as if we right clicked the program and selected "Run on robot" in the RoboDK GUI ''' ''' Number of instructions that can be executed Public Function RunCode(Optional parameters As String = Nothing) As Integer link._check_connection() If parameters Is Nothing Then Dim command As String = "RunProg" link._send_line(command) link._send_item(Me) Else Dim command As String = "RunProgParam" link._send_line(command) link._send_item(Me) link._send_line(parameters) End If Dim progstatus As Integer = link._rec_int() link._check_status() Return progstatus End Function ''' ''' Adds a program call, code, message or comment inside a program. ''' ''' string of the code or program to run ''' INSTRUCTION_* variable to specify if the code is a progra Public Function RunCodeCustom(code As String, Optional run_type As Integer = INSTRUCTION_CALL_PROGRAM) As Integer link._check_connection() Dim command As String = "RunCode2" link._send_line(command) link._send_item(Me) link._send_line(code.Replace(vbLf & vbLf, "
").Replace(vbLf, "
")) link._send_int(run_type) Dim progstatus As Integer = link._rec_int() link._check_status() Return progstatus End Function ''' ''' Generates a pause instruction for a robot or a program when generating code. Set it to -1 (default) if you want the robot to stop and let the user resume the program anytime. ''' ''' Time in milliseconds Public Sub Pause(Optional time_ms As Double = -1) link._check_connection() Dim command As String = "RunPause" link._send_line(command) link._send_item(Me) link._send_int(CInt(time_ms * 1000.0)) link._check_status() End Sub ''' ''' Sets a variable (output) to a given value. This can also be used to set any variables to a desired value. ''' ''' io_var -> digital output (string or number) ''' io_value -> value (string or number) Public Sub setDO(io_var As String, io_value As String) link._check_connection() Dim command As String = "setDO" link._send_line(command) link._send_item(Me) link._send_line(io_var) link._send_line(io_value) link._check_status() End Sub ''' ''' Waits for an input io_id to attain a given value io_value. Optionally, a timeout can be provided. ''' ''' io_var -> digital output (string or number) ''' io_value -> value (string or number) ''' int (optional) -> timeout in miliseconds Public Sub waitDI(io_var As String, io_value As String, Optional timeout_ms As Double = -1) link._check_connection() Dim command As String = "waitDI" link._send_line(command) link._send_item(Me) link._send_line(io_var) link._send_line(io_value) link._send_int(CInt(timeout_ms * 1000.0)) link._check_status() End Sub ''' ''' Adds a new robot move joint instruction to a program. ''' ''' target to move to Public Sub addMoveJ(itemtarget As Item) link._check_connection() Dim command As String = "Add_INSMOVE" link._send_line(command) link._send_item(itemtarget) link._send_item(Me) link._send_int(1) link._check_status() End Sub ''' ''' Adds a new robot move linear instruction to a program. ''' ''' target to move to Public Sub addMoveL(itemtarget As Item) link._check_connection() Dim command As String = "Add_INSMOVE" link._send_line(command) link._send_item(itemtarget) link._send_item(Me) link._send_int(2) link._check_status() End Sub '/////// ADD MORE METHODS ''' ''' Show instructions inside a program in RoboDK tree ''' ''' optional Public Sub ShowInstructions(Optional show As Boolean = True) link._check_connection() Dim command As String = "Prog_ShowIns" link._send_line(command) link._send_item(Me) If show Then link._send_int(1) Else link._send_int(0) End If link._check_status() End Sub ''' ''' Returns the number of instructions of a program. ''' ''' Public Function InstructionCount() As Integer link._check_connection() Dim command As String = "Prog_Nins" link._send_line(command) link._send_item(Me) Dim nins As Integer = link._rec_int() link._check_status() Return nins End Function ''' ''' Returns the program instruction at position id ''' ''' ''' ''' ''' ''' ''' ''' Public Sub Instruction(ins_id As Integer, ByRef name As String, ByRef instype As Integer, ByRef movetype As Integer, ByRef isjointtarget As Boolean, ByRef target As Mat, _ ByRef joints As Double()) link._check_connection() Dim command As String = "Prog_GIns" link._send_line(command) link._send_item(Me) link._send_int(ins_id) name = link._rec_line() instype = link._rec_int() movetype = 0 isjointtarget = False target = Nothing joints = Nothing If instype = INS_TYPE_MOVE Then movetype = link._rec_int() isjointtarget = If(link._rec_int() > 0, True, False) target = link._rec_pose() joints = link._rec_array() End If link._check_status() End Sub ''' ''' Updates a program and returns the estimated time and the number of valid instructions. ''' ''' Returns the number of valid instructions ''' Estimated cycle time (in seconds) ''' Distance that the robot TCP will travel (in mm) ''' It is True if the program has no issues, False otherwise. Public Sub Update(ByRef valid_instructions_number As Integer, ByRef program_time_seconds As Double, ByRef program_distance_mm As Double, ByRef valid_program As Boolean) ' Out 1: ' Out 2: ' Out 3: ' Out 4: """ link._check_connection() Dim command As String = "Update" link._send_line(command) link._send_item(Me) Dim values As Double() = link._rec_array() link._check_status() valid_instructions_number = CInt(values(0)) program_time_seconds = values(1) program_distance_mm = values(2) valid_program = values(3) > 0 End Sub ''' ''' Sets the program instruction at position id ''' ''' ''' ''' ''' ''' ''' ''' Public Sub setInstruction(ins_id As Integer, name As String, instype As Integer, movetype As Integer, isjointtarget As Boolean, target As Mat, _ joints As Double()) link._check_connection() Dim command As String = "Prog_SIns" link._send_line(command) link._send_item(Me) link._send_int(ins_id) link._send_line(name) link._send_int(instype) If instype = INS_TYPE_MOVE Then link._send_int(movetype) link._send_int(If(isjointtarget, 1, 0)) link._send_pose(target) link._send_array(joints) End If link._check_status() End Sub ''' ''' Returns the list of program instructions as an MxN matrix, where N is the number of instructions and M equals to 1 plus the number of robot axes. ''' ''' the matrix of instructions ''' Returns 0 if success Public Function InstructionList(ByRef instructions As Mat) As Integer link._check_connection() Dim command As String = "G_ProgInsList" link._send_line(command) link._send_item(Me) instructions = link._rec_matrix() Dim errors As Integer = link._rec_int() link._check_status() Return errors End Function ''' ''' Returns a list of joints an MxN matrix, where M is the number of robot axes plus 4 columns. Linear moves are rounded according to the smoothing parameter set inside the program. ''' ''' Returns a human readable error message (if any) ''' Returns the list of joints as [J1, J2, ..., Jn, ERROR, MM_STEP, DEG_STEP, MOVE_ID] if a file name is not specified ''' Maximum step in millimeters for linear movements (millimeters) ''' Maximum step for joint movements (degrees) ''' Provide a file name to directly save the output to a file. If the file name is not provided it will return the matrix. If step values are very small, the returned matrix can be very large. ''' Returns 0 if success, otherwise, it will return negative values Public Function InstructionListJoints(ByRef error_msg As String, ByRef joint_list As Mat, Optional mm_step As Double = 10.0, Optional deg_step As Double = 5.0, Optional save_to_file As String = "") As Integer link._check_connection() Dim command As String = "G_ProgJointList" link._send_line(command) link._send_item(Me) Dim ste_mm_deg As Double() = {mm_step, deg_step} link._send_array(ste_mm_deg) 'joint_list = save_to_file; If save_to_file.Length <= 0 Then link._send_line("") joint_list = link._rec_matrix() Else link._send_line(save_to_file) joint_list = Nothing End If Dim error_code As Integer = link._rec_int() error_msg = link._rec_line() link._check_status() Return error_code End Function ''' ''' Disconnect from the RoboDK API. This flushes any pending program generation. ''' ''' Public Function Finish() As Boolean Return link.Finish() End Function End Class End Class