Team DIANA / Mbed 2 deprecated IPKF_Test_nucleo

test_IPKF

Dependencies:   mbed

source/kinematic.c@0:fb6e494a7656, 2016-04-20 (annotated)

Committer:
LudovicoDani
Date:
Wed Apr 20 10:03:58 2016 +0000
Revision:
0:fb6e494a7656
IPKF_Test_nucleo

Who changed what in which revision?

UserRevisionLine numberNew contents of line
LudovicoDani 0:fb6e494a7656 1 #include "kinematic.h"
LudovicoDani 0:fb6e494a7656 2 #include <stdio.h>
LudovicoDani 0:fb6e494a7656 3
LudovicoDani 0:fb6e494a7656 4 /**
LudovicoDani 0:fb6e494a7656 5 * @defgroup MACRO_GROUP
LudovicoDani 0:fb6e494a7656 6 *
LudovicoDani 0:fb6e494a7656 7 * @{
LudovicoDani 0:fb6e494a7656 8 */
LudovicoDani 0:fb6e494a7656 9 /**
LudovicoDani 0:fb6e494a7656 10 * @brief
LudovicoDani 0:fb6e494a7656 11 * Macro used to build the jacobian
LudovicoDani 0:fb6e494a7656 12 * matrix column by column
LudovicoDani 0:fb6e494a7656 13 */
LudovicoDani 0:fb6e494a7656 14 #define BUILD_JACOBIAN(J, p, z, i) \
LudovicoDani 0:fb6e494a7656 15 J->element[0][i]=p.element[0][0];\
LudovicoDani 0:fb6e494a7656 16 J->element[1][i]=p.element[1][0];\
LudovicoDani 0:fb6e494a7656 17 J->element[2][i]=p.element[2][0];\
LudovicoDani 0:fb6e494a7656 18 J->element[3][i]=z.element[0][0];\
LudovicoDani 0:fb6e494a7656 19 J->element[4][i]=z.element[1][0];\
LudovicoDani 0:fb6e494a7656 20 J->element[5][i]=z.element[2][0];
LudovicoDani 0:fb6e494a7656 21
LudovicoDani 0:fb6e494a7656 22 /**
LudovicoDani 0:fb6e494a7656 23 * @brief
LudovicoDani 0:fb6e494a7656 24 * Macro used to check if the joint angle
LudovicoDani 0:fb6e494a7656 25 * are in the interval between -pi and pi
LudovicoDani 0:fb6e494a7656 26 */
LudovicoDani 0:fb6e494a7656 27 #define CHECK_ANGLE()\
LudovicoDani 0:fb6e494a7656 28 while(joint->element[0][0] > PI)\
LudovicoDani 0:fb6e494a7656 29 joint->element[0][0] = joint->element[0][0] - 2*PI;\
LudovicoDani 0:fb6e494a7656 30 while(joint->element[0][0] < -PI)\
LudovicoDani 0:fb6e494a7656 31 joint->element[0][0] = joint->element[0][0] + 2*PI;\
LudovicoDani 0:fb6e494a7656 32 \
LudovicoDani 0:fb6e494a7656 33 while(joint->element[1][0] > PI)\
LudovicoDani 0:fb6e494a7656 34 joint->element[1][0] = joint->element[1][0] - 2*PI;\
LudovicoDani 0:fb6e494a7656 35 while(joint->element[1][0] < -PI)\
LudovicoDani 0:fb6e494a7656 36 joint->element[1][0] = joint->element[1][0] + 2*PI;\
LudovicoDani 0:fb6e494a7656 37 \
LudovicoDani 0:fb6e494a7656 38 while(joint->element[2][0] > PI)\
LudovicoDani 0:fb6e494a7656 39 joint->element[2][0] = joint->element[2][0] - 2*PI;\
LudovicoDani 0:fb6e494a7656 40 while(joint->element[2][0] < -PI)\
LudovicoDani 0:fb6e494a7656 41 joint->element[2][0] = joint->element[2][0] + 2*PI;\
LudovicoDani 0:fb6e494a7656 42 \
LudovicoDani 0:fb6e494a7656 43 while(joint->element[3][0] > PI)\
LudovicoDani 0:fb6e494a7656 44 joint->element[3][0] = joint->element[3][0] - 2*PI;\
LudovicoDani 0:fb6e494a7656 45 while(joint->element[3][0] < -PI)\
LudovicoDani 0:fb6e494a7656 46 joint->element[3][0] = joint->element[3][0] + 2*PI;\
LudovicoDani 0:fb6e494a7656 47 \
LudovicoDani 0:fb6e494a7656 48 while(joint->element[4][0] > PI)\
LudovicoDani 0:fb6e494a7656 49 joint->element[4][0] = joint->element[4][0] - 2*PI;\
LudovicoDani 0:fb6e494a7656 50 while(joint->element[4][0] < -PI)\
LudovicoDani 0:fb6e494a7656 51 joint->element[4][0] = joint->element[4][0] + 2*PI;\
LudovicoDani 0:fb6e494a7656 52 \
LudovicoDani 0:fb6e494a7656 53 while(joint->element[5][0] > PI)\
LudovicoDani 0:fb6e494a7656 54 joint->element[5][0] = joint->element[5][0] - 2*PI;\
LudovicoDani 0:fb6e494a7656 55 while(joint->element[5][0] < -PI)\
LudovicoDani 0:fb6e494a7656 56 joint->element[5][0] = joint->element[5][0] + 2*PI;\
LudovicoDani 0:fb6e494a7656 57 /** @} */
LudovicoDani 0:fb6e494a7656 58
LudovicoDani 0:fb6e494a7656 59 void DHP_update(DHP* dhp,matrix* joints)
LudovicoDani 0:fb6e494a7656 60 {
LudovicoDani 0:fb6e494a7656 61 dhp[0].d = l1;
LudovicoDani 0:fb6e494a7656 62 dhp[0].theta = joints->element[0][0];
LudovicoDani 0:fb6e494a7656 63 dhp[0].a = 0;
LudovicoDani 0:fb6e494a7656 64 dhp[0].alpha = PI/2;
LudovicoDani 0:fb6e494a7656 65
LudovicoDani 0:fb6e494a7656 66 dhp[1].d = 0;
LudovicoDani 0:fb6e494a7656 67 dhp[1].theta = joints->element[1][0];
LudovicoDani 0:fb6e494a7656 68 dhp[1].a = l2;
LudovicoDani 0:fb6e494a7656 69 dhp[1].alpha = 0;
LudovicoDani 0:fb6e494a7656 70
LudovicoDani 0:fb6e494a7656 71 dhp[2].d = 0;
LudovicoDani 0:fb6e494a7656 72 dhp[2].theta = joints->element[2][0];
LudovicoDani 0:fb6e494a7656 73 dhp[2].a = l3;
LudovicoDani 0:fb6e494a7656 74 dhp[2].alpha = 0;
LudovicoDani 0:fb6e494a7656 75
LudovicoDani 0:fb6e494a7656 76 dhp[3].d = 0;
LudovicoDani 0:fb6e494a7656 77 dhp[3].theta = joints->element[3][0];
LudovicoDani 0:fb6e494a7656 78 dhp[3].a= l4;
LudovicoDani 0:fb6e494a7656 79 dhp[3].alpha = -PI/2;
LudovicoDani 0:fb6e494a7656 80
LudovicoDani 0:fb6e494a7656 81 dhp[4].d = 0;
LudovicoDani 0:fb6e494a7656 82 dhp[4].theta = joints->element[4][0] + PI/2;
LudovicoDani 0:fb6e494a7656 83 dhp[4].a = 0;
LudovicoDani 0:fb6e494a7656 84 dhp[4].alpha = PI/2;
LudovicoDani 0:fb6e494a7656 85
LudovicoDani 0:fb6e494a7656 86 dhp[5].d = l5;
LudovicoDani 0:fb6e494a7656 87 dhp[5].theta = joints->element[5][0];
LudovicoDani 0:fb6e494a7656 88 dhp[5].a = 0;
LudovicoDani 0:fb6e494a7656 89 dhp[5].alpha = 0;
LudovicoDani 0:fb6e494a7656 90
LudovicoDani 0:fb6e494a7656 91 return;
LudovicoDani 0:fb6e494a7656 92 }
LudovicoDani 0:fb6e494a7656 93
LudovicoDani 0:fb6e494a7656 94 void DH_Transf(DHP* dhp, matrix* T)
LudovicoDani 0:fb6e494a7656 95 {
LudovicoDani 0:fb6e494a7656 96
LudovicoDani 0:fb6e494a7656 97 T->element[0][0] = (float)cos(dhp->theta);
LudovicoDani 0:fb6e494a7656 98 T->element[0][1] = (float)(-sin(dhp->theta)*cos(dhp->alpha));
LudovicoDani 0:fb6e494a7656 99 T->element[0][2] = (float)(sin(dhp->theta)*sin(dhp->alpha));
LudovicoDani 0:fb6e494a7656 100 T->element[0][3] = (float)(dhp->a * cos(dhp->theta));
LudovicoDani 0:fb6e494a7656 101
LudovicoDani 0:fb6e494a7656 102 T->element[1][0] = (float)sin(dhp->theta);
LudovicoDani 0:fb6e494a7656 103 T->element[1][1] = (float)(cos(dhp->theta)*cos(dhp->alpha));
LudovicoDani 0:fb6e494a7656 104 T->element[1][2] = (float)(-cos(dhp->theta)*sin(dhp->alpha));
LudovicoDani 0:fb6e494a7656 105 T->element[1][3] = (float)(dhp->a * sin(dhp->theta));
LudovicoDani 0:fb6e494a7656 106
LudovicoDani 0:fb6e494a7656 107 T->element[2][0] = 0;
LudovicoDani 0:fb6e494a7656 108 T->element[2][1] = (float)sin(dhp->alpha);
LudovicoDani 0:fb6e494a7656 109 T->element[2][2] = (float)cos(dhp->alpha);
LudovicoDani 0:fb6e494a7656 110 T->element[2][3] = dhp->d;
LudovicoDani 0:fb6e494a7656 111
LudovicoDani 0:fb6e494a7656 112 T->element[3][0] = 0;
LudovicoDani 0:fb6e494a7656 113 T->element[3][1] = 0;
LudovicoDani 0:fb6e494a7656 114 T->element[3][2] = 0;
LudovicoDani 0:fb6e494a7656 115 T->element[3][3] = 1;
LudovicoDani 0:fb6e494a7656 116
LudovicoDani 0:fb6e494a7656 117 return;
LudovicoDani 0:fb6e494a7656 118 }
LudovicoDani 0:fb6e494a7656 119
LudovicoDani 0:fb6e494a7656 120 void DPKF(matrix* joint, matrix* p)
LudovicoDani 0:fb6e494a7656 121 {
LudovicoDani 0:fb6e494a7656 122
LudovicoDani 0:fb6e494a7656 123 int val = 0;
LudovicoDani 0:fb6e494a7656 124 matrix T01;
LudovicoDani 0:fb6e494a7656 125 matrix T12;
LudovicoDani 0:fb6e494a7656 126 matrix T23;
LudovicoDani 0:fb6e494a7656 127 matrix T34;
LudovicoDani 0:fb6e494a7656 128 matrix T45;
LudovicoDani 0:fb6e494a7656 129 matrix T56;
LudovicoDani 0:fb6e494a7656 130 matrix T_TCP;
LudovicoDani 0:fb6e494a7656 131
LudovicoDani 0:fb6e494a7656 132 DHP dhp[6];
LudovicoDani 0:fb6e494a7656 133
LudovicoDani 0:fb6e494a7656 134 /**< initialization of matrices */
LudovicoDani 0:fb6e494a7656 135 InitMatrix(&T01,4,4);
LudovicoDani 0:fb6e494a7656 136 InitMatrix(&T12,4,4);
LudovicoDani 0:fb6e494a7656 137 InitMatrix(&T23,4,4);
LudovicoDani 0:fb6e494a7656 138 InitMatrix(&T34,4,4);
LudovicoDani 0:fb6e494a7656 139 InitMatrix(&T45,4,4);
LudovicoDani 0:fb6e494a7656 140 InitMatrix(&T56,4,4);
LudovicoDani 0:fb6e494a7656 141 InitMatrix(&T_TCP,4,4);
LudovicoDani 0:fb6e494a7656 142
LudovicoDani 0:fb6e494a7656 143 /**< DHP initialization*/
LudovicoDani 0:fb6e494a7656 144 DHP_update(dhp, joint);
LudovicoDani 0:fb6e494a7656 145
LudovicoDani 0:fb6e494a7656 146 /**< Homogeneous transformations */
LudovicoDani 0:fb6e494a7656 147 DH_Transf(&dhp[0], &T01);
LudovicoDani 0:fb6e494a7656 148 DH_Transf(&dhp[1], &T12);
LudovicoDani 0:fb6e494a7656 149 DH_Transf(&dhp[2], &T23);
LudovicoDani 0:fb6e494a7656 150 DH_Transf(&dhp[3], &T34);
LudovicoDani 0:fb6e494a7656 151 DH_Transf(&dhp[4], &T45);
LudovicoDani 0:fb6e494a7656 152 DH_Transf(&dhp[5], &T56);
LudovicoDani 0:fb6e494a7656 153
LudovicoDani 0:fb6e494a7656 154 /**< Tool center point transformation*/
LudovicoDani 0:fb6e494a7656 155 val = MxM(&T01, &T12, &T_TCP);
LudovicoDani 0:fb6e494a7656 156 val += MxM(&T_TCP, &T23, &T_TCP);
LudovicoDani 0:fb6e494a7656 157 val += MxM(&T_TCP, &T34, &T_TCP);
LudovicoDani 0:fb6e494a7656 158 val += MxM(&T_TCP, &T45, &T_TCP);
LudovicoDani 0:fb6e494a7656 159 val += MxM(&T_TCP, &T56, &T_TCP);
LudovicoDani 0:fb6e494a7656 160
LudovicoDani 0:fb6e494a7656 161 if (val!=0)
LudovicoDani 0:fb6e494a7656 162 {
LudovicoDani 0:fb6e494a7656 163 return;
LudovicoDani 0:fb6e494a7656 164 }
LudovicoDani 0:fb6e494a7656 165
LudovicoDani 0:fb6e494a7656 166 /**< pose extrapolation*/
LudovicoDani 0:fb6e494a7656 167 p->element[0][0] = T_TCP.element[0][3];
LudovicoDani 0:fb6e494a7656 168 p->element[1][0] = T_TCP.element[1][3];
LudovicoDani 0:fb6e494a7656 169 p->element[2][0] = T_TCP.element[2][3];
LudovicoDani 0:fb6e494a7656 170 p->element[3][0] = (float)atan2(T_TCP.element[2][1],T_TCP.element[2][2]);
LudovicoDani 0:fb6e494a7656 171 p->element[4][0] = (float)atan2(-T_TCP.element[2][0],sin(p->element[3][0])*T_TCP.element[2][1] + cos(p->element[3][0])*T_TCP.element[2][2]);
LudovicoDani 0:fb6e494a7656 172 p->element[5][0] = (float)atan2(-cos(p->element[3][0])*T_TCP.element[0][1]+ sin(p->element[3][0]) * T_TCP.element[0][2],cos(p->element[3][0]) * T_TCP.element[1][1]- sin(p->element[3][0]) * T_TCP.element[1][2]);
LudovicoDani 0:fb6e494a7656 173
LudovicoDani 0:fb6e494a7656 174
LudovicoDani 0:fb6e494a7656 175 /**< matrix delation*/
LudovicoDani 0:fb6e494a7656 176 DeleteMatrix(&T01);
LudovicoDani 0:fb6e494a7656 177 DeleteMatrix(&T12);
LudovicoDani 0:fb6e494a7656 178 DeleteMatrix(&T23);
LudovicoDani 0:fb6e494a7656 179 DeleteMatrix(&T34);
LudovicoDani 0:fb6e494a7656 180 DeleteMatrix(&T45);
LudovicoDani 0:fb6e494a7656 181 DeleteMatrix(&T56);
LudovicoDani 0:fb6e494a7656 182 DeleteMatrix(&T_TCP);
LudovicoDani 0:fb6e494a7656 183
LudovicoDani 0:fb6e494a7656 184 return;
LudovicoDani 0:fb6e494a7656 185 }
LudovicoDani 0:fb6e494a7656 186
LudovicoDani 0:fb6e494a7656 187 void JacobianG(matrix* joint, matrix* Jg, matrix* T_TCP)
LudovicoDani 0:fb6e494a7656 188 {
LudovicoDani 0:fb6e494a7656 189 int i = 0;
LudovicoDani 0:fb6e494a7656 190 DHP dhp[6];
LudovicoDani 0:fb6e494a7656 191
LudovicoDani 0:fb6e494a7656 192 matrix T01;
LudovicoDani 0:fb6e494a7656 193 matrix T12;
LudovicoDani 0:fb6e494a7656 194 matrix T23;
LudovicoDani 0:fb6e494a7656 195 matrix T34;
LudovicoDani 0:fb6e494a7656 196 matrix T45;
LudovicoDani 0:fb6e494a7656 197 matrix T56;
LudovicoDani 0:fb6e494a7656 198
LudovicoDani 0:fb6e494a7656 199 matrix p0;
LudovicoDani 0:fb6e494a7656 200 matrix p1;
LudovicoDani 0:fb6e494a7656 201 matrix p2;
LudovicoDani 0:fb6e494a7656 202 matrix p3;
LudovicoDani 0:fb6e494a7656 203 matrix p4;
LudovicoDani 0:fb6e494a7656 204 matrix p5;
LudovicoDani 0:fb6e494a7656 205 matrix p_TCP;
LudovicoDani 0:fb6e494a7656 206
LudovicoDani 0:fb6e494a7656 207 matrix z0;
LudovicoDani 0:fb6e494a7656 208 matrix z1;
LudovicoDani 0:fb6e494a7656 209 matrix z2;
LudovicoDani 0:fb6e494a7656 210 matrix z3;
LudovicoDani 0:fb6e494a7656 211 matrix z4;
LudovicoDani 0:fb6e494a7656 212 matrix z5;
LudovicoDani 0:fb6e494a7656 213
LudovicoDani 0:fb6e494a7656 214 matrix CrossP;
LudovicoDani 0:fb6e494a7656 215 //p is matrix of RF origin positions
LudovicoDani 0:fb6e494a7656 216 //z is matrix of k axis for each RF
LudovicoDani 0:fb6e494a7656 217
LudovicoDani 0:fb6e494a7656 218 /*Matrix creation*/
LudovicoDani 0:fb6e494a7656 219 InitMatrix(&T01,4,4);
LudovicoDani 0:fb6e494a7656 220 InitMatrix(&T12,4,4);
LudovicoDani 0:fb6e494a7656 221 InitMatrix(&T23,4,4);
LudovicoDani 0:fb6e494a7656 222 InitMatrix(&T34,4,4);
LudovicoDani 0:fb6e494a7656 223 InitMatrix(&T45,4,4);
LudovicoDani 0:fb6e494a7656 224 InitMatrix(&T56,4,4);
LudovicoDani 0:fb6e494a7656 225
LudovicoDani 0:fb6e494a7656 226 InitMatrix(&p0,3,1);
LudovicoDani 0:fb6e494a7656 227 InitMatrix(&p1,3,1);
LudovicoDani 0:fb6e494a7656 228 InitMatrix(&p2,3,1);
LudovicoDani 0:fb6e494a7656 229 InitMatrix(&p3,3,1);
LudovicoDani 0:fb6e494a7656 230 InitMatrix(&p4,3,1);
LudovicoDani 0:fb6e494a7656 231 InitMatrix(&p5,3,1);
LudovicoDani 0:fb6e494a7656 232 InitMatrix(&p_TCP,3,1);
LudovicoDani 0:fb6e494a7656 233
LudovicoDani 0:fb6e494a7656 234 InitMatrix(&z0,3,1);
LudovicoDani 0:fb6e494a7656 235 InitMatrix(&z1,3,1);
LudovicoDani 0:fb6e494a7656 236 InitMatrix(&z2,3,1);
LudovicoDani 0:fb6e494a7656 237 InitMatrix(&z3,3,1);
LudovicoDani 0:fb6e494a7656 238 InitMatrix(&z4,3,1);
LudovicoDani 0:fb6e494a7656 239 InitMatrix(&z5,3,1);
LudovicoDani 0:fb6e494a7656 240
LudovicoDani 0:fb6e494a7656 241 InitMatrix(&CrossP,3,1);
LudovicoDani 0:fb6e494a7656 242
LudovicoDani 0:fb6e494a7656 243
LudovicoDani 0:fb6e494a7656 244 /**<compute dh_par */
LudovicoDani 0:fb6e494a7656 245 DHP_update(dhp,joint);
LudovicoDani 0:fb6e494a7656 246
LudovicoDani 0:fb6e494a7656 247 /**>compute T from dhp*/
LudovicoDani 0:fb6e494a7656 248 DH_Transf(&dhp[0], &T01);
LudovicoDani 0:fb6e494a7656 249 DH_Transf(&dhp[1], &T12);
LudovicoDani 0:fb6e494a7656 250 DH_Transf(&dhp[2], &T23);
LudovicoDani 0:fb6e494a7656 251 DH_Transf(&dhp[3], &T34);
LudovicoDani 0:fb6e494a7656 252 DH_Transf(&dhp[4], &T45);
LudovicoDani 0:fb6e494a7656 253 DH_Transf(&dhp[5], &T56);
LudovicoDani 0:fb6e494a7656 254
LudovicoDani 0:fb6e494a7656 255 /* R0 - JOINT 1
LudovicoDani 0:fb6e494a7656 256 distance between Global RF origin & R0 origin + R0 k axis */
LudovicoDani 0:fb6e494a7656 257 p0.element[0][0] = 0;
LudovicoDani 0:fb6e494a7656 258 p0.element[1][0] = 0;
LudovicoDani 0:fb6e494a7656 259 p0.element[2][0] = 0;
LudovicoDani 0:fb6e494a7656 260
LudovicoDani 0:fb6e494a7656 261 z0.element[0][0] = 0;
LudovicoDani 0:fb6e494a7656 262 z0.element[1][0] = 0;
LudovicoDani 0:fb6e494a7656 263 z0.element[2][0] = 1;
LudovicoDani 0:fb6e494a7656 264
LudovicoDani 0:fb6e494a7656 265
LudovicoDani 0:fb6e494a7656 266 /* R1 -JOINT 2
LudovicoDani 0:fb6e494a7656 267 distance between Global RF origin & R1 origin + R1 k axis */
LudovicoDani 0:fb6e494a7656 268 for(i = 0; i < 3; i++) {
LudovicoDani 0:fb6e494a7656 269 p1.element[i][0] = T01.element[i][3];
LudovicoDani 0:fb6e494a7656 270 }
LudovicoDani 0:fb6e494a7656 271 // z1 = omo2rot(T02)*[0;0;1];
LudovicoDani 0:fb6e494a7656 272 for(i = 0; i < 3; i++) {
LudovicoDani 0:fb6e494a7656 273 z1.element[i][0] = T01.element[i][2];
LudovicoDani 0:fb6e494a7656 274 }
LudovicoDani 0:fb6e494a7656 275
LudovicoDani 0:fb6e494a7656 276 /* R2 -JOINT 3
LudovicoDani 0:fb6e494a7656 277 distance between Global RF origin & R2 origin + R2 k axis */
LudovicoDani 0:fb6e494a7656 278 MxM(&T01, &T12, T_TCP);
LudovicoDani 0:fb6e494a7656 279 for(i = 0; i < 3; i++) {
LudovicoDani 0:fb6e494a7656 280 p2.element[i][0] = T_TCP->element[i][3];
LudovicoDani 0:fb6e494a7656 281 }
LudovicoDani 0:fb6e494a7656 282 // z2 = omo2rot(T02)*[0;0;1];
LudovicoDani 0:fb6e494a7656 283 for(i=0; i<3; i++) {
LudovicoDani 0:fb6e494a7656 284 z2.element[i][0] = T_TCP->element[i][2];
LudovicoDani 0:fb6e494a7656 285 }
LudovicoDani 0:fb6e494a7656 286
LudovicoDani 0:fb6e494a7656 287 /* R3 -JOINT 4
LudovicoDani 0:fb6e494a7656 288 distance between Global RF origin & R3 origin + R3 k axis */
LudovicoDani 0:fb6e494a7656 289 MxM(T_TCP, &T23, T_TCP);
LudovicoDani 0:fb6e494a7656 290 for(i = 0; i < 3; i++) {
LudovicoDani 0:fb6e494a7656 291 p3.element[i][0] = T_TCP->element[i][3];
LudovicoDani 0:fb6e494a7656 292 }
LudovicoDani 0:fb6e494a7656 293 // z3 = omo2rot(T03)*[0;0;1];
LudovicoDani 0:fb6e494a7656 294 for(i = 0; i < 3; i++) {
LudovicoDani 0:fb6e494a7656 295 z3.element[i][0] = T_TCP->element[i][2];
LudovicoDani 0:fb6e494a7656 296 }
LudovicoDani 0:fb6e494a7656 297
LudovicoDani 0:fb6e494a7656 298
LudovicoDani 0:fb6e494a7656 299 /* R4 -JOINT 5
LudovicoDani 0:fb6e494a7656 300 distance between Global RF origin & R4 origin + R4 k axis */
LudovicoDani 0:fb6e494a7656 301 MxM(T_TCP, &T34, T_TCP);
LudovicoDani 0:fb6e494a7656 302 for(i = 0; i < 3; i++) {
LudovicoDani 0:fb6e494a7656 303 p4.element[i][0] = T_TCP->element[i][3];
LudovicoDani 0:fb6e494a7656 304 }
LudovicoDani 0:fb6e494a7656 305 // z4 = omo2rot(T03)*[0;0;1];
LudovicoDani 0:fb6e494a7656 306 for(i = 0; i < 3; i++) {
LudovicoDani 0:fb6e494a7656 307 z4.element[i][0] = T_TCP->element[i][2];
LudovicoDani 0:fb6e494a7656 308 }
LudovicoDani 0:fb6e494a7656 309
LudovicoDani 0:fb6e494a7656 310 /* R5 -JOINT 6
LudovicoDani 0:fb6e494a7656 311 distance between Global RF origin & R5 origin + R5 k axis */
LudovicoDani 0:fb6e494a7656 312 MxM(T_TCP, &T45, T_TCP);
LudovicoDani 0:fb6e494a7656 313 for(i = 0; i < 3; i++) {
LudovicoDani 0:fb6e494a7656 314 p5.element[i][0] = T_TCP->element[i][3];
LudovicoDani 0:fb6e494a7656 315 }
LudovicoDani 0:fb6e494a7656 316 // z5 = omo2rot(T03)*[0;0;1];
LudovicoDani 0:fb6e494a7656 317 for(i = 0; i < 3; i++) {
LudovicoDani 0:fb6e494a7656 318 z5.element[i][0] = T_TCP->element[i][2];
LudovicoDani 0:fb6e494a7656 319 }
LudovicoDani 0:fb6e494a7656 320
LudovicoDani 0:fb6e494a7656 321 MxM(T_TCP, &T56, T_TCP);
LudovicoDani 0:fb6e494a7656 322 for(i = 0; i < 3; i++) {
LudovicoDani 0:fb6e494a7656 323 p_TCP.element[i][0] = T_TCP->element[i][3];
LudovicoDani 0:fb6e494a7656 324 }
LudovicoDani 0:fb6e494a7656 325
LudovicoDani 0:fb6e494a7656 326 /**compute distance from TCP*/
LudovicoDani 0:fb6e494a7656 327
LudovicoDani 0:fb6e494a7656 328 Sub(&p_TCP, &p0, &p0);
LudovicoDani 0:fb6e494a7656 329 Sub(&p_TCP, &p1, &p1);
LudovicoDani 0:fb6e494a7656 330 Sub(&p_TCP, &p2, &p2);
LudovicoDani 0:fb6e494a7656 331 Sub(&p_TCP, &p3, &p3);
LudovicoDani 0:fb6e494a7656 332 Sub(&p_TCP, &p4, &p4);
LudovicoDani 0:fb6e494a7656 333 Sub(&p_TCP, &p5, &p5);
LudovicoDani 0:fb6e494a7656 334
LudovicoDani 0:fb6e494a7656 335 /**compute jacobian*/
LudovicoDani 0:fb6e494a7656 336
LudovicoDani 0:fb6e494a7656 337 CrossProd(&z0, &p0, &CrossP);
LudovicoDani 0:fb6e494a7656 338
LudovicoDani 0:fb6e494a7656 339 BUILD_JACOBIAN(Jg, CrossP, z0, 0)
LudovicoDani 0:fb6e494a7656 340
LudovicoDani 0:fb6e494a7656 341
LudovicoDani 0:fb6e494a7656 342 CrossProd(&z1, &p1, &CrossP);
LudovicoDani 0:fb6e494a7656 343
LudovicoDani 0:fb6e494a7656 344 BUILD_JACOBIAN(Jg, CrossP, z1, 1)
LudovicoDani 0:fb6e494a7656 345
LudovicoDani 0:fb6e494a7656 346
LudovicoDani 0:fb6e494a7656 347 CrossProd(&z2, &p2, &CrossP);
LudovicoDani 0:fb6e494a7656 348
LudovicoDani 0:fb6e494a7656 349 BUILD_JACOBIAN(Jg, CrossP, z2, 2)
LudovicoDani 0:fb6e494a7656 350
LudovicoDani 0:fb6e494a7656 351
LudovicoDani 0:fb6e494a7656 352 CrossProd(&z3, &p3, &CrossP);
LudovicoDani 0:fb6e494a7656 353
LudovicoDani 0:fb6e494a7656 354 BUILD_JACOBIAN(Jg, CrossP, z3, 3)
LudovicoDani 0:fb6e494a7656 355
LudovicoDani 0:fb6e494a7656 356
LudovicoDani 0:fb6e494a7656 357 CrossProd(&z4, &p4, &CrossP);
LudovicoDani 0:fb6e494a7656 358
LudovicoDani 0:fb6e494a7656 359 BUILD_JACOBIAN(Jg, CrossP, z4, 4)
LudovicoDani 0:fb6e494a7656 360
LudovicoDani 0:fb6e494a7656 361
LudovicoDani 0:fb6e494a7656 362
LudovicoDani 0:fb6e494a7656 363 CrossProd(&z5, &p5, &CrossP);
LudovicoDani 0:fb6e494a7656 364
LudovicoDani 0:fb6e494a7656 365 BUILD_JACOBIAN(Jg, CrossP, z5, 5)
LudovicoDani 0:fb6e494a7656 366
LudovicoDani 0:fb6e494a7656 367
LudovicoDani 0:fb6e494a7656 368 DeleteMatrix(&p0);
LudovicoDani 0:fb6e494a7656 369 DeleteMatrix(&p1);
LudovicoDani 0:fb6e494a7656 370 DeleteMatrix(&p2);
LudovicoDani 0:fb6e494a7656 371 DeleteMatrix(&p3);
LudovicoDani 0:fb6e494a7656 372 DeleteMatrix(&p4);
LudovicoDani 0:fb6e494a7656 373 DeleteMatrix(&p5);
LudovicoDani 0:fb6e494a7656 374 DeleteMatrix(&p_TCP);
LudovicoDani 0:fb6e494a7656 375
LudovicoDani 0:fb6e494a7656 376 DeleteMatrix(&z0);
LudovicoDani 0:fb6e494a7656 377 DeleteMatrix(&z1);
LudovicoDani 0:fb6e494a7656 378 DeleteMatrix(&z2);
LudovicoDani 0:fb6e494a7656 379 DeleteMatrix(&z3);
LudovicoDani 0:fb6e494a7656 380 DeleteMatrix(&z4);
LudovicoDani 0:fb6e494a7656 381 DeleteMatrix(&z5);
LudovicoDani 0:fb6e494a7656 382
LudovicoDani 0:fb6e494a7656 383 DeleteMatrix(&T01);
LudovicoDani 0:fb6e494a7656 384 DeleteMatrix(&T12);
LudovicoDani 0:fb6e494a7656 385 DeleteMatrix(&T23);
LudovicoDani 0:fb6e494a7656 386 DeleteMatrix(&T34);
LudovicoDani 0:fb6e494a7656 387 DeleteMatrix(&T45);
LudovicoDani 0:fb6e494a7656 388 DeleteMatrix(&T56);
LudovicoDani 0:fb6e494a7656 389
LudovicoDani 0:fb6e494a7656 390 DeleteMatrix(&CrossP);
LudovicoDani 0:fb6e494a7656 391
LudovicoDani 0:fb6e494a7656 392 return;
LudovicoDani 0:fb6e494a7656 393 }
LudovicoDani 0:fb6e494a7656 394
LudovicoDani 0:fb6e494a7656 395 void JacobianA(matrix* joint, matrix* Ja)
LudovicoDani 0:fb6e494a7656 396 {
LudovicoDani 0:fb6e494a7656 397
LudovicoDani 0:fb6e494a7656 398 int i = 0;
LudovicoDani 0:fb6e494a7656 399 int j = 0;
LudovicoDani 0:fb6e494a7656 400
LudovicoDani 0:fb6e494a7656 401 matrix Jg;
LudovicoDani 0:fb6e494a7656 402 matrix M;
LudovicoDani 0:fb6e494a7656 403 matrix Ttcp;
LudovicoDani 0:fb6e494a7656 404 matrix JgA;
LudovicoDani 0:fb6e494a7656 405 matrix JaA;
LudovicoDani 0:fb6e494a7656 406
LudovicoDani 0:fb6e494a7656 407 matrix p;
LudovicoDani 0:fb6e494a7656 408
LudovicoDani 0:fb6e494a7656 409 InitMatrix(&Jg, Ja->row, Ja->col );
LudovicoDani 0:fb6e494a7656 410 InitMatrix(&M, 3 ,3 );
LudovicoDani 0:fb6e494a7656 411 InitMatrix(&JgA, 3, 6 );
LudovicoDani 0:fb6e494a7656 412 InitMatrix(&JaA, 3, 6 );
LudovicoDani 0:fb6e494a7656 413 InitMatrix(&Ttcp, 4, 4 );
LudovicoDani 0:fb6e494a7656 414 InitMatrix(&p, 6, 1);
LudovicoDani 0:fb6e494a7656 415
LudovicoDani 0:fb6e494a7656 416 JacobianG(joint, &Jg, &Ttcp);
LudovicoDani 0:fb6e494a7656 417
LudovicoDani 0:fb6e494a7656 418 p.element[0][0] = Ttcp.element[0][3];
LudovicoDani 0:fb6e494a7656 419 p.element[1][0] = Ttcp.element[1][3];
LudovicoDani 0:fb6e494a7656 420 p.element[2][0] = Ttcp.element[2][3];
LudovicoDani 0:fb6e494a7656 421 p.element[3][0] = (float)atan2(Ttcp.element[2][1],Ttcp.element[2][2]);
LudovicoDani 0:fb6e494a7656 422 p.element[4][0] = (float)atan2(-Ttcp.element[2][0],sin(p.element[3][0])*Ttcp.element[2][1]+cos(p.element[3][0])*Ttcp.element[2][2]);
LudovicoDani 0:fb6e494a7656 423 p.element[5][0] = (float)atan2(-cos(p.element[3][0])*Ttcp.element[0][1]+ sin(p.element[3][0])*Ttcp.element[0][2],cos(p.element[3][0])*Ttcp.element[1][1]-sin(p.element[3][0])*Ttcp.element[1][2]);
LudovicoDani 0:fb6e494a7656 424
LudovicoDani 0:fb6e494a7656 425
LudovicoDani 0:fb6e494a7656 426 // Transformation matrix
LudovicoDani 0:fb6e494a7656 427 M.element[0][0] = (float)(cos(p.element[5][0])/cos(p.element[4][0]));
LudovicoDani 0:fb6e494a7656 428 M.element[0][1] = (float)(sin(p.element[5][0])/cos(p.element[4][0]));
LudovicoDani 0:fb6e494a7656 429 M.element[0][2] = 0;
LudovicoDani 0:fb6e494a7656 430
LudovicoDani 0:fb6e494a7656 431 M.element[1][0] = (float)-sin(p.element[5][0]);
LudovicoDani 0:fb6e494a7656 432 M.element[1][1] = (float)cos(p.element[5][0]);
LudovicoDani 0:fb6e494a7656 433 M.element[1][2] = 0;
LudovicoDani 0:fb6e494a7656 434
LudovicoDani 0:fb6e494a7656 435 M.element[2][0] = (float)(cos(p.element[5][0])*sin(p.element[4][0])/cos(p.element[4][0]));
LudovicoDani 0:fb6e494a7656 436 M.element[2][1] = (float)(-sin(p.element[4][0])*sin(p.element[5][0])/cos(p.element[4][0]));
LudovicoDani 0:fb6e494a7656 437 M.element[2][2] = 1;
LudovicoDani 0:fb6e494a7656 438
LudovicoDani 0:fb6e494a7656 439
LudovicoDani 0:fb6e494a7656 440 for(i = 0; i < JgA.row; i++) {
LudovicoDani 0:fb6e494a7656 441 for(j = 0; j< JgA.col; j++) {
LudovicoDani 0:fb6e494a7656 442 JgA.element[i][j] = Jg.element[i+3][j];
LudovicoDani 0:fb6e494a7656 443 }
LudovicoDani 0:fb6e494a7656 444 }
LudovicoDani 0:fb6e494a7656 445
LudovicoDani 0:fb6e494a7656 446 MxM(&M, &JgA, &JaA);
LudovicoDani 0:fb6e494a7656 447
LudovicoDani 0:fb6e494a7656 448
LudovicoDani 0:fb6e494a7656 449 for(i = 0; i < JaA.row; i++) {
LudovicoDani 0:fb6e494a7656 450 for(j = 0; j< JaA.col; j++) {
LudovicoDani 0:fb6e494a7656 451 Ja->element[i][j] = Jg.element[i][j];
LudovicoDani 0:fb6e494a7656 452 }
LudovicoDani 0:fb6e494a7656 453 }
LudovicoDani 0:fb6e494a7656 454
LudovicoDani 0:fb6e494a7656 455 for(i = 0; i < JaA.row; i++) {
LudovicoDani 0:fb6e494a7656 456 for(j = 0; j< JaA.col; j++) {
LudovicoDani 0:fb6e494a7656 457 Ja->element[i+3][j] = JaA.element[i][j];
LudovicoDani 0:fb6e494a7656 458 }
LudovicoDani 0:fb6e494a7656 459 }
LudovicoDani 0:fb6e494a7656 460
LudovicoDani 0:fb6e494a7656 461 DeleteMatrix(&M);
LudovicoDani 0:fb6e494a7656 462 DeleteMatrix(&Jg);
LudovicoDani 0:fb6e494a7656 463 DeleteMatrix(&JgA);
LudovicoDani 0:fb6e494a7656 464 DeleteMatrix(&JaA);
LudovicoDani 0:fb6e494a7656 465 DeleteMatrix(&Ttcp);
LudovicoDani 0:fb6e494a7656 466 DeleteMatrix(&p);
LudovicoDani 0:fb6e494a7656 467
LudovicoDani 0:fb6e494a7656 468 return;
LudovicoDani 0:fb6e494a7656 469 }
LudovicoDani 0:fb6e494a7656 470
LudovicoDani 0:fb6e494a7656 471 int IPKF (matrix* p, matrix* joint)
LudovicoDani 0:fb6e494a7656 472 {
LudovicoDani 0:fb6e494a7656 473
LudovicoDani 0:fb6e494a7656 474 float sentinel = 2;
LudovicoDani 0:fb6e494a7656 475 float sum = 0.0;
LudovicoDani 0:fb6e494a7656 476 float error_G = 0.5;
LudovicoDani 0:fb6e494a7656 477 float error_N = (float)0.0001;
LudovicoDani 0:fb6e494a7656 478 float alpha = (float)0.27; //after several trials
LudovicoDani 0:fb6e494a7656 479 int c = 0;
LudovicoDani 0:fb6e494a7656 480 int i = 0;
LudovicoDani 0:fb6e494a7656 481
LudovicoDani 0:fb6e494a7656 482
LudovicoDani 0:fb6e494a7656 483 DHP dhp[6];
LudovicoDani 0:fb6e494a7656 484
LudovicoDani 0:fb6e494a7656 485 matrix f;//computed position through Direct Kinematics
LudovicoDani 0:fb6e494a7656 486 matrix J;
LudovicoDani 0:fb6e494a7656 487 matrix delta_p;
LudovicoDani 0:fb6e494a7656 488 matrix delta_q;
LudovicoDani 0:fb6e494a7656 489 matrix inv_J;
LudovicoDani 0:fb6e494a7656 490
LudovicoDani 0:fb6e494a7656 491
LudovicoDani 0:fb6e494a7656 492 InitMatrix(&f, 6, 1);
LudovicoDani 0:fb6e494a7656 493 InitMatrix(&J, 6, 6);
LudovicoDani 0:fb6e494a7656 494 InitMatrix(&inv_J, 6, 6);
LudovicoDani 0:fb6e494a7656 495 InitMatrix(&delta_q, 6,1);
LudovicoDani 0:fb6e494a7656 496 InitMatrix(&delta_p, 6,1);
LudovicoDani 0:fb6e494a7656 497
LudovicoDani 0:fb6e494a7656 498
LudovicoDani 0:fb6e494a7656 499 /*gradient method*/
LudovicoDani 0:fb6e494a7656 500 while(sentinel >= error_G) {
LudovicoDani 0:fb6e494a7656 501 //
LudovicoDani 0:fb6e494a7656 502 /*compute the direct kinematics*/
LudovicoDani 0:fb6e494a7656 503 DPKF(joint, &f);
LudovicoDani 0:fb6e494a7656 504
LudovicoDani 0:fb6e494a7656 505 /*compute dh_par*/
LudovicoDani 0:fb6e494a7656 506 DHP_update(dhp,joint);
LudovicoDani 0:fb6e494a7656 507
LudovicoDani 0:fb6e494a7656 508 /*compute jacobian*/
LudovicoDani 0:fb6e494a7656 509 JacobianA(joint, &J);// to be defined
LudovicoDani 0:fb6e494a7656 510
LudovicoDani 0:fb6e494a7656 511 /*compute the next value of angle*/
LudovicoDani 0:fb6e494a7656 512 Traspost(&J, &J); //Transpose of J saved in JT
LudovicoDani 0:fb6e494a7656 513 axM(&J, alpha, &J);// Transpose of J (J) multiplied by alpha and saved in J
LudovicoDani 0:fb6e494a7656 514
LudovicoDani 0:fb6e494a7656 515 /*subtraction of two pose objects into matrix*/
LudovicoDani 0:fb6e494a7656 516 Sub(p , &f, &delta_p);
LudovicoDani 0:fb6e494a7656 517
LudovicoDani 0:fb6e494a7656 518 //alpha*J'*(p-f)
LudovicoDani 0:fb6e494a7656 519 MxM(&J, &delta_p, &delta_q);
LudovicoDani 0:fb6e494a7656 520
LudovicoDani 0:fb6e494a7656 521 Sum(joint, &delta_q, joint);
LudovicoDani 0:fb6e494a7656 522
LudovicoDani 0:fb6e494a7656 523 CHECK_ANGLE()
LudovicoDani 0:fb6e494a7656 524
LudovicoDani 0:fb6e494a7656 525 c = c+1;
LudovicoDani 0:fb6e494a7656 526
LudovicoDani 0:fb6e494a7656 527 /*if the gradient method does not converge break the cycle*/
LudovicoDani 0:fb6e494a7656 528 if (c > 1000) {
LudovicoDani 0:fb6e494a7656 529 break;
LudovicoDani 0:fb6e494a7656 530 }
LudovicoDani 0:fb6e494a7656 531
LudovicoDani 0:fb6e494a7656 532 /* norm of p_minus_f*/
LudovicoDani 0:fb6e494a7656 533
LudovicoDani 0:fb6e494a7656 534 for(i=0; i<6; i++) {
LudovicoDani 0:fb6e494a7656 535 sum += (delta_p.element[i][0] * delta_p.element[i][0]);
LudovicoDani 0:fb6e494a7656 536 }
LudovicoDani 0:fb6e494a7656 537 sentinel = (float)sqrt(sum);
LudovicoDani 0:fb6e494a7656 538 sum = 0;
LudovicoDani 0:fb6e494a7656 539 }
LudovicoDani 0:fb6e494a7656 540
LudovicoDani 0:fb6e494a7656 541 //printf("c = %d\n", c);
LudovicoDani 0:fb6e494a7656 542
LudovicoDani 0:fb6e494a7656 543 /*Newton method*/
LudovicoDani 0:fb6e494a7656 544 while (sentinel >= error_N) {
LudovicoDani 0:fb6e494a7656 545
LudovicoDani 0:fb6e494a7656 546 /*compute the direct kinematics*/
LudovicoDani 0:fb6e494a7656 547 DPKF(joint,&f);
LudovicoDani 0:fb6e494a7656 548
LudovicoDani 0:fb6e494a7656 549 /*compute dh_par*/
LudovicoDani 0:fb6e494a7656 550 DHP_update(dhp,joint);
LudovicoDani 0:fb6e494a7656 551
LudovicoDani 0:fb6e494a7656 552 /*compute jacobian*/
LudovicoDani 0:fb6e494a7656 553 JacobianA(joint, &J);// to be defined
LudovicoDani 0:fb6e494a7656 554
LudovicoDani 0:fb6e494a7656 555 /*compute the next value of angle*/
LudovicoDani 0:fb6e494a7656 556
LudovicoDani 0:fb6e494a7656 557 //check if Jacobian is invertible
LudovicoDani 0:fb6e494a7656 558 if(Inv(&J, &inv_J) == 6 ) {
LudovicoDani 0:fb6e494a7656 559 break;
LudovicoDani 0:fb6e494a7656 560 //printf("error due to jacobuan singularities");
LudovicoDani 0:fb6e494a7656 561 }
LudovicoDani 0:fb6e494a7656 562
LudovicoDani 0:fb6e494a7656 563 Sub(p, &f, &delta_p);
LudovicoDani 0:fb6e494a7656 564
LudovicoDani 0:fb6e494a7656 565 MxM(&inv_J, &delta_p, &delta_q);
LudovicoDani 0:fb6e494a7656 566
LudovicoDani 0:fb6e494a7656 567 //q = q + J \ (p-f);
LudovicoDani 0:fb6e494a7656 568 Sum(joint, &delta_q, joint);
LudovicoDani 0:fb6e494a7656 569
LudovicoDani 0:fb6e494a7656 570 CHECK_ANGLE();
LudovicoDani 0:fb6e494a7656 571
LudovicoDani 0:fb6e494a7656 572 c = c+1;
LudovicoDani 0:fb6e494a7656 573
LudovicoDani 0:fb6e494a7656 574 /*if the newton method does not converge break the cycle*/
LudovicoDani 0:fb6e494a7656 575 if (c > 2000) {
LudovicoDani 0:fb6e494a7656 576 break;
LudovicoDani 0:fb6e494a7656 577 }
LudovicoDani 0:fb6e494a7656 578 /* norm of p_minus_f*/
LudovicoDani 0:fb6e494a7656 579 sum = 0.0;
LudovicoDani 0:fb6e494a7656 580
LudovicoDani 0:fb6e494a7656 581 for(i=0; i<6; i++) {
LudovicoDani 0:fb6e494a7656 582 sum += delta_p.element[i][0] * delta_p.element[i][0];
LudovicoDani 0:fb6e494a7656 583 }
LudovicoDani 0:fb6e494a7656 584 sentinel = (float)sqrt(sum);
LudovicoDani 0:fb6e494a7656 585 }
LudovicoDani 0:fb6e494a7656 586
LudovicoDani 0:fb6e494a7656 587 DeleteMatrix(&f);
LudovicoDani 0:fb6e494a7656 588 DeleteMatrix(&J);
LudovicoDani 0:fb6e494a7656 589 DeleteMatrix(&delta_p);
LudovicoDani 0:fb6e494a7656 590 DeleteMatrix(&delta_q);
LudovicoDani 0:fb6e494a7656 591 DeleteMatrix(&inv_J);
LudovicoDani 0:fb6e494a7656 592 //printf("c = %d\n",c);
LudovicoDani 0:fb6e494a7656 593 if(c >=2000)
LudovicoDani 0:fb6e494a7656 594 {
LudovicoDani 0:fb6e494a7656 595 return 1;
LudovicoDani 0:fb6e494a7656 596 }
LudovicoDani 0:fb6e494a7656 597 else
LudovicoDani 0:fb6e494a7656 598 {
LudovicoDani 0:fb6e494a7656 599 return 0;
LudovicoDani 0:fb6e494a7656 600 }
LudovicoDani 0:fb6e494a7656 601 }
LudovicoDani 0:fb6e494a7656 602
LudovicoDani 0:fb6e494a7656 603 void Print_DHP(DHP* dhp)
LudovicoDani 0:fb6e494a7656 604 {
LudovicoDani 0:fb6e494a7656 605 printf("d = %f\n",dhp->d);
LudovicoDani 0:fb6e494a7656 606 printf("theta = %f\n",dhp->theta);
LudovicoDani 0:fb6e494a7656 607 printf("a = %f\n",dhp->a);
LudovicoDani 0:fb6e494a7656 608 printf("alpha = %f\n",dhp->alpha);
LudovicoDani 0:fb6e494a7656 609 printf("\n");
LudovicoDani 0:fb6e494a7656 610 }
LudovicoDani 0:fb6e494a7656 611
LudovicoDani 0:fb6e494a7656 612
LudovicoDani 0:fb6e494a7656 613
LudovicoDani 0:fb6e494a7656 614