101 lines
2.8 KiB
Python
Executable File
101 lines
2.8 KiB
Python
Executable File
#! /usr/bin/env python
|
|
import os
|
|
import json
|
|
from time import sleep
|
|
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
|
|
|
|
|
|
_inform_controller = inform_controller_factory('imitator')
|
|
|
|
|
|
_FRAME_TORSO = 0
|
|
arm = None
|
|
|
|
|
|
def _elbow(arm_):
|
|
quot = min(1.0, arm_ / arm)
|
|
return radians(180 * (1 - quot))
|
|
|
|
|
|
if __name__ == '__main__':
|
|
rospy.init_node('imitator')
|
|
rospy.wait_for_service('inform_controller')
|
|
ll = tf.TransformListener()
|
|
am = ALProxy('ALAutonomousMoves', os.environ['NAO_IP'], 9559)
|
|
am.setExpressiveListeningEnabled(False)
|
|
mp = ALProxy('ALMotion', os.environ['NAO_IP'], 9559)
|
|
mp.wakeUp()
|
|
mp.setAngles(['LElbowRoll', 'RElbowRoll', 'LElbowYaw', 'RElbowYaw'],
|
|
[radians(-70), radians(70), -radians(40), radians(40)], 0.5)
|
|
|
|
here = os.path.dirname(os.path.realpath(__file__))
|
|
with open('{}/../config/default.yaml'.format(here)) as f:
|
|
x = json.load(f)
|
|
|
|
arm = x['arm']
|
|
|
|
while not rospy.is_shutdown():
|
|
sleep(0.1)
|
|
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:
|
|
trans, _ = ll.lookupTransform(
|
|
'Aruco_0_frame',
|
|
'Aruco_{}_frame'.format(i),
|
|
rospy.Time(0)
|
|
)
|
|
except Exception as e:
|
|
rospy.logwarn(e)
|
|
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]))
|
|
|
|
eroll = -sign * _elbow(np.linalg.norm(np.array(trans)))
|
|
|
|
mp.setAngles([
|
|
'{}ShoulderRoll'.format(eff),
|
|
'{}ShoulderPitch'.format(eff),
|
|
'{}ElbowRoll'.format(eff)
|
|
], [
|
|
roll,
|
|
pitch,
|
|
eroll
|
|
], 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()
|