Commit ee2c282c by Olivier Lantsoght 🏁

### [SolveKin][New] New mode to retrieve the iterations of newton-raphson done for one configuration.

parent 5f0598b3
 ... ... @@ -57,7 +57,7 @@ int mbs_Rred(MbsAux *mbs_aux, MbsData *s); /*! \brief compute a position of the multibody system which solves the constraints * * The algorithm is a Newton/Raphson procedure which solves the equation: \f$v^{k+1} = v^{k}-\left(J_{v}\right)^{-1} h\f$.\n * The algorithm calls a Newton/Raphson procedure which solves the equation: \f$v^{k+1} = v^{k}-\left(J_{v}\right)^{-1} h\f$.\n * Robotran Basics Eq(17), chapter 3.2.1, pp. 12 * * \param[in,out] s the MbsData structure. ... ... @@ -67,6 +67,18 @@ int mbs_Rred(MbsAux *mbs_aux, MbsData *s); */ int mbs_close_geo(MbsData *s, MbsAux *mbs_aux); /*! \brief compute an iteration of the position of the multibody system to solve the constraints * * The algorithm is one step of a Newton/Raphson procedure which solves the equation: \f$v^{k+1} = v^{k}-\left(J_{v}\right)^{-1} h\f$.\n * Robotran Basics Eq(17), chapter 3.2.1, pp. 12 * * \param[in,out] s the MbsData structure. * \param[in,out] mbs_aux the MbsAux structure related to the MbsData structure. * * \return success (0) or error (<0) */ int mbs_step_close_geo(MbsData *s, MbsAux *mbs_aux); /*! \brief compute the dependent velocities that solves the constraints. * * The function solves the linear equation: \f$\dot{v} = \textbf{B}_{vu} \dot{u} \f$.\n ... ...
 //------------------------------- // UCL-CEREM-MBS // // @version MBsysLab_s 1.7.a // // Creation : 2008 by JF Collard // Last update : 01/10/2008 //------------------------------- // // Gestion via Bugzilla : // 01/10/2008 : JFC : Bug n40 // #include "MBSfun.h" #include "nrfct.h" #include "mbs_project_interface.h" #include "mbs_message.h" double norm_vector(double *v, int n); double norminf_vector(double *v, int n); int mbs_close_geo(MbsData *s, MbsAux *mbs_aux) { int i,j; int iter=0; int nL,nC; int err = 0; double d; iter = 0; mbs_aux->norm_h=1.0; while((mbs_aux->norm_h > mbs_aux->NRerr) && (iter++ <= mbs_aux->MAX_NR_ITER)) { // Calcul des contraintes et de la Jacobienne mbs_calc_hJ(s,mbs_aux); // Norme des contraintes (en supposant que toutes les contraintes independantes sont au debut ???) mbs_aux->norm_h = norminf_vector(mbs_aux->h,s->nhu); // -Jv nL = s->nhu; nC = s->nqv; for(i=1;i<=nL;i++) { for(j=1;j<=nC;j++) { mbs_aux->mJv[i][j] = -mbs_aux->Jac[s->hu[i]][s->qv[j]]; } } // Decomposition LU de la matrice -Jv err = ludcmp(mbs_aux->mJv,s->nqv,mbs_aux->ind_mJv,&d); if (err < 0) // Error management { mbs_msg("\t \t >CLOSE GEO> Error in mbs_close_geo during LU decomposition of Jv matrix \n"); return -50+err; } if(mbs_aux->norm_h > mbs_aux->NRerr) { // err for(i=1;i<=s->nhu;i++) { mbs_aux->mJv_h[i] = mbs_aux->h[s->hu[i]]; // mbs_aux->mJv_h[i][1] = mbs_aux->h[s->hu[i]]; } lubksb(mbs_aux->mJv,s->nqv,mbs_aux->ind_mJv,mbs_aux->mJv_h); // gaussj(mbs_aux->mJv, s->nqv, mbs_aux->mJv_h, 1); // Correction des qv for(i=1;i<=s->nhu;i++) { s->q[s->qv[i]] += mbs_aux->mJv_h[i]; // s->q[s->qv[i]] += mbs_aux->mJv_h[i][1]; } } } if (iter >= mbs_aux->MAX_NR_ITER) // Error management { mbs_msg("\t \t >CLOSE GEO> Impossible to close the geometry after %d iterations \n", iter); return -50; } return iter; } void mbs_close_kin(MbsData *s, MbsAux *mbs_aux) { int i,j,k; int nL,nC,nk; double term; nL = s->nhu; nC = s->nqu + s->nqc; for(i=1;i<=nL;i++) { for(j=1;j<=nC;j++) { mbs_aux->Juct[j][i] = mbs_aux->Jac[s->hu[i]][mbs_aux->iquc[j]]; // mbs_aux->Juc[i][j] = mbs_aux->Jac[s->hu[i]][mbs_aux->iquc[j]]; } } // calcul de la matrice de couplage des vitesses for (j=1;j<=nC;j++) { lubksb(mbs_aux->mJv,s->nqv,mbs_aux->ind_mJv,mbs_aux->Juct[j]); } // gaussj(mbs_aux->mJv, s->nqv, mbs_aux->Juc, nC); for(i=1;i<=nL;i++) { for(j=1;j<=nC;j++) { mbs_aux->Bvuc[i][j] = mbs_aux->Juct[j][i]; // mbs_aux->Bvuc[i][j] = mbs_aux->Juc[i][j]; } } // calcul des vitesses dependantes (qdv = Bvuc * qduc) nL = s->nqv; nk = s->nqu + s->nqc; for(i=1;i<=nL;i++) { term = 0.0; for(k=1;k<=nk;k++) { term += mbs_aux->Bvuc[i][k]*s->qd[mbs_aux->iquc[k]]; } s->qd[s->qv[i]] = term; } } void mbs_calc_hJ(MbsData *s, MbsAux *mbs_aux) { int i,j; // contraintes de fermeture de boucles mbs_cons_hJ(mbs_aux->h,mbs_aux->Jac,s,s->tsim); // contraintes user if (s->Nuserc>0) { user_cons_hJ(mbs_aux->huserc, mbs_aux->Juserc, s, s->tsim); // ajout des contraintes user aux contraintes de fermeture for (i=1;i<=s->Nuserc;i++) { mbs_aux->h[s->Nloopc+i] = mbs_aux->huserc[i]; for (j=1;j<=s->njoint;j++) mbs_aux->Jac[s->Nloopc+i][j] = mbs_aux->Juserc[i][j]; } } } void mbs_calc_jdqd(MbsData *s, MbsAux *mbs_aux) { int i; // compute contribution of symbolic constraints to jdqd mbs_cons_jdqd(mbs_aux->jdqd,s,s->tsim); // compute contribution of user constraints to jdqd if (s->Nuserc>0) { user_cons_jdqd(mbs_aux->jdqduserc, s, s->tsim); // ajout des contraintes user aux contraintes de fermeture for (i=1;i<=s->Nuserc;i++) { mbs_aux->jdqd[s->Nloopc+i] = mbs_aux->jdqduserc[i]; } } // bp = (-Jv)\jdqd for(i=1;i<=s->nhu;i++) { mbs_aux->bp[i] = mbs_aux->jdqd[s->hu[i]]; } lubksb(mbs_aux->mJv,s->nqv,mbs_aux->ind_mJv,mbs_aux->bp); } //------------------------------- // UCL-CEREM-MBS // // @version MBsysLab_s 1.7.a // // Creation : 2008 by JF Collard // Last update : 01/10/2008 //------------------------------- // // Gestion via Bugzilla : // 01/10/2008 : JFC : Bug n°40 // #include "MBSfun.h" #include "nrfct.h" #include "mbs_project_interface.h" #include "mbs_message.h" double norm_vector(double *v, int n); double norminf_vector(double *v, int n); int mbs_close_geo(MbsData *s, MbsAux *mbs_aux) { int i,j; int iter=0; int nL,nC; int err = 0; double d; iter = 0; mbs_aux->norm_h=1.0; while((mbs_aux->norm_h > mbs_aux->NRerr) && (iter++ <= mbs_aux->MAX_NR_ITER)) { err = mbs_step_close_geo(s, mbs_aux); if (err <0){ mbs_msg("\t \t >CLOSE GEO> Error in mbs_close_geo during a NR iteration \n"); return -50+err; } } if (iter >= mbs_aux->MAX_NR_ITER) // Error management { mbs_msg("\t \t >CLOSE GEO> Impossible to close the geometry after %d iterations \n", iter); return -50; } return iter; } int mbs_step_close_geo(MbsData *s, MbsAux *mbs_aux){ int i,j; int nL,nC; int err = 0; double d; // Calcul des contraintes et de la Jacobienne mbs_calc_hJ(s,mbs_aux); // Norme des contraintes (en supposant que toutes les contraintes independantes sont au debut ???) mbs_aux->norm_h = norminf_vector(mbs_aux->h,s->nhu); // -Jv nL = s->nhu; nC = s->nqv; for(i=1;i<=nL;i++) { for(j=1;j<=nC;j++) { mbs_aux->mJv[i][j] = -mbs_aux->Jac[s->hu[i]][s->qv[j]]; } } // Decomposition LU de la matrice -Jv err = ludcmp(mbs_aux->mJv,s->nqv,mbs_aux->ind_mJv,&d); if (err < 0) // Error management { mbs_msg("\t \t >CLOSE GEO> Error in mbs_step_close_geo during LU decomposition of Jv matrix \n"); return err; } if(mbs_aux->norm_h > mbs_aux->NRerr) { // err for(i=1;i<=s->nhu;i++) { mbs_aux->mJv_h[i] = mbs_aux->h[s->hu[i]]; } lubksb(mbs_aux->mJv,s->nqv,mbs_aux->ind_mJv,mbs_aux->mJv_h); // Correction des qv for(i=1;i<=s->nhu;i++) { s->q[s->qv[i]] += mbs_aux->mJv_h[i]; } } return 0; } void mbs_close_kin(MbsData *s, MbsAux *mbs_aux) { int i,j,k; int nL,nC,nk; double term; nL = s->nhu; nC = s->nqu + s->nqc; for(i=1;i<=nL;i++) { for(j=1;j<=nC;j++) { mbs_aux->Juct[j][i] = mbs_aux->Jac[s->hu[i]][mbs_aux->iquc[j]]; // mbs_aux->Juc[i][j] = mbs_aux->Jac[s->hu[i]][mbs_aux->iquc[j]]; } } // calcul de la matrice de couplage des vitesses for (j=1;j<=nC;j++) { lubksb(mbs_aux->mJv,s->nqv,mbs_aux->ind_mJv,mbs_aux->Juct[j]); } // gaussj(mbs_aux->mJv, s->nqv, mbs_aux->Juc, nC); for(i=1;i<=nL;i++) { for(j=1;j<=nC;j++) { mbs_aux->Bvuc[i][j] = mbs_aux->Juct[j][i]; // mbs_aux->Bvuc[i][j] = mbs_aux->Juc[i][j]; } } // calcul des vitesses dependantes (qdv = Bvuc * qduc) nL = s->nqv; nk = s->nqu + s->nqc; for(i=1;i<=nL;i++) { term = 0.0; for(k=1;k<=nk;k++) { term += mbs_aux->Bvuc[i][k]*s->qd[mbs_aux->iquc[k]]; } s->qd[s->qv[i]] = term; } } void mbs_calc_hJ(MbsData *s, MbsAux *mbs_aux) { int i,j; // contraintes de fermeture de boucles mbs_cons_hJ(mbs_aux->h,mbs_aux->Jac,s,s->tsim); // contraintes user if (s->Nuserc>0) { user_cons_hJ(mbs_aux->huserc, mbs_aux->Juserc, s, s->tsim); // ajout des contraintes user aux contraintes de fermeture for (i=1;i<=s->Nuserc;i++) { mbs_aux->h[s->Nloopc+i] = mbs_aux->huserc[i]; for (j=1;j<=s->njoint;j++) mbs_aux->Jac[s->Nloopc+i][j] = mbs_aux->Juserc[i][j]; } } } void mbs_calc_jdqd(MbsData *s, MbsAux *mbs_aux) { int i; // compute contribution of symbolic constraints to jdqd mbs_cons_jdqd(mbs_aux->jdqd,s,s->tsim); // compute contribution of user constraints to jdqd if (s->Nuserc>0) { user_cons_jdqd(mbs_aux->jdqduserc, s, s->tsim); // ajout des contraintes user aux contraintes de fermeture for (i=1;i<=s->Nuserc;i++) { mbs_aux->jdqd[s->Nloopc+i] = mbs_aux->jdqduserc[i]; } } // bp = (-Jv)\jdqd for(i=1;i<=s->nhu;i++) { mbs_aux->bp[i] = mbs_aux->jdqd[s->hu[i]]; } lubksb(mbs_aux->mJv,s->nqv,mbs_aux->ind_mJv,mbs_aux->bp); }
 ... ... @@ -149,7 +149,7 @@ void mbs_solvekin_init(MbsSolvekin* sk, MbsData* mbs_data) // base value, all cases sk->tindex = 0; // Motion identification if (sk->options->motion==oneshot){ if (sk->options->motion==oneshot || sk->options->motion==closeloop){ sk->trajectorytype = 0; sk->tsim = mbs_data->tsim; sk->options->buffersize=1; ... ... @@ -367,52 +367,66 @@ void mbs_solvekin_loop(MbsSolvekin* sk, MbsData* mbs_data) { int err = 0, err2=0, i; if (!sk->trajectorytype){ // Oneshot case, just call the mbs_fct_solvekin err = mbs_fct_solvekin(mbs_data, sk); if (err < 0) // Error management { mbs_msg(">>SOLVEKIN>>\n"); mbs_msg(">>SOLVEKIN>> Error during inverse kinematic analysis at time %g for oneshot case !\n", mbs_data->tsim); mbs_error_msg("[%d] in mbs_solvekin_loop !! \n", -500 + err); int iter = 0; if (sk->options->motion==closeloop){ mbs_solvekin_save(sk, mbs_data, iter); sk->mbs_aux->norm_h=1.0; } while(err == 0 && iter <= sk->mbs_aux->MAX_NR_ITER && sk->mbs_aux->norm_h > sk->mbs_aux->NRerr){ // Oneshot or close loop case, just call the mbs_fct_solvekin err = mbs_fct_solvekin(mbs_data, sk); iter ++; // Check contents of vectors for (i=1; i<=mbs_data->njoint;i++){ if (isnan(mbs_data->q[i])) { // Error management mbs_msg(">>SOLVEKIN>> q[%d] is Nan at time %g s ! \n", i, mbs_data->tsim ); err2 = mbs_solvekin_write_buffers(sk); if (err2<0){ mbs_msg(">>SOLVEKIN>>\n"); mbs_msg(">>SOLVEKIN>> Error [%d] while saving results to disk !\n", -500 + err2); mbs_msg(">>SOLVEKIN>>\n"); } mbs_error_msg("[%d] in mbs_solvekin_loop !! \n", -537); if (err < 0) // Error management { mbs_msg(">>SOLVEKIN>>\n"); mbs_msg(">>SOLVEKIN>> Error during inverse kinematic analysis at time %g for oneshot case !\n", mbs_data->tsim); mbs_error_msg("[%d] in mbs_solvekin_loop !! \n", -500 + err); } if (isnan(mbs_data->qd[i])) { mbs_msg(">>SOLVEKIN>> qd[%d] is Nan at time %g s ! \n", i, mbs_data->tsim ); err2 = mbs_solvekin_write_buffers(sk); if (err2<0){ mbs_msg(">>SOLVEKIN>>\n"); mbs_msg(">>SOLVEKIN>> Error [%d] while saving results to disk !\n", -500 + err2); mbs_msg(">>SOLVEKIN>>\n"); // Check contents of vectors for (i=1; i<=mbs_data->njoint;i++){ if (isnan(mbs_data->q[i])) { // Error management mbs_msg(">>SOLVEKIN>> q[%d] is Nan at time %g s ! \n", i, mbs_data->tsim ); err2 = mbs_solvekin_write_buffers(sk); if (err2<0){ mbs_msg(">>SOLVEKIN>>\n"); mbs_msg(">>SOLVEKIN>> Error [%d] while saving results to disk !\n", -500 + err2); mbs_msg(">>SOLVEKIN>>\n"); } mbs_error_msg("[%d] in mbs_solvekin_loop !! \n", -537); } mbs_error_msg("[%d] in mbs_solvekin_loop !! \n", -537); } if (isnan(mbs_data->qdd[i])) { mbs_msg(">>SOLVEKIN>> qdd[%d] is Nan at time %g s ! \n", i, mbs_data->tsim ); err2 = mbs_solvekin_write_buffers(sk); if (err2<0){ mbs_msg(">>SOLVEKIN>>\n"); mbs_msg(">>SOLVEKIN>> Error [%d] while saving results to disk !\n", -500 + err2); mbs_msg(">>SOLVEKIN>>\n"); if (isnan(mbs_data->qd[i])) { mbs_msg(">>SOLVEKIN>> qd[%d] is Nan at time %g s ! \n", i, mbs_data->tsim ); err2 = mbs_solvekin_write_buffers(sk); if (err2<0){ mbs_msg(">>SOLVEKIN>>\n"); mbs_msg(">>SOLVEKIN>> Error [%d] while saving results to disk !\n", -500 + err2); mbs_msg(">>SOLVEKIN>>\n"); } mbs_error_msg("[%d] in mbs_solvekin_loop !! \n", -537); } if (isnan(mbs_data->qdd[i])) { mbs_msg(">>SOLVEKIN>> qdd[%d] is Nan at time %g s ! \n", i, mbs_data->tsim ); err2 = mbs_solvekin_write_buffers(sk); if (err2<0){ mbs_msg(">>SOLVEKIN>>\n"); mbs_msg(">>SOLVEKIN>> Error [%d] while saving results to disk !\n", -500 + err2); mbs_msg(">>SOLVEKIN>>\n"); } mbs_error_msg("[%d] in mbs_solvekin_loop !! \n", -537); } mbs_error_msg("[%d] in mbs_solvekin_loop !! \n", -537); } } // adding to buffer mbs_solvekin_save(sk, mbs_data, mbs_data->tsim); // adding to buffer if (sk->options->motion==oneshot){ mbs_solvekin_save(sk, mbs_data, mbs_data->tsim); return; } else{ mbs_solvekin_save(sk, mbs_data, iter); } } } else{ // Time loop, time updated at the end of loop ... ... @@ -627,11 +641,17 @@ int mbs_fct_solvekin(MbsData *s, MbsSolvekin *sk) // Solving constraints, Ncons checked in initialization // Geometric resolution err = mbs_close_geo(s, sk->mbs_aux); if (err<0) // Error management { mbs_msg("\t \t in >SOLVEKIN> error while closing geometry in mbs_fct_solvekin \n"); return err; if (!sk->options->motion==closeloop){ err = mbs_close_geo(s, sk->mbs_aux); if (err<0) // Error management { mbs_msg("\t \t in >SOLVEKIN> error while closing geometry in mbs_fct_solvekin \n"); return err; } } else{ // We only do one iteration of N-R err = mbs_step_close_geo(s, sk->mbs_aux); } // Kinematic resolution ... ...
 ... ... @@ -22,7 +22,7 @@ /** @brief Enumeration of the possible motion for solvekin */ enum { oneshot, trajectory }; enum { oneshot, closeloop, trajectory}; /** * Structure defining the option of an inverse kinematics analysis ... ... @@ -32,6 +32,7 @@ typedef struct MbsSolvekinOptions /** \brief Determines how the evolution of q, qd qdd are provided and computed. * * - oneshot (default) : Compute q, qd and qdd at the current time and configuration of the MBS. * - closeloop : Same as oneshot but save the iteration of the close loop process (NR). * - trajectory : Compute the evolution of between t0 and tf. */ int motion; ... ...
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!