The journey of fixing IK/FK continues..

1. added zero frame to base frame transformation 2. added crtk frame to base frame transformation (for better intuition of the correctness of the returned cartesian pose from FK)
cshreyastech · May 1, 2019 · 1abca6d · 1abca6d
1 parent 885bf6e
commit 1abca6d
Show file tree

Hide file tree

Showing 3 changed files with 23 additions and 6 deletions.
diff --git a/ambf_controller/include/ambf_defines.h b/ambf_controller/include/ambf_defines.h
@@ -81,7 +81,8 @@ class AMBFDef {
   static const vector<vector<float>>   raven_dh_theta;
   static const vector<vector<float>>   raven_kin_offset;
   static const vector<float>           raven_ikin_param;
-
+  static const vector<tf::Transform>   raven_T_B0;  // raven base frame to raven zero frame (have seperate ones for each arm)
+  static const tf::Transform           raven_T_CB;  // raven crtk frame to base frame (have seperate ones for each arm)
 };
 
 #endif
diff --git a/ambf_controller/src/ambf_defines.cpp b/ambf_controller/src/ambf_defines.cpp
@@ -71,6 +71,18 @@ const vector<unsigned char>   AMBFDef::false_joints         = {          0,
 
 const vector<vector<float>>   AMBFDef::raven_joint_limit    = {{         0,   M_PI/4, -0.17,  -M_PI*2,   -2,       -2,        -2},
                                                                {  M_PI*1/2, M_PI*3/4,  0.10,   M_PI*2,    2,        2,         2}};  // TODO: finalize this
+const vector<vector<float>>   AMBFDef::raven_dh_alpha       = {{         0,       75,   128,        0,   90,       90,         0},
+                                                               {       180,       75,    52,        0,   90,       90,         0}};
+const vector<vector<float>>   AMBFDef::raven_dh_a           = {{         0,        0,     0,        0,    0,    0.013,         0},
+                                                               {         0,        0,     0,        0,    0,    0.013,         0}};
+const vector<vector<float>>   AMBFDef::raven_dh_d           = {{         0,        0,     V,    -0.47,    0,        0,         0},
+                                                               {         0,        0,     V,    -0.47,    0,        0,         0}};
+const vector<vector<float>>   AMBFDef::raven_dh_theta       = {{         V,        V,    90,        V,    V,        V,         0},
+                                                               {         V,        V,   -90,        V,    V,        V,         0}};
+const vector<vector<float>>   AMBFDef::raven_kin_offset     = {{         0,        0,     0,        0,  -90,        0,         0},
+                                                               {         0,        0,     0,        0,  -90,        0,         0}};
+
+/*
 const vector<vector<float>>   AMBFDef::raven_dh_alpha       = {{         0,       75,   128,        0,   90,       90,         0},
                                                                {       180,       75,    52,        0,   90,       90,         0}};
 const vector<vector<float>>   AMBFDef::raven_dh_a           = {{         0,        0,     0,        0,    0,    0.013,         0},
@@ -80,14 +92,18 @@ const vector<vector<float>>   AMBFDef::raven_dh_d           = {{         0,
 const vector<vector<float>>   AMBFDef::raven_dh_theta       = {{         V,        V,    90,        V,    V,        V,         0},
                                                                {         V,        V,   -90,        V,    V,        V,         0}};
 const vector<vector<float>>   AMBFDef::raven_kin_offset     = {{       205,      180,     0,        0,  -90,        0,         0},
-                                                               {        25,        0,     0,        0,  -90,        0,         0}};
+                                                               {        25,        0,     0,        0,  -90,        0,         0}};*/
 
 const vector<float>           AMBFDef::raven_ikin_param     = { (float)sin(AMBFDef::raven_dh_alpha[0][1] Deg2Rad),     // 0: GM1
                                                                 (float)cos(AMBFDef::raven_dh_alpha[0][1] Deg2Rad),     // 1: GM2
                                                                 (float)sin(AMBFDef::raven_dh_alpha[1][2] Deg2Rad),     // 2: GM3
                                                                 (float)cos(AMBFDef::raven_dh_alpha[1][2] Deg2Rad),     // 3: GM4
-                                                                -0.47,                                          // 4: d4
-                                                                0.013                                    };     // 5: Lw 
+                                                                -0.47,                                                 // 4: d4
+                                                                0.013                                    };            // 5: Lw 
+
+const vector<tf::Transform>   AMBFDef::raven_T_B0           = { tf::Transform(tf::Matrix3x3(0,0,1,0,-1,0,1,0,0),tf::Vector3(0.30071, 0.061, -0.007)),
+                                                                tf::Transform(tf::Matrix3x3(0,0,-1,0,1,0,1,0,0),tf::Vector3(-0.30071, 0.061, -0.007))};
+const tf::Transform           AMBFDef::raven_T_CB           =   tf::Transform(tf::Quaternion(tf::Vector3(0,0,1),-M_PI/2),tf::Vector3(0,0,0));
 
 // Raven joints:
 // joint -: 0_link-base_link_L:             fixed

diff --git a/ambf_controller/src/ambf_motion_planner.cpp b/ambf_controller/src/ambf_motion_planner.cpp
@@ -198,7 +198,7 @@ bool AMBFRavenPlanner::fwd_kinematics(int arm, vector<float> input_jp, tf::Trans
 	}
 
 	// computes forward kinematics
-	output_cp = fwd_trans(0, 6, dh_alpha, dh_theta, dh_a, dh_d);
+	output_cp = AMBFDef::raven_T_CB * AMBFDef::raven_T_B0[arm] * fwd_trans(0, 6, dh_alpha, dh_theta, dh_a, dh_d);
 
 	success = true;
 	return success;
@@ -261,7 +261,7 @@ bool AMBFRavenPlanner::inv_kinematics(int arm, tf::Transform& input_cp, float in
 {
 	bool success = false;
 
-	tf::Transform xf = input_cp;
+	tf::Transform xf =  (AMBFDef::raven_T_CB * AMBFDef::raven_T_B0[arm]).inverse() * input_cp;
 
 	vector<vector<float>> iksol(AMBFDef::raven_iksols, vector<float>(AMBFDef::raven_joints-1));
 	vector<bool>ikcheck(AMBFDef::raven_iksols);