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

refactored the code, fixed config bug

Browse Source
This commit is contained in:
Pavel Lutskov
2019-02-08 16:42:48 +01:00
parent 949f657a75
commit 8861d215c5
11 changed files with 1022 additions and 1064 deletions
Download Patch File Download Diff File Expand all files Collapse all files

6
script/controller.py
View File

@@ -5,12 +5,12 @@ import json
from math import asin, atan, radians
import numpy as np
from naoqi import ALProxy
from masterloop import mp
FRAME_TORSO = 0
K = 0.1
mp = ALProxy('ALMotion', os.environ['NAO_IP'], 9559)
_here = os.path.dirname(os.path.realpath(__file__))
with open('{}/../config/default.yaml'.format(_here)) as f:
@@ -63,7 +63,7 @@ def our_cartesian(my_xyz, side):
nao_xyz = to_nao(my_xyz, side)
delta_r = K * (nao_xyz - xyz('{}Arm'.format(side)))
crt_q = mp.getAngles([side + j for j in JOINTS], False)
delta_q = np.linalg.pinv(jacobian()).dot(delta_r).flatten()
delta_q = np.linalg.pinv(jacobian(side)).dot(delta_r).flatten()
return crt_q + delta_q