Commit 9276dbe1 authored by Nicolas Docquier's avatar Nicolas Docquier
Browse files

Merge branch 'accelred' into 'master'

Accelred

In this merge request, accelred in the generic files is implemented.

Pay attention that, as @fisette asked me, I changed all the 'qddu' of the project by 'udd'.

@averle @TimotheeHabra @lantsoght

See merge request !16
parents 466f9eee 81ec4f36
//
// MBsysTran - Release 8.1 (built: August 08, 2015)
//
// Copyright
// Universite catholique de Louvain (UCL)
// Center for Research in Energy and Mechatronics (CEREM)
// 2, Place du Levant
// 1348 Louvain-la-Neuve
// Belgium
//
// http://www.robotran.be
//
// ==> Generation Date: Thu Sep 03 09:41:26 2015
//
// ==> Project name: PendulumSpringC
//
// ==> Number of joints: 5
//
// ==> Function: F10 - Direct Dynamics (Explicit formulation) - RNEA + CP
//
#include <math.h>
#include "mbs_data.h"
#include "mbs_project_interface.h"
int mbs_accelred(MbsData *s, double tsim)
{
#include "mbs_accelred_PendulumSpringC.h"
#define MAX_NR_ITER 20
#define NR_ERR 1.0e-20
double *q, *qd, *qdd, *udd, *Qq;
double *lrod;
double **dpt, **l;
double *m;
double **In;
double *g;
int iter = 0;
q = s->q;
qd = s->qd;
qdd = s->qdd;
udd = s->udd;
Qq = s->Qq;
dpt = s->dpt;
l = s->l;
lrod = s->lrod;
m = s->m;
In = s->In;
g = s->g;
// Driven Variables
user_DrivenJoints(s,tsim);
NRh2 = 1.0;
while((NRh2 > NR_ERR) && (iter++ < MAX_NR_ITER))
{
// Trigonometric functions
S4 = sin(q[4]);
C4 = cos(q[4]);
S5 = sin(q[5]);
C5 = cos(q[5]);
S1 = sin(q[1]);
C1 = cos(q[1]);
S3 = sin(q[3]);
C3 = cos(q[3]);
// Augmented Joint Position Vectors
Dz23 = q[2]+dpt[3][3];
// Augmented Joint Position Vectors
// Constraints and Constraints Jacobian
ROlp1_12 = C4*C5-S4*S5;
ROlp1_32 = -C4*S5-S4*C5;
RLlp1_12 = dpt[3][6]*S4;
RLlp1_32 = dpt[3][6]*C4;
POlp1_12 = RLlp1_12+dpt[1][2];
RLlp1_13 = ROlp1_12*dpt[1][7];
RLlp1_33 = ROlp1_32*dpt[1][7];
POlp1_13 = POlp1_12+RLlp1_13;
POlp1_33 = RLlp1_32+RLlp1_33;
JTlp1_13_1 = RLlp1_32+RLlp1_33;
JTlp1_33_1 = -RLlp1_12-RLlp1_13;
RLlp2_12 = dpt[3][4]*S1;
RLlp2_32 = dpt[3][4]*C1;
h_1 = POlp1_13-RLlp2_12;
h_3 = POlp1_33-RLlp2_32;
// Loop Closure Thresold
NRh2_1 = h_1*h_1;
NRh2 = NRh2_1+h_3*h_3;
// LU Factorisation of the Jacobian (Jv)
Jvlu2_1 = JTlp1_33_1/JTlp1_13_1;
Jvlu2_2 = -RLlp1_13-Jvlu2_1*RLlp1_33;
//
if(NRh2 > NR_ERR)
{
// Newton-Raphson: Loop Closure
Jv_hy2 = h_3-Jvlu2_1*h_1;
Jv_hx2 = Jv_hy2/Jvlu2_2;
Jv_hx1 = (h_1-Jv_hx2*RLlp1_33)/JTlp1_13_1;
q[4] = q[4]-Jv_hx1;
q[5] = q[5]-Jv_hx2;
//
}
}
if(iter > MAX_NR_ITER) return(-1);
// Bvu Matrix
Bvu2y1 = -RLlp2_12-Jvlu2_1*RLlp2_32;
Bvu2x1 = Bvu2y1/Jvlu2_2;
Bvu1x1 = (RLlp2_32-Bvu2x1*RLlp1_33)/JTlp1_13_1;
// Dependent Velocities
qd[4] = qd[1]*Bvu1x1;
qd[5] = qd[1]*Bvu2x1;
// Constraints Quadratic Terms
ROjdqd1_12 = C4*C5-S4*S5;
ROjdqd1_32 = -C4*S5-S4*C5;
RLjdqd1_12 = dpt[3][6]*S4;
RLjdqd1_32 = dpt[3][6]*C4;
OMjdqd1_22 = qd[4]+qd[5];
ORjdqd1_12 = RLjdqd1_32*qd[4];
ORjdqd1_32 = -RLjdqd1_12*qd[4];
Apqpjdqd1_12 = ORjdqd1_32*qd[4];
Apqpjdqd1_32 = -ORjdqd1_12*qd[4];
RLjdqd1_13 = ROjdqd1_12*dpt[1][7];
RLjdqd1_33 = ROjdqd1_32*dpt[1][7];
ORjdqd1_13 = OMjdqd1_22*RLjdqd1_33;
ORjdqd1_33 = -OMjdqd1_22*RLjdqd1_13;
Apqpjdqd1_13 = Apqpjdqd1_12+OMjdqd1_22*ORjdqd1_33;
Apqpjdqd1_33 = Apqpjdqd1_32-OMjdqd1_22*ORjdqd1_13;
RLjdqd2_12 = dpt[3][4]*S1;
RLjdqd2_32 = dpt[3][4]*C1;
ORjdqd2_12 = qd[1]*RLjdqd2_32;
ORjdqd2_32 = -qd[1]*RLjdqd2_12;
Apqpjdqd2_12 = qd[1]*ORjdqd2_32;
Apqpjdqd2_32 = -qd[1]*ORjdqd2_12;
jdqd1 = Apqpjdqd1_13-Apqpjdqd2_12;
jdqd3 = Apqpjdqd1_33-Apqpjdqd2_32;
// b Vector
b1y2 = -jdqd3+Jvlu2_1*jdqd1;
b1x2 = b1y2/Jvlu2_2;
b1x1 = (-jdqd1-RLlp1_33*b1x2)/JTlp1_13_1;
// Forces and Torques computation
// Joint Forces
Qq = user_JointForces(s,tsim);
// Link Kinematics: Distance Z , Relative Velocity ZD
RLlnk2_12 = dpt[3][4]*S1;
RLlnk2_32 = dpt[3][4]*C1;
ORlnk2_12 = qd[1]*RLlnk2_32;
ORlnk2_32 = -qd[1]*RLlnk2_12;
Plnk11 = RLlnk2_12-dpt[1][1];
PPlnk1 = Plnk11*Plnk11+RLlnk2_32*RLlnk2_32;
Z1 = sqrt(PPlnk1);
e11 = Plnk11/Z1;
e31 = RLlnk2_32/Z1;
Zd1 = ORlnk2_12*e11+ORlnk2_32*e31;
// Link Forces
Flnk1 = user_LinkForces(Z1,Zd1,s,tsim,1);
// Link Dynamics : Forces projection on body-fixed frames
fSlnk11 = Flnk1*(e11*C1-e31*S1);
trqlnk1_1_2 = -fSlnk11*(dpt[3][4]-l[3][1]);
// Sensor Kinematics
// Forward Kinematics
BS91 = -qd[1]*qd[1];
AlF11 = g[3]*S1;
AlF31 = -g[3]*C1;
AlF12 = AlF11+(2.0)*qd[1]*qd[2];
AlF32 = AlF31+BS91*Dz23;
OM13 = qd[1]*S3;
OM23 = qd[1]*C3;
AlF13 = AlF12*C3;
AlF23 = -AlF12*S3;
AlM13_1 = Dz23*C3;
AlM23_1 = -Dz23*S3;
BS94 = -qd[4]*qd[4];
AlF14 = g[3]*S4;
AlF34 = -g[3]*C4;
OM25 = qd[4]+qd[5];
BS15 = -OM25*OM25;
AlF15 = AlF14*C5-S5*(AlF34+BS94*dpt[3][6]);
AlF35 = AlF14*S5+C5*(AlF34+BS94*dpt[3][6]);
AlM15_4 = dpt[3][6]*C5;
AlM35_4 = dpt[3][6]*S5;
// Backward Dynamics
FA15 = m[5]*(AlF15+BS15*l[1][5]);
FA35 = m[5]*AlF35;
CF25 = -FA35*l[1][5];
FB15_4 = m[5]*AlM15_4;
FB35_4 = m[5]*(AlM35_4-l[1][5]);
CM25_4 = In[5][5]-FB35_4*l[1][5];
FB35_5 = -m[5]*l[1][5];
CM25_5 = In[5][5]-FB35_5*l[1][5];
FA14 = m[4]*AlF14;
CF24 = CF25+FA14*l[3][4]+dpt[3][6]*(FA15*C5+FA35*S5);
FB14_4 = m[4]*l[3][4];
CM24_4 = In[5][4]+CM25_4+FB14_4*l[3][4]+dpt[3][6]*(FB15_4*C5+FB35_4*S5);
FA13 = m[3]*AlF13;
FA23 = m[3]*AlF23;
FA33 = m[3]*AlF32;
CF13 = qd[3]*In[9][3]*OM23;
CF23 = -qd[3]*In[9][3]*OM13;
FB13_1 = m[3]*AlM13_1;
FB23_1 = m[3]*AlM23_1;
FF12 = FA13*C3-FA23*S3;
CF22 = CF13*S3+CF23*C3;
FM12_1 = FB13_1*C3-FB23_1*S3;
FA11 = fSlnk11+m[1]*AlF11;
CF21 = CF22-trqlnk1_1_2+FA11*l[3][1]+FF12*Dz23;
FB11_1 = m[1]*l[3][1];
CM21_1 = In[5][1]+FB11_1*l[3][1]+FM12_1*Dz23;
// Reduction (Coordinate Partitioning)
MMB1_1 = Bvu1x1*CM24_4+Bvu2x1*CM25_4;
MMB2_1 = Bvu1x1*CM25_4+Bvu2x1*CM25_5;
Mr1_1 = CM21_1+Bvu1x1*MMB1_1+Bvu2x1*MMB2_1;
FMB1 = CF24-Qq[4]+CM24_4*b1x1+CM25_4*b1x2;
FMB2 = CF25-Qq[5]+CM25_4*b1x1+CM25_5*b1x2;
Fr1 = CF21-Qq[1]+Bvu1x1*FMB1+Bvu2x1*FMB2;
Fr2 = FA33-Qq[2];
Fr3 = -Qq[3];
// Linear System Resolution
qppux3 = -Fr3/In[9][3];
qppux2 = -Fr2/m[3];
qppux1 = -Fr1/Mr1_1;
// Symbolic Output number 1
qdd[1] = qppux1;
qdd[2] = qppux2;
qdd[3] = qppux3;
// Symbolic Output number 2
udd[1] = qppux1;
udd[2] = qppux2;
udd[3] = qppux3;
// Symbolic Output number 3
qdd[4] = b1x1+Bvu1x1*qdd[1];
qdd[5] = b1x2+Bvu2x1*qdd[1];
return iter;
// Number of continuation lines = 0
}
double S4;
double C4;
double S5;
double C5;
double S1;
double C1;
double S3;
double C3;
double Dz23;
double ROlp1_12;
double ROlp1_32;
double RLlp1_12;
double RLlp1_32;
double POlp1_12;
double RLlp1_13;
double RLlp1_33;
double POlp1_13;
double POlp1_33;
double JTlp1_13_1;
double JTlp1_33_1;
double RLlp2_12;
double RLlp2_32;
double h_1;
double h_3;
double NRh2_1;
double NRh2;
double Jvlu2_1;
double Jvlu2_2;
double Jv_hy2;
double Jv_hx2;
double Jv_hx1;
double Bvu2y1;
double Bvu2x1;
double Bvu1x1;
double ROjdqd1_12;
double ROjdqd1_32;
double RLjdqd1_12;
double RLjdqd1_32;
double OMjdqd1_22;
double ORjdqd1_12;
double ORjdqd1_32;
double Apqpjdqd1_12;
double Apqpjdqd1_32;
double RLjdqd1_13;
double RLjdqd1_33;
double ORjdqd1_13;
double ORjdqd1_33;
double Apqpjdqd1_13;
double Apqpjdqd1_33;
double RLjdqd2_12;
double RLjdqd2_32;
double ORjdqd2_12;
double ORjdqd2_32;
double Apqpjdqd2_12;
double Apqpjdqd2_32;
double jdqd1;
double jdqd3;
double b1y2;
double b1x2;
double b1x1;
double RLlnk2_12;
double RLlnk2_32;
double ORlnk2_12;
double ORlnk2_32;
double Plnk11;
double PPlnk1;
double Z1;
double e11;
double e31;
double Zd1;
double Flnk1;
double fSlnk11;
double trqlnk1_1_2;
double BS91;
double AlF11;
double AlF31;
double AlF12;
double AlF32;
double OM13;
double OM23;
double AlF13;
double AlF23;
double AlM13_1;
double AlM23_1;
double BS94;
double AlF14;
double AlF34;
double OM25;
double BS15;
double AlF15;
double AlF35;
double AlM15_4;
double AlM35_4;
double FA15;
double FA35;
double CF25;
double FB15_4;
double FB35_4;
double CM25_4;
double FB35_5;
double CM25_5;
double FA14;
double CF24;
double FB14_4;
double CM24_4;
double FA13;
double FA23;
double FA33;
double CF13;
double CF23;
double FB13_1;
double FB23_1;
double FF12;
double CF22;
double FM12_1;
double FA11;
double CF21;
double FB11_1;
double CM21_1;
double MMB1_1;
double MMB2_1;
double Mr1_1;
double FMB1;
double FMB2;
double Fr1;
double Fr2;
double Fr3;
double qppux3;
double qppux2;
double qppux1;
......@@ -63,23 +63,23 @@ function(increment_void_symbolic arg0)
set(VOID_SYMBOLIC ${VOID_SYMBOLIC} ${ROBOTRAN_SOURCE_DIR}/mbs_common/mbs_void_symbolicR/mbs_cons_hJ_void.c)
endif()
file(GLOB SYMBOLIC_FILES "${PROJECT_SOURCE_DIR}/../${arg0}/mbs_cons_jdqd*")
if (NOT SYMBOLIC_FILES)
message("Symbolic mbs_cons_jdqd not found, linking to void function")
set(VOID_SYMBOLIC ${VOID_SYMBOLIC} ${ROBOTRAN_SOURCE_DIR}/mbs_common/mbs_void_symbolicR/mbs_cons_jdqd_void.c)
endif()
file(GLOB SYMBOLIC_FILES "${PROJECT_SOURCE_DIR}/../${arg0}/mbs_dirdyna*")
if (NOT SYMBOLIC_FILES)
message("Symbolic mbs_dirdyna not found, linking to void function")
set(VOID_SYMBOLIC ${VOID_SYMBOLIC} ${ROBOTRAN_SOURCE_DIR}/mbs_common/mbs_void_symbolicR/mbs_dirdyna_void.c)
endif()
file(GLOB SYMBOLIC_FILES "${PROJECT_SOURCE_DIR}/../${arg0}/mbs_accelred*")
if (NOT SYMBOLIC_FILES)
message("Symbolic mbs_accelred not found, linking to void function")
set(VOID_SYMBOLIC ${VOID_SYMBOLIC} ${ROBOTRAN_SOURCE_DIR}/mbs_common/mbs_void_symbolicR/mbs_accelred_void.c)
endif()
file(GLOB SYMBOLIC_FILES "${PROJECT_SOURCE_DIR}/../${arg0}/mbs_extforces*")
if (NOT SYMBOLIC_FILES)
......@@ -111,7 +111,6 @@ function(increment_void_symbolic arg0)
set(VOID_SYMBOLIC ${VOID_SYMBOLIC} ${ROBOTRAN_SOURCE_DIR}/mbs_common/mbs_void_symbolicR/mbs_link3D_void.c)
endif()
file(GLOB SYMBOLIC_FILES "${PROJECT_SOURCE_DIR}/../${arg0}/mbs_sensor*")
if (NOT SYMBOLIC_FILES)
message("Symbolic mbs_sensor not found, linking to void function")
......
......@@ -407,14 +407,14 @@ MbsData* MDS_create_MBSdataStruct(MDS_gen_strct* mds_gen_strct)
// Other
// qddu
// udd
if (s->nqu)
{
s->qddu = (double*) calloc(s->nqu+1,sizeof(double));
s->qddu[0] = (double) s->nqu;
s->udd = (double*) calloc(s->nqu+1,sizeof(double));
s->udd[0] = (double) s->nqu;
}
else
s->qddu = NULL;
s->udd = NULL;
s->DonePart = 0;
......@@ -534,7 +534,7 @@ void mbs_delete_data(MbsData *s)
// Other
if (s->nqu)
{
free(s->qddu);
free(s->udd);
}
if (s->nhu) // to change
......
......@@ -28,9 +28,6 @@
int dirdynared(MbsAux *mbs_aux,MbsData *s);
#endif
/* accelred */
//int accelred(double* qddu, MbsData* s, double tsim);
/* invdynared */
#ifdef INVDYNARED
......
......@@ -54,7 +54,8 @@ MbsDirdyn* mbs_new_dirdyn_aux(MbsData* mbs_data, MbsAux* mbs_aux){
opts->buffersize = -1;
opts->resfilename = NULL;
opts->respath = NULL;
opts->realtime = 0;
opts->realtime = 0;
opts->accelred = 0;
dirdyn->options = opts;
// initial the saving counter
......@@ -258,8 +259,8 @@ void mbs_dirdyn_loop(MbsDirdyn* dd, MbsData* mbs_data){
// user loop
user_loop(mbs_data, dd->mbs_aux);
mbs_fct_dirdyn(dd->tsim, dd->y, dd->yd, mbs_data, dd->mbs_aux);
rk4(dd->y, dd->yd, dd->nState, dd->tsim, dd->dt, dd->yout, mbs_fct_dirdyn, mbs_data, dd->mbs_aux);
mbs_fct_dirdyn(dd->tsim, dd->y, dd->yd, mbs_data, dd->mbs_aux, dd->options);
rk4(dd->y, dd->yd, dd->nState, dd->tsim, dd->dt, dd->yout, mbs_fct_dirdyn, mbs_data, dd->mbs_aux, dd->options);
dd->tsim += dd->dt;
......@@ -354,12 +355,10 @@ void mbs_dirdyn_finish(MbsDirdyn* dd, MbsData* mbs_data){
/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
void mbs_fct_dirdyn(double tsim, double y[], double dydt[], MbsData *s, MbsAux *mbs_aux)
void mbs_fct_dirdyn(double tsim, double y[], double dydt[], MbsData *s, MbsAux *mbs_aux, MbsDirdynOptions *opts)
{
int i;
int accelred_on_off = 0.; // Should be improved!
s->tsim = tsim;
// Update state variables
......@@ -373,13 +372,13 @@ void mbs_fct_dirdyn(double tsim, double y[], double dydt[], MbsData *s, MbsAux *
s->ux[i] = y[i+2*s->nqu-1];
}
if (accelred_on_off) // Full symbolic computation of qdd (including solving constraint, inv(M), ...)
if (opts->accelred) // Full symbolic computation of qdd (including solving constraint, inv(M), ...)
{
// ! ! ! ! i = accelred(s->qddu, s, x);
i = mbs_accelred(s, tsim);
}
else // Numerical solving of constraints and inv(M)
{
i = dirdynared(mbs_aux,s);
i = dirdynared(mbs_aux, s);
}
if(i<0){
......@@ -392,8 +391,8 @@ void mbs_fct_dirdyn(double tsim, double y[], double dydt[], MbsData *s, MbsAux *
// Update state vector
for(i=1;i<=s->nqu;i++)
{
dydt[i-1] = s->qd[s->qu[i]];
dydt[i+s->nqu-1] = s->qddu[i];
dydt[i-1] = s->qd [s->qu[i]];
dydt[i+s->nqu-1] = s->qdd[s->qu[i]];
}
for(i=1;i<=s->Nux;i++)
{
......
......@@ -62,7 +62,8 @@ typedef struct MbsDirdynOptions
int buffersize;
int realtime; ///< 1 to activate to real-time features, 0 to deactivate them
int accelred; ///< 1 to use accelred, 0 otherwise
} MbsDirdynOptions;
......@@ -177,6 +178,6 @@ void mbs_delete_dirdyn(MbsDirdyn* dirdyn, MbsData* mbs_data);
* \p mbs_aux the local data structure
*/
void mbs_fct_dirdyn(double t, double y[], double dydt[], MbsData *s, MbsAux *mbs_aux);
void mbs_fct_dirdyn(double t, double y[], double dydt[], MbsData *s, MbsAux *mbs_aux, MbsDirdynOptions *opts);
#endif
......@@ -264,14 +264,14 @@ int dirdynared(MbsAux *mbs_aux,MbsData *s)