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

MBprojects/PendulumSpringC/symbolicR/mbs_accelred_PendulumSpringC.c

+//
+//	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
+
+}

MBprojects/PendulumSpringC/symbolicR/mbs_accelred_PendulumSpringC.h

+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;

MBsysC/cmake_aux/listing/CMakeLists.txt

@@ -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")

MBsysC/mbs_common/mbs_load_xml/mbs_load_xml.c

@@ -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 

MBsysC/mbs_common/mbs_module/MBSfun.h

@@ -28,9 +28,6 @@
 int dirdynared(MbsAux *mbs_aux,MbsData *s);
 #endif
 
-/* accelred */
-//int accelred(double* qddu, MbsData* s, double tsim);
-
 /* invdynared */
 
 #ifdef INVDYNARED

MBsysC/mbs_common/mbs_module/mbs_dirdyn.c

@@ -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++)
     {

MBsysC/mbs_common/mbs_module/mbs_dirdyn.h

@@ -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

MBsysC/mbs_common/mbs_module/mbs_dirdynared.c

@@ -264,14 +264,14 @@ int dirdynared(MbsAux *mbs_aux,MbsData *s)