merge with rviz branch

script/cartesian_controller.py

@@ -0,0 +1,139 @@
+#! /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
+    jac_mat = jacobian()   
+
+    # use jacobian to compute desired joint speed
+    
+    derivative_speed = (p_d - end_position) / 5
+    inv_jac = pseudo_inverse(jac_mat)
+    angular_velocity = derivative_speed * inv_jac
+    
+        
+    # integrate over desired speed to get desired joint position
+    
+    names         = "LArm"
+    useSensors    = False
+    commandAnglesLArm = motionProxy.getAngles(names, useSensors)
+
+    #names         = "RArm"
+    #useSensors    = False
+    #commandAngles = motionProxy.getAngles(names, useSensors)
+
+    goal_angleL = commandAnglesLArm + (angular_velocity * 5)
+    
+
+    # return desired joint position and speed
+    
+    
+
+    return goal_angleL, commandAnglesLArm
+
+
+
+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()
+