Marco De Silva / ARMControl

con arm control a libreria

InverseKinematicsControl.cpp@0:843c9f29adde, 2021-10-04 (annotated)

Committer:
marcodesilva
Date:
Mon Oct 04 13:34:22 2021 +0000
Revision:
0:843c9f29adde
arm control rendi libreria ;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
marcodesilva 0:843c9f29adde 1 #include "InverseKinematicsControl.h"
marcodesilva 0:843c9f29adde 2
marcodesilva 0:843c9f29adde 3 InverseKinematicsControl::InverseKinematicsControl(ARAP180_WITH_ROVER *_arm ,Vector6f _K_first, Vector6f _K_second, Vector6f _D, Vector8f _maxAcc, Vector8f _maxVel, Vector8f _maxPos, Vector8f _minPos, int _n_max_iter, float _max_pError, float _max_oError){
marcodesilva 0:843c9f29adde 4
marcodesilva 0:843c9f29adde 5 arm = _arm;
marcodesilva 0:843c9f29adde 6 maxAcc = _maxAcc;
marcodesilva 0:843c9f29adde 7 maxVel = _maxVel;
marcodesilva 0:843c9f29adde 8 maxPos = _maxPos;
marcodesilva 0:843c9f29adde 9 minPos = _minPos;
marcodesilva 0:843c9f29adde 10
marcodesilva 0:843c9f29adde 11 K_first = _K_first;
marcodesilva 0:843c9f29adde 12 K_second = _K_second;
marcodesilva 0:843c9f29adde 13
marcodesilva 0:843c9f29adde 14 D = _D;
marcodesilva 0:843c9f29adde 15
marcodesilva 0:843c9f29adde 16 n_max_iter = _n_max_iter;
marcodesilva 0:843c9f29adde 17 max_pError = _max_pError;
marcodesilva 0:843c9f29adde 18 max_oError = _max_oError;
marcodesilva 0:843c9f29adde 19
marcodesilva 0:843c9f29adde 20 q = Vector8f::Zero();
marcodesilva 0:843c9f29adde 21 dq = Vector8f::Zero();
marcodesilva 0:843c9f29adde 22 ddq = Vector8f::Zero();
marcodesilva 0:843c9f29adde 23
marcodesilva 0:843c9f29adde 24 ddx = Vector6f::Zero();
marcodesilva 0:843c9f29adde 25
marcodesilva 0:843c9f29adde 26 Te = Matrix4f::Identity();
marcodesilva 0:843c9f29adde 27
marcodesilva 0:843c9f29adde 28 J = Matrix<float,6,8>::Zero();
marcodesilva 0:843c9f29adde 29 pinvJ = Matrix<float,8,6>::Zero();
marcodesilva 0:843c9f29adde 30 J_T = Matrix<float,8,6>::Zero();
marcodesilva 0:843c9f29adde 31 L = Matrix3f::Zero();
marcodesilva 0:843c9f29adde 32 pinvL = Matrix3f::Zero();
marcodesilva 0:843c9f29adde 33
marcodesilva 0:843c9f29adde 34 Jp = Matrix<float,6,8>::Zero();
marcodesilva 0:843c9f29adde 35
marcodesilva 0:843c9f29adde 36 Vector8f W_vec;
marcodesilva 0:843c9f29adde 37 W_vec << 10,1000,1,1,1,1,1,1;
marcodesilva 0:843c9f29adde 38
marcodesilva 0:843c9f29adde 39 W = W_vec.cast<float>().asDiagonal();
marcodesilva 0:843c9f29adde 40 }
marcodesilva 0:843c9f29adde 41
marcodesilva 0:843c9f29adde 42 void InverseKinematicsControl::setInverseKinematicsControlParameters(Vector6f _K_first, Vector6f _K_second, Vector6f _D, Vector8f WeightVector, Vector8f _maxAcc, Vector8f _maxVel, Vector8f _maxPos, Vector8f _minPos, int _n_max_iter, float _max_pError, float _max_oError){
marcodesilva 0:843c9f29adde 43
marcodesilva 0:843c9f29adde 44 maxAcc = _maxAcc;
marcodesilva 0:843c9f29adde 45 maxVel = _maxVel;
marcodesilva 0:843c9f29adde 46 maxPos = _maxPos;
marcodesilva 0:843c9f29adde 47 minPos = _minPos;
marcodesilva 0:843c9f29adde 48
marcodesilva 0:843c9f29adde 49 K_first = _K_first;
marcodesilva 0:843c9f29adde 50 K_second = _K_second;
marcodesilva 0:843c9f29adde 51
marcodesilva 0:843c9f29adde 52 n_max_iter = _n_max_iter;
marcodesilva 0:843c9f29adde 53 max_pError = _max_pError;
marcodesilva 0:843c9f29adde 54 max_oError = _max_oError;
marcodesilva 0:843c9f29adde 55
marcodesilva 0:843c9f29adde 56 W = WeightVector.cast<float>().asDiagonal();
marcodesilva 0:843c9f29adde 57
marcodesilva 0:843c9f29adde 58 }
marcodesilva 0:843c9f29adde 59
marcodesilva 0:843c9f29adde 60
marcodesilva 0:843c9f29adde 61 void InverseKinematicsControl::initInverseKinematicsControl(Vector8f _first_q){
marcodesilva 0:843c9f29adde 62
marcodesilva 0:843c9f29adde 63 q = _first_q;
marcodesilva 0:843c9f29adde 64 dq = Vector8f::Zero();
marcodesilva 0:843c9f29adde 65 ddq = Vector8f::Zero();
marcodesilva 0:843c9f29adde 66 ddx = Vector6f::Zero();
marcodesilva 0:843c9f29adde 67
marcodesilva 0:843c9f29adde 68 }
marcodesilva 0:843c9f29adde 69
marcodesilva 0:843c9f29adde 70 void InverseKinematicsControl::getJointCmd(VectorXf &q_cmd, VectorXf &dq_cmd,VectorXf &ddq_cmd){
marcodesilva 0:843c9f29adde 71 q_cmd.resize(q.size(),1);
marcodesilva 0:843c9f29adde 72 dq_cmd.resize(q.size(),1);
marcodesilva 0:843c9f29adde 73 ddq_cmd.resize(q.size(),1);
marcodesilva 0:843c9f29adde 74 q_cmd = q;
marcodesilva 0:843c9f29adde 75 dq_cmd = dq;
marcodesilva 0:843c9f29adde 76 ddq_cmd = ddq;
marcodesilva 0:843c9f29adde 77
marcodesilva 0:843c9f29adde 78 }
marcodesilva 0:843c9f29adde 79
marcodesilva 0:843c9f29adde 80 void InverseKinematicsControl::getDebugVector(Matrix<float, 24, 1> &_debugVector){
marcodesilva 0:843c9f29adde 81 _debugVector = debugVector;
marcodesilva 0:843c9f29adde 82
marcodesilva 0:843c9f29adde 83 }
marcodesilva 0:843c9f29adde 84
marcodesilva 0:843c9f29adde 85
marcodesilva 0:843c9f29adde 86
marcodesilva 0:843c9f29adde 87 void InverseKinematicsControl::updateInverseKinematicsControl_firstOrder(Matrix4f Td, Vector6f dx_d, Vector8f dq_null_space, float deltaT) {
marcodesilva 0:843c9f29adde 88
marcodesilva 0:843c9f29adde 89 Matrix3f Kp = Matrix3f::Zero();
marcodesilva 0:843c9f29adde 90 Matrix3f Ko = Matrix3f::Zero();
marcodesilva 0:843c9f29adde 91 Kp = K_first.block(0,0,3,1).cast<float>().asDiagonal();
marcodesilva 0:843c9f29adde 92 Ko = K_first.block(3,0,3,1).cast<float>().asDiagonal();
marcodesilva 0:843c9f29adde 93
marcodesilva 0:843c9f29adde 94 //Matrix6f W_inv = Matrix6f::Identity();
marcodesilva 0:843c9f29adde 95
marcodesilva 0:843c9f29adde 96 Vector3f dxp = Vector3f::Zero();
marcodesilva 0:843c9f29adde 97 Vector3f dxo = Vector3f::Zero();
marcodesilva 0:843c9f29adde 98 dx = Vector6f::Zero();
marcodesilva 0:843c9f29adde 99
marcodesilva 0:843c9f29adde 100 int n_iter = 0;
marcodesilva 0:843c9f29adde 101
marcodesilva 0:843c9f29adde 102 float pError = 0.0;
marcodesilva 0:843c9f29adde 103 float oError = 0.0;
marcodesilva 0:843c9f29adde 104
marcodesilva 0:843c9f29adde 105 Te = arm->forwardKinematics(q);
marcodesilva 0:843c9f29adde 106 J = arm->jacobianMatrix(q);
marcodesilva 0:843c9f29adde 107
marcodesilva 0:843c9f29adde 108 ep = Td.block(0,3,3,1) - Te.block(0,3,3,1);
marcodesilva 0:843c9f29adde 109 eo = utilities::rotationMatrixError(Td, Te);
marcodesilva 0:843c9f29adde 110
marcodesilva 0:843c9f29adde 111 pError = ep.norm(); //L_2 norm of vector
marcodesilva 0:843c9f29adde 112 oError = eo.norm();
marcodesilva 0:843c9f29adde 113
marcodesilva 0:843c9f29adde 114 //while(n_iter<n_max_iter && (pError>max_pError || oError>max_oError)){
marcodesilva 0:843c9f29adde 115
marcodesilva 0:843c9f29adde 116 L = utilities::L_matrix(Td.block(0,0,3,3), Te.block(0,0,3,3));
marcodesilva 0:843c9f29adde 117
marcodesilva 0:843c9f29adde 118 pinvL = L.inverse();
marcodesilva 0:843c9f29adde 119
marcodesilva 0:843c9f29adde 120 dxp = dx_d.block(0,0,3,1) + Kp*ep;
marcodesilva 0:843c9f29adde 121
marcodesilva 0:843c9f29adde 122 dxo = pinvL*(L.transpose()*dx_d.block(3,0,3,1) + Ko*eo);
marcodesilva 0:843c9f29adde 123
marcodesilva 0:843c9f29adde 124 dx << dxp[0], dxp[1], dxp[2], dxo[0], dxo[1], dxo[2];
marcodesilva 0:843c9f29adde 125
marcodesilva 0:843c9f29adde 126 J_T = J.transpose();
marcodesilva 0:843c9f29adde 127
marcodesilva 0:843c9f29adde 128 // Pseudo inversa a destra perchè ho più giunti che gradi di libertà
marcodesilva 0:843c9f29adde 129 FullPivLU<Matrix6f> Core_J(J*W.inverse()*J_T);
marcodesilva 0:843c9f29adde 130 pinvJ = W.inverse()*J_T*Core_J.inverse();
marcodesilva 0:843c9f29adde 131
marcodesilva 0:843c9f29adde 132 //pinvJ = J.inverse();
marcodesilva 0:843c9f29adde 133
marcodesilva 0:843c9f29adde 134 dq = pinvJ*dx + (Matrix<float,8,8>::Identity() - pinvJ*J)*dq_null_space;
marcodesilva 0:843c9f29adde 135
marcodesilva 0:843c9f29adde 136 /*for(int j = 0; j<dq.size(); j++){
marcodesilva 0:843c9f29adde 137 if(dq(j) > maxVel(j)){ dq(j) = maxVel(j);}
marcodesilva 0:843c9f29adde 138 if(dq(j) < -maxVel(j)){ dq(j) = -maxVel(j);}
marcodesilva 0:843c9f29adde 139 }*/
marcodesilva 0:843c9f29adde 140
marcodesilva 0:843c9f29adde 141
marcodesilva 0:843c9f29adde 142 q += dq*deltaT;
marcodesilva 0:843c9f29adde 143
marcodesilva 0:843c9f29adde 144 for(int j = 0; j<q.size(); j++){
marcodesilva 0:843c9f29adde 145 if(q(j) > maxPos(j)){ q(j) = maxPos(j);}
marcodesilva 0:843c9f29adde 146 if(q(j) < minPos(j)){ q(j) = minPos(j);}
marcodesilva 0:843c9f29adde 147 }
marcodesilva 0:843c9f29adde 148
marcodesilva 0:843c9f29adde 149
marcodesilva 0:843c9f29adde 150 /*Te = arm->forwardKinematics(q);
marcodesilva 0:843c9f29adde 151 J = arm->jacobianMatrix(q);
marcodesilva 0:843c9f29adde 152
marcodesilva 0:843c9f29adde 153 ep = Td.block(0,3,3,1) - Te.block(0,3,3,1);
marcodesilva 0:843c9f29adde 154 eo = utilities::rotationMatrixError(Td, Te);
marcodesilva 0:843c9f29adde 155
marcodesilva 0:843c9f29adde 156 pError = ep.norm(); //L_2 norm of vector
marcodesilva 0:843c9f29adde 157 oError = eo.norm();
marcodesilva 0:843c9f29adde 158
marcodesilva 0:843c9f29adde 159 debugVector.block(0,0,6,1) << ep[0], ep[1], ep[2], eo[0], eo[1], eo[2];
marcodesilva 0:843c9f29adde 160 debugVector.block(6,0,6,1) << de;
marcodesilva 0:843c9f29adde 161 //debugVector.block(12,0,6,1) << dq;
marcodesilva 0:843c9f29adde 162 //debugVector.block(18,0,6,1) << ddq;
marcodesilva 0:843c9f29adde 163
marcodesilva 0:843c9f29adde 164 n_iter++;*/
marcodesilva 0:843c9f29adde 165
marcodesilva 0:843c9f29adde 166 //}
marcodesilva 0:843c9f29adde 167
marcodesilva 0:843c9f29adde 168 }
marcodesilva 0:843c9f29adde 169
marcodesilva 0:843c9f29adde 170
marcodesilva 0:843c9f29adde 171 //SECOND ORDER CLICK
marcodesilva 0:843c9f29adde 172
marcodesilva 0:843c9f29adde 173 void InverseKinematicsControl::updateInverseKinematicsControl_secondOrder(Matrix4f Td, Vector6f dx_d, Vector6f ddx_d, float deltaT) {
marcodesilva 0:843c9f29adde 174
marcodesilva 0:843c9f29adde 175
marcodesilva 0:843c9f29adde 176 Matrix6f K_mat = Matrix6f::Zero();
marcodesilva 0:843c9f29adde 177 Matrix6f D_mat = Matrix6f::Zero();
marcodesilva 0:843c9f29adde 178 K_mat = K_second.cast<float>().asDiagonal();
marcodesilva 0:843c9f29adde 179 D_mat = D.cast<float>().asDiagonal();
marcodesilva 0:843c9f29adde 180
marcodesilva 0:843c9f29adde 181 int n_iter = 0;
marcodesilva 0:843c9f29adde 182
marcodesilva 0:843c9f29adde 183 float pError = 0.0;
marcodesilva 0:843c9f29adde 184 float oError = 0.0;
marcodesilva 0:843c9f29adde 185
marcodesilva 0:843c9f29adde 186 Te = arm->forwardKinematics(q);
marcodesilva 0:843c9f29adde 187 J = arm->jacobianMatrix(q);
marcodesilva 0:843c9f29adde 188 L = utilities::L_matrix(Td.block(0,0,3,3), Te.block(0,0,3,3));
marcodesilva 0:843c9f29adde 189
marcodesilva 0:843c9f29adde 190 ep = Td.block(0,3,3,1) - Te.block(0,3,3,1);
marcodesilva 0:843c9f29adde 191 eo = utilities::rotationMatrixError(Td, Te);
marcodesilva 0:843c9f29adde 192
marcodesilva 0:843c9f29adde 193 dep = dx_d.block(0,0,3,1) - J.block(0,0,3,8)*dq; //dq deve entrare
marcodesilva 0:843c9f29adde 194 deo = L.transpose()*dx_d.block(3,0,3,1) - L*J.block(3,0,3,8)*dq; //
marcodesilva 0:843c9f29adde 195 de << dep[0], dep[1], dep[2], deo[0], deo[1], deo[2];
marcodesilva 0:843c9f29adde 196 e << ep[0], ep[1], ep[2], eo[0], eo[1], eo[2];
marcodesilva 0:843c9f29adde 197
marcodesilva 0:843c9f29adde 198 pError = ep.norm(); //L_2 norm of vector
marcodesilva 0:843c9f29adde 199 oError = eo.norm();
marcodesilva 0:843c9f29adde 200 Matrix<float,6,8> J_dot;
marcodesilva 0:843c9f29adde 201
marcodesilva 0:843c9f29adde 202 while(n_iter<n_max_iter && (pError>max_pError || oError>max_oError)){
marcodesilva 0:843c9f29adde 203
marcodesilva 0:843c9f29adde 204
marcodesilva 0:843c9f29adde 205 J_dot = (J - Jp)/deltaT;
marcodesilva 0:843c9f29adde 206 Jp = J;
marcodesilva 0:843c9f29adde 207
marcodesilva 0:843c9f29adde 208 ddx = ddx_d + D_mat*de + K_mat*e - J_dot*dq; //arm->jacobianTimeDerivativeMatrix(q, dq)*dq
marcodesilva 0:843c9f29adde 209
marcodesilva 0:843c9f29adde 210 J_T = J.transpose();
marcodesilva 0:843c9f29adde 211
marcodesilva 0:843c9f29adde 212 FullPivLU<Matrix6f> Core_J(J*W.inverse()*J_T);
marcodesilva 0:843c9f29adde 213 pinvJ = W.inverse()*J_T*Core_J.inverse();
marcodesilva 0:843c9f29adde 214
marcodesilva 0:843c9f29adde 215
marcodesilva 0:843c9f29adde 216 //pinvJ = J.inverse();
marcodesilva 0:843c9f29adde 217
marcodesilva 0:843c9f29adde 218 ddq = pinvJ*ddx;
marcodesilva 0:843c9f29adde 219
marcodesilva 0:843c9f29adde 220
marcodesilva 0:843c9f29adde 221 /*for(int j = 0; j<ddq.size(); j++){
marcodesilva 0:843c9f29adde 222 if(ddq(j) > maxAcc(j)){ ddq(j) = maxAcc(j);}
marcodesilva 0:843c9f29adde 223 if(ddq(j) < -maxAcc(j)){ ddq(j) = -maxAcc(j);}
marcodesilva 0:843c9f29adde 224 }*/
marcodesilva 0:843c9f29adde 225
marcodesilva 0:843c9f29adde 226 dq += ddq*deltaT;
marcodesilva 0:843c9f29adde 227
marcodesilva 0:843c9f29adde 228 /*for(int j = 0; j<dq.size(); j++){
marcodesilva 0:843c9f29adde 229 if(dq(j) > maxVel(j)){ dq(j) = maxVel(j);}
marcodesilva 0:843c9f29adde 230 if(dq(j) < -maxVel(j)){ dq(j) = -maxVel(j);}
marcodesilva 0:843c9f29adde 231 }*/
marcodesilva 0:843c9f29adde 232
marcodesilva 0:843c9f29adde 233 q += dq*deltaT;
marcodesilva 0:843c9f29adde 234
marcodesilva 0:843c9f29adde 235 for(int j = 0; j<q.size(); j++){
marcodesilva 0:843c9f29adde 236 if(q(j) > maxPos(j)){ q(j) = maxPos(j);}
marcodesilva 0:843c9f29adde 237 if(q(j) < minPos(j)){ q(j) = minPos(j);}
marcodesilva 0:843c9f29adde 238 }
marcodesilva 0:843c9f29adde 239
marcodesilva 0:843c9f29adde 240
marcodesilva 0:843c9f29adde 241 Te = arm->forwardKinematics(q);
marcodesilva 0:843c9f29adde 242 J = arm->jacobianMatrix(q);
marcodesilva 0:843c9f29adde 243 L = utilities::L_matrix(Td.block(0,0,3,3), Te.block(0,0,3,3));
marcodesilva 0:843c9f29adde 244
marcodesilva 0:843c9f29adde 245
marcodesilva 0:843c9f29adde 246 ep = Td.block(0,3,3,1) - Te.block(0,3,3,1);
marcodesilva 0:843c9f29adde 247 eo = utilities::rotationMatrixError(Td, Te);
marcodesilva 0:843c9f29adde 248
marcodesilva 0:843c9f29adde 249 dep = dx_d.block(0,0,3,1) - J.block(0,0,3,8)*dq; //dq deve entrare
marcodesilva 0:843c9f29adde 250 deo = L.transpose()*dx_d.block(3,0,3,1) - L*J.block(3,0,3,8)*dq; //
marcodesilva 0:843c9f29adde 251 de << dep[0], dep[1], dep[2], deo[0], deo[1], deo[2];
marcodesilva 0:843c9f29adde 252 e << ep[0], ep[1], ep[2], eo[0], eo[1], eo[2];
marcodesilva 0:843c9f29adde 253
marcodesilva 0:843c9f29adde 254 pError = ep.norm(); //L_2 norm of vector
marcodesilva 0:843c9f29adde 255 oError = eo.norm();
marcodesilva 0:843c9f29adde 256
marcodesilva 0:843c9f29adde 257 debugVector.block(0,0,6,1) << e;
marcodesilva 0:843c9f29adde 258 debugVector.block(6,0,6,1) << de;
marcodesilva 0:843c9f29adde 259 //debugVector.block(12,0,6,1) << dq;
marcodesilva 0:843c9f29adde 260 //debugVector.block(18,0,6,1) << ddq;
marcodesilva 0:843c9f29adde 261
marcodesilva 0:843c9f29adde 262 n_iter++;
marcodesilva 0:843c9f29adde 263
marcodesilva 0:843c9f29adde 264 }
marcodesilva 0:843c9f29adde 265
marcodesilva 0:843c9f29adde 266 }
marcodesilva 0:843c9f29adde 267