diff --git a/.gitignore b/.gitignore
index b5da835..c09a496 100644
--- a/.gitignore
+++ b/.gitignore
@@ -10,5 +10,8 @@ experiments.py
 # Gedit stuff
 *~
 
+# QTCreator Stuff?
+CMakeLists.txt.user
+
 # Ignore PDFs on master
 literature/
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 @@
+<? xml version ="1.0"? >
+< robot name =" robot0 " >
+
+< link name =" world " / >
+
+< joint name =" joint_0 " type =" fixed " >
+< parent link =" world "/ >
+< child link =" link_0 "/ >
+</ joint >
+
+< link name =" link_0 " >
+< visual >
+< geometry >
+< cylinder length ="0.6" radius ="0.2"/ >
+</ geometry >
+< material name =" gray " >
+< color rgba ="0.5 0.5 0.5 1"/ >
+</ material >
+</ visual >
+</ link >
+
+< 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"/ >
+</ joint >
+
+< link name =" link_1 " >
+< visual >
+< origin xyz ="0 0 0.35" rpy ="0 0 0"/ >
+< geometry >
+< box size ="0.1 0.1 0.7" / >
+</ geometry >
+< material name =" red " >
+< color rgba ="0.5 0.0 0.0 1"/ >
+</ material >
+</ visual >
+</ link >
+
+< 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 "/ >
+</ joint >
+
+< link name =" end_effector " / >
+
+</ robot >
diff --git a/script/cartesian_controller.py b/script/cartesian_controller.py
new file mode 100755
index 0000000..ec584d0
--- /dev/null
+++ b/script/cartesian_controller.py
@@ -0,0 +1,120 @@
+#! /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/ accordint to control figure these values should actually come from the
+    # integration step in the previous first control loop
+
+    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, transformation to torso frame
+
+    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
+    
+    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)
+
+    # build jacobian matrix
+    jacobian = np.concatenate([shoulder_basis, bicep_basis, elbow_basis, forearm_basis], axis=0).T
+
+    return jacobian
+
+
+def pseudo_inverse(jacobian):
+    return np.linalg.pinv(jacobian)
+
+
+def reference_generator(p_d)
+
+    # calculate jacobian
+
+    # use jacobian to compute desired joint speed
+
+    # integrate over desired speed to get desired joint position
+
+    # return desired joint position and speed
+
+    return
+
+
+def movement(e)
+
+    # scale joint states with matrix K
+
+    # add desired joint speed
+
+    # move robot arm    
+
+    return
+ 
+
+if __name__ == '__main__':
+
+    motionProxy = ALProxy("ALMotion", os.environ['NAO_IP'], 9559)
+    jacob = jacobian()
+    print jacob
+    jacob = pseudo_inverse(jacob)
+    print(jacob)
+
+    # given new desired coordinates
+
+    e = 1;
+    """ 
+    while e bigger some value
+
+        # run reference generator to get desired joint postion and speed
+
+        # subtract current joint states
+
+        # movement
+ 
+    """    
+    #rospy.init_node('cartesian_controller')
+    #rospy.spin()
+