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

imitation works for 2dof in arms

Browse Source
This commit is contained in:
Pavel Lutskov
2019-01-25 17:09:55 +01:00
parent fd36b56631
commit f10e9d3ca7
1 changed files with 15 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files

22
script/imitator.py
View File

@@ -1,11 +1,10 @@
#! /usr/bin/env python #! /usr/bin/env python
import os import os
from time import sleep from time import sleep
from math import atan, degrees, radians from math import atan, asin, radians, sqrt
import rospy import rospy
import tf import tf
import numpy as np
from naoqi import ALProxy from naoqi import ALProxy
from controller import inform_controller_factory from controller import inform_controller_factory
@@ -21,7 +20,8 @@ if __name__ == '__main__':
ll = tf.TransformListener() ll = tf.TransformListener()
mp = ALProxy('ALMotion', os.environ['NAO_IP'], 9559) mp = ALProxy('ALMotion', os.environ['NAO_IP'], 9559)
mp.wakeUp() mp.wakeUp()
mp.setAngles(['LElbowRoll', 'RElbowRoll'], [0, 0], 0.5) mp.setAngles(['LElbowRoll', 'RElbowRoll'],
[radians(-50), radians(50)], 0.5)
while not rospy.is_shutdown(): while not rospy.is_shutdown():
sleep(0.1) sleep(0.1)
@@ -39,14 +39,22 @@ if __name__ == '__main__':
permission = _inform_controller('imitate') permission = _inform_controller('imitate')
if not permission: if not permission:
continue continue
roll = atan(trans[1] / abs(trans[0])) 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])) pitch = atan(-trans[2] / abs(trans[0]))
# sign = 1 if roll > 0 else -1 # sign = 1 if roll > 0 else -1
# roll -= sign * radians(10) # roll -= sign * radians(10)
print degrees(roll) # print degrees(roll)
mp.setAngles(['{}ShoulderRoll'.format(eff), mp.setAngles([
'{}ShoulderPitch'.format(eff)], [roll, pitch], 0.3) '{}ShoulderRoll'.format(eff),
'{}ShoulderPitch'.format(eff)
], [
roll,
pitch
], 0.3)
# targ = np.array(trans) # targ = np.array(trans)
# targ = targ / np.linalg.norm(targ) * 0.3 # targ = targ / np.linalg.norm(targ) * 0.3
# mp.setPositions(eff, _FRAME_TORSO, # mp.setPositions(eff, _FRAME_TORSO,