109 lines
2.7 KiB
Python
Executable File
109 lines
2.7 KiB
Python
Executable File
#! /usr/bin/env python
|
|
"""Controller should execute control for a given effector"""
|
|
from __future__ import division
|
|
import os
|
|
import json
|
|
from math import asin, atan, radians
|
|
|
|
import numpy as np
|
|
|
|
from masterloop import mp
|
|
|
|
|
|
FRAME_TORSO = 0
|
|
AXIS_XYZ = 7
|
|
K = 0.1
|
|
|
|
_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 = {
|
|
'L': _xxx['lsh'],
|
|
'R': _xxx['rsh']
|
|
}
|
|
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 inv_jacobian(j):
|
|
u, s, vt = np.linalg.svd(j)
|
|
s_inv = np.zeros((vt.shape[0], u.shape[1]))
|
|
np.fill_diagonal(s_inv, 1 // s)
|
|
s_inv[np.abs(s_inv) > 30] = 0
|
|
return vt.T.dot(s_inv).dot(u.T)
|
|
|
|
|
|
def our_cartesian(my_xyz, side):
|
|
nao_xyz = to_nao(my_xyz, side)
|
|
delta_r = 0.1 * (nao_xyz - xyz('{}Arm'.format(side)))
|
|
crt_q = mp.getAngles([side + j for j in JOINTS], False)
|
|
delta_q = inv_jacobian(jacobian(side)).dot(delta_r).flatten()
|
|
return crt_q + delta_q
|
|
|
|
|
|
def _elbow(arm_):
|
|
quot = min(1.0, arm_ / 0.70)
|
|
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 = -sign * radians(40)
|
|
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)
|
|
|
|
|
|
def nao_cart_movement(my_arm_xyz, side, *_):
|
|
nao_xyz = to_nao(my_arm_xyz, side)
|
|
mp.setPositions('{}Arm'.format(side), FRAME_TORSO,
|
|
tuple(nao_xyz.tolist()) + (0, 0, 0),
|
|
1.0, AXIS_XYZ)
|