MBSdataStruct* loadMBSdata_xml(xmlDocPtr doc){ int i,j, ncol; xmlNodePtr root = NULL; xmlNodePtr cur = NULL; MBSdataStruct *s; s = loadMBSsizes_xml(doc); root = xmlDocGetRootElement(doc); cur = root; //=== Index parameters ======================================// /* qc */ if (s->nqc) { s->qc = (int*) calloc(s->nqc+1,sizeof(int)); s->qc[0] = s->nqc; mbs_xml_parseVectorInt("qc", doc, cur, s->qc); } else s->qc = NULL; /* qu */ if (s->nqu) { s->qu = (int*) calloc(s->nqu+1,sizeof(int)); s->qu[0] = s->nqu; mbs_xml_parseVectorInt("qu", doc, cur, s->qu); } else s->qu = NULL; /* qa */ if (s->nqa) { s->qa = (int*) calloc(s->nqa+1,sizeof(int)); s->qa[0] = s->nqa; mbs_xml_parseVectorInt("qa", doc, cur, s->qa); } else s->qa = NULL; /* qv */ if (s->nqv) { s->qv = (int*) calloc(s->nqv+1,sizeof(int)); s->qv[0] = s->nqv; mbs_xml_parseVectorInt("qv", doc, cur, s->qv); } else s->qv = NULL; /* hu */ if (s->nhu) { s->hu = (int*) calloc(s->nhu+1,sizeof(int)); s->hu[0] = s->nhu; mbs_xml_parseVectorInt("hu", doc, cur, s->hu); } else s->hu = NULL; /* qlocked */ if (s->nqlocked) { s->qlocked = (int*) calloc(s->nqlocked+1,sizeof(int)); s->qlocked[0] = s->nqlocked; mbs_xml_parseVectorInt("qlocked", doc, cur, s->qlocked); } else s->qlocked = NULL; /* qdriven */ if (s->nqdriven) { s->qdriven = (int*) calloc(s->nqdriven+1,sizeof(int)); s->qdriven[0] = s->nqdriven; mbs_xml_parseVectorInt("qdriven", doc, cur, s->qdriven); } else s->qdriven = NULL; //=== Work Variables ==========================================================// /* q,qd,qdd */ s->q = (double*) calloc(s->njoint+1,sizeof(double)); s->qd = (double*) calloc(s->njoint+1,sizeof(double)); s->qdd = (double*) calloc(s->njoint+1,sizeof(double)); s->q[0] = (double) s->njoint; s->qd[0] = (double) s->njoint; s->qdd[0] = (double) s->njoint; mbs_xml_parseVectorDouble("q", doc, cur, s->q); mbs_xml_parseVectorDouble("qd", doc, cur, s->qd); mbs_xml_parseVectorDouble("qdd", doc, cur, s->qdd); /* frc */ s->frc[0] = NULL; for(i=1;i<=3;i++) { s->frc[i] = (double*) calloc(s->nbody+1,sizeof(double)); s->frc[i][0] = (double) s->nbody; for(j=1;j<=s->nbody;j++) s->frc[i][j] = 0.0; } /* trq */ s->trq[0] = NULL; for(i=1;i<=3;i++) { s->trq[i] = (double*) calloc(s->nbody+1,sizeof(double)); s->trq[i][0] = (double) s->nbody; for(j=1;j<=s->nbody;j++) s->trq[i][j] = 0.0; } /* Qq */ s->Qq = (double*) calloc(s->njoint+1,sizeof(double)); s->Qq[0] = (double) s->njoint; for(i=1;i<=s->njoint;i++) s->Qq[i] = 0.0; /* tsim */ s->tsim = 0.0; //=== System parameters ====================================================// /* dpt */ s->dpt[0] = NULL; if (s->npt) { for(i=1;i<=3;i++) { s->dpt[i] = (double*) calloc(s->npt+1,sizeof(double)); s->dpt[i][0] = (double) s->npt; } mbs_xml_parseMatrixDouble("dpt", doc, cur, s->dpt); } else // je pense que ce cas est impossible mais il faudrait vérifier avant de supprimer for(i=1;i<=3;i++) s->dpt[i] = NULL; /* l */ s->l[0] = NULL; for(i=1;i<=3;i++) { s->l[i] = (double*) calloc(s->nbody+1,sizeof(double)); s->l[i][0] = (double) s->nbody; } mbs_xml_parseMatrixDouble("l", doc, cur, s->l); /* m */ s->m = (double*) calloc(s->nbody+1,sizeof(double)); s->m[0] = (double) s->nbody; mbs_xml_parseVectorDouble("m", doc, cur, s->m); /* In */ s->In[0] = NULL; for(i=1;i<=9;i++) { s->In[i] = (double*) calloc(s->nbody+1,sizeof(double)); s->In[i][0] = (double) s->nbody; } mbs_xml_parseMatrixDouble("In", doc, cur, s->In); /* g */ s->g[0] = 0.0; mbs_xml_parseVectorDouble("g", doc, cur, s->g); /* Constraint data */ /* lrod */ ncol = mbs_xml_getNbColElement("lrod", doc, cur); if (ncol) { s->lrod = (double*) calloc(ncol+1,sizeof(double)); s->lrod[0] = ncol; mbs_xml_parseVectorDouble("lrod", doc, cur, s->lrod); } else s->lrod = NULL; /* NRerr */ if (!mbs_xml_isElement(doc,"/mbsdata/NRerr")) s->NRerr = 1e-9; //default value else s->NRerr = mbs_xml_parseScalarInt("NRerr", doc, cur); /* Link Data */ /* Z,Zd,Fl */ if (s->Nlink) { s->Z = (double*) calloc(s->Nlink+1,sizeof(double)); s->Zd = (double*) calloc(s->Nlink+1,sizeof(double)); s->Fl = (double*) calloc(s->Nlink+1,sizeof(double)); s->Z[0] = (double) s->Nlink; s->Zd[0] = (double) s->Nlink; s->Fl[0] = (double) s->Nlink; for(i=1;i<=s->Nlink;i++) { s->Z[i] = 0.0; s->Zd[i] = 0.0; s->Fl[i] = 0.0; } } else { s->Z = NULL; s->Zd = NULL; s->Fl = NULL; } /* l3DWr */ if (s->Nlink3D) { s->l3DWr = (double**) calloc(s->Nlink3D+1,sizeof(double*)); s->l3DWr[0] = NULL; for(i=1;i<=s->Nlink3D;i++) { s->l3DWr[i] = (double*) calloc(6+1,sizeof(double)); for(j=0;j<=6;j++) s->l3DWr[i][j] = 0.0; } } else s->l3DWr = NULL; /* Ext. Forces Data */ /* xfidpt */ if (s->Nxfrc) { s->xfidpt = (int*) calloc(s->Nxfrc+1,sizeof(int)); s->xfidpt[0] = s->Nxfrc; mbs_xml_parseVectorInt("xfidpt", doc, cur, s->xfidpt); } else s->xfidpt = NULL; /* SWr */ if (s->Nxfrc) { s->SWr = (double**) calloc(s->Nxfrc+1,sizeof(double*)); s->SWr[0] = NULL; for(i=1;i<=s->Nxfrc;i++) { s->SWr[i] = (double*) calloc(9+1,sizeof(double)); for(j=0;j<=9;j++) s->SWr[i][j] = 0.0; } } else s->SWr = NULL; /* Wheel Data */ /* rnom */ if (s->Nwheel) { s->rnom = (double*) calloc(s->Nwheel+1,sizeof(double)); s->rnom[0] = (double) s->Nwheel; mbs_xml_parseVectorDouble("rnom", doc, cur, s->rnom); } else s->rnom = NULL; #if !defined SENSORKIN /* User Model */ /* user_model */ if (s->Nuser_model) { s->user_model = loadUserModel_xml(doc, cur); } /* ux,uxd,ux0 */ if (s->Nux) { s->ux = (double*) calloc(s->Nux+1,sizeof(double)); s->uxd = (double*) calloc(s->Nux+1,sizeof(double)); s->ux0 = (double*) calloc(s->Nux+1,sizeof(double)); s->ux[0] = (double) s->Nux; s->uxd[0] = (double) s->Nux; s->ux0[0] = (double) s->Nux; mbs_xml_parseVectorDouble("ux", doc, cur, s->ux); mbs_xml_parseVectorDouble("uxd", doc, cur, s->uxd); mbs_xml_parseVectorDouble("ux0", doc, cur, s->ux0); } else { s->ux = NULL; s->uxd = NULL; s->ux0 = NULL; } /* User IO */ s->user_IO = initUserIO(s); #endif //=== Other ========================================================// /* qddu */ if (s->nqu) { s->qddu = (double*) calloc(s->nqu+1,sizeof(double)); s->qddu[0] = (double) s->nqu; } else s->qddu = NULL; /* DonePart */ s->DonePart = mbs_xml_parseScalarInt("DonePart", doc, cur); return s; }
MBSdataStruct* MDS_create_MBSdataStruct(MDS_gen_strct* mds_gen_strct) { int i,j; int ind_joint = 0; int ind_state_value = 0; MBSdataStruct *s; s = (MBSdataStruct*) malloc(sizeof(MBSdataStruct)); s->nbody = mds_gen_strct->bodytree->n_joint; s->njoint = mds_gen_strct->bodytree->n_joint; s->npt = mds_gen_strct->n_point; s->nqu = mds_gen_strct->bodytree->n_qu; s->nqv = mds_gen_strct->bodytree->n_qv; s->nqc = mds_gen_strct->bodytree->n_qc; s->nqlocked = mds_gen_strct->bodytree->n_qlocked; s->nqdriven = mds_gen_strct->bodytree->n_qdriven; s->nqa = mds_gen_strct->bodytree->n_qa; mds_gen_strct->cuts = init_MDS_cuts_strct(); // debug (to be corrected) mds_gen_strct->links = init_MDS_links_strct(); // debug (to be corrected) s->Nloopc = mds_gen_strct->cuts->n_rod + 3 * mds_gen_strct->cuts->n_ball + 6 * mds_gen_strct->cuts->n_solid; s->Nuserc = 0; s->Ncons = s->Nloopc + s->Nuserc; //s->nhu = 8; // caution WEMOOV dependent !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! s->nhu = s->Ncons; s->Nlink = mds_gen_strct->links->n_link; s->Nlink3D = mds_gen_strct->links->n_link3D; s->Nsensor = mds_gen_strct->n_sensor; s->Nxfrc = mds_gen_strct->n_extforce; s->Nwheel = 0; // dead function (at this time) #if !defined SENSORKIN s->Nuser_model = mds_gen_strct->user_models->n_user_model; #endif s->Nux = mds_gen_strct->n_state_value; // geometric ans dynamic data //pt s->dpt[0] = NULL; if (s->npt) { for(i=1;i<=3;i++) { s->dpt[i] = (double*) calloc(s->npt+1,sizeof(double)); s->dpt[i][0] = (double) s->npt; } for(i=0; i<mds_gen_strct->n_point; i++) { for(j=0; j<3; j++) { s->dpt[j+1][i+1] = mds_gen_strct->point_list[i]->pt[j]; } } } else for(i=1;i<=3;i++) s->dpt[i] = NULL; // l, m, In ind_joint = 0; s->l[0] = NULL; for(i=1;i<=3;i++) { s->l[i] = (double*) calloc(s->nbody+1,sizeof(double)); s->l[i][0] = (double) s->nbody; } s->m = (double*) calloc(s->nbody+1,sizeof(double)); s->m[0] = (double) s->nbody; s->In[0] = NULL; for(i=1;i<=9;i++) { s->In[i] = (double*) calloc(s->nbody+1,sizeof(double)); s->In[i][0] = (double) s->nbody; } for(i=0; i<mds_gen_strct->bodytree->n_body; i++) { ind_joint = ind_joint + mds_gen_strct->bodytree->body_list[i]->n_joint; for(j=0; j<3; j++) { s->l[j+1][ind_joint] = mds_gen_strct->bodytree->body_list[i]->com[j]; } s->m[ind_joint] = mds_gen_strct->bodytree->body_list[i]->mass; s->In[1][ind_joint] = mds_gen_strct->bodytree->body_list[i]->inertia[0]; s->In[2][ind_joint] = mds_gen_strct->bodytree->body_list[i]->inertia[1]; s->In[3][ind_joint] = mds_gen_strct->bodytree->body_list[i]->inertia[2]; s->In[4][ind_joint] = mds_gen_strct->bodytree->body_list[i]->inertia[1]; s->In[5][ind_joint] = mds_gen_strct->bodytree->body_list[i]->inertia[3]; s->In[6][ind_joint] = mds_gen_strct->bodytree->body_list[i]->inertia[4]; s->In[7][ind_joint] = mds_gen_strct->bodytree->body_list[i]->inertia[2]; s->In[8][ind_joint] = mds_gen_strct->bodytree->body_list[i]->inertia[4]; s->In[9][ind_joint] = mds_gen_strct->bodytree->body_list[i]->inertia[5]; } // g s->g[0] = 0.0; copy_double_vec(mds_gen_strct->base->gravity, &(s->g[1]), 3); // partitionning infos s->qu = MDS_translate_q(mds_gen_strct->bodytree->qu, mds_gen_strct->bodytree->n_qu); s->qv = MDS_translate_q(mds_gen_strct->bodytree->qv, mds_gen_strct->bodytree->n_qv); s->qc = MDS_translate_q(mds_gen_strct->bodytree->qc, mds_gen_strct->bodytree->n_qc); s->qlocked = MDS_translate_q(mds_gen_strct->bodytree->qlocked, mds_gen_strct->bodytree->n_qlocked); s->qdriven = MDS_translate_q(mds_gen_strct->bodytree->qdriven, mds_gen_strct->bodytree->n_qdriven); s->qa = MDS_translate_q(mds_gen_strct->bodytree->qa, mds_gen_strct->bodytree->n_qa); if (s->nhu) // to change { s->hu = (int*) calloc(s->nhu+1,sizeof(int)); s->hu[0] = s->nhu; for(i=0; i<s->nhu; i++) { s->hu[i+1] = i+1; } } else s->hu = NULL; // joints data s->q0 = (double*) calloc(s->njoint+1,sizeof(double)); s->qd0 = (double*) calloc(s->njoint+1,sizeof(double)); s->qdd0 = (double*) calloc(s->njoint+1,sizeof(double)); s->q0[0] = (double) s->njoint; s->qd0[0] = (double) s->njoint; s->qdd0[0] = (double) s->njoint; s->q = (double*) calloc(s->njoint+1,sizeof(double)); s->qd = (double*) calloc(s->njoint+1,sizeof(double)); s->qdd = (double*) calloc(s->njoint+1,sizeof(double)); for(i=0; i<mds_gen_strct->bodytree->n_joint; i++) { s->q0[i+1] = mds_gen_strct->bodytree->joint_list[i]->q0; s->qd0[i+1] = mds_gen_strct->bodytree->joint_list[i]->qd0; s->qdd0[i+1] = mds_gen_strct->bodytree->joint_list[i]->qdd0; } copy_double_vec(s->q0, s->q, s->njoint+1); copy_double_vec(s->qd0, s->qd, s->njoint+1); copy_double_vec(s->qdd0, s->qdd, s->njoint+1); s->qmin = NULL; // dead function (at this time) s->qmax = NULL; // Frc, Trq, Qq, tsim // frc s->frc[0] = NULL; for(i=1;i<=3;i++) { s->frc[i] = (double*) calloc(s->nbody+1,sizeof(double)); s->frc[i][0] = (double) s->nbody; for(j=1;j<=s->nbody;j++) s->frc[i][j] = 0.0; } // trq s->trq[0] = NULL; for(i=1;i<=3;i++) { s->trq[i] = (double*) calloc(s->nbody+1,sizeof(double)); s->trq[i][0] = (double) s->nbody; for(j=1;j<=s->nbody;j++) s->trq[i][j] = 0.0; } // Qq s->Qq = (double*) calloc(s->njoint+1,sizeof(double)); s->Qq[0] = (double) s->njoint; for(i=1;i<=s->njoint;i++) s->Qq[i] = 0.0; // tsim s->tsim = 0.0; // Constraint data // lrod if (mds_gen_strct->cuts->n_rod) { s->lrod = (double*) calloc(mds_gen_strct->cuts->n_rod+1,sizeof(double)); s->lrod[0] = mds_gen_strct->cuts->n_rod; for(i=0; i<mds_gen_strct->cuts->n_rod; i++) { s->lrod[i+1] = mds_gen_strct->cuts->rod_list[i]->length; } } else s->lrod = NULL; // NRerr s->NRerr = 1e-9; //default value // Link Data // Z, Zd, Fl if (s->Nlink) { s->Z = (double*) calloc(s->Nlink+1,sizeof(double)); s->Zd = (double*) calloc(s->Nlink+1,sizeof(double)); s->Fl = (double*) calloc(s->Nlink+1,sizeof(double)); s->Z[0] = (double) s->Nlink; s->Zd[0] = (double) s->Nlink; s->Fl[0] = (double) s->Nlink; for(i=1;i<=s->Nlink;i++) { s->Z[i] = 0.0; s->Zd[i] = 0.0; s->Fl[i] = 0.0; } } else { s->Z = NULL; s->Zd = NULL; s->Fl = NULL; } // l3DWr if (s->Nlink3D) { s->l3DWr = (double**) calloc(s->Nlink3D+1,sizeof(double*)); s->l3DWr[0] = NULL; for(i=1;i<=s->Nlink3D;i++) { s->l3DWr[i] = (double*) calloc(6+1,sizeof(double)); for(j=0;j<=6;j++) s->l3DWr[i][j] = 0.0; } } else s->l3DWr = NULL; // Ext. Forces Data // xfidpt if (s->Nxfrc) { s->xfidpt = (int*) calloc(s->Nxfrc+1,sizeof(int)); s->xfidpt[0] = s->Nxfrc; for(i=0;i<mds_gen_strct->n_extforce;i++) { s->xfidpt[i+1] = mds_gen_strct->extforce_point_list[i]; } } else s->xfidpt = NULL; // SWr if (s->Nxfrc) { s->SWr = (double**) calloc(s->Nxfrc+1,sizeof(double*)); s->SWr[0] = NULL; for(i=1;i<=s->Nxfrc;i++) { s->SWr[i] = (double*) calloc(9+1,sizeof(double)); for(j=0;j<=9;j++) s->SWr[i][j] = 0.0; } } else s->SWr = NULL; // Wheel Data // rnom if (s->Nwheel) { s->rnom = (double*) calloc(s->Nwheel+1,sizeof(double)); s->rnom[0] = (double) s->Nwheel; // to complet } else s->rnom = NULL; #if !defined SENSORKIN // User Model // user_model if (s->Nuser_model) { s->user_model = init_UserModelStruct(); load_UserModelStruct(mds_gen_strct, s->user_model); } // ux, uxd, ux0 if (s->Nux) { s->ux = (double*) calloc(s->Nux+1,sizeof(double)); s->uxd = (double*) calloc(s->Nux+1,sizeof(double)); s->ux0 = (double*) calloc(s->Nux+1,sizeof(double)); s->ux[0] = (double) s->Nux; s->uxd[0] = (double) s->Nux; s->ux0[0] = (double) s->Nux; for(i=0; i<mds_gen_strct->n_state; i++) { for(j=0; j<mds_gen_strct->state_list[i]->n_value; j++) { s->ux0[ind_state_value+1] = mds_gen_strct->state_list[i]->value_list[j]; ind_state_value++; } } copy_double_vec(s->ux0, s->ux, s->Nux); } else { s->ux = NULL; s->uxd = NULL; s->ux0 = NULL; } s->user_IO = initUserIO(s); #endif // Other // qddu if (s->nqu) { s->qddu = (double*) calloc(s->nqu+1,sizeof(double)); s->qddu[0] = (double) s->nqu; } else s->qddu = NULL; s->DonePart = 0; s->DoneEquil = 0; return s; }
MBSdataStruct * loadMBSdata(const mxArray *s_ptr) { MBSdataStruct *s; const mxArray *field_value_ptr,*field_value_ptr2,*field_value_ptr3; mxArray *usm_ptr; int i,j, ncol; s = loadMBSsizes(s_ptr); //=== Index parameters ======================================// /* qc */ if (s->nqc) { field_value_ptr = mxGetField(s_ptr, 0, "qc"); if (field_value_ptr == NULL) mexErrMsgTxt("Could not get field \"qc\"\n"); s->qc = (int*) calloc(s->nqc+1,sizeof(int)); s->qc[0] = s->nqc; for(i=1;i<=s->nqc;i++) s->qc[i] =(int) mxGetPr(field_value_ptr)[i-1]; } else s->qc = NULL; /* qu */ if (s->nqu) { field_value_ptr = mxGetField(s_ptr, 0, "qu"); if (field_value_ptr == NULL) mexErrMsgTxt("Could not get field \"qu\"\n"); s->qu = (int*) calloc(s->nqu+1,sizeof(int)); s->qu[0] = s->nqu; for(i=1;i<=s->nqu;i++) s->qu[i] =(int) mxGetPr(field_value_ptr)[i-1]; } else s->qu = NULL; /* qa */ if (s->nqa) { field_value_ptr = mxGetField(s_ptr, 0, "qa"); if (field_value_ptr == NULL) mexErrMsgTxt("Could not get field \"qa\"\n"); s->qa = (int*) calloc(s->nqa+1,sizeof(int)); s->qa[0] = s->nqa; for(i=1;i<=s->nqa;i++) s->qa[i] =(int) mxGetPr(field_value_ptr)[i-1]; } else s->qa = NULL; /* qv */ if (s->nqv) { field_value_ptr = mxGetField(s_ptr, 0, "qv"); if (field_value_ptr == NULL) mexErrMsgTxt("Could not get field \"qv\"\n"); s->qv = (int*) calloc(s->nqv+1,sizeof(int)); s->qv[0] = s->nqv; for(i=1;i<=s->nqv;i++) s->qv[i] =(int) mxGetPr(field_value_ptr)[i-1]; } else s->qv = NULL; /* hu */ // JFC : 15/01/2008 : ajout if (s->nhu) { field_value_ptr = mxGetField(s_ptr, 0, "hu"); if (field_value_ptr == NULL) mexErrMsgTxt("Could not get field \"hu\"\n"); s->hu = (int*) calloc(s->nhu+1,sizeof(int)); s->hu[0] = s->nhu; for(i=1;i<=s->nhu;i++) s->hu[i] =(int) mxGetPr(field_value_ptr)[i-1]; } else s->hu = NULL; /* qlocked */ if (s->nqlocked) { field_value_ptr = mxGetField(s_ptr, 0, "qlocked"); if (field_value_ptr == NULL) mexErrMsgTxt("Could not get field \"qlocked\"\n"); s->qlocked = (int*) calloc(s->nqlocked+1,sizeof(int)); s->qlocked[0] = s->nqlocked; for(i=1;i<=s->nqlocked;i++) s->qlocked[i] =(int) mxGetPr(field_value_ptr)[i-1]; } else s->qlocked = NULL; /* qdriven */ if (s->nqdriven) { field_value_ptr = mxGetField(s_ptr, 0, "qdriven"); if (field_value_ptr == NULL) mexErrMsgTxt("Could not get field \"qdriven\"\n"); s->qdriven = (int*) calloc(s->nqdriven+1,sizeof(int)); s->qdriven[0] = s->nqdriven; for(i=1;i<=s->nqdriven;i++) s->qdriven[i] =(int) mxGetPr(field_value_ptr)[i-1]; } else s->qdriven = NULL; //=== Initial values ==========================================================// /* q0,qd0,qdd0 * field_value_ptr = mxGetField(s_ptr, 0, "q0"); field_value_ptr2 = mxGetField(s_ptr, 0, "qd0"); field_value_ptr3 = mxGetField(s_ptr, 0, "qdd0"); if (field_value_ptr == NULL) mexErrMsgTxt("Could not get field \"q0\"\n"); if (field_value_ptr2 == NULL) mexErrMsgTxt("Could not get field \"qd0\"\n"); if (field_value_ptr3 == NULL) mexErrMsgTxt("Could not get field \"qdd0\"\n"); s->q0 = (double*) calloc(s->njoint+1,sizeof(double)); s->qd0 = (double*) calloc(s->njoint+1,sizeof(double)); s->qdd0 = (double*) calloc(s->njoint+1,sizeof(double)); s->q0[0] = (double) s->njoint; s->qd0[0] = (double) s->njoint; s->qdd0[0] = (double) s->njoint; for(i=1;i<=s->njoint;i++) { s->q0[i] = mxGetPr(field_value_ptr) [i-1]; s->qd0[i] = mxGetPr(field_value_ptr2)[i-1]; s->qdd0[i] = mxGetPr(field_value_ptr3)[i-1]; } //=== Range values ==========================================================// /* qmin */ field_value_ptr = mxGetField(s_ptr, 0, "qmin"); if (field_value_ptr != NULL) { // vérifier la taille de qmin // ... s->qmin = (double*) calloc(s->njoint+1,sizeof(double)); s->qmin[0] = (double) s->njoint; // copier les valeurs de qmin // ... } else s->qmin = NULL; /* qmax */ field_value_ptr = mxGetField(s_ptr, 0, "qmax"); if (field_value_ptr != NULL) { // vérifier la taille de qmax // ... s->qmax = (double*) calloc(s->njoint+1,sizeof(double)); s->qmax[0] = (double) s->njoint; // copier les valeurs de qmax // ... } else s->qmax = NULL; //=== Work Variables ==========================================================// /* q,qd,qdd */ field_value_ptr = mxGetField(s_ptr, 0, "q"); field_value_ptr2 = mxGetField(s_ptr, 0, "qd"); field_value_ptr3 = mxGetField(s_ptr, 0, "qdd"); if (field_value_ptr == NULL) mexErrMsgTxt("Could not get field \"q\n"); if (field_value_ptr2 == NULL) mexErrMsgTxt("Could not get field \"qd\n"); if (field_value_ptr3 == NULL) mexErrMsgTxt("Could not get field \"qdd\n"); s->q = (double*) calloc(s->njoint+1,sizeof(double)); s->qd = (double*) calloc(s->njoint+1,sizeof(double)); s->qdd = (double*) calloc(s->njoint+1,sizeof(double)); s->q[0] = (double) s->njoint; s->qd[0] = (double) s->njoint; s->qdd[0] = (double) s->njoint; for(i=1;i<=s->njoint;i++) { s->q[i] = mxGetPr(field_value_ptr) [i-1]; s->qd[i] = mxGetPr(field_value_ptr2)[i-1]; s->qdd[i] = mxGetPr(field_value_ptr3)[i-1]; } /* frc */ s->frc[0] = NULL; for(i=1;i<=3;i++) { s->frc[i] = (double*) calloc(s->nbody+1,sizeof(double)); s->frc[i][0] = (double) s->nbody; for(j=1;j<=s->nbody;j++) s->frc[i][j] = 0.0; } /* trq */ s->trq[0] = NULL; for(i=1;i<=3;i++) { s->trq[i] = (double*) calloc(s->nbody+1,sizeof(double)); s->trq[i][0] = (double) s->nbody; for(j=1;j<=s->nbody;j++) s->trq[i][j] = 0.0; } /* Qq */ s->Qq = (double*) calloc(s->njoint+1,sizeof(double)); s->Qq[0] = (double) s->njoint; for(i=1;i<=s->njoint;i++) s->Qq[i] = 0.0; /* tsim */ s->tsim = 0.0; //=== System parameters ====================================================// /* dpt */ s->dpt[0] = NULL; if (s->npt) { field_value_ptr = mxGetField(s_ptr, 0, "dpt"); if (field_value_ptr == NULL) mexErrMsgTxt("Could not get field \"dpt\"\n"); for(i=1;i<=3;i++) { s->dpt[i] = (double*) calloc(s->npt+1,sizeof(double)); s->dpt[i][0] = (double) s->npt; for(j=1;j<=s->npt;j++) s->dpt[i][j] = mxGetPr(field_value_ptr)[(i-1)+3*(j-1)]; } } else // je pense que ce cas est impossible mais il faudrait vérifier avant de supprimer for(i=1;i<=3;i++) s->dpt[i] = NULL; /* l */ s->l[0] = NULL; field_value_ptr = mxGetField(s_ptr, 0, "l"); if (field_value_ptr == NULL) mexErrMsgTxt("Could not get field \"l\"\n"); for(i=1;i<=3;i++) { s->l[i] = (double*) calloc(s->nbody+1,sizeof(double)); s->l[i][0] = (double) s->nbody; for(j=1;j<=s->nbody;j++) s->l[i][j] = mxGetPr(field_value_ptr)[(i-1)+3*(j-1)]; } /* m */ field_value_ptr = mxGetField(s_ptr, 0, "m"); if (field_value_ptr == NULL) mexErrMsgTxt("Could not get field \"m\"\n"); s->m = (double*) calloc(s->nbody+1,sizeof(double)); s->m[0] = (double) s->nbody; for(i=1;i<=s->nbody;i++) s->m[i] = mxGetPr(field_value_ptr)[i-1]; /* In */ s->In[0] = NULL; field_value_ptr = mxGetField(s_ptr, 0, "In"); if (field_value_ptr == NULL) mexErrMsgTxt("Could not get field \"In\"\n"); for(i=1;i<=9;i++) { s->In[i] = (double*) calloc(s->nbody+1,sizeof(double)); s->In[i][0] = (double) s->nbody; for(j=1;j<=s->nbody;j++) s->In[i][j] = mxGetPr(field_value_ptr)[(i-1)+9*(j-1)]; } /* g */ s->g[0] = 0.0; field_value_ptr = mxGetField(s_ptr, 0, "g"); if (field_value_ptr == NULL) mexErrMsgTxt("Could not get field \"g\"\n"); for(i=1;i<=3;i++) s->g[i] = mxGetPr(field_value_ptr)[i-1]; /* Constraint data */ /* lrod */ field_value_ptr = mxGetField(s_ptr, 0, "lrod"); if (field_value_ptr == NULL) mexErrMsgTxt("Could not get field \"lrod\"\n"); ncol = mxGetN(field_value_ptr); if (ncol) { s->lrod = (double*) calloc(ncol+1,sizeof(double)); s->lrod[0] = ncol; for(i=1;i<=ncol;i++) s->lrod[i] = mxGetPr(field_value_ptr)[i-1]; } else s->lrod = NULL; /* NRerr */ field_value_ptr = mxGetField(s_ptr, 0, "NRerr"); if (field_value_ptr == NULL) s->NRerr = 1e-9; //default value else s->NRerr = mxGetScalar(field_value_ptr); /* Link Data */ /* Z,Zd,Fl */ if (s->Nlink) { s->Z = (double*) calloc(s->Nlink+1,sizeof(double)); s->Zd = (double*) calloc(s->Nlink+1,sizeof(double)); s->Fl = (double*) calloc(s->Nlink+1,sizeof(double)); s->Z[0] = (double) s->Nlink; s->Zd[0] = (double) s->Nlink; s->Fl[0] = (double) s->Nlink; for(i=1;i<=s->Nlink;i++) { s->Z[i] = 0.0; s->Zd[i] = 0.0; s->Fl[i] = 0.0; } } else { s->Z = NULL; s->Zd = NULL; s->Fl = NULL; } /* l3DWr */ if (s->Nlink3D) { s->l3DWr = (double**) calloc(s->Nlink3D+1,sizeof(double*)); s->l3DWr[0] = NULL; for(i=1;i<=s->Nlink3D;i++) { s->l3DWr[i] = (double*) calloc(6+1,sizeof(double)); for(j=0;j<=6;j++) s->l3DWr[i][j] = 0.0; } } else s->l3DWr = NULL; /* Ext. Forces Data */ /* xfidpt */ if (s->Nxfrc) { field_value_ptr = mxGetField(s_ptr, 0, "xfidpt"); if (field_value_ptr == NULL) mexErrMsgTxt("Could not get field \"xfidpt\"\n"); s->xfidpt = (int*) calloc(s->Nxfrc+1,sizeof(int)); s->xfidpt[0] = s->Nxfrc; ncol = mxGetN(field_value_ptr); for(i=1;i<=ncol;i++) s->xfidpt[i] =(int) mxGetPr(field_value_ptr)[i-1]; } else s->xfidpt = NULL; /* SWr */ if (s->Nxfrc) { s->SWr = (double**) calloc(s->Nxfrc+1,sizeof(double*)); s->SWr[0] = NULL; for(i=1;i<=s->Nxfrc;i++) { s->SWr[i] = (double*) calloc(9+1,sizeof(double)); for(j=0;j<=9;j++) s->SWr[i][j] = 0.0; } } else s->SWr = NULL; /* Wheel Data */ /* rnom */ if (s->Nwheel) { field_value_ptr = mxGetField(s_ptr, 0, "rnom"); if (field_value_ptr == NULL) mexErrMsgTxt("Could not get field \"rnom\"\n"); s->rnom = (double*) calloc(s->Nwheel+1,sizeof(double)); s->rnom[0] = (double) s->Nwheel; for(i=1;i<=s->Nwheel;i++) s->rnom[i] = mxGetPr(field_value_ptr)[i-1]; } else s->rnom = NULL; #if !defined SENSORKIN /* User Model */ /* user_model */ if (s->Nuser_model) { usm_ptr = mxGetField(s_ptr, 0, "user_model"); if (usm_ptr == NULL) mexErrMsgTxt("Could not get field \"user_model\"\n"); s->user_model = loadUserModel(usm_ptr); } /* ux,uxd,ux0 */ if (s->Nux) { field_value_ptr = mxGetField(s_ptr, 0, "ux"); field_value_ptr2 = mxGetField(s_ptr, 0, "uxd"); field_value_ptr3 = mxGetField(s_ptr, 0, "ux0"); if ((field_value_ptr == NULL) || (field_value_ptr2 == NULL) || (field_value_ptr3 == NULL)) mexErrMsgTxt("Could not get field \"ux\",\"uxd\" or \"ux0\"\n"); s->ux = (double*) calloc(s->Nux+1,sizeof(double)); s->uxd = (double*) calloc(s->Nux+1,sizeof(double)); s->ux0 = (double*) calloc(s->Nux+1,sizeof(double)); s->ux[0] = (double) s->Nux; s->uxd[0] = (double) s->Nux; s->ux0[0] = (double) s->Nux; for(i=1;i<=s->Nux;i++) { s->ux[i] = mxGetPr(field_value_ptr)[i-1]; s->uxd[i] = mxGetPr(field_value_ptr2)[i-1]; s->ux0[i] = mxGetPr(field_value_ptr3)[i-1]; } } else { s->ux = NULL; s->uxd = NULL; s->ux0 = NULL; } //#ifndef CMEX /* User IO */ s->user_IO = initUserIO(s); //#endif #endif //=== Other ========================================================// /* qddu */ if (s->nqu) { s->qddu = (double*) calloc(s->nqu+1,sizeof(double)); s->qddu[0] = (double) s->nqu; } else s->qddu = NULL; /* DonePart */ field_value_ptr = mxGetField(s_ptr, 0, "DonePart"); if (field_value_ptr == NULL) mexErrMsgTxt("Could not get field \"DonePart\"\n"); s->DonePart =(int) *mxGetPr(field_value_ptr); return s; }
/* * Code based on loadMBSdata (in MBSdataStruct.c) */ void loadMBSelements_xml(MBSdataStruct *s, xmlDocPtr doc, xmlNodePtr cur) { int i,j, ncol; //=== Index parameters ======================================// /* qc */ if (s->nqc) { s->qc = (int*) calloc(s->nqc+1,sizeof(int)); s->qc[0] = s->nqc; parseVectorInteger_xml("qc", doc, cur, s->qc); } else s->qc = NULL; /* qu */ if (s->nqu) { s->qu = (int*) calloc(s->nqu+1,sizeof(int)); s->qu[0] = s->nqu; parseVectorInteger_xml("qu", doc, cur, s->qu); } else s->qu = NULL; /* qa */ if (s->nqa) { s->qa = (int*) calloc(s->nqa+1,sizeof(int)); s->qa[0] = s->nqa; parseVectorInteger_xml("qa", doc, cur, s->qa); } else s->qa = NULL; /* qv */ if (s->nqv) { s->qv = (int*) calloc(s->nqv+1,sizeof(int)); s->qv[0] = s->nqv; parseVectorInteger_xml("qv", doc, cur, s->qv); } else s->qv = NULL; /* hu */ // JFC : 15/01/2008 : ajout if (s->nhu) { s->hu = (int*) calloc(s->nhu+1,sizeof(int)); s->hu[0] = s->nhu; parseVectorInteger_xml("hu", doc, cur, s->hu); } else s->hu = NULL; /* qlocked */ if (s->nqlocked) { s->qlocked = (int*) calloc(s->nqlocked+1,sizeof(int)); s->qlocked[0] = s->nqlocked; parseVectorInteger_xml("qlocked", doc, cur, s->qlocked); } else s->qlocked = NULL; /* qdriven */ if (s->nqdriven) { s->qdriven = (int*) calloc(s->nqdriven+1,sizeof(int)); s->qdriven[0] = s->nqdriven; parseVectorInteger_xml("qdriven", doc, cur, s->qdriven); } else s->qdriven = NULL; ////=== Initial values ==========================================================// // // /* q0,qd0,qdd0 * // field_value_ptr = mxGetField(s_ptr, 0, "q0"); // field_value_ptr2 = mxGetField(s_ptr, 0, "qd0"); // field_value_ptr3 = mxGetField(s_ptr, 0, "qdd0"); // if (field_value_ptr == NULL) // mexErrMsgTxt("Could not get field \"q0\"\n"); // if (field_value_ptr2 == NULL) // mexErrMsgTxt("Could not get field \"qd0\"\n"); // if (field_value_ptr3 == NULL) // mexErrMsgTxt("Could not get field \"qdd0\"\n"); // s->q0 = (double*) calloc(s->njoint+1,sizeof(double)); // s->qd0 = (double*) calloc(s->njoint+1,sizeof(double)); // s->qdd0 = (double*) calloc(s->njoint+1,sizeof(double)); // s->q0[0] = (double) s->njoint; // s->qd0[0] = (double) s->njoint; // s->qdd0[0] = (double) s->njoint; // for(i=1;i<=s->njoint;i++) // { // s->q0[i] = mxGetPr(field_value_ptr) [i-1]; // s->qd0[i] = mxGetPr(field_value_ptr2)[i-1]; // s->qdd0[i] = mxGetPr(field_value_ptr3)[i-1]; // } //=== Range values ==========================================================// /* qmin */ if(isElement_xml(cur, "qmin")) { // vérifier la taille de qmin // ... s->qmin = (double*) calloc(s->njoint+1,sizeof(double)); s->qmin[0] = (double) s->njoint; // copier les valeurs de qmin // ... } else s->qmin = NULL; /* qmax */ if(isElement_xml(cur, "qmax")) { // vérifier la taille de qmax // ... s->qmax = (double*) calloc(s->njoint+1,sizeof(double)); s->qmax[0] = (double) s->njoint; // copier les valeurs de qmax // ... } else s->qmax = NULL; //=== Work Variables ==========================================================// /* q,qd,qdd */ s->q = (double*) calloc(s->njoint+1,sizeof(double)); s->qd = (double*) calloc(s->njoint+1,sizeof(double)); s->qdd = (double*) calloc(s->njoint+1,sizeof(double)); s->q[0] = (double) s->njoint; s->qd[0] = (double) s->njoint; s->qdd[0] = (double) s->njoint; parseVectorDouble_xml("q", doc, cur, s->q); parseVectorDouble_xml("qd", doc, cur, s->qd); parseVectorDouble_xml("qdd", doc, cur, s->qdd); /* frc */ s->frc[0] = NULL; for(i=1;i<=3;i++) { s->frc[i] = (double*) calloc(s->nbody+1,sizeof(double)); s->frc[i][0] = (double) s->nbody; for(j=1;j<=s->nbody;j++) s->frc[i][j] = 0.0; } /* trq */ s->trq[0] = NULL; for(i=1;i<=3;i++) { s->trq[i] = (double*) calloc(s->nbody+1,sizeof(double)); s->trq[i][0] = (double) s->nbody; for(j=1;j<=s->nbody;j++) s->trq[i][j] = 0.0; } /* Qq */ s->Qq = (double*) calloc(s->njoint+1,sizeof(double)); s->Qq[0] = (double) s->njoint; for(i=1;i<=s->njoint;i++) s->Qq[i] = 0.0; /* tsim */ s->tsim = 0.0; //=== System parameters ====================================================// /* dpt */ s->dpt[0] = NULL; if (s->npt) { for(i=1;i<=3;i++) { s->dpt[i] = (double*) calloc(s->npt+1,sizeof(double)); s->dpt[i][0] = (double) s->npt; } parseMatrixDouble_xml("dpt", doc, cur, s->dpt); } else // je pense que ce cas est impossible mais il faudrait vérifier avant de supprimer for(i=1;i<=3;i++) s->dpt[i] = NULL; /* l */ s->l[0] = NULL; for(i=1;i<=3;i++) { s->l[i] = (double*) calloc(s->nbody+1,sizeof(double)); s->l[i][0] = (double) s->nbody; } parseMatrixDouble_xml("l", doc, cur, s->l); /* m */ s->m = (double*) calloc(s->nbody+1,sizeof(double)); s->m[0] = (double) s->nbody; parseVectorDouble_xml("m", doc, cur, s->m); /* In */ s->In[0] = NULL; for(i=1;i<=9;i++) { s->In[i] = (double*) calloc(s->nbody+1,sizeof(double)); s->In[i][0] = (double) s->nbody; } parseMatrixDouble_xml("In", doc, cur, s->In); /* g */ s->g[0] = 0.0; parseVectorDouble_xml("g", doc, cur, s->g); /* Constraint data */ /* lrod */ ncol = getNbColElement_xml("lrod", doc, cur); if (ncol) { s->lrod = (double*) calloc(ncol+1,sizeof(double)); s->lrod[0] = ncol; parseVectorDouble_xml("lrod", doc, cur, s->lrod); } else s->lrod = NULL; /* NRerr */ if (!isElement_xml(cur,"NRerr")) s->NRerr = 1e-9; //default value else s->NRerr = parseScalarInteger_xml("NRerr", doc, cur); /* Link Data */ /* Z,Zd,Fl */ if (s->Nlink) { s->Z = (double*) calloc(s->Nlink+1,sizeof(double)); s->Zd = (double*) calloc(s->Nlink+1,sizeof(double)); s->Fl = (double*) calloc(s->Nlink+1,sizeof(double)); s->Z[0] = (double) s->Nlink; s->Zd[0] = (double) s->Nlink; s->Fl[0] = (double) s->Nlink; for(i=1;i<=s->Nlink;i++) { s->Z[i] = 0.0; s->Zd[i] = 0.0; s->Fl[i] = 0.0; } } else { s->Z = NULL; s->Zd = NULL; s->Fl = NULL; } /* l3DWr */ if (s->Nlink3D) { s->l3DWr = (double**) calloc(s->Nlink3D+1,sizeof(double*)); s->l3DWr[0] = NULL; for(i=1;i<=s->Nlink3D;i++) { s->l3DWr[i] = (double*) calloc(6+1,sizeof(double)); for(j=0;j<=6;j++) s->l3DWr[i][j] = 0.0; } } else s->l3DWr = NULL; /* Ext. Forces Data */ /* xfidpt */ if (s->Nxfrc) { s->xfidpt = (int*) calloc(s->Nxfrc+1,sizeof(int)); s->xfidpt[0] = s->Nxfrc; parseVectorInteger_xml("xfidpt", doc, cur, s->xfidpt); } else s->xfidpt = NULL; /* SWr */ if (s->Nxfrc) { s->SWr = (double**) calloc(s->Nxfrc+1,sizeof(double*)); s->SWr[0] = NULL; for(i=1;i<=s->Nxfrc;i++) { s->SWr[i] = (double*) calloc(9+1,sizeof(double)); for(j=0;j<=9;j++) s->SWr[i][j] = 0.0; } } else s->SWr = NULL; /* Wheel Data */ /* rnom */ if (s->Nwheel) { s->rnom = (double*) calloc(s->Nwheel+1,sizeof(double)); s->rnom[0] = (double) s->Nwheel; parseVectorDouble_xml("rnom", doc, cur, s->rnom); } else s->rnom = NULL; #if !defined SENSORKIN /* User Model */ /* user_model */ if (s->Nuser_model) { s->user_model = loadUserModel_xml(doc, cur); } /* ux,uxd,ux0 */ if (s->Nux) { s->ux = (double*) calloc(s->Nux+1,sizeof(double)); s->uxd = (double*) calloc(s->Nux+1,sizeof(double)); s->ux0 = (double*) calloc(s->Nux+1,sizeof(double)); s->ux[0] = (double) s->Nux; s->uxd[0] = (double) s->Nux; s->ux0[0] = (double) s->Nux; parseVectorDouble_xml("ux", doc, cur, s->ux); parseVectorDouble_xml("uxd", doc, cur, s->uxd); parseVectorDouble_xml("ux0", doc, cur, s->ux0); } else { s->ux = NULL; s->uxd = NULL; s->ux0 = NULL; } //#ifndef CMEX /* User IO */ s->user_IO = initUserIO(s); //#endif #endif //=== Other ========================================================// /* qddu */ if (s->nqu) { s->qddu = (double*) calloc(s->nqu+1,sizeof(double)); s->qddu[0] = (double) s->nqu; } else s->qddu = NULL; /* DonePart */ s->DonePart = parseScalarInteger_xml("DonePart", doc, cur); }