implemented Jacobian calculation
This commit is contained in:
HRS_D
183
CMakeLists.txt.user
Normal file
183
CMakeLists.txt.user
Normal file
@@ -0,0 +1,183 @@
|
|||||||
|
<?xml version="1.0" encoding="UTF-8"?>
|
||||||
|
<!DOCTYPE QtCreatorProject>
|
||||||
|
<!-- Written by QtCreator 3.0.1, 2019-01-31T10:27:10. -->
|
||||||
|
<qtcreator>
|
||||||
|
<data>
|
||||||
|
<variable>ProjectExplorer.Project.ActiveTarget</variable>
|
||||||
|
<value type="int">0</value>
|
||||||
|
</data>
|
||||||
|
<data>
|
||||||
|
<variable>ProjectExplorer.Project.EditorSettings</variable>
|
||||||
|
<valuemap type="QVariantMap">
|
||||||
|
<value type="bool" key="EditorConfiguration.AutoIndent">true</value>
|
||||||
|
<value type="bool" key="EditorConfiguration.AutoSpacesForTabs">false</value>
|
||||||
|
<value type="bool" key="EditorConfiguration.CamelCaseNavigation">true</value>
|
||||||
|
<valuemap type="QVariantMap" key="EditorConfiguration.CodeStyle.0">
|
||||||
|
<value type="QString" key="language">Cpp</value>
|
||||||
|
<valuemap type="QVariantMap" key="value">
|
||||||
|
<value type="QByteArray" key="CurrentPreferences">CppGlobal</value>
|
||||||
|
</valuemap>
|
||||||
|
</valuemap>
|
||||||
|
<valuemap type="QVariantMap" key="EditorConfiguration.CodeStyle.1">
|
||||||
|
<value type="QString" key="language">QmlJS</value>
|
||||||
|
<valuemap type="QVariantMap" key="value">
|
||||||
|
<value type="QByteArray" key="CurrentPreferences">QmlJSGlobal</value>
|
||||||
|
</valuemap>
|
||||||
|
</valuemap>
|
||||||
|
<value type="int" key="EditorConfiguration.CodeStyle.Count">2</value>
|
||||||
|
<value type="QByteArray" key="EditorConfiguration.Codec">UTF-8</value>
|
||||||
|
<value type="bool" key="EditorConfiguration.ConstrainTooltips">false</value>
|
||||||
|
<value type="int" key="EditorConfiguration.IndentSize">4</value>
|
||||||
|
<value type="bool" key="EditorConfiguration.KeyboardTooltips">false</value>
|
||||||
|
<value type="bool" key="EditorConfiguration.MouseNavigation">true</value>
|
||||||
|
<value type="int" key="EditorConfiguration.PaddingMode">1</value>
|
||||||
|
<value type="bool" key="EditorConfiguration.ScrollWheelZooming">true</value>
|
||||||
|
<value type="int" key="EditorConfiguration.SmartBackspaceBehavior">0</value>
|
||||||
|
<value type="bool" key="EditorConfiguration.SpacesForTabs">true</value>
|
||||||
|
<value type="int" key="EditorConfiguration.TabKeyBehavior">0</value>
|
||||||
|
<value type="int" key="EditorConfiguration.TabSize">8</value>
|
||||||
|
<value type="bool" key="EditorConfiguration.UseGlobal">true</value>
|
||||||
|
<value type="int" key="EditorConfiguration.Utf8BomBehavior">1</value>
|
||||||
|
<value type="bool" key="EditorConfiguration.addFinalNewLine">true</value>
|
||||||
|
<value type="bool" key="EditorConfiguration.cleanIndentation">true</value>
|
||||||
|
<value type="bool" key="EditorConfiguration.cleanWhitespace">true</value>
|
||||||
|
<value type="bool" key="EditorConfiguration.inEntireDocument">false</value>
|
||||||
|
</valuemap>
|
||||||
|
</data>
|
||||||
|
<data>
|
||||||
|
<variable>ProjectExplorer.Project.PluginSettings</variable>
|
||||||
|
<valuemap type="QVariantMap"/>
|
||||||
|
</data>
|
||||||
|
<data>
|
||||||
|
<variable>ProjectExplorer.Project.Target.0</variable>
|
||||||
|
<valuemap type="QVariantMap">
|
||||||
|
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Desktop</value>
|
||||||
|
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">Desktop</value>
|
||||||
|
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">{52dc460e-6709-4680-9540-0b7a75df0c6e}</value>
|
||||||
|
<value type="int" key="ProjectExplorer.Target.ActiveBuildConfiguration">0</value>
|
||||||
|
<value type="int" key="ProjectExplorer.Target.ActiveDeployConfiguration">0</value>
|
||||||
|
<value type="int" key="ProjectExplorer.Target.ActiveRunConfiguration">0</value>
|
||||||
|
<valuemap type="QVariantMap" key="ProjectExplorer.Target.BuildConfiguration.0">
|
||||||
|
<value type="bool" key="CMakeProjectManager.CMakeBuildConfiguration.UseNinja">false</value>
|
||||||
|
<value type="QString" key="ProjectExplorer.BuildConfiguration.BuildDirectory">/home/hrs_d/catkin_ws/src/build-teleoperation-Desktop-Default</value>
|
||||||
|
<valuemap type="QVariantMap" key="ProjectExplorer.BuildConfiguration.BuildStepList.0">
|
||||||
|
<valuemap type="QVariantMap" key="ProjectExplorer.BuildStepList.Step.0">
|
||||||
|
<value type="QString" key="CMakeProjectManager.MakeStep.AdditionalArguments"></value>
|
||||||
|
<valuelist type="QVariantList" key="CMakeProjectManager.MakeStep.BuildTargets"/>
|
||||||
|
<value type="bool" key="CMakeProjectManager.MakeStep.Clean">false</value>
|
||||||
|
<value type="bool" key="CMakeProjectManager.MakeStep.UseNinja">false</value>
|
||||||
|
<value type="bool" key="ProjectExplorer.BuildStep.Enabled">true</value>
|
||||||
|
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Make</value>
|
||||||
|
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
|
||||||
|
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">CMakeProjectManager.MakeStep</value>
|
||||||
|
</valuemap>
|
||||||
|
<value type="int" key="ProjectExplorer.BuildStepList.StepsCount">1</value>
|
||||||
|
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Build</value>
|
||||||
|
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
|
||||||
|
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.BuildSteps.Build</value>
|
||||||
|
</valuemap>
|
||||||
|
<valuemap type="QVariantMap" key="ProjectExplorer.BuildConfiguration.BuildStepList.1">
|
||||||
|
<valuemap type="QVariantMap" key="ProjectExplorer.BuildStepList.Step.0">
|
||||||
|
<value type="QString" key="CMakeProjectManager.MakeStep.AdditionalArguments">clean</value>
|
||||||
|
<valuelist type="QVariantList" key="CMakeProjectManager.MakeStep.BuildTargets"/>
|
||||||
|
<value type="bool" key="CMakeProjectManager.MakeStep.Clean">true</value>
|
||||||
|
<value type="bool" key="CMakeProjectManager.MakeStep.UseNinja">false</value>
|
||||||
|
<value type="bool" key="ProjectExplorer.BuildStep.Enabled">true</value>
|
||||||
|
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Make</value>
|
||||||
|
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
|
||||||
|
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">CMakeProjectManager.MakeStep</value>
|
||||||
|
</valuemap>
|
||||||
|
<value type="int" key="ProjectExplorer.BuildStepList.StepsCount">1</value>
|
||||||
|
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Clean</value>
|
||||||
|
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
|
||||||
|
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.BuildSteps.Clean</value>
|
||||||
|
</valuemap>
|
||||||
|
<value type="int" key="ProjectExplorer.BuildConfiguration.BuildStepListCount">2</value>
|
||||||
|
<value type="bool" key="ProjectExplorer.BuildConfiguration.ClearSystemEnvironment">false</value>
|
||||||
|
<valuelist type="QVariantList" key="ProjectExplorer.BuildConfiguration.UserEnvironmentChanges"/>
|
||||||
|
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Default</value>
|
||||||
|
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">Default</value>
|
||||||
|
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">CMakeProjectManager.CMakeBuildConfiguration</value>
|
||||||
|
</valuemap>
|
||||||
|
<value type="int" key="ProjectExplorer.Target.BuildConfigurationCount">1</value>
|
||||||
|
<valuemap type="QVariantMap" key="ProjectExplorer.Target.DeployConfiguration.0">
|
||||||
|
<valuemap type="QVariantMap" key="ProjectExplorer.BuildConfiguration.BuildStepList.0">
|
||||||
|
<value type="int" key="ProjectExplorer.BuildStepList.StepsCount">0</value>
|
||||||
|
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Deploy</value>
|
||||||
|
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
|
||||||
|
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.BuildSteps.Deploy</value>
|
||||||
|
</valuemap>
|
||||||
|
<value type="int" key="ProjectExplorer.BuildConfiguration.BuildStepListCount">1</value>
|
||||||
|
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Deploy locally</value>
|
||||||
|
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
|
||||||
|
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.DefaultDeployConfiguration</value>
|
||||||
|
</valuemap>
|
||||||
|
<value type="int" key="ProjectExplorer.Target.DeployConfigurationCount">1</value>
|
||||||
|
<valuemap type="QVariantMap" key="ProjectExplorer.Target.PluginSettings"/>
|
||||||
|
<valuemap type="QVariantMap" key="ProjectExplorer.Target.RunConfiguration.0">
|
||||||
|
<valuelist type="QVariantList" key="Analyzer.Valgrind.AddedSuppressionFiles"/>
|
||||||
|
<value type="bool" key="Analyzer.Valgrind.Callgrind.CollectBusEvents">false</value>
|
||||||
|
<value type="bool" key="Analyzer.Valgrind.Callgrind.CollectSystime">false</value>
|
||||||
|
<value type="bool" key="Analyzer.Valgrind.Callgrind.EnableBranchSim">false</value>
|
||||||
|
<value type="bool" key="Analyzer.Valgrind.Callgrind.EnableCacheSim">false</value>
|
||||||
|
<value type="bool" key="Analyzer.Valgrind.Callgrind.EnableEventToolTips">true</value>
|
||||||
|
<value type="double" key="Analyzer.Valgrind.Callgrind.MinimumCostRatio">0.01</value>
|
||||||
|
<value type="double" key="Analyzer.Valgrind.Callgrind.VisualisationMinimumCostRatio">10</value>
|
||||||
|
<value type="bool" key="Analyzer.Valgrind.FilterExternalIssues">true</value>
|
||||||
|
<value type="int" key="Analyzer.Valgrind.LeakCheckOnFinish">1</value>
|
||||||
|
<value type="int" key="Analyzer.Valgrind.NumCallers">25</value>
|
||||||
|
<valuelist type="QVariantList" key="Analyzer.Valgrind.RemovedSuppressionFiles"/>
|
||||||
|
<value type="int" key="Analyzer.Valgrind.SelfModifyingCodeDetection">1</value>
|
||||||
|
<value type="bool" key="Analyzer.Valgrind.Settings.UseGlobalSettings">true</value>
|
||||||
|
<value type="bool" key="Analyzer.Valgrind.ShowReachable">false</value>
|
||||||
|
<value type="bool" key="Analyzer.Valgrind.TrackOrigins">true</value>
|
||||||
|
<value type="QString" key="Analyzer.Valgrind.ValgrindExecutable">valgrind</value>
|
||||||
|
<valuelist type="QVariantList" key="Analyzer.Valgrind.VisibleErrorKinds">
|
||||||
|
<value type="int">0</value>
|
||||||
|
<value type="int">1</value>
|
||||||
|
<value type="int">2</value>
|
||||||
|
<value type="int">3</value>
|
||||||
|
<value type="int">4</value>
|
||||||
|
<value type="int">5</value>
|
||||||
|
<value type="int">6</value>
|
||||||
|
<value type="int">7</value>
|
||||||
|
<value type="int">8</value>
|
||||||
|
<value type="int">9</value>
|
||||||
|
<value type="int">10</value>
|
||||||
|
<value type="int">11</value>
|
||||||
|
<value type="int">12</value>
|
||||||
|
<value type="int">13</value>
|
||||||
|
<value type="int">14</value>
|
||||||
|
</valuelist>
|
||||||
|
<value type="int" key="PE.EnvironmentAspect.Base">2</value>
|
||||||
|
<valuelist type="QVariantList" key="PE.EnvironmentAspect.Changes"/>
|
||||||
|
<value type="QString" key="ProjectExplorer.CustomExecutableRunConfiguration.Arguments"></value>
|
||||||
|
<value type="QString" key="ProjectExplorer.CustomExecutableRunConfiguration.Executable"></value>
|
||||||
|
<value type="bool" key="ProjectExplorer.CustomExecutableRunConfiguration.UseTerminal">false</value>
|
||||||
|
<value type="QString" key="ProjectExplorer.CustomExecutableRunConfiguration.WorkingDirectory">%{buildDir}</value>
|
||||||
|
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Custom Executable</value>
|
||||||
|
<value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
|
||||||
|
<value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.CustomExecutableRunConfiguration</value>
|
||||||
|
<value type="uint" key="RunConfiguration.QmlDebugServerPort">3768</value>
|
||||||
|
<value type="bool" key="RunConfiguration.UseCppDebugger">true</value>
|
||||||
|
<value type="bool" key="RunConfiguration.UseCppDebuggerAuto">false</value>
|
||||||
|
<value type="bool" key="RunConfiguration.UseMultiProcess">false</value>
|
||||||
|
<value type="bool" key="RunConfiguration.UseQmlDebugger">false</value>
|
||||||
|
<value type="bool" key="RunConfiguration.UseQmlDebuggerAuto">true</value>
|
||||||
|
</valuemap>
|
||||||
|
<value type="int" key="ProjectExplorer.Target.RunConfigurationCount">1</value>
|
||||||
|
</valuemap>
|
||||||
|
</data>
|
||||||
|
<data>
|
||||||
|
<variable>ProjectExplorer.Project.TargetCount</variable>
|
||||||
|
<value type="int">1</value>
|
||||||
|
</data>
|
||||||
|
<data>
|
||||||
|
<variable>ProjectExplorer.Project.Updater.EnvironmentId</variable>
|
||||||
|
<value type="QByteArray">{9ce0fc2b-7b92-4c88-a61a-8b1884a0af3f}</value>
|
||||||
|
</data>
|
||||||
|
<data>
|
||||||
|
<variable>ProjectExplorer.Project.Updater.FileVersion</variable>
|
||||||
|
<value type="int">15</value>
|
||||||
|
</data>
|
||||||
|
</qtcreator>
|
||||||
51
launch/rviz_config/test.rviz
Normal file
51
launch/rviz_config/test.rviz
Normal file
@@ -0,0 +1,51 @@
|
|||||||
|
<? xml version ="1.0"? >
|
||||||
|
< robot name =" robot0 " >
|
||||||
|
|
||||||
|
< link name =" world " / >
|
||||||
|
|
||||||
|
< joint name =" joint_0 " type =" fixed " >
|
||||||
|
< parent link =" world "/ >
|
||||||
|
< child link =" link_0 "/ >
|
||||||
|
</ joint >
|
||||||
|
|
||||||
|
< link name =" link_0 " >
|
||||||
|
< visual >
|
||||||
|
< geometry >
|
||||||
|
< cylinder length ="0.6" radius ="0.2"/ >
|
||||||
|
</ geometry >
|
||||||
|
< material name =" gray " >
|
||||||
|
< color rgba ="0.5 0.5 0.5 1"/ >
|
||||||
|
</ material >
|
||||||
|
</ visual >
|
||||||
|
</ link >
|
||||||
|
|
||||||
|
< 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"/ >
|
||||||
|
</ joint >
|
||||||
|
|
||||||
|
< link name =" link_1 " >
|
||||||
|
< visual >
|
||||||
|
< origin xyz ="0 0 0.35" rpy ="0 0 0"/ >
|
||||||
|
< geometry >
|
||||||
|
< box size ="0.1 0.1 0.7" / >
|
||||||
|
</ geometry >
|
||||||
|
< material name =" red " >
|
||||||
|
< color rgba ="0.5 0.0 0.0 1"/ >
|
||||||
|
</ material >
|
||||||
|
</ visual >
|
||||||
|
</ link >
|
||||||
|
|
||||||
|
< 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 "/ >
|
||||||
|
</ joint >
|
||||||
|
|
||||||
|
< link name =" end_effector " / >
|
||||||
|
|
||||||
|
</ robot >
|
||||||
94
script/cartesian_controller.py
Executable file
94
script/cartesian_controller.py
Executable file
@@ -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()
|
||||||
|
|
||||||
60
src/NAO_Jacobian.cpp
Normal file
60
src/NAO_Jacobian.cpp
Normal file
@@ -0,0 +1,60 @@
|
|||||||
|
#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.push_back(e_diff.x);
|
||||||
|
postion_vec.push_back(e_diff.y);
|
||||||
|
postion_vec.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);
|
||||||
|
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