93 lines
2.3 KiB
Python
Executable File
93 lines
2.3 KiB
Python
Executable File
#! /usr/bin/env python
|
|
"""Controller should execute control for a given effector"""
|
|
import os
|
|
import json
|
|
from math import asin, atan, radians
|
|
|
|
import numpy as np
|
|
from naoqi import ALProxy
|
|
|
|
|
|
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:
|
|
_xxx = json.load(f)
|
|
|
|
ARM = _xxx['arm']
|
|
MY_SHOULDERS = _xxx['should']
|
|
JOINTS = ('ShoulderPitch', 'ShoulderRoll', 'ElbowYaw', 'ElbowRoll')
|
|
NAO_ARM = 0.22
|
|
|
|
|
|
def get_transform(joint):
|
|
return np.array(
|
|
mp.getTransform(joint, FRAME_TORSO, False)
|
|
).reshape((4, 4))
|
|
|
|
|
|
def xyz(joint):
|
|
return np.array(mp.getPosition(joint, FRAME_TORSO, False))[:3]
|
|
|
|
|
|
def jacobian(side):
|
|
end_xyz = xyz('{}Arm'.format(side))
|
|
xyzs = np.array([xyz(side + j) for j in JOINTS])
|
|
|
|
x_axis = np.array([[1, 0, 0, 1]]).T
|
|
y_axis = np.array([[0, 1, 0, 1]]).T
|
|
z_axis = np.array([[0, 0, 1, 1]]).T
|
|
|
|
ax_order = (y_axis, z_axis, x_axis, z_axis)
|
|
|
|
axes = np.concatenate(
|
|
[get_transform(side + j).dot(ax) for j, ax in zip(JOINTS, ax_order)],
|
|
axis=1
|
|
)[:3].T
|
|
|
|
jacobian = np.cross(axes, end_xyz - xyzs).T
|
|
return jacobian
|
|
|
|
|
|
def to_nao(my_xyz, side):
|
|
sh_offs = np.array(MY_SHOULDERS[side])
|
|
my_sh_xyz = my_xyz - sh_offs
|
|
nao_sh_xyz = my_sh_xyz / ARM * NAO_ARM
|
|
nao_xyz = nao_sh_xyz + xyz(side + 'Shoulder')
|
|
return nao_xyz
|
|
|
|
|
|
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()
|
|
return crt_q + delta_q
|
|
|
|
|
|
def nao_cartesian(my_xyz, side):
|
|
pass
|
|
|
|
|
|
def _elbow(arm_):
|
|
quot = min(1.0, arm_ / ARM)
|
|
return radians(180 * (1 - quot))
|
|
|
|
|
|
def dumb(my_xyz, side):
|
|
sign = 1 if side == 'L' else -1
|
|
roll = asin(my_xyz[1] / np.linalg.norm(my_xyz))
|
|
roll -= sign * radians(25)
|
|
pitch = atan(-my_xyz[2] / np.abs(my_xyz[0]))
|
|
|
|
eroll = -sign * _elbow(np.linalg.norm(my_xyz))
|
|
eyaw = mp.getAngles(['{}ElbowYaw'.format(side)], False)[0]
|
|
return np.array([pitch, roll, eyaw, eroll])
|
|
|
|
|
|
def movement(my_xyz, side, control):
|
|
ref = control(np.array(my_xyz), side).tolist()
|
|
mp.setAngles([side + j for j in JOINTS], ref, 0.3)
|