did some cartesian control

script/cartesian_controller.py

@@ -1,127 +0,0 @@
-#! /usr/bin/env python
-import os
-
-import rospy
-import numpy as np
-
-from naoqi import ALProxy
-import motion
-
-
-mp = None
-
-
-def get_transform(joint):
-    frame = motion.FRAME_TORSO
-    useSensorValues = True
-    result = mp.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 = mp.getPosition(joint, frame, useSensorValues)
-    #print result
-    return np.array(result[:3])
-
-
-def jacobian():
-    # get current positions
-    # according 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__':
-    mp = 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_masterloop')
-    #rospy.spin()
-

script/controller.py

@@ -0,0 +1,90 @@
+#! /usr/bin/env python
+"""Controller should execute control for a given effector"""
+import os
+
+# import rospy
+import numpy as np
+from naoqi import ALProxy
+
+
+FRAME_TORSO = 0
+K = 0.1
+mp = ALProxy('ALMotion', os.environ['NAO_IP'], 9559)
+mp.wakeUp()
+
+
+def _get_transform(joint):
+    return np.array(
+        mp.getTransform(joint, FRAME_TORSO, False)
+    ).reshape((4, 4))
+
+
+def _xyz(joint):
+    return np.array(mp.getPosition(joint, FRAME_TORSO, False))[:3]
+
+
+def jacobian():
+    # get current positions
+    # according to control figure these values should actually come
+    # from the integration step in the previous first control loop
+
+    end_position = _xyz('LArm')
+    shoulder_position = _xyz('LShoulderPitch')
+    bicep_position = _xyz('LShoulderRoll')
+    elbow_position = _xyz('LElbowYaw')
+    forearm_position = _xyz('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)
+
+    xyzs = np.array([shoulder_position, bicep_position, elbow_position,
+                     forearm_position])
+
+    axes = np.concatenate(
+        [shoulder_axis, bicep_axis, elbow_axis, forearm_axis],
+        axis=1
+    )[:3].T
+
+    # get basis vectors of jacobian
+
+    jacobian = np.cross(axes, end_position - xyzs).T
+
+    # 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(xyz):
+    delta = K * (xyz - _xyz('LArm'))
+    return np.linalg.pinv(jacobian()).dot(delta).flatten()
+
+
+def movement(xyz):
+    ref = reference_generator(np.array(xyz)).tolist()
+    mp.changeAngles(['LShoulderPitch', 'LShoulderRoll',
+                     'LElbowYaw', 'LElbowRoll'], ref, 0.2)

script/imitator.py

@@ -1,7 +1,6 @@
 #! /usr/bin/env python
 import os
 import json
-from time import sleep
 from math import atan, asin, radians, sqrt
 
 import rospy
@@ -42,7 +41,7 @@ if __name__ == '__main__':
     arm = x['arm']
 
     while not rospy.is_shutdown():
-        sleep(0.1)
+        rospy.Rate(10).sleep()
         if not _inform_masterloop('imitate'):
             continue
         rospy.logdebug('IMITATOR: ACTIVE')