1 #include "header/graspConstrainer.h" 3 GraspConstrainer::GraspConstrainer() {}
6 Eigen::Matrix<double, 6, ARM_DOF> w_J_man,
7 Eigen::Matrix4d otherEE_T_EE) {
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);
13 CMAT::Vect6 errorSwapped = CMAT::CartError(desiredT, otherEE_T_EE_cmat);
16 error.SetFirstVect3(errorSwapped.GetSecondVect3());
17 error.SetSecondVect3(errorSwapped.GetFirstVect3());
26 CMAT::Matrix velocities_cmat =
27 (w_J_man_cmat.RegPseudoInverse(THRESHOLD_DEFAULT, LAMBDA_DEFAULT)) * error;
30 std::vector<double> velocities;
31 velocities.resize(ARM_DOF);
32 velocities = CONV::vector_cmat2std(velocities_cmat);
39 Eigen::Matrix<double, 6, TOT_DOF> w_Jee_robot,
40 Eigen::Matrix4d wTotherPeg, Eigen::Matrix4d wTee) {
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);
46 CMAT::Vect6 errorSwapped = CMAT::CartError(wTotherPeg_cmat, wTee_cmat);
49 error.SetFirstVect3(errorSwapped.GetSecondVect3());
50 error.SetSecondVect3(errorSwapped.GetFirstVect3());
59 CMAT::Matrix velocities_cmat =
60 (w_Jee_robot_cmat.RegPseudoInverse(THRESHOLD_DEFAULT, LAMBDA_DEFAULT)) * error;
63 std::vector<double> velocities;
64 velocities.resize(TOT_DOF);
65 velocities = CONV::vector_cmat2std(velocities_cmat);
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