65 lines
1.9 KiB
Python
Executable File
65 lines
1.9 KiB
Python
Executable File
#! /usr/bin/env python
|
|
import os
|
|
from time import sleep
|
|
from math import atan, asin, radians, sqrt
|
|
|
|
import rospy
|
|
import tf
|
|
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'],
|
|
[radians(-50), radians(50)], 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
|
|
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)
|
|
# 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()
|