Add roboview-from-csv.py

- Used for reviewing kinematics of TEO at roboticslab-uc3m/teo-main#38

scripts/python/roboview-from-csv.py

@@ -0,0 +1,80 @@
+# Install dependecies:
+# - Fork of https://github.com/arcoslab/roboview
+#    - git clone -b vtk6 https://github.com/jgvictores/roboview  
+# - Examples hard-coded from:
+#    - git clone https://github.com/roboticslab-uc3m/teo-developer-manual
+#
+# Run:
+# ./roboview roboview-from-csv.py
+
+from PyKDL import *
+from math import pi
+import csv
+
+lengthsFileName = '/home/yo/repos/teo-developer-manual/csv/lengths.csv'
+dhFileName = '/home/yo/repos/teo-developer-manual/csv/dh-rightArm.csv'
+
+segments = [] # required for roboview
+
+def degToRad(deg):
+    return deg*pi/180.0
+
+#-- initPoss
+# Small hack: first replace two digits, e.g. else can replace q1 before q13
+twoDigitJoints = {'q'+str(i):'0.0' for i in range(10,28)} # dof
+oneDigitjoints = {'q'+str(i):'0.0' for i in range(1,10)} # dof
+
+#-- lengths
+lengthsFile = open(lengthsFileName, 'r')
+reader = csv.reader(lengthsFile, delimiter=',')
+next(reader, None)  # skip the header
+lengths = {row[0]:str(float(row[1])/1000.0) for row in reader} # [mm] to [m]
+print('lengths: ',lengths)
+
+#-- dh params
+dhFile = open(dhFileName, 'r')
+reader = csv.reader(dhFile, delimiter=',')
+
+#-- populate segments
+next(reader, None)  # skip the header
+for row in reader:
+    print('row: ',row)
+
+    linkOffset = row[1]
+    for first, second in twoDigitJoints.items():
+        linkOffset = str(linkOffset).replace(first, second)
+    for first, second in oneDigitjoints.items():
+        linkOffset = str(linkOffset).replace(first, second)
+    linkOffset = eval(linkOffset)
+
+    linkD = row[2]
+    for first, second in twoDigitJoints.items():
+        linkD = str(linkD).replace(first, second)
+    for first, second in oneDigitjoints.items():
+        linkD = str(linkD).replace(first, second)
+    for first, second in lengths.items():
+        linkD = str(linkD).replace(first, second)
+    linkD = eval(linkD)
+
+    linkA = row[3]
+    for first, second in lengths.items():
+        linkA = str(linkA).replace(first, second)
+    linkA = eval(linkA)
+
+    linkAlpha = eval(row[4])
+
+    print('* linkOffset ', linkOffset)
+    print('* linkD ', linkD)
+    print('* linkA ', linkA)
+    print('* linkAlpha ', linkAlpha)
+    s = Segment(Joint(Joint.RotZ),
+                Frame().DH(linkA,degToRad(linkAlpha),linkD,degToRad(linkOffset)))
+    segments.append(s)
+
+
+#-- populate limits (hard-coded for now)
+limits_min = [-180.0] * len(segments)
+limits_max = [180.0] * len(segments)
+
+limits_min = [degToRad(v) for v in limits_min]
+limits_max = [degToRad(v) for v in limits_max]