fix merge forgot to delete files
This commit is contained in:
Pavel Lutskov
@@ -1,139 +0,0 @@
|
||||
#! /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/ accordint 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
|
||||
jac_mat = jacobian()
|
||||
|
||||
# use jacobian to compute desired joint speed
|
||||
|
||||
derivative_speed = (p_d - end_position) / 5
|
||||
inv_jac = pseudo_inverse(jac_mat)
|
||||
angular_velocity = derivative_speed * inv_jac
|
||||
|
||||
|
||||
# integrate over desired speed to get desired joint position
|
||||
|
||||
names = "LArm"
|
||||
useSensors = False
|
||||
commandAnglesLArm = motionProxy.getAngles(names, useSensors)
|
||||
|
||||
#names = "RArm"
|
||||
#useSensors = False
|
||||
#commandAngles = motionProxy.getAngles(names, useSensors)
|
||||
|
||||
goal_angleL = commandAnglesLArm + (angular_velocity * 5)
|
||||
|
||||
|
||||
# return desired joint position and speed
|
||||
|
||||
|
||||
|
||||
return goal_angleL, commandAnglesLArm
|
||||
|
||||
|
||||
|
||||
def movement(e)
|
||||
|
||||
# scale joint states with matrix K
|
||||
|
||||
# add desired joint speed
|
||||
|
||||
# move robot arm
|
||||
|
||||
return
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
|
||||
motionProxy = 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_controller')
|
||||
#rospy.spin()
|
||||
|
||||
@@ -1,61 +0,0 @@
|
||||
#include <Vector3.h>
|
||||
|
||||
using namespace std;
|
||||
// Calculate the Jacobian Matrix for the NAO
|
||||
// inputs: original angles tau1 - tau4 and new angles tau1' - tau4'
|
||||
// original endeffector position e1 - e4 and new endeffector position e1' - e4'
|
||||
|
||||
typedef struct position{
|
||||
float x;
|
||||
float y;
|
||||
float z;
|
||||
}position;
|
||||
|
||||
typedef struct angles{
|
||||
float tau_1;
|
||||
float tau_2;
|
||||
float tau_3;
|
||||
float tau_4;
|
||||
}angles;
|
||||
|
||||
angles a_end, a, a_diff;
|
||||
position e_end, e, e_diff;
|
||||
|
||||
e_diff = diff(e_end, e);
|
||||
a_diff = diff(a_end, a);
|
||||
|
||||
vector<float> postion_vec;
|
||||
postion_vec.vector::push_back(e_diff.x);
|
||||
postion_vec.vector::push_back(e_diff.y);
|
||||
postion_vec.vector::push_back(e_diff.z);
|
||||
vector<float> angles_vec;
|
||||
angles_vec.vector::push_back(a_diff.tau_1);
|
||||
angles_vec.vector::push_back(a_diff.tau_2);
|
||||
angles_vec.vector::push_back(a_diff.tau_3);
|
||||
angles_vec.vector::push_back(a_diff.tau_4);s
|
||||
vector<vector<float>> Jacobian;
|
||||
|
||||
|
||||
for (int i = 0; i<3; i++) {
|
||||
for(int j = 0; j<4; j++ ) {
|
||||
Jacobian[i][j] = postion_vec[i]/angles_vec[j];
|
||||
}
|
||||
}
|
||||
|
||||
position diff(position end, position actual){
|
||||
position temp;
|
||||
temp.x = end.x - actual.x;
|
||||
temp.y = end.y - actual.y;
|
||||
temp.z = end.z - actual.z;
|
||||
return temp;
|
||||
}
|
||||
|
||||
angles diff(angles end, angles actual){
|
||||
angles temp;
|
||||
temp.tau_1 = end.tau_1 - actual.tau_1;
|
||||
temp.tau_2 = end.tau_2 - actual.tau_2;
|
||||
temp.tau_3 = end.tau_3 - actual.tau_3;
|
||||
temp.tau_4 = end.tau_4 - actual.tau_4;
|
||||
|
||||
return temp;
|
||||
}
|
||||
Reference in New Issue
Block a user