Commit 472f93e7 authored by Nicolas Van der Noot's avatar Nicolas Van der Noot
Browse files

merge accelred, user_hardcoded, visu, mbs_dirdyn user

parents d2393f8f 07d900a4
//
// 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;
//---------------------------
// UCL-CEREM-MBS
//
// @version MBsysLab_s 1.7.a
//
// Creation : 2015
// Last update : 08/05/2015
//---------------------------
#include "math.h"
#include "MBSdef.h"
#include "mbs_data.h"
#include "mbs_dirdyn.h"
/*! \brief user own initialization functions
*
* \param[in,out] mbs_data data structure of the model
* \param[in,out] mbs_dd general structure of the direct dynamic module (for advance users)
*
* For beginners, it is advised to only use the MbsData structure.
* The field MbsDirdyn is provided for more advance users.
*/
void user_init(MbsData *mbs_data, MbsDirdyn *mbs_dd)
{
}
/*! \brief user own loop functions
*
* \param[in,out] mbs_data data structure of the model
* \param[in,out] mbs_dd general structure of the direct dynamic module (for advance users)
*
* For beginners, it is advised to only use the MbsData structure.
* The field MbsDirdyn is provided for more advance users.
*/
void user_loop(MbsData *mbs_data, MbsDirdyn *mbs_dd)
{
}
/*! \brief user own finishing functions
*
* \param[in,out] mbs_data data structure of the model
* \param[in,out] mbs_dd general structure of the direct dynamic module (for advance users)
*
* For beginners, it is advised to only use the MbsData structure.
* The field MbsDirdyn is provided for more advance users.
*/
void user_finish(MbsData *mbs_data, MbsDirdyn *mbs_dd)
{
}
//---------------------------
// UCL-CEREM-MBS
//
// @version MBsysLab_s 1.7.a
//
// Creation : 2015
// Last update : 08/05/2015
//---------------------------
#include "math.h"
#include "MBSdef.h"
#include "mbs_data.h"
#include "mbs_aux.h"
void user_finish(MbsData *mbs_data, MbsAux *mbs_aux)
{
}
/* --------------------------------------------------------
* This code was generated automatically by MBsysC modules.
* MBsysC modules are distributed as part of the ROBOTRAN
* software. They provides functionalities for dealing with
* symbolic equations generated by ROBOTRAN.
*
* More info on www.robotran.be
*
* Universite catholique de Louvain, Belgium
*
* Last update : Wed Sep 9 13:31:19 2015
* --------------------------------------------------------
*
*/
#ifndef USER_HARD_PARAM_h
#define USER_HARD_PARAM_h
// ============================================================ //
// gravity
#define G_3 9.81
// mass
#define M_1 5
#define M_3 2
#define M_4 2
#define M_5 2
// center of mass
#define L_3_1 0.4
#define L_3_4 0.075
#define L_1_5 0.29
// inertia
#define IN_5_1 0.1
#define IN_9_3 1
#define IN_5_4 0.1
#define IN_5_5 0.1
// body point
#define DPT_1_1 0.25
#define DPT_1_2 -0.5
#define DPT_3_3 0.5
#define DPT_3_4 0.25
#define DPT_3_5 1.5
#define DPT_3_6 0.15
#define DPT_1_7 0.58
#define DPT_1_8 0.29
// ============================================================ //
# endif
//---------------------------
// UCL-CEREM-MBS
//
// @version MBsysLab_s 1.7.a
//
// Creation : 2015
// Last update : 08/05/2015
//---------------------------
#include "math.h"
#include "MBSdef.h"
#include "mbs_data.h"
#include "mbs_aux.h"
void user_init(MbsData *mbs_data, MbsAux *mbs_aux)
{
}
//---------------------------
// UCL-CEREM-MBS
//
// @version MBsysLab_s 1.7.a
//
// Creation : 2015
// Last update : 08/05/2015
//---------------------------
#include "math.h"
#include "MBSdef.h"