DOCUMENT IT

script/controller.py

@@ -1,5 +1,6 @@
 #! /usr/bin/env python
-"""Controller should execute control for a given effector"""
+"""Module for executing control for a given effector"""
+
 from __future__ import division
 import os
 import json
@@ -53,12 +54,14 @@ best_guess = {
 
 
 def get_transform(joint):
+    """Retrieve a transform matrix of a joint in the torso frame."""
     return np.array(
         mp.getTransform(joint, FRAME_TORSO, False), dtype=np.float64
     ).reshape((4, 4))
 
 
 def diff_jacobian(side):
+    """Update and return the differential Jacobian. NOT GOOD."""
     new_end_xyz = xyz('{}Arm'.format(side))
     delta_r = new_end_xyz - crt_xyz[side]
     crt_xyz[side] = new_end_xyz.copy()
@@ -75,6 +78,7 @@ def diff_jacobian(side):
 
 
 def jacobian(side):
+    """Calculate the analytical Jacobian for side 'L' or 'R'."""
     end_xyz = xyz('{}Arm'.format(side))
     xyzs = np.array([xyz(side + j) for j in JOINTS])
 
@@ -94,6 +98,7 @@ def jacobian(side):
 
 
 def to_nao(my_xyz, side):
+    """Transform object coordinates from operator chest frame to NAO torso."""
     sh_offs = np.array(MY_SHOULDERS[side])
     my_sh_xyz = my_xyz - sh_offs
     nao_sh_xyz = my_sh_xyz / ARM * NAO_ARM
@@ -102,6 +107,7 @@ def to_nao(my_xyz, side):
 
 
 def inv_jacobian(j):
+    """Singluarity robust inverse Jacobian."""
     u, s, vt = np.linalg.svd(j)
     s_inv = np.zeros((vt.shape[0], u.shape[1]))
     np.fill_diagonal(s_inv, 1 / s)
@@ -110,6 +116,22 @@ def inv_jacobian(j):
 
 
 def _some_cartesian(my_xyz, side, jfunc):
+    """A generic cartesian controller.
+
+    Parameters
+    ----------
+    my_xyz : numpy.array
+        Coordinates of the object/target in the operator chest frame.
+    side : str, 'L' or 'R'
+    jfunc : func
+        A function that will return a jacobian matrix for the given side.
+
+    Returns
+    -------
+    numpy.array
+        The control to execute in the joint space.
+
+    """
     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)
@@ -118,19 +140,23 @@ def _some_cartesian(my_xyz, side, jfunc):
 
 
 def our_cartesian(my_xyz, side):
+    """Our cartesian controller."""
     return _some_cartesian(my_xyz, side, jacobian)
 
 
 def our_diff_cartesian(my_xyz, side):
+    """Our differential cartesian controller."""
     return _some_cartesian(my_xyz, side, diff_jacobian)
 
 
 def _elbow(arm_):
+    "Dumb way to calculate the elbow roll."""
     quot = min(1.0, arm_ / 0.70)
     return radians(180 * (1 - quot))
 
 
 def dumb(my_xyz, side):
+    """Our dumb controller that directly maps 3D coords into joint space."""
     sign = 1 if side == 'L' else -1
     roll = asin(my_xyz[1] / np.linalg.norm(my_xyz))
     roll -= sign * radians(25)
@@ -142,11 +168,24 @@ def dumb(my_xyz, side):
 
 
 def movement(my_xyz, side, control):
+    """Execute the movement.
+
+    my_xyz : numpy.array
+        Coordinates of the object/target in the operator chest frame.
+    side : str, 'L' or 'R'
+    control : func
+        A function to calculate the conrol for the side, depending on the
+        target coordinates. It must return returning the `numpy.array`
+        joint vector of format
+        [ShoulderPitch, ShoulderRoll, ElbowYaw, ElbowRoll].
+
+    """
     ref = control(np.array(my_xyz), side).tolist()
     mp.setAngles([side + j for j in JOINTS], ref, 0.3)
 
 
 def nao_cart_movement(my_arm_xyz, side, *_):
+    """Execute the movement using the NAO cartesian controller."""
     nao_xyz = to_nao(my_arm_xyz, side)
     mp.setPositions('{}Arm'.format(side), FRAME_TORSO,
                     tuple(nao_xyz.tolist()) + (0, 0, 0),