Merge branch 'xgnitive-tools' into develop, #7

README.md

@@ -26,6 +26,16 @@ Installation instructions for installing from source can be found [here]( doc/to
 #### Manuals
 1. Read [development manual](https://github.com/roboticslab-uc3m/tools/blob/develop/doc/tools-development-manual.md) if you want to build a new language model
 
+#### Manuals
+
+List of programs presented in this repo:
+
+- [openrave-to-stl](https://github.com/roboticslab-uc3m/tools/tree/develop/programs/openraveppToSTL) 
+- [meanVar](https://github.com/roboticslab-uc3m/tools/tree/develop/programs/meanVar)
+- [boxPlot](https://github.com/roboticslab-uc3m/tools/tree/xgnitive-tools/programs/boxPlot)
+- [plotTrajectories](https://github.com/roboticslab-uc3m/tools/tree/xgnitive-tools/programs/plotTrajectories)
+- [plotTrajectories3D](https://github.com/roboticslab-uc3m/tools/tree/xgnitive-tools/programs/plotTrajectories3D)
+
 ## Status
 
 [![Build Status (Linux/OSX)](https://travis-ci.org/roboticslab-uc3m/tools.svg?branch=develop)](https://travis-ci.org/roboticslab-uc3m/tools)
@@ -38,4 +48,3 @@ Installation instructions for installing from source can be found [here]( doc/to
 
 * [kinematics-dynamics](https://github.com/roboticslab-uc3m/kinematics-dynamics)
 * [yarp-devices](https://github.com/roboticslab-uc3m/yarp-devices)
-

programs/boxPlot/README.md

@@ -0,0 +1,5 @@
+# mean-var tool
+
+This tool is used to calculate the mean and variance from a dataset.
+
+The algorithm returns the mean and variance of each of the selected columns from the file selected by the user, and save it on results-mean.txt and results-var.txt.

programs/boxPlot/boxPlot.py

@@ -0,0 +1,47 @@
+#!/usr/bin/python
+
+## numpy is used for creating fake data
+import numpy as np 
+import matplotlib as mpl 
+import matplotlib.pyplot as plt 
+
+#import matplotlib
+
+#matplotlib.rcParams['ps.useafm'] = True
+#matplotlib.rcParams['pdf.use14corefonts'] = True
+#matplotlib.rcParams['text.usetex'] = True
+
+font = {'family' : 'Helvetica',
+        'size'   : 36}
+
+plt.rc('font', **font)
+
+
+fte=[263,345,209]
+iet=[115,148,166]
+oet=[3.9810779095, 3.8074719906, 4.1091489792, 3.8754820824, 3.8158481121, 3.9638550282, 3.8815281391, 3.6324350834, 3.8728020191, 3.6472148895, 3.8645160198, 3.8971121311,3.3999831677, 3.415995121, 3.7009780407, 3.8986778259, 3.6791219711, 4.4722249508, 4.4507529736, 5.6388869286, 3.6090362072, 3.954447031, 3.3659791946, 3.9020190239, 3.7741010189, 4.011754036, 4.0648510456, 7.3000378609, 3.9543938637, 3.9721271992, 3.9382181168, 3.9818081856, 3.734610080, 3.5969200134, 3.9504361153, 5.4207720757]
+
+
+data_to_plot = [fte,iet,oet]
+
+fig = plt.figure(1, figsize=(30, 20))
+
+# Create an axes instance
+ax = fig.add_subplot(111)
+
+ax.set_xticklabels(['FTE', 'IET', 'OET'])
+
+ax.set_ylabel('Real Iteration Time (s)')
+
+# Create the boxplot
+bp = ax.boxplot(data_to_plot, widths = 0.9)
+
+
+
+plt.show()
+
+
+
+
+
+

programs/meanVar/README.md

@@ -0,0 +1,5 @@
+# meanVar tool
+
+This tool is used to calculate the mean and variance from a dataset.
+
+The algorithm returns the mean and variance of each of the selected columns from the file selected by the user, and save it on results-mean.txt and results-var.txt..

programs/meanVar/meanVar.py

@@ -0,0 +1,36 @@
+# -*- coding: utf-8 -*-
+"""
+Author: Raul Fernandez-Fernandez (rauferna@ing.uc3m.es)
+Robotics Lab. Universidad Carlos III de Madrid
+"""
+
+################### GENERAL ##############################
+import numpy as np
+
+################### TO USE NOW ###########################
+
+features1=[3,4] #column features to use from trajectory1
+
+d1="data/Trajectory-IET-Iron.txt" #Trajectory 1 file name
+
+##########################################################
+def main():
+
+    #Read Trajectory 1
+    data1=np.loadtxt(d1)
+    d1_clean=data1[:,features1]
+
+    #Calculate mean and variance from trajectories, indeed we just need 1
+    meanArray=[]
+    varArray=[]
+
+    for i in range(d1_clean.shape[1]):
+        meanArray.append(np.mean(d1_clean[:][:,i]))
+        varArray.append(np.var(d1_clean[:][:,i]))
+
+    np.savetxt("results-mean.txt",meanArray,"%f")
+    np.savetxt("results-var.txt",varArray, "%f")
+
+if __name__ == "__main__":
+    main()
+

programs/openraveppToSTL/README.md

@@ -0,0 +1,13 @@
+# openrave-to-stl script
+
+This script is designed to do the conversion between the .pp files obtained with the openrave convex decomposition model used/created [here](https://github.com/roboticslab-uc3m/openrave-yarp-plugins/blob/develop/examples/openrave-YarpRobot-collision-sim.py), 
+to a more standard file format (stl), in order to be used with other frameworks like ArmarX, and so on.
+
+## How to use it
+
+Just change this [line](https://github.com/roboticslab-uc3m/tools/blob/b565045f11cd0b2f0ec0fc403823d679b8a79fd3/programs/openraveppToSTL/openraveppToSTL.py#L21) to be the filename of the pp file you want to convert.
+
+then run the python script in this folder
+```
+python openravepp-to-stl.py 
+```

programs/openraveppToSTL/openraveppToSTL.py

@@ -0,0 +1,54 @@
+#!/usr/bin/env python
+
+'''
+python-picker-loader.py program
+
+Authors: Raul Fernandez-Fernandez
+
+Universidad Carlos III de Madrid 
+'''
+
+try:
+    import cPickle as pickle
+except:
+    import pickle
+
+import pprint
+import numpy as np
+from stl import mesh
+
+
+filename='/home/raul/repos/teo-openrave-models/openrave/teo/pp/convexdecomposition.pp'
+modelversion,params = pickle.load(open(filename, 'r'))
+
+#pprint.pprint(modelversion)
+#pprint.pprint(params)
+
+#links
+linkcd, convexparams=params
+
+print('processing '+str(len(linkcd))+' links')
+
+iteration = 0
+for linkcd_for in linkcd:
+        for ig, hulls in linkcd_for:
+            vertices = np.zeros((0, 3), np.float64)
+            faces = np.zeros((0, 3), int)
+            for hull in hulls:
+                faces = np.r_[faces, hull[1] + len(vertices)]
+                vertices = np.r_[vertices, hull[0]]
+            #print vertices
+            #print faces
+
+            #Create cube mesh object
+            mesh_object = mesh.Mesh(np.zeros(faces.shape[0], dtype=mesh.Mesh.dtype))
+            for i, f in enumerate(faces):
+                for j in range(3):
+                    mesh_object.vectors[i][j] = vertices[f[j], :]
+
+            #print(cube)
+
+            # Write the mesh to file "cube.stl"
+            mesh_object.save('link-'+str(iteration)+'.stl')
+            iteration=iteration+1
+            print ('link '+str(iteration))

programs/plotTrajectories/README.md

@@ -0,0 +1,3 @@
+# plotTrajectories
+
+plot 2D trajectories from [file](https://github.com/roboticslab-uc3m/tools/blob/b48e62e62f99c465982ab2bfd42c50cb80fb3a38/programs/plotTrajectories/plotTrajectories.py#L16).

programs/plotTrajectories/plotTrajectories.py

@@ -0,0 +1,32 @@
+#!/usr/bin/python
+import matplotlib.pyplot as plt 
+
+#import matplotlib
+
+#matplotlib.rcParams['ps.useafm'] = True
+#matplotlib.rcParams['pdf.use14corefonts'] = True
+#matplotlib.rcParams['text.usetex'] = True
+
+font = {'family' : 'Helvetica',
+        'size'   : 36}
+
+plt.rc('font', **font)
+
+
+plt.plotfile('Generalized.txt', delimiter=' ', cols=(0, 1),
+                     names=('Time interval', 'Wall Painted (%)'), marker='s', label='Generalized', linewidth=7.0)
+
+plt.plotfile('FTE-Paint-Percentage.txt', delimiter=' ', cols=(0, 1), newfig=False, 
+                     names=('Time interval', 'Wall Painted (%)'), marker='x', label='FTE', linewidth=2.0)
+
+plt.plotfile('IET-Paint-Percentage.txt', delimiter=' ', cols=(0, 1), newfig=False,
+                     names=('Time interval', 'Wall Painted (%)'), marker='o', label='IET', linewidth=2.0)
+
+plt.plotfile('OET-Paint-Percentage.txt', delimiter=' ', cols=(0, 1), newfig=False,
+                     names=('Time interval', 'Wall Painted (%)'), marker='^', label='OET', linewidth=2.0)
+
+plt.legend(loc=2)
+
+plt.savefig('Gen-paint.pdf')
+
+plt.show()

programs/plotTrajectories3D/README.md

@@ -0,0 +1,3 @@
+# plotTrajectories3D
+
+Plot 3D trajectories from [file](https://github.com/roboticslab-uc3m/tools/blob/bee2c7640a999f47b07c5bd155397e195c7d2e90/programs/plotTrajectories3D/plotTrajectories3D.py#L31).

programs/plotTrajectories3D/plotTrajectories3D.py

@@ -0,0 +1,77 @@
+# -*- coding: utf-8 -*-
+"""
+Author: Raul Fernandez-Fernandez (rauferna@ing.uc3m.es)
+Robotics Lab. Universidad Carlos III de Madrid
+"""
+
+################### GENERAL ##############################
+import glob
+import numpy as np
+import matplotlib.pyplot as plt
+from mpl_toolkits.mplot3d import Axes3D
+
+font = {'family' : 'normal',
+        'size'   : 18}
+
+plt.rc('font', **font)
+
+
+################### TO USE NOW ###########################
+
+#3D
+X_COL=0
+Y_COL=1
+Z_COL=2
+
+##########################################################
+def main():
+
+    #Read Trajectory 1
+
+    Traj1=np.loadtxt("Trajectory-generalized.txt")
+
+    #Read Trajectory 2
+
+    #Traj2= np.loadtxt("Trajectory2.txt")
+    #Traj2= np.loadtxt("iron/Trajectory-OET.txt") #First line of OET is to specify the Time interval, not always T.
+
+    #Traj3= np.loadtxt("iron/Trajectory-IET.txt")
+
+    #Traj4= np.loadtxt("iron/Trajectory-FTE.txt")
+
+    ############################### CHOOSE BEST RESULT #########################
+
+    #TODO implement this to be automatic
+
+    ############################### PLOT 3D ###########################################
+    fig = plt.figure()
+    ax = fig.gca(projection='3d')
+
+    ax.plot(Traj1[:][:,X_COL],Traj1[:][:,Y_COL],Traj1[:][:,Z_COL])
+    #ax.plot(Traj2[:][:,X_COL+1],Traj2[:][:,Y_COL+1],Traj2[:][:,Z_COL+1])
+    #ax.plot(Traj3[:][:,X_COL],Traj3[:][:,Y_COL],Traj3[:][:,Z_COL])
+    #ax.plot(Traj4[:][:,X_COL],Traj4[:][:,Y_COL],Traj4[:][:,Z_COL])
+
+
+    ############################### PLOT ALL 2D #######################################
+
+    #for i in range(4):  # features
+#        plt.figure()
+#	plt.xlabel('Force (N)') #Features
+#	plt.ylabel('Time Interval')
+#        plt.plot(np.linspace(0,9,Traj1.shape[0]),Traj1[:][:,i])
+#        #plt.plot(Traj2[:][:,0]*1/8,Traj2[:][:,i+1])
+#	plt.plot(Traj2[:][:,0],Traj2[:][:,i+1])
+#	plt.plot(np.linspace(0,9,9),Traj3[:][:,i])
+#        plt.plot(np.linspace(0,9,9),Traj4[:][:,i])
+
+
+
+    plt.show()
+    #plt.savefig('foo.png')
+
+
+
+
+if __name__ == "__main__":
+    main()