72 lines
1.9 KiB
Python
Executable File
72 lines
1.9 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)
|
|
|
|
|
|
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
|
|
|
|
jacobian = np.cross(axes, end_position - xyzs).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)
|