AUV-Coop-Assembly
Master Thesis for Robotics Engineering. Cooperative peg-in-hole assembly with two underwater vehicles guided by vision
jacobianHelper.cpp
1 #include "header/jacobianHelper.h"
2 
3 
4 //TODO: store internally matrix and vector that remain fixed? (eg posiz of link0 respect to vehicle)
5 
11 int computeWholeJacobianEE(struct Infos *robInfo){
12 
13 
14  //positional and orientational part of arm jacobian PROJECTED ON WORLD
15  Eigen::Matrix <double, 3, ARM_DOF> w_J_man_pos;
16  Eigen::Matrix <double, 3, ARM_DOF> w_J_man_or;
17  Eigen::Matrix3d wR0 = robInfo->robotState.wTv_eigen.topLeftCorner<3,3>() *
18  robInfo->robotStruct.vTlink0.topLeftCorner<3,3>();
19 
20  //J_man_xxx = wR0 * zeroJ_man_xxx
21  w_J_man_pos = wR0 * robInfo->robotState.link0_Jee_man.topRows<3>();
22  w_J_man_or = wR0 * robInfo->robotState.link0_Jee_man.bottomRows<3>();
23 
25  Eigen::Matrix<double, 3, TOT_DOF> w_J_pos;
26 
27  // part relative to joints
28  w_J_pos.leftCols<ARM_DOF>() = w_J_man_pos;
29 
30  // part relative to linear velocities
31  Eigen::Matrix3d wRv = robInfo->robotState.wTv_eigen.topLeftCorner<3,3>();
32  w_J_pos.block<3,3>(0,ARM_DOF) = wRv; //with block indexes start from 0
33 
34  // part relative to angular velocities
35  Eigen::Matrix3d vRlink0 = robInfo->robotStruct.vTlink0.topLeftCorner<3,3>();
36  //distance from vehicle to link 0 respect on vehicle frame
37  Eigen::Vector3d v_Distv0 = robInfo->robotStruct.vTlink0.topRightCorner<3,1>();
38  //distance from link0 to end effector respect to link0
39  Eigen::Vector3d link0_link0DistEe = robInfo->robotState.link0Tee_eigen.topRightCorner<3,1>();
40  Eigen::Matrix3d wRlink0 = wRv*vRlink0;
41  //skew__XXX : skew of XXX
42  Eigen::Matrix3d skew__w_Dist_v0 = FRM::skewMat(wRv * v_Distv0);
43  Eigen::Matrix3d skew__w_Dist_0ee = FRM::skewMat(wRlink0 * link0_link0DistEe);
44  w_J_pos.rightCols<3>() = -(skew__w_Dist_v0 + skew__w_Dist_0ee) * wRv;
45 
46 
48  Eigen::Matrix<double, 3, TOT_DOF> w_J_or;
49  w_J_or.leftCols<ARM_DOF>() = w_J_man_or; //relative to arm
50  w_J_or.block<3,3>(0,ARM_DOF) = Eigen::Matrix3d::Zero(); //relative to linear vel
51  w_J_or.rightCols<3>() = wRv; //relative to angular vel
52 
54  robInfo->robotState.w_Jee_robot.topRows<3>() = w_J_pos;
55  robInfo->robotState.w_Jee_robot.bottomRows<3>() = w_J_or;
56 
57  return 0;
58 }
59 
65 int computeWholeJacobianTool(struct Infos *robInfo){
66 
67 
68  //positional and orientational part of arm jacobian PROJECTED ON WORLD
69  Eigen::Matrix <double, 3, ARM_DOF> w_J_man_pos;
70  Eigen::Matrix <double, 3, ARM_DOF> w_J_man_or;
71  Eigen::Matrix3d wRv = robInfo->robotState.wTv_eigen.topLeftCorner<3,3>();
72 
73  //J_man_xxx = wR0 * zeroJ_man_xxx
74  w_J_man_pos = wRv * robInfo->robotState.v_Jtool_man.topRows<3>();
75  w_J_man_or = wRv * robInfo->robotState.v_Jtool_man.bottomRows<3>();
76 
78  Eigen::Matrix<double, 3, TOT_DOF> w_J_pos;
79 
80  // part relative to joints
81  w_J_pos.leftCols<ARM_DOF>() = w_J_man_pos;
82 
83  // part relative to linear velocities
84  w_J_pos.block<3,3>(0,ARM_DOF) = wRv; //with block indexes start from 0
85 
86  // part relative to angular velocities
87  //distance from vehicle to TOOL respect to world (wRv * v_eta_v,ee)
88  Eigen::Vector3d w_DistVTool =
89  robInfo->transforms.wTt_eigen.topRightCorner<3,1>() -
90  robInfo->robotState.wTv_eigen.topRightCorner<3,1>();
91 
92 
93  //skew__XXX : skew of XXX
94  Eigen::Matrix3d skew__w_Dist_vtool = FRM::skewMat(w_DistVTool);
95  w_J_pos.rightCols<3>() = -(skew__w_Dist_vtool) * wRv;
96 
97 
99  Eigen::Matrix<double, 3, TOT_DOF> w_J_or;
100  w_J_or.leftCols<ARM_DOF>() = w_J_man_or; //relative to arm
101  w_J_or.block<3,3>(0,ARM_DOF) = Eigen::Matrix3d::Zero(); //relative to linear vel
102  w_J_or.rightCols<3>() = wRv; //relative to angular vel
103 
105 
106  robInfo->robotState.w_Jtool_robot.topRows<3>() = w_J_pos;
107  robInfo->robotState.w_Jtool_robot.bottomRows<3>() = w_J_or;
108 
109 
110 
111  //DEBUG
112 // std::cout << "kdl first Skew\n" << skew__w_Dist_v0 << "\n\n";
113 // std::cout << "kdl secodn Skew\n" << skew__w_Dist_0tool << "\n\n";
114 // std::cout << "eta1\n"
115 // << robInfo->robotState.wTv_eigen.topRightCorner<3,1>()
116 // << "\n\n";
117 // std::cout << "eta2\n"
118 // << robInfo->robotState.wTv_eigen.topLeftCorner<3,3>()
119 // << "\n\n";
120 // std::cout << "eta_ee1\n"
121 // << robInfo->transforms.wTt_eigen.topRightCorner<3,1>()
122 // << "\n\n";
123 
124  return 0;
125 }
126 
127 
128 
129 
130 
131 //int computeJacobianToolNoKdl(struct Infos *robInfo, std::string robotName){
132 
133 // Eigen::MatrixXd J_eigen = Eigen::MatrixXd::Zero(6, TOT_DOF);
134 
135 // Eigen::Vector3d w_rtool = robInfo->transforms.wTt_eigen.topRightCorner(3,1); //position of tool respect world
136 // Eigen::Vector3d khat; //versor along z axis
137 // khat << 0, 0, 1;
138 
139 // /// taking rotation matrices and translational vectors
140 // Eigen::Matrix3d w_R_[ARM_DOF];
141 // Eigen::Vector3d w_r[ARM_DOF];
142 
143 // for (int i =0; i<ARM_DOF; i++){
144 // w_R_[i] = robInfo->robotState.wTjoints[i].topLeftCorner(3,3);
145 // w_r[i] = robInfo->robotState.wTjoints[i].topRightCorner(3,1); //position of joint i respect world
146 // //std::cout <<"\n\n JOINT "<< i << ": \n" << w_R_[i] << "\n\n";
147 // }
148 
149 // ///JOINT part (0 to 3 column)
150 // for (int i = 0; i<ARM_DOF; i++){
151 
152 // Eigen::VectorXd g(6);
153 // g << 0,0,0, 0,0,0;
154 
155 // Eigen::Vector3d w_kHat = w_R_[i] * khat; //versor k respect vehicle frame
156 
157 // g.head(3) = w_kHat.cross(w_rtool - w_r[i]); //linear part
158 // // std::cout <<"\n\n JOINT "<< i << ": \n" << v_kHat << " x (" << v_ree << " - " << v_r[i] << ")" << "\n"
159 // // "result: " << g.head(3)<<"\n\n";
160 
161 
162 // g.tail(3) = w_kHat; //angular part
163 
164 // J_eigen.col(i) = g;
165 // }
166 
167 
168 // ///VEHICLE part (4 to 9 columns)
169 
170 // //linear part (top rows)
171 // J_eigen.block<3,3>(0,4) = robInfo->robotState.wTv_eigen.topLeftCorner<3,3>(); //linear contribution to linear velocities
172 
173 // Eigen::Matrix3d firstSkew = FRM::skewMat(robInfo->robotState.wTv_eigen.topLeftCorner<3,3>() * robInfo->robotStruct.vTlink0.topRightCorner<3,1>());
174 // Eigen::Vector3d ee_distEeTool;
175 // if (robotName.compare("g500_A") == 0){
176 // ee_distEeTool << -0.000, 2.000, -0.017;
177 // }else{
178 // ee_distEeTool <<-0.000, -2.000, 0.007;
179 
180 // }
181 // Eigen::Vector3d link0_DistLink0Tool = robInfo->robotState.link0Tee_eigen.topRightCorner<3,1>() +
182 // (robInfo->robotState.link0Tee_eigen.topLeftCorner<3,3>()*ee_distEeTool);
183 // Eigen::Matrix3d secondSkew =
184 // FRM::skewMat(robInfo->robotState.wTv_eigen.topLeftCorner<3,3>() *
185 // robInfo->robotStruct.vTlink0.topLeftCorner<3,3>() *
186 // link0_DistLink0Tool);
187 
192 // J_eigen.topRightCorner<3,3>() = - (firstSkew * robInfo->robotState.wTv_eigen.topLeftCorner<3,3>())
193 // - (secondSkew * robInfo->robotState.wTv_eigen.topLeftCorner<3,3>() *
194 // robInfo->robotStruct.vTlink0.topLeftCorner<3,3>());
195 
196 // //J_eigen.topRightCorner(3,3) = -(FRM::skewMat(w_rtool)); //angular contribution to linear velocites
197 // //youbot version??
198 // CMAT::Vect3 link0_DistLink0Tool_cmat;
199 // link0_DistLink0Tool_cmat(1) = link0_DistLink0Tool(0);
200 // link0_DistLink0Tool_cmat(2) = link0_DistLink0Tool(1);
201 // link0_DistLink0Tool_cmat(3) = link0_DistLink0Tool(2);
202 // std::cout<< "OOOOO\n\n";
203 // Eigen::MatrixXd rigBodySwapped = CONV::matrix_cmat2eigen(link0_DistLink0Tool_cmat.GetRigidBodyMatrix());
204 // Eigen::MatrixXd rigBody(6,6);
205 // rigBody.topLeftCorner<3,3>() = (robInfo->robotState.wTv_eigen * robInfo->robotStruct.vTlink0).topLeftCorner<3,3>() *
206 // rigBodySwapped.bottomLeftCorner<3,3>();
207 // rigBody.topRightCorner<3,3>() = (robInfo->robotState.wTv_eigen * robInfo->robotStruct.vTlink0).topLeftCorner<3,3>() *
208 // rigBodySwapped.bottomRightCorner<3,3>();
209 // rigBody.bottomLeftCorner<3,3>() = (robInfo->robotState.wTv_eigen * robInfo->robotStruct.vTlink0).topLeftCorner<3,3>() *
210 // rigBodySwapped.topLeftCorner<3,3>();
211 // rigBody.bottomRightCorner<3,3>() = (robInfo->robotState.wTv_eigen * robInfo->robotStruct.vTlink0).topLeftCorner<3,3>() *
212 // rigBodySwapped.topRightCorner<3,3>();
213 
214 // J_eigen.rightCols(6) = rigBody;
215 // // J_eigen.rightCols(6) = (link0_DistLink0Tool_cmat.GetRigidBodyMatrix()); //angular contribution to linear velocites
216 
217 // //angualr part (bottom rows)
218 // J_eigen.block<3,3>(3,4) = Eigen::Matrix3d::Zero(); //linear contribution to angular velocities
219 // J_eigen.bottomRightCorner(3,3) = robInfo->robotState.wTv_eigen.topLeftCorner<3,3>(); //angular contribution to angular velocities
220 
221 // robInfo->robotState.w_JNoKdltool_robot = J_eigen;
222 
223 // return 0;
224 
225 //}
226 
227 
228 
234 /*
235 int computeWholeJacobianTool_old(struct Infos *robInfo){
236 
237 
238  //positional and orientational part of arm jacobian PROJECTED ON WORLD
239  Eigen::Matrix <double, 3, ARM_DOF> w_J_man_pos;
240  Eigen::Matrix <double, 3, ARM_DOF> w_J_man_or;
241  Eigen::Matrix3d wR0 = robInfo->robotState.wTv_eigen.topLeftCorner<3,3>() *
242  robInfo->robotStruct.vTlink0.topLeftCorner<3,3>();
243 
244  //J_man_xxx = wR0 * zeroJ_man_xxx
245  w_J_man_pos = wR0 * robInfo->robotState.link0_Jtool_man.topRows<3>();
246  w_J_man_or = wR0 * robInfo->robotState.link0_Jtool_man.bottomRows<3>();
247 
249  Eigen::Matrix<double, 3, TOT_DOF> w_J_pos;
250 
251  // part relative to joints
252  w_J_pos.leftCols<ARM_DOF>() = w_J_man_pos;
253 
254  // part relative to linear velocities
255  Eigen::Matrix3d wRv = robInfo->robotState.wTv_eigen.topLeftCorner<3,3>();
256  w_J_pos.block<3,3>(0,ARM_DOF) = wRv; //with block indexes start from 0
257 
258  // part relative to angular velocities
259  //distance from vehicle to link 0 respect on vehicle frame
260  Eigen::Vector3d v_Distv0 = robInfo->robotStruct.vTlink0.topRightCorner<3,1>();
261  Eigen::Vector3d w_Distv0 = wRv * v_Distv0;
262  //distance from link0 to TOOL respect to world (wR0 * 0_eta_0,ee in book)
263  Eigen::Vector3d w_Dist0Tool =
264  robInfo->transforms.wTt_eigen.topRightCorner<3,1>() -
265  robInfo->robotState.wTv_eigen.topRightCorner<3,1>() -
266  w_Distv0;
267  Eigen::Vector3d w_DistVTool =
268  robInfo->transforms.wTt_eigen.topRightCorner<3,1>() -
269  robInfo->robotState.wTv_eigen.topRightCorner<3,1>();
270 
271 
272  //skew__XXX : skew of XXX
273  Eigen::Matrix3d skew__w_Dist_v0 = FRM::skewMat(w_Distv0);
274  Eigen::Matrix3d skew__w_Dist_0tool = FRM::skewMat(w_Dist0Tool);
275  Eigen::Matrix3d skew__w_Dist_Vtool = FRM::skewMat(w_DistVTool);
276  //w_J_pos.rightCols<3>() = -(skew__w_Dist_v0 + skew__w_Dist_0tool) * wRv;
277  w_J_pos.rightCols<3>() = -(skew__w_Dist_Vtool) * wRv;
278 
279 
281  Eigen::Matrix<double, 3, TOT_DOF> w_J_or;
282  w_J_or.leftCols<ARM_DOF>() = w_J_man_or; //relative to arm
283  w_J_or.block<3,3>(0,ARM_DOF) = Eigen::Matrix3d::Zero(); //relative to linear vel
284  w_J_or.rightCols<3>() = wRv; //relative to angular vel
285 
287 
288  robInfo->robotState.w_Jtool_robot.topRows<3>() = w_J_pos;
289  robInfo->robotState.w_Jtool_robot.bottomRows<3>() = w_J_or;
290 
291 
292 
293  //DEBUG
294 // std::cout << "kdl first Skew\n" << skew__w_Dist_v0 << "\n\n";
295 // std::cout << "kdl secodn Skew\n" << skew__w_Dist_0tool << "\n\n";
296 // std::cout << "eta1\n"
297 // << robInfo->robotState.wTv_eigen.topRightCorner<3,1>()
298 // << "\n\n";
299 // std::cout << "eta2\n"
300 // << robInfo->robotState.wTv_eigen.topLeftCorner<3,3>()
301 // << "\n\n";
302 // std::cout << "eta_ee1\n"
303 // << robInfo->transforms.wTt_eigen.topRightCorner<3,1>()
304 // << "\n\n";
305 
306 
307 // std::cout << "J: \n"
308 // << robInfo->robotState.w_Jtool_robot
309 // << "\n\n";
310 
311 
312  return 0;
313 }
314 */
RobotState::link0_Jee_man
Eigen::Matrix< double, 6, ARM_DOF > link0_Jee_man
Jacobians.
Definition: infos.h:52
Infos
Definition: infos.h:117
RobotState::wTv_eigen
Eigen::Matrix4d wTv_eigen
Transforms.
Definition: infos.h:34
Generated by   doxygen 1.8.11