91 lines
2.6 KiB
Python
Executable File
91 lines
2.6 KiB
Python
Executable File
#! /usr/bin/env python
|
|
"""Controller should execute control for a given effector"""
|
|
import os
|
|
|
|
# import rospy
|
|
import numpy as np
|
|
from naoqi import ALProxy
|
|
|
|
|
|
FRAME_TORSO = 0
|
|
K = 0.1
|
|
mp = ALProxy('ALMotion', os.environ['NAO_IP'], 9559)
|
|
mp.wakeUp()
|
|
|
|
|
|
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():
|
|
# get current positions
|
|
# according to control figure these values should actually come
|
|
# from the integration step in the previous first control loop
|
|
|
|
end_position = _xyz('LArm')
|
|
shoulder_position = _xyz('LShoulderPitch')
|
|
bicep_position = _xyz('LShoulderRoll')
|
|
elbow_position = _xyz('LElbowYaw')
|
|
forearm_position = _xyz('LElbowRoll')
|
|
|
|
# get transformed rotation axes, transformation to torso frame
|
|
|
|
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
|
|
|
|
shoulder_axis = _get_transform('LShoulderPitch').dot(y_axis)
|
|
bicep_axis = _get_transform('LShoulderRoll').dot(z_axis)
|
|
elbow_axis = _get_transform('LElbowYaw').dot(x_axis)
|
|
forearm_axis = _get_transform('LElbowRoll').dot(z_axis)
|
|
|
|
xyzs = np.array([shoulder_position, bicep_position, elbow_position,
|
|
forearm_position])
|
|
|
|
axes = np.concatenate(
|
|
[shoulder_axis, bicep_axis, elbow_axis, forearm_axis],
|
|
axis=1
|
|
)[:3].T
|
|
|
|
# get basis vectors of jacobian
|
|
|
|
jacobian = np.cross(axes, end_position - xyzs).T
|
|
|
|
# shoulder_basis = np.cross(shoulder_axis[:3].flatten(),
|
|
# end_position - shoulder_position)
|
|
# bicep_basis = np.cross(bicep_axis[:3].flatten(),
|
|
# end_position - bicep_position)
|
|
# elbow_basis = np.cross(elbow_axis[:3].flatten(),
|
|
# end_position - elbow_position)
|
|
# forearm_basis = np.cross(forearm_axis[:3].flatten(),
|
|
# end_position - forearm_position)
|
|
|
|
# build jacobian matrix
|
|
# jacobian = np.concatenate(
|
|
# [shoulder_basis, bicep_basis, elbow_basis, forearm_basis],
|
|
# axis=0
|
|
# ).T
|
|
|
|
return jacobian
|
|
|
|
|
|
def pseudo_inverse(jacobian):
|
|
return np.linalg.pinv(jacobian)
|
|
|
|
|
|
def reference_generator(xyz):
|
|
delta = K * (xyz - _xyz('LArm'))
|
|
return np.linalg.pinv(jacobian()).dot(delta).flatten()
|
|
|
|
|
|
def movement(xyz):
|
|
ref = reference_generator(np.array(xyz)).tolist()
|
|
mp.changeAngles(['LShoulderPitch', 'LShoulderRoll',
|
|
'LElbowYaw', 'LElbowRoll'], ref, 0.2)
|