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);
}
