pavel/teleoperation
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

refactored controller code. now going to test it

Browse Source
This commit is contained in:
Pavel Lutskov
2019-02-08 15:02:22 +01:00
parent 98ce555fe9
commit 949f657a75
3 changed files with 75 additions and 90 deletions
Download Patch File Download Diff File Expand all files Collapse all files

78
script/imitator.py
View File

@@ -1,44 +1,30 @@
#! /usr/bin/env python
import os
import json
from math import atan, asin, radians, sqrt
import rospy
import tf
import numpy as np
from naoqi import ALProxy
from masterloop import inform_masterloop_factory
from controller import movement, dumb, mp
_inform_masterloop = inform_masterloop_factory('imitator')
_FRAME_TORSO = 0
arm = None
def _elbow(arm_):
quot = min(1.0, arm_ / arm)
return radians(180 * (1 - quot))
TORSO = False
if __name__ == '__main__':
rospy.init_node('imitator')
rospy.wait_for_service('inform_masterloop')
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():
rospy.Rate(10).sleep()
@@ -46,24 +32,20 @@ if __name__ == '__main__':
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):
if TORSO:
try:
trans, _ = ll.lookupTransform(
_, 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:
my_arm_xyz, _ = ll.lookupTransform(
'Aruco_0_frame',
'Aruco_{}_frame'.format(i),
rospy.Time(0)
@@ -72,28 +54,6 @@ if __name__ == '__main__':
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
movement(my_arm_xyz, side, dumb)
mp.rest()