pavel/teleoperation
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

fix merge forgot to delete files

Browse Source
This commit is contained in:
Pavel Lutskov
2019-02-08 14:01:40 +01:00
parent 622ed91470
commit 98ce555fe9
2 changed files with 0 additions and 200 deletions
Download Patch File Download Diff File Expand all files Collapse all files

139
script/cartesian_controller.py
View File

@@ -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()

61
src/NAO_Jacobian.cpp
View File

@@ -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;
}