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

small update

Browse Source
This commit is contained in:
HRS_D
2019-02-05 13:44:24 +01:00
parent 193dcff7a8
commit 3b16ae6fcd
2 changed files with 26 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files

19
script/cartesian_controller.py
View File

@@ -73,12 +73,29 @@ def pseudo_inverse(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
commandAngles = motionProxy.getAngles(names, useSensors)
names = "RArm"
useSensors = False
commandAngles = motionProxy.getAngles(names, useSensors)
goal_angle = mom_angle + (angular_velocity * 5)
# return desired joint position and speed
return

15
src/NAO_Jacobian.cpp
View File

@@ -1,4 +1,5 @@
#include <Vector3.h>
using namespace std;
// Calculate the Jacobian Matrix for the NAO
// inputs: original angles tau1 - tau4 and new angles tau1' - tau4'
@@ -24,14 +25,14 @@ e_diff = diff(e_end, e);
a_diff = diff(a_end, a);
vector<float> postion_vec;
postion_vec.push_back(e_diff.x);
postion_vec.push_back(e_diff.y);
postion_vec.push_back(e_diff.z);
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.push_back(a_diff.tau_1);
angles_vec.push_back(a_diff.tau_2);
angles_vec.push_back(a_diff.tau_3);
angles_vec.push_back(a_diff.tau_4);
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;