clean the package space

Author: chenlx92

README.md

@@ -1,5 +1,9 @@
 # Interaction ProMPs  
-Interaction ProMPs generate a robot collaborative motion based on the prediction from a set of partial human motion observations. The approach also works in multi-task scenarios. This package use EMG signals to enhance the task recognition.   
+Interaction ProMPs generate a robot collaborative motion based on the prediction from a set of partial human motion observations. 
+
+![generalization](./docs/media/generalization.png  "generalization.png")
+
+The approach also works in multi-task scenarios. This package use EMG signals to enhance the task recognition.   
 Not make sure if the EMG signals are correlated with robot motion and we will confirm it latter. 
 
 

datasets/guilherme_datasets/main.m

@@ -0,0 +1,60 @@
+
+% Read and plot data from human-robot collaboration
+% taskProMP{1}: task 1
+% taskProMP{2}: task 2
+% taskProMP{3}: task 3
+%
+% Trajectories of both human and robot are time aligned.
+% Each column is one degree of freedom.
+% Each row is a time step.
+% data{1}(:,1:3)   xyz coordinates of the human wrist recorded via optical
+%                  marker traking
+% data{1}(:,4:10)  joint angles of the KUKA lightweight arm, starting from
+%                  base towards the end effector
+% data{1}(:,11:13) xyz coordinates of the robot end-effector
+
+function main()
+
+    clear; clc; close all;
+    dbstop if error;
+
+
+    load('taskProMP.mat');
+
+    number_of_tasks = numel(taskProMP);
+    
+    for j=1:number_of_tasks 
+        plot_data(taskProMP{j});
+    end
+    
+    for j=1:number_of_tasks 
+        gen_csv(taskProMP{j}, j);
+    end
+
+end
+
+
+function plot_data(data)
+    h1 = figure; axis equal; grid on; hold on; 
+    xlabel 'x (m)';
+    ylabel 'y (m)'; 
+    zlabel 'z (m)'; 
+    view([-1 1 1])
+    title('Blue: Human wrist. Red: robot hand')
+
+    number_of_training_data = numel(data);
+    for k=1:number_of_training_data
+        plot3(data{k}(:,1), data{k}(:,2), data{k}(:,3), 'b'  );
+        plot3(data{k}(:,11), data{k}(:,12), data{k}(:,13), 'r'  );
+    end
+end
+    
+%  created by Longxin to output the csv file that can be read by python
+function gen_csv(data, task_idx)
+	number_of_training_data = numel(data);
+    for k=1:number_of_training_data
+        data_temp = [data{k}(:,1), data{k}(:,2), data{k}(:,3), data{k}(:,11), data{k}(:,12), data{k}(:,13)];
+        file_name = ['./csv_datasets/', 'task', num2str(task_idx), '_', num2str(k)];
+        csvwrite(file_name, data_temp);
+    end
+end

datasets/guilherme_datasets/scripts/bag_to_csv.sh

@@ -0,0 +1,49 @@
+#!/usr/bin/env bash
+
+function usage() {
+    echo
+    echo "Usage:"
+    echo "bag_to_csv.sh <input bag> <output directory>"
+    echo 
+    echo "<input bag>        - the input bag file"
+    echo "<output directory> - the directory to output the csv in"
+    echo
+    echo "e.g."
+    echo "bag_to_csv.sh input.bag output_dir"
+    echo
+}
+
+if [[ $# -ne 2 ]]; then
+    echo "Invalid parameters"
+    usage
+fi
+
+# make the output directory
+output_dir=$2/$(basename $1 | sed 's/\..*$//')
+mkdir -p $output_dir
+
+# find where topics starts
+topics_line=$(rosbag info $1 | grep -n "topics:" | cut -f1 -d:)
+
+# get the number of lines
+end_line=$(rosbag info $1 | wc -l)
+
+# get the topic on the "topics:" line
+topics[0]=$(rosbag info $1 | sed -n "${topics_line},${topics_line} p" | awk '{ print $2; }')
+
+# get the rest of the topics
+for (( i=$topics_line + 1; i <= $end_line; i++)); do
+    topics[$i]=$(rosbag info $1 | sed -n "$i,$i p" | awk '{ print $1; }')
+done
+
+# for each topic
+for topic in ${topics[@]}; do
+    # create the csv output directory
+    dir_name=$(dirname $topic)
+    base_name=$(basename $topic)
+
+    mkdir -p $output_dir/$dir_name
+
+    echo "Creating csv: $output_dir/$dir_name/$base_name.csv"
+    rostopic echo -b $1 -p $topic > $output_dir/$dir_name/$base_name.csv
+done

datasets/guilherme_datasets/scripts/batch_proc.py

@@ -0,0 +1,23 @@
+#!/usr/bin/env python
+import glob
+import os
+import subprocess
+
+# load the raw data set
+file_path = os.path.dirname(__file__)
+datasets_raw_path = '../datasets/tape/raw'
+
+
+def main():
+    # run sh script for each rosbag file in datasets path
+    task_path_list = glob.glob(os.path.join(datasets_raw_path, "*"))
+    for task_path in task_path_list:
+        demo_path_list = glob.glob(os.path.join(task_path, "*.bag"))
+        for demo_path in demo_path_list:
+            subprocess.Popen([os.path.join(file_path, './bag_to_csv.sh') + ' ' +
+                              demo_path + ' ' +
+                              os.path.join(task_path, 'csv')], shell=True)  # non block function
+                              # os.path.join(task_path, 'csv')], shell = True).wait()   # the block function
+
+if __name__ == '__main__':
+    main()

datasets/guilherme_datasets/scripts/load_data.py

@@ -0,0 +1,32 @@
+#!/usr/bin/python
+import numpy as np
+import pandas as pd
+import glob
+import os
+from sklearn.externals import joblib
+import scipy.signal as signal
+from scipy.ndimage.filters import gaussian_filter1d
+import scipy.io as sio
+
+
+data_mat = sio.loadmat('./../taskProMP.mat')
+data = data_mat['taskProMP'][0]
+
+datasets_raw = []
+for task_idx in range(len(data)):
+    demo_temp = []
+    for demo_idx in range(len(data[task_idx])):
+        # the info of interest: convert the object to int / float
+        demo_temp.append({
+            'stamp': demo_idx,
+            'left_hand': data[task_idx][demo_idx][0][:, 0:3].astype(float),
+            'left_joints': data[task_idx][demo_idx][0][:, 10:13].astype(float)
+        })
+    datasets_raw.append(demo_temp)
+
+
+
+
+
+print('Saving the datasets as pkl ...')
+joblib.dump(datasets_raw, '../datasets_raw.pkl')

datasets/guilherme_datasets/scripts/plot_traj.py

@@ -0,0 +1,25 @@
+#!/usr/bin/python
+import numpy as np
+import pandas as pd
+import glob
+import os
+import matplotlib.pyplot as plt
+from mpl_toolkits.mplot3d import Axes3D
+
+datasets_path = '../datasets/raw/rect/2018-01-18-10-12-41/multiModal_states.csv'
+data_csv = pd.read_csv(datasets_path)
+data = data_csv.values[:, 207:210].astype(float)
+
+fig = plt.figure()
+ax = fig.add_subplot(111, projection='3d')
+
+ax.scatter(data[:,0], data[:,1], data[:,2], c='r', marker='o')
+plt.xlim([-0.5, 1.5])
+plt.ylim([-0.5, 1.5])
+ax.set_zlim(-0.5, 1.5)
+ax.set_xlabel('X Label')
+ax.set_ylabel('Y Label')
+ax.set_zlabel('Z Label')
+
+plt.show()
+

datasets/guilherme_datasets/scripts/test_zhulei.py

@@ -0,0 +1,29 @@
+#!/usr/bin/python
+import numpy as np
+from scipy.stats import multivariate_normal as mvn
+import matplotlib.pyplot as plt
+from scipy.stats import multivariate_normal as mvn
+from mpl_toolkits.mplot3d import Axes3D
+
+
+sample = np.random.random((2, 50))*100
+
+cov = np.cov(sample)
+mean = np.mean(sample, 1)
+x, y = np.mgrid[(mean[0]-10):(mean[0]+10):1.0, (mean[1]-10):(mean[1]+10):1.0]
+
+
+x = x.reshape(400, 1)[:, 0]
+y = y.reshape(400, 1)[:, 0]
+
+test = [[x[i], y[i]] for i in range(400)]
+
+h = mvn.pdf(test, mean, cov)
+
+fig = plt.figure()
+ax = Axes3D(fig)
+ax.scatter3D(x, y, h)
+# ax.plot_surface(x, y, h)
+# ax.plot_surface(x, y, h, color='b')
+
+plt.show()

datasets/guilherme_datasets/scripts/visulization.py

@@ -0,0 +1,32 @@
+#!/usr/bin/python
+import numpy as np
+import matplotlib.pyplot as plt
+import os
+from sklearn.externals import joblib
+
+joint_num = 3
+datasets_path = '../datasets/tape/'
+datasets_raw = joblib.load(os.path.join(datasets_path, 'pkl/datasets_raw.pkl'))
+datasets_filtered = joblib.load(os.path.join(datasets_path, 'pkl/datasets_filtered.pkl'))
+num_demo = 15
+publish_rate = 50.0
+info = 'left_hand'
+
+fig = plt.figure(0)
+fig.suptitle('the raw data of ' + info)
+for demo_idx in range(num_demo):
+    for joint_idx in range(joint_num):
+        ax = fig.add_subplot(joint_num, 1, 1 + joint_idx)
+        data = datasets_raw[0][demo_idx][info][:, joint_idx]
+        plt.plot(np.array(range(len(data)))/publish_rate, data)
+
+# plot the filtered data
+fig = plt.figure(2)
+fig.suptitle('the filtered data of ' + info)
+for demo_idx in range(num_demo):
+    for joint_idx in range(joint_num):
+        ax = fig.add_subplot(joint_num, 1, 1 + joint_idx)
+        data = datasets_filtered[0][demo_idx][info][:, joint_idx]
+        plt.plot(np.array(range(len(data)))/publish_rate, data)
+
+plt.show()

datasets/guilherme_datasets/taskProMP.mat

@@ -2,3 +2,8 @@
 data_index0: 0, 1, 2, 3, 10, 23, 24
 data_index1: 0, 1, 5, 7, 8, 14, 24
 data_index2: 0, 12, 14, 15, 19, 22, 23
+
+[index_12]
+data_index0: 0, 1, 3, 10, 11, 14, 17, 18, 21, 22, 23, 24
+data_index1: 0, 1, 2, 3, 4, 5, 6, 7, 8, 10, 14, 24
+data_index2: 0, 1, 2, 3, 4, 5, 6, 8, 15, 19, 22, 23

datasets/handover_20171228/info/cfg/datasets.cfg

@@ -1,4 +1,3 @@
 [index_12]
 data_index0: 0,1,3,4,14,15,17,18,19,21,22,23,25,27,28
-#5,6,7,8,9,13
-# 0,1,3,4,5,6,7,8,9,13,14,15,17,18,19,21,22,23,25,27,28
+#5,6,7,8,9,13