Commit ee2c282c authored by Olivier Lantsoght's avatar Olivier Lantsoght 🏁
Browse files

[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!
Please register or to comment