diff --git a/CMakeLists.txt.user b/CMakeLists.txt.user
new file mode 100644
index 0000000..9a1fa5b
--- /dev/null
+++ b/CMakeLists.txt.user
@@ -0,0 +1,183 @@
+
+
+
+
+
+ ProjectExplorer.Project.ActiveTarget
+ 0
+
+
+ ProjectExplorer.Project.EditorSettings
+
+ true
+ false
+ true
+
+ Cpp
+
+ CppGlobal
+
+
+
+ QmlJS
+
+ QmlJSGlobal
+
+
+ 2
+ UTF-8
+ false
+ 4
+ false
+ true
+ 1
+ true
+ 0
+ true
+ 0
+ 8
+ true
+ 1
+ true
+ true
+ true
+ false
+
+
+
+ ProjectExplorer.Project.PluginSettings
+
+
+
+ ProjectExplorer.Project.Target.0
+
+ Desktop
+ Desktop
+ {52dc460e-6709-4680-9540-0b7a75df0c6e}
+ 0
+ 0
+ 0
+
+ false
+ /home/hrs_d/catkin_ws/src/build-teleoperation-Desktop-Default
+
+
+
+
+ false
+ false
+ true
+ Make
+
+ CMakeProjectManager.MakeStep
+
+ 1
+ Build
+
+ ProjectExplorer.BuildSteps.Build
+
+
+
+ clean
+
+ true
+ false
+ true
+ Make
+
+ CMakeProjectManager.MakeStep
+
+ 1
+ Clean
+
+ ProjectExplorer.BuildSteps.Clean
+
+ 2
+ false
+
+ Default
+ Default
+ CMakeProjectManager.CMakeBuildConfiguration
+
+ 1
+
+
+ 0
+ Deploy
+
+ ProjectExplorer.BuildSteps.Deploy
+
+ 1
+ Deploy locally
+
+ ProjectExplorer.DefaultDeployConfiguration
+
+ 1
+
+
+
+ false
+ false
+ false
+ false
+ true
+ 0.01
+ 10
+ true
+ 1
+ 25
+
+ 1
+ true
+ false
+ true
+ valgrind
+
+ 0
+ 1
+ 2
+ 3
+ 4
+ 5
+ 6
+ 7
+ 8
+ 9
+ 10
+ 11
+ 12
+ 13
+ 14
+
+ 2
+
+
+
+ false
+ %{buildDir}
+ Custom Executable
+
+ ProjectExplorer.CustomExecutableRunConfiguration
+ 3768
+ true
+ false
+ false
+ false
+ true
+
+ 1
+
+
+
+ ProjectExplorer.Project.TargetCount
+ 1
+
+
+ ProjectExplorer.Project.Updater.EnvironmentId
+ {9ce0fc2b-7b92-4c88-a61a-8b1884a0af3f}
+
+
+ ProjectExplorer.Project.Updater.FileVersion
+ 15
+
+
diff --git a/launch/rviz_config/test.rviz b/launch/rviz_config/test.rviz
new file mode 100644
index 0000000..77b9627
--- /dev/null
+++ b/launch/rviz_config/test.rviz
@@ -0,0 +1,51 @@
+
+< robot name =" robot0 " >
+
+< link name =" world " / >
+
+< joint name =" joint_0 " type =" fixed " >
+< parent link =" world "/ >
+< child link =" link_0 "/ >
+
+
+< link name =" link_0 " >
+< visual >
+< geometry >
+< cylinder length ="0.6" radius ="0.2"/ >
+
+< material name =" gray " >
+< color rgba ="0.5 0.5 0.5 1"/ >
+
+
+
+
+< joint name =" joint_1 " type =" revolute " >
+< origin xyz ="0 0 0.35" rpy ="1.57079632679 0 0"/ >
+< parent link =" link_0 "/ >
+< child link =" link_1 "/ >
+< limit effort ="30" velocity ="1.0" lower =" -3.1415926535897931" upper
+="3 .141 59265 3589 7931 " / >
+< axis xyz ="0 1 0"/ >
+
+
+< link name =" link_1 " >
+< visual >
+< origin xyz ="0 0 0.35" rpy ="0 0 0"/ >
+< geometry >
+< box size ="0.1 0.1 0.7" / >
+
+< material name =" red " >
+< color rgba ="0.5 0.0 0.0 1"/ >
+
+
+
+
+< joint name =" end_efector_joint " type =" fixed " >
+< origin xyz ="0 0 0.7" rpy ="1.57079632679 0 0"/ >
+< parent link =" link_1 "/ >
+< child link =" end_effector "/ >
+
+
+< link name =" end_effector " / >
+
+
diff --git a/script/cartesian_controller.py b/script/cartesian_controller.py
new file mode 100755
index 0000000..5463b9c
--- /dev/null
+++ b/script/cartesian_controller.py
@@ -0,0 +1,94 @@
+#! /usr/bin/env python
+
+import os
+
+import rospy
+import numpy as np
+
+import sys
+from naoqi import ALProxy
+import motion
+
+motionProxy = 0
+
+def get_transform(joint):
+
+ frame = motion.FRAME_TORSO
+ useSensorValues = True
+ result = motionProxy.getTransform(joint,frame,useSensorValues)
+ result = np.matrix(result)
+ print result
+ result = np.reshape(result, (4,4))
+ print result
+ return result
+
+
+def cartesian_position(joint):
+ print 'function'
+ frame = motion.FRAME_TORSO
+ useSensorValues = True
+ result = motionProxy.getPosition(joint, frame, useSensorValues)
+ #print result
+ return np.array(result[:3])
+
+
+def jacobian():
+
+ # get current positions
+ end_position = cartesian_position('LArm')
+
+ shoulder_position = cartesian_position('LShoulderPitch')
+ bicep_position = cartesian_position('LShoulderRoll')
+ elbow_position = cartesian_position('LElbowYaw')
+ forearm_position = cartesian_position('LElbowRoll')
+
+ # get transformed rotation axes
+
+ x_axis = np.array([[1, 0, 0, 1]]).T
+ y_axis = np.array([[0, 1, 0, 1]]).T
+ z_axis = np.array([[0, 0, 1, 1]]).T
+
+ #print np.shape(x_axis)
+ #print np.shape(get_transform('LShoulderPitch'))
+
+ shoulder_axis = get_transform('LShoulderPitch').dot(y_axis)
+ bicep_axis = get_transform('LShoulderRoll').dot(z_axis)
+ elbow_axis = get_transform('LElbowYaw').dot(x_axis)
+ forearm_axis = get_transform('LElbowRoll').dot(z_axis)
+
+ # get basis vectors of jacobian
+
+ shoulder_basis = np.cross(shoulder_axis[:3].flatten(), end_position - shoulder_position)
+ bicep_basis = np.cross(bicep_axis[:3].flatten(), end_position - bicep_position)
+ elbow_basis = np.cross(elbow_axis[:3].flatten(), end_position - elbow_position)
+ forearm_basis = np.cross(forearm_axis[:3].flatten(), end_position - forearm_position)
+
+ print shoulder_basis.T
+
+ # build jacobian matrix
+ jacobian = np.concatenate([shoulder_basis, bicep_basis, elbow_basis, forearm_basis], axis=0).T
+
+
+ print np.shape(jacobian)
+ print 'jacobian'
+ print jacobian
+
+ return jacobian
+
+
+def pseudo_inverse(jacobian):
+
+ return np.linalg.pinv(jacobian)
+
+
+if __name__ == '__main__':
+
+ motionProxy = ALProxy("ALMotion", os.environ['NAO_IP'], 9559)
+ jacob = jacobian()
+ print jacob
+ jacob = pseudo_inverse(jacob)
+ print(jacob)
+
+ #rospy.init_node('cartesian_controller')
+ #rospy.spin()
+
diff --git a/src/NAO_Jacobian.cpp b/src/NAO_Jacobian.cpp
new file mode 100644
index 0000000..764e11c
--- /dev/null
+++ b/src/NAO_Jacobian.cpp
@@ -0,0 +1,60 @@
+#include
+using namespace std;
+// Calculate the Jacobian Matrix for the NAO
+// inputs: original angles tau1 - tau4 and new angles tau1' - tau4'
+// original endeffector position e1 - e4 and new endeffector position e1' - e4'
+
+typedef struct position{
+ float x;
+ float y;
+ float z;
+}position;
+
+typedef struct angles{
+ float tau_1;
+ float tau_2;
+ float tau_3;
+ float tau_4;
+}angles;
+
+angles a_end, a, a_diff;
+position e_end, e, e_diff;
+
+e_diff = diff(e_end, e);
+a_diff = diff(a_end, a);
+
+vector postion_vec;
+postion_vec.push_back(e_diff.x);
+postion_vec.push_back(e_diff.y);
+postion_vec.push_back(e_diff.z);
+vector angles_vec;
+angles_vec.push_back(a_diff.tau_1);
+angles_vec.push_back(a_diff.tau_2);
+angles_vec.push_back(a_diff.tau_3);
+angles_vec.push_back(a_diff.tau_4);
+vector> Jacobian;
+
+
+for (int i = 0; i<3; i++) {
+ for(int j = 0; j<4; j++ ) {
+ Jacobian[i][j] = postion_vec[i]/angles_vec[j];
+ }
+}
+
+position diff(position end, position actual){
+ position temp;
+ temp.x = end.x - actual.x;
+ temp.y = end.y - actual.y;
+ temp.z = end.z - actual.z;
+ return temp;
+}
+
+angles diff(angles end, angles actual){
+ angles temp;
+ temp.tau_1 = end.tau_1 - actual.tau_1;
+ temp.tau_2 = end.tau_2 - actual.tau_2;
+ temp.tau_3 = end.tau_3 - actual.tau_3;
+ temp.tau_4 = end.tau_4 - actual.tau_4;
+
+ return temp;
+}