finalizing

script/controller.py

@@ -27,14 +27,51 @@ JOINTS = ('ShoulderPitch', 'ShoulderRoll', 'ElbowYaw', 'ElbowRoll')
 NAO_ARM = 0.22
 
 
+def xyz(joint):
+    return np.array(mp.getPosition(joint, FRAME_TORSO, False),
+                    dtype=np.float64)[:3]
+
+
+crt_xyz = {
+    'L': xyz('LArm'),
+    'R': xyz('RArm')
+}
+
+
+crt_joint = {
+    'L': np.array(mp.getAngles(['L' + j for j in JOINTS], False),
+                  dtype=np.float64),
+    'R': np.array(mp.getAngles(['R' + j for j in JOINTS], False),
+                  dtype=np.float64)
+}
+
+
+best_guess = {
+    'L': np.random.randn(3, 4),
+    'R': np.random.randn(3, 4)
+}
+
+
 def get_transform(joint):
     return np.array(
-        mp.getTransform(joint, FRAME_TORSO, False)
+        mp.getTransform(joint, FRAME_TORSO, False), dtype=np.float64
     ).reshape((4, 4))
 
 
-def xyz(joint):
-    return np.array(mp.getPosition(joint, FRAME_TORSO, False))[:3]
+def diff_jacobian(side):
+    new_end_xyz = xyz('{}Arm'.format(side))
+    delta_r = new_end_xyz - crt_xyz[side]
+    crt_xyz[side] = new_end_xyz.copy()
+
+    new_joint = np.array(mp.getAngles([side + j for j in JOINTS], False))
+    delta_j = new_joint - crt_joint[side]
+    crt_joint[side] = new_joint.copy()
+
+    J = delta_r[:,None] / delta_j
+    okval = np.isfinite(J)
+    J[~okval] = best_guess[side][~okval]
+    best_guess[side][okval] = J[okval]
+    return J
 
 
 def jacobian(side):
@@ -52,8 +89,8 @@ def jacobian(side):
         axis=1
     )[:3].T
 
-    jacobian = np.cross(axes, end_xyz - xyzs).T
-    return jacobian
+    J = np.cross(axes, end_xyz - xyzs).T
+    return J
 
 
 def to_nao(my_xyz, side):
@@ -67,19 +104,27 @@ def to_nao(my_xyz, side):
 def inv_jacobian(j):
     u, s, vt = np.linalg.svd(j)
     s_inv = np.zeros((vt.shape[0], u.shape[1]))
-    np.fill_diagonal(s_inv, 1 // s)
+    np.fill_diagonal(s_inv, 1 / s)
     s_inv[np.abs(s_inv) > 30] = 0
     return vt.T.dot(s_inv).dot(u.T)
 
 
-def our_cartesian(my_xyz, side):
+def _some_cartesian(my_xyz, side, jfunc):
     nao_xyz = to_nao(my_xyz, side)
     delta_r = 0.1 * (nao_xyz - xyz('{}Arm'.format(side)))
     crt_q = mp.getAngles([side + j for j in JOINTS], False)
-    delta_q =  inv_jacobian(jacobian(side)).dot(delta_r).flatten()
+    delta_q =  inv_jacobian(jfunc(side)).dot(delta_r).flatten()
     return crt_q + delta_q
 
 
+def our_cartesian(my_xyz, side):
+    return _some_cartesian(my_xyz, side, jacobian)
+
+
+def our_diff_cartesian(my_xyz, side):
+    return _some_cartesian(my_xyz, side, diff_jacobian)
+
+
 def _elbow(arm_):
     quot = min(1.0, arm_ / 0.70)
     return radians(180 * (1 - quot))