From d1dc5d43990ea313c7a81636feffc3823d0d2b9d Mon Sep 17 00:00:00 2001 From: HRS_D Date: Thu, 31 Jan 2019 18:02:03 +0100 Subject: [PATCH] implemented Jacobian calculation --- CMakeLists.txt.user | 183 +++++++++++++++++++++++++++++++++ launch/rviz_config/test.rviz | 51 +++++++++ script/cartesian_controller.py | 94 +++++++++++++++++ src/NAO_Jacobian.cpp | 60 +++++++++++ 4 files changed, 388 insertions(+) create mode 100644 CMakeLists.txt.user create mode 100644 launch/rviz_config/test.rviz create mode 100755 script/cartesian_controller.py create mode 100644 src/NAO_Jacobian.cpp diff --git a/CMakeLists.txt.user b/CMakeLists.txt.user new file mode 100644 index 0000000..9a1fa5b --- /dev/null +++ b/CMakeLists.txt.user @@ -0,0 +1,183 @@ + + + + + + ProjectExplorer.Project.ActiveTarget + 0 + + + ProjectExplorer.Project.EditorSettings + + true + false + true + + Cpp + + CppGlobal + + + + QmlJS + + QmlJSGlobal + + + 2 + UTF-8 + false + 4 + false + true + 1 + true + 0 + true + 0 + 8 + true + 1 + true + true + true + false + + + + ProjectExplorer.Project.PluginSettings + + + + ProjectExplorer.Project.Target.0 + + Desktop + Desktop + {52dc460e-6709-4680-9540-0b7a75df0c6e} + 0 + 0 + 0 + + false + /home/hrs_d/catkin_ws/src/build-teleoperation-Desktop-Default + + + + + false + false + true + Make + + CMakeProjectManager.MakeStep + + 1 + Build + + ProjectExplorer.BuildSteps.Build + + + + clean + + true + false + true + Make + + CMakeProjectManager.MakeStep + + 1 + Clean + + ProjectExplorer.BuildSteps.Clean + + 2 + false + + Default + Default + CMakeProjectManager.CMakeBuildConfiguration + + 1 + + + 0 + Deploy + + ProjectExplorer.BuildSteps.Deploy + + 1 + Deploy locally + + ProjectExplorer.DefaultDeployConfiguration + + 1 + + + + false + false + false + false + true + 0.01 + 10 + true + 1 + 25 + + 1 + true + false + true + valgrind + + 0 + 1 + 2 + 3 + 4 + 5 + 6 + 7 + 8 + 9 + 10 + 11 + 12 + 13 + 14 + + 2 + + + + false + %{buildDir} + Custom Executable + + ProjectExplorer.CustomExecutableRunConfiguration + 3768 + true + false + false + false + true + + 1 + + + + ProjectExplorer.Project.TargetCount + 1 + + + ProjectExplorer.Project.Updater.EnvironmentId + {9ce0fc2b-7b92-4c88-a61a-8b1884a0af3f} + + + ProjectExplorer.Project.Updater.FileVersion + 15 + + diff --git a/launch/rviz_config/test.rviz b/launch/rviz_config/test.rviz new file mode 100644 index 0000000..77b9627 --- /dev/null +++ b/launch/rviz_config/test.rviz @@ -0,0 +1,51 @@ + +< robot name =" robot0 " > + +< link name =" world " / > + +< joint name =" joint_0 " type =" fixed " > +< parent link =" world "/ > +< child link =" link_0 "/ > + + +< link name =" link_0 " > +< visual > +< geometry > +< cylinder length ="0.6" radius ="0.2"/ > + +< material name =" gray " > +< color rgba ="0.5 0.5 0.5 1"/ > + + + + +< joint name =" joint_1 " type =" revolute " > +< origin xyz ="0 0 0.35" rpy ="1.57079632679 0 0"/ > +< parent link =" link_0 "/ > +< child link =" link_1 "/ > +< limit effort ="30" velocity ="1.0" lower =" -3.1415926535897931" upper +="3 .141 59265 3589 7931 " / > +< axis xyz ="0 1 0"/ > + + +< link name =" link_1 " > +< visual > +< origin xyz ="0 0 0.35" rpy ="0 0 0"/ > +< geometry > +< box size ="0.1 0.1 0.7" / > + +< material name =" red " > +< color rgba ="0.5 0.0 0.0 1"/ > + + + + +< joint name =" end_efector_joint " type =" fixed " > +< origin xyz ="0 0 0.7" rpy ="1.57079632679 0 0"/ > +< parent link =" link_1 "/ > +< child link =" end_effector "/ > + + +< link name =" end_effector " / > + + diff --git a/script/cartesian_controller.py b/script/cartesian_controller.py new file mode 100755 index 0000000..5463b9c --- /dev/null +++ b/script/cartesian_controller.py @@ -0,0 +1,94 @@ +#! /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() + diff --git a/src/NAO_Jacobian.cpp b/src/NAO_Jacobian.cpp new file mode 100644 index 0000000..764e11c --- /dev/null +++ b/src/NAO_Jacobian.cpp @@ -0,0 +1,60 @@ +#include +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 postion_vec; +postion_vec.push_back(e_diff.x); +postion_vec.push_back(e_diff.y); +postion_vec.push_back(e_diff.z); +vector 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); +vector> 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; +}