57 lines
1.7 KiB
Python
Executable File
57 lines
1.7 KiB
Python
Executable File
#! /usr/bin/env python
|
|
import os
|
|
from time import sleep
|
|
from math import atan, degrees, radians
|
|
|
|
import rospy
|
|
import tf
|
|
import numpy as np
|
|
from naoqi import ALProxy
|
|
|
|
from controller import inform_controller_factory
|
|
|
|
|
|
_inform_controller = inform_controller_factory('imitator')
|
|
_FRAME_TORSO = 0
|
|
|
|
|
|
if __name__ == '__main__':
|
|
rospy.init_node('imitator')
|
|
rospy.wait_for_service('inform_controller')
|
|
ll = tf.TransformListener()
|
|
mp = ALProxy('ALMotion', os.environ['NAO_IP'], 9559)
|
|
mp.wakeUp()
|
|
mp.setAngles(['LElbowRoll', 'RElbowRoll'], [0, 0], 0.5)
|
|
|
|
while not rospy.is_shutdown():
|
|
sleep(0.1)
|
|
for i, eff in enumerate(['L',
|
|
'R'], 1):
|
|
try:
|
|
trans, _ = ll.lookupTransform(
|
|
'Aruco_0_frame',
|
|
'Aruco_{}_frame'.format(i),
|
|
rospy.Time(0)
|
|
)
|
|
except Exception as e:
|
|
print e
|
|
continue
|
|
permission = _inform_controller('imitate')
|
|
if not permission:
|
|
continue
|
|
roll = atan(trans[1] / abs(trans[0]))
|
|
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)
|
|
# targ = np.array(trans)
|
|
# targ = targ / np.linalg.norm(targ) * 0.3
|
|
# mp.setPositions(eff, _FRAME_TORSO,
|
|
# targ.tolist() + [0, 0, 0], 0.5, 7)
|
|
# print eff, targ
|
|
|
|
mp.rest()
|