AUV-Coop-Assembly
Master Thesis for Robotics Engineering. Cooperative peg-in-hole assembly with two underwater vehicles guided by vision
graspConstrainer.cpp
1 #include "header/graspConstrainer.h"
2 
3 GraspConstrainer::GraspConstrainer() {} //private, uncallable
4 
6  Eigen::Matrix<double, 6, ARM_DOF> w_J_man,
7  Eigen::Matrix4d otherEE_T_EE) {
8 
9  CMAT::TransfMatrix otherEE_T_EE_cmat = CONV::matrix_eigen2cmat(otherEE_T_EE);
10  CMAT::TransfMatrix desiredT = CMAT::Matrix::Eye(4);
11  CMAT::Matrix w_J_man_cmat = CONV::matrix_eigen2cmat(w_J_man);
12 
13  CMAT::Vect6 errorSwapped = CMAT::CartError(desiredT, otherEE_T_EE_cmat);//ang;lin
14  // ang and lin must be swapped because in yDot and jacob linear part is before
15  CMAT::Vect6 error;
16  error.SetFirstVect3(errorSwapped.GetSecondVect3());
17  error.SetSecondVect3(errorSwapped.GetFirstVect3());
18 
19  //gain
20  error = 1 * error;
21 
22  //saturate
23  //error.SetFirstVect3(FRM::saturateCmat(error.GetFirstVect3(), 0.5)); //linear
24  //error.SetSecondVect3(FRM::saturateCmat(error.GetSecondVect3(), 0.2)); //angular
25 
26  CMAT::Matrix velocities_cmat =
27  (w_J_man_cmat.RegPseudoInverse(THRESHOLD_DEFAULT, LAMBDA_DEFAULT)) * error;
28 
29 
30  std::vector<double> velocities;
31  velocities.resize(ARM_DOF); //to be sure about dimension
32  velocities = CONV::vector_cmat2std(velocities_cmat);
33 
34  return velocities;
35 
36 }
37 
39  Eigen::Matrix<double, 6, TOT_DOF> w_Jee_robot,
40  Eigen::Matrix4d wTotherPeg, Eigen::Matrix4d wTee) {
41 
42  CMAT::TransfMatrix wTotherPeg_cmat = CONV::matrix_eigen2cmat(wTotherPeg);
43  CMAT::TransfMatrix wTee_cmat = CONV::matrix_eigen2cmat(wTee);
44  CMAT::Matrix w_Jee_robot_cmat = CONV::matrix_eigen2cmat(w_Jee_robot);
45 
46  CMAT::Vect6 errorSwapped = CMAT::CartError(wTotherPeg_cmat, wTee_cmat);//ang;lin
47  // ang and lin must be swapped because in yDot and jacob linear part is before
48  CMAT::Vect6 error;
49  error.SetFirstVect3(errorSwapped.GetSecondVect3());
50  error.SetSecondVect3(errorSwapped.GetFirstVect3());
51 
52  //gain
53  error = 3 * error;
54 
55  //saturate
56  //error.SetFirstVect3(FRM::saturateCmat(error.GetFirstVect3(), 0.5)); //linear
57  //error.SetSecondVect3(FRM::saturateCmat(error.GetSecondVect3(), 0.2)); //angular
58 
59  CMAT::Matrix velocities_cmat =
60  (w_Jee_robot_cmat.RegPseudoInverse(THRESHOLD_DEFAULT, LAMBDA_DEFAULT)) * error;
61 
62 
63  std::vector<double> velocities;
64  velocities.resize(TOT_DOF); //to be sure about dimension
65  velocities = CONV::vector_cmat2std(velocities_cmat);
66 
67  return velocities;
68 
69 
70 
71 }
72 
73 
74 //std::vector<double> GraspConstrainer::calculateGraspVelocities_armVeh(
75 // Eigen::Matrix<double, 6, TOT_DOF> w_Jee_robot,
76 // Eigen::Matrix4d otherEE_T_EE) {
77 
78 // CMAT::TransfMatrix otherEE_T_EE_cmat = CONV::matrix_eigen2cmat(otherEE_T_EE);
79 // CMAT::TransfMatrix desiredT = CMAT::Matrix::Eye(4);
80 // CMAT::Matrix w_Jee_robot_cmat = CONV::matrix_eigen2cmat(w_Jee_robot);
81 
82 // CMAT::Vect6 errorSwapped = CMAT::CartError(desiredT, otherEE_T_EE_cmat);//ang;lin
83 // // ang and lin must be swapped because in yDot and jacob linear part is before
84 // CMAT::Vect6 error;
85 // error.SetFirstVect3(errorSwapped.GetSecondVect3());
86 // error.SetSecondVect3(errorSwapped.GetFirstVect3());
87 
88 // //gain
89 // error = 0.1 * error;
90 
91 // //saturate
92 // //error.SetFirstVect3(FRM::saturateCmat(error.GetFirstVect3(), 0.5)); //linear
93 // //error.SetSecondVect3(FRM::saturateCmat(error.GetSecondVect3(), 0.2)); //angular
94 
95 // CMAT::Matrix velocities_cmat =
96 // (w_Jee_robot_cmat.RegPseudoInverse(THRESHOLD_DEFAULT, LAMBDA_DEFAULT)) * error;
97 
98 
99 // std::vector<double> velocities;
100 // velocities.resize(TOT_DOF); //to be sure about dimension
101 // velocities = CONV::vector_cmat2std(velocities_cmat);
102 
103 // return velocities;
104 
105 
106 
107 //}
static std::vector< double > calculateGraspVelocities_armVeh(Eigen::Matrix< double, 6, TOT_DOF > w_Jee_robot, Eigen::Matrix4d wTotherPeg, Eigen::Matrix4d wTee)
calculateGraspVelocities_armVeh for effect of firm grasp on the arm and on the vehicle ...
static std::vector< double > calculateGraspVelocities_arm(Eigen::Matrix< double, 6, ARM_DOF > w_J_man, Eigen::Matrix4d otherEE_T_EE)
calculateGraspVelocities_arm for effect of firm grasp only on the arm