95 lines
2.4 KiB
Python
Executable File
95 lines
2.4 KiB
Python
Executable File
#! /usr/bin/env python
|
|
|
|
import os
|
|
|
|
import rospy
|
|
import numpy as np
|
|
|
|
import sys
|
|
from naoqi import ALProxy
|
|
import motion
|
|
|
|
motionProxy = 0
|
|
|
|
def get_transform(joint):
|
|
|
|
frame = motion.FRAME_TORSO
|
|
useSensorValues = True
|
|
result = motionProxy.getTransform(joint,frame,useSensorValues)
|
|
result = np.matrix(result)
|
|
print result
|
|
result = np.reshape(result, (4,4))
|
|
print result
|
|
return result
|
|
|
|
|
|
def cartesian_position(joint):
|
|
print 'function'
|
|
frame = motion.FRAME_TORSO
|
|
useSensorValues = True
|
|
result = motionProxy.getPosition(joint, frame, useSensorValues)
|
|
#print result
|
|
return np.array(result[:3])
|
|
|
|
|
|
def jacobian():
|
|
|
|
# get current positions
|
|
end_position = cartesian_position('LArm')
|
|
|
|
shoulder_position = cartesian_position('LShoulderPitch')
|
|
bicep_position = cartesian_position('LShoulderRoll')
|
|
elbow_position = cartesian_position('LElbowYaw')
|
|
forearm_position = cartesian_position('LElbowRoll')
|
|
|
|
# get transformed rotation axes
|
|
|
|
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
|
|
|
|
#print np.shape(x_axis)
|
|
#print np.shape(get_transform('LShoulderPitch'))
|
|
|
|
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)
|
|
|
|
# get basis vectors of jacobian
|
|
|
|
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)
|
|
|
|
print shoulder_basis.T
|
|
|
|
# build jacobian matrix
|
|
jacobian = np.concatenate([shoulder_basis, bicep_basis, elbow_basis, forearm_basis], axis=0).T
|
|
|
|
|
|
print np.shape(jacobian)
|
|
print 'jacobian'
|
|
print jacobian
|
|
|
|
return jacobian
|
|
|
|
|
|
def pseudo_inverse(jacobian):
|
|
|
|
return np.linalg.pinv(jacobian)
|
|
|
|
|
|
if __name__ == '__main__':
|
|
|
|
motionProxy = ALProxy("ALMotion", os.environ['NAO_IP'], 9559)
|
|
jacob = jacobian()
|
|
print jacob
|
|
jacob = pseudo_inverse(jacob)
|
|
print(jacob)
|
|
|
|
#rospy.init_node('cartesian_controller')
|
|
#rospy.spin()
|
|
|