Merge branch 'roboview-from-csv' into develop

- Used to review TEO kinematics:
   - roboticslab-uc3m/teo-developer-manual#40
   - roboticslab-uc3m/teo-main#38

scripts/python/README.md

@@ -0,0 +1,16 @@
+# scripts/python
+
+## roboview-from-csv
+Provides a kinematic chain description for [roboview](https://github.com/arcoslab/roboview), given a `.csv` file.
+
+### Install dependecies
+- [roboview](https://github.com/arcoslab/roboview)
+   - At time of writing, best use `vtk6` branch of <https://github.com/jgvictores/roboview> fork
+   - Requires [Python YARP](http://robots.uc3m.es/gitbook-installation-guides/install-yarp.html#install-bindings) and Python [KDL](http://robots.uc3m.es/gitbook-installation-guides/install-kdl.html) (PyKDL)
+- Examples hard-coded from https://github.com/roboticslab-uc3m/teo-developer-manual
+
+### Run
+```bash
+yarp server &
+./roboview roboview-from-csv.py
+```

scripts/python/roboview-from-csv.py

@@ -0,0 +1,73 @@
+# See README.md
+
+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]