implemented torso imitation (DANGEROUS!!!)

also fixed usb_cam launch bug

script/imitator.py

@@ -30,6 +30,21 @@ if __name__ == '__main__':
         if not _inform_controller('imitate'):
             continue
         rospy.logdebug('IMITATOR: ACTIVE')
+
+        # torso movement
+
+        try:
+            _, q = ll.lookupTransform('odom',
+                                      'Aruco_0_frame',
+                                      rospy.Time(0))
+            rot = tf.transformations.euler_from_quaternion(q)
+            mp.setAngles(['LHipYawPitch', 'RHipYawPitch'],
+                         [-rot[1], -rot[1]], 0.3)
+        except Exception as e:
+            rospy.logwarn(e)
+
+        # continue
+
         for i, eff in enumerate(['L',
                                  'R'], 1):
             try:

script/walker.py

@@ -10,7 +10,7 @@ from controller import inform_controller_factory
 
 
       #min   #max
-FW = -1.65, -1.45
+FW = -1.70, -1.40
 BK = -2.20, -2.40
 LT =  0.35,  0.53
 RT = -0.35, -0.53
@@ -58,10 +58,6 @@ if __name__ == '__main__':
             _inform_controller('stop')
             continue
 
-        # print trans
-        # print rot
-        # continue
-
         movement = [0, 0, 0]
         ref_vec = trans[:2] + rot[2:]
                                #-1   1  -1   1  -1   1