diff --git a/scripts/python/README.md b/scripts/python/README.md new file mode 100644 index 000000000..0303c5f5c --- /dev/null +++ b/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 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 +``` diff --git a/scripts/python/roboview-from-csv.py b/scripts/python/roboview-from-csv.py new file mode 100644 index 000000000..4ea99ddc7 --- /dev/null +++ b/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]