From f10e9d3ca74f3794cdc6a255997012b7651fd9db Mon Sep 17 00:00:00 2001
From: Pavel Lutskov <pavel.lutskov@tum.de>
Date: Fri, 25 Jan 2019 17:09:55 +0100
Subject: [PATCH] imitation works for 2dof in arms

---
 script/imitator.py | 22 +++++++++++++++-------
 1 file changed, 15 insertions(+), 7 deletions(-)

diff --git a/script/imitator.py b/script/imitator.py
index 4dc46f4..28b37f3 100755
--- a/script/imitator.py
+++ b/script/imitator.py
@@ -1,11 +1,10 @@
 #! /usr/bin/env python
 import os
 from time import sleep
-from math import atan, degrees, radians
+from math import atan, asin, radians, sqrt
 
 import rospy
 import tf
-import numpy as np
 from naoqi import ALProxy
 
 from controller import inform_controller_factory
@@ -21,7 +20,8 @@ if __name__ == '__main__':
     ll = tf.TransformListener()
     mp = ALProxy('ALMotion', os.environ['NAO_IP'], 9559)
     mp.wakeUp()
-    mp.setAngles(['LElbowRoll', 'RElbowRoll'], [0, 0], 0.5)
+    mp.setAngles(['LElbowRoll', 'RElbowRoll'],
+                 [radians(-50), radians(50)], 0.5)
 
     while not rospy.is_shutdown():
         sleep(0.1)
@@ -39,14 +39,22 @@ if __name__ == '__main__':
             permission = _inform_controller('imitate')
             if not permission:
                 continue
-            roll = atan(trans[1] / abs(trans[0]))
+            sign = 1 if eff == 'L' else -1
+            roll = asin(trans[1] /
+                        sqrt(trans[0]**2 + trans[1]**2 + trans[2]**2))
+            roll -= sign * radians(25)
             pitch = atan(-trans[2] / abs(trans[0]))
 
             # sign = 1 if roll > 0 else -1
             # roll -= sign * radians(10)
-            print degrees(roll)
-            mp.setAngles(['{}ShoulderRoll'.format(eff),
-                          '{}ShoulderPitch'.format(eff)], [roll, pitch], 0.3)
+            # print degrees(roll)
+            mp.setAngles([
+                '{}ShoulderRoll'.format(eff),
+                '{}ShoulderPitch'.format(eff)
+            ], [
+                roll,
+                pitch
+            ], 0.3)
             # targ = np.array(trans)
             # targ = targ / np.linalg.norm(targ) * 0.3
             # mp.setPositions(eff, _FRAME_TORSO,