128 lines
3.0 KiB
Python
Executable File
128 lines
3.0 KiB
Python
Executable File
#! /usr/bin/env python
|
|
import os
|
|
|
|
import rospy
|
|
import numpy as np
|
|
|
|
from naoqi import ALProxy
|
|
import motion
|
|
|
|
|
|
mp = None
|
|
|
|
|
|
def get_transform(joint):
|
|
frame = motion.FRAME_TORSO
|
|
useSensorValues = True
|
|
result = mp.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 = mp.getPosition(joint, frame, useSensorValues)
|
|
#print result
|
|
return np.array(result[: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 = 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, 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)
|
|
|
|
# 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)
|
|
|
|
# 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(p_d):
|
|
|
|
# calculate jacobian
|
|
|
|
# use jacobian to compute desired joint speed
|
|
|
|
# integrate over desired speed to get desired joint position
|
|
|
|
# return desired joint position and speed
|
|
|
|
return
|
|
|
|
|
|
def movement(e):
|
|
|
|
# scale joint states with matrix K
|
|
|
|
# add desired joint speed
|
|
|
|
# move robot arm
|
|
|
|
return
|
|
|
|
|
|
if __name__ == '__main__':
|
|
mp = ALProxy("ALMotion", os.environ['NAO_IP'], 9559)
|
|
jacob = jacobian()
|
|
print jacob
|
|
jacob = pseudo_inverse(jacob)
|
|
print(jacob)
|
|
|
|
# given new desired coordinates
|
|
|
|
e = 1;
|
|
"""
|
|
while e bigger some value
|
|
|
|
# run reference generator to get desired joint postion and speed
|
|
|
|
# subtract current joint states
|
|
|
|
# movement
|
|
|
|
"""
|
|
#rospy.init_node('cartesian_masterloop')
|
|
#rospy.spin()
|
|
|