60 lines
1.8 KiB
Python
Executable File
60 lines
1.8 KiB
Python
Executable File
#! /usr/bin/env python
|
|
from argparse import ArgumentParser
|
|
|
|
import rospy
|
|
import tf
|
|
import numpy as np
|
|
|
|
from masterloop import inform_masterloop_factory
|
|
from controller import nao_cart_movement, movement, dumb, our_cartesian
|
|
|
|
|
|
_inform_masterloop = inform_masterloop_factory('imitator')
|
|
|
|
|
|
TORSO = False
|
|
|
|
|
|
if __name__ == '__main__':
|
|
rospy.init_node('imitator')
|
|
rospy.wait_for_service('inform_masterloop')
|
|
|
|
ll = tf.TransformListener()
|
|
|
|
while not rospy.is_shutdown():
|
|
rospy.Rate(20).sleep()
|
|
if not _inform_masterloop('imitate'):
|
|
continue
|
|
rospy.logdebug('IMITATOR: ACTIVE')
|
|
|
|
# if TORSO:
|
|
# 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)
|
|
|
|
for i, side in enumerate(['L', 'R'], 1):
|
|
try:
|
|
a0, _ = ll.lookupTransform(
|
|
'odom',
|
|
'Aruco_0_frame',
|
|
rospy.Time(0)
|
|
)
|
|
arm, _ = ll.lookupTransform(
|
|
'odom',
|
|
'Aruco_{}_frame'.format(i),
|
|
rospy.Time(0)
|
|
)
|
|
except Exception as e:
|
|
rospy.logwarn(e)
|
|
continue
|
|
my_arm_xyz = np.array(arm) - np.array(a0)
|
|
# rospy.loginfo('{}'.format(my_arm_xyz))
|
|
# rospy.loginfo('{}'.format(dumb(my_arm_xyz, side)))
|
|
movement(my_arm_xyz, side, dumb)
|