Robotran C Documentation
mbs_project_interface.h
Go to the documentation of this file.
1 
9 #include "mbs_data.h"
10 #include "mbs_aux.h"
11 #include "mbs_dirdyn_struct.h"
12 #include "mbs_equil_struct.h"
13 
17 void mbs_get_project_functions(MbsData *mbs_data);
18 
19 /* * * * * * * * * * * * * * * * * * * * * * * * * * * * */
20 /* User functions */
21 /* * * * * * * * * * * * * * * * * * * * * * * * * * * * */
22 void user_load_post(MbsData *MBSdata);
23 void user_free(MbsData *MBSdata);
24 
25 void user_dirdyn_init(MbsData *MBSdata, MbsDirdyn *mbs_dd);
26 void user_dirdyn_loop(MbsData *MBSdata, MbsDirdyn *mbs_dd);
27 void user_dirdyn_finish(MbsData *MBSdata, MbsDirdyn *mbs_dd);
28 
29 void user_invdyn_init(MbsData *MBSdata, MbsInvdyn *mbs_invd);
30 void user_invdyn_loop(MbsData *MBSdata, MbsInvdyn *mbs_invd);
31 void user_invdyn_finish(MbsData *MBSdata, MbsInvdyn *mbs_invd);
32 
33 void user_equil_init(MbsData *MBSdata, MbsEquil *mbs_equil);
34 void user_equil_loop(MbsData *MBSdata, MbsEquil *mbs_equil);
35 void user_equil_finish(MbsData *MBSdata, MbsEquil *mbs_equil);
36 void user_equil_fxe(MbsData *mbs_data, double* f);
37 
38 void user_cons_hJ(double *h, double **Jac, MbsData *s, double tsim);
39 
40 void user_cons_jdqd(double *jdqd, MbsData *s, double tsim);
41 
42 void user_Derivative(MbsData *s);
43 
51 void user_DrivenJoints(MbsData *s, double tsim);
52 
71 double* user_ExtForces(double PxF[4], double RxF[4][4],
72  double VxF[4], double OMxF[4],
73  double AxF[4], double OMPxF[4],
74  MbsData *s, double tsim, int ixF);
75 
76 double* user_GenExtForces(double PxF[4], double RxF[4][4],
77  double VxF[4], double OMxF[4],
78  double AxF[4], double OMPxF[4],
79  MbsData *s, double tsim,int iBody);
80 
88 double* user_JointForces(MbsData *s, double tsim);
89 
100 double user_LinkForces(double Z, double Zd, MbsData *s, double tsim, int ilnk);
101 
139 double* user_Link3DForces(double PxF[4], double RxF[4][4],
140  double VxF[4], double OMxF[4],
141  double AxF[4], double OMPxF[4],
142  MbsData *s, double tsim,int ixF);
143 
157 int user_cons_J_accelred(MbsData* s, double tsim);
158 
159 /* * * * * * * * * * * * * * * * * * * * * * * * * * * * */
160 /* Symbolic functions */
161 /* * * * * * * * * * * * * * * * * * * * * * * * * * * * */
162 
163 void mbs_cons_hJ(double *h,double **Jac,MbsData *s, double tsim);
164 
165 void mbs_cons_jdqd(double *Jdqd,MbsData *s, double tsim);
166 
167 void mbs_link(double **frc,double **trq,double *Flnk,double *Z,double *Zd,MbsData *s, double tsim);
168 
169 void mbs_link3D(double **frc,double **trq, MbsData *s, double tsim);
170 
171 void mbs_extforces(double **frc, double **trq, MbsData *s, double tsim);
172 
173 int mbs_accelred(MbsData *s, double tsim);
174 
175 void mbs_gensensor(MbsSensor *sens, MbsData *s, int isens);
176 
184 void mbs_sensor(MbsSensor *sens, MbsData *s, int isens);
185 
186 void mbs_invdyna(double *Qq, MbsData *s, double tsim);
187 
188 void mbs_dirdyna(double **M,double *c, MbsData *s, double tsim);
user_ExtForces
double * user_ExtForces(double PxF[4], double RxF[4][4], double VxF[4], double OMxF[4], double AxF[4], double OMPxF[4], MbsData *s, double tsim, int ixF)
Compute an user-specified external force.
Definition: user_ExtForces.c:15
mbs_get_project_functions
void mbs_get_project_functions(MbsData *mbs_data)
loads the project function into the pointers defined in MbsData::fct
user_Derivative
void user_Derivative(MbsData *s)
Definition: user_Derivative.c:15
MbsInvdyn
General structure of the inverse dynamics module.
Definition: mbs_invdyn_struct.h:27
user_cons_jdqd
void user_cons_jdqd(double *jdqd, MbsData *s, double tsim)
Definition: user_cons_jdqd.c:15
mbs_link
void mbs_link(double **frc, double **trq, double *Flnk, double *Z, double *Zd, MbsData *s, double tsim)
Definition: mbs_project_fct_ptr.c:24
mbs_cons_jdqd
void mbs_cons_jdqd(double *Jdqd, MbsData *s, double tsim)
Definition: mbs_project_fct_ptr.c:52
mbs_data.h
mbs_dirdyn_struct.h
mbs_link3D
void mbs_link3D(double **frc, double **trq, MbsData *s, double tsim)
Definition: mbs_project_fct_ptr.c:28
mbs_dirdyna
void mbs_dirdyna(double **M, double *c, MbsData *s, double tsim)
Definition: mbs_project_fct_ptr.c:40
user_DrivenJoints
void user_DrivenJoints(MbsData *s, double tsim)
Compute the positions, velocities and acceleration of driven joint.
Definition: user_DrivenJoints.c:15
user_LinkForces
double user_LinkForces(double Z, double Zd, MbsData *s, double tsim, int ilnk)
Compute the value of a link forces.
Definition: user_LinkForces.c:15
user_JointForces
double * user_JointForces(MbsData *s, double tsim)
Compute the user-specified joint force in all joint.
Definition: user_JointForces.c:17
user_equil_loop
void user_equil_loop(MbsData *MBSdata, MbsEquil *mbs_equil)
user own loop functions
Definition: user_equil.c:40
mbs_gensensor
void mbs_gensensor(MbsSensor *sens, MbsData *s, int isens)
Definition: mbs_project_fct_ptr.c:60
user_invdyn_loop
void user_invdyn_loop(MbsData *MBSdata, MbsInvdyn *mbs_invd)
Definition: user_invdyn.c:21
user_dirdyn_finish
void user_dirdyn_finish(MbsData *MBSdata, MbsDirdyn *mbs_dd)
user own finishing functions
Definition: user_dirdyn.c:59
user_equil_fxe
void user_equil_fxe(MbsData *mbs_data, double *f)
user own implementation of added equilibrium equations Fxe Necessary to express equilibrium f(x)=0
Definition: user_equil.c:65
mbs_cons_hJ
void mbs_cons_hJ(double *h, double **Jac, MbsData *s, double tsim)
Definition: mbs_project_fct_ptr.c:48
user_invdyn_finish
void user_invdyn_finish(MbsData *MBSdata, MbsInvdyn *mbs_invd)
Definition: user_invdyn.c:27
MbsEquil
Definition: mbs_equil_struct.h:68
mbs_accelred
int mbs_accelred(MbsData *s, double tsim)
Definition: mbs_project_fct_ptr.c:36
mbs_invdyna
void mbs_invdyna(double *Qq, MbsData *s, double tsim)
Definition: mbs_project_fct_ptr.c:44
mbs_equil_struct.h
Simu_realtime::tsim
double tsim
current simulation time [s]
Definition: realtime.h:114
mbs_sensor
void mbs_sensor(MbsSensor *sens, MbsData *s, int isens)
Compute all fields of a sensor (force sensor or classic sensor).
Definition: mbs_project_fct_ptr.c:56
user_invdyn_init
void user_invdyn_init(MbsData *MBSdata, MbsInvdyn *mbs_invd)
Definition: user_invdyn.c:16
MbsData
Definition: mbs_data.h:149
user_cons_hJ
void user_cons_hJ(double *h, double **Jac, MbsData *s, double tsim)
Definition: user_cons_hJ.c:15
user_load_post
void user_load_post(MbsData *MBSdata)
User own initialization operations.
Definition: user_load.c:24
MbsSensor
Definition: mbs_sensor_struct.h:18
user_GenExtForces
double * user_GenExtForces(double PxF[4], double RxF[4][4], double VxF[4], double OMxF[4], double AxF[4], double OMPxF[4], MbsData *s, double tsim, int iBody)
mbs_extforces
void mbs_extforces(double **frc, double **trq, MbsData *s, double tsim)
Definition: mbs_project_fct_ptr.c:32
user_free
void user_free(MbsData *MBSdata)
User own freeing operations.
Definition: user_load.c:43
user_Link3DForces
double * user_Link3DForces(double PxF[4], double RxF[4][4], double VxF[4], double OMxF[4], double AxF[4], double OMPxF[4], MbsData *s, double tsim, int ixF)
Compute an user-specified point to point force, with arbitrary line of action.
Definition: user_Link3DForces.c:15
user_cons_J_accelred
int user_cons_J_accelred(MbsData *s, double tsim)
[Accelred only] Compute the Jac_user matrix nqv x Njoint matrix (starting at one) for custom user con...
Definition: user_cons_J_accelred.c:16
MbsDirdyn
General structure of the direct dynamic module.
Definition: mbs_dirdyn_struct.h:144
user_dirdyn_init
void user_dirdyn_init(MbsData *MBSdata, MbsDirdyn *mbs_dd)
user own initialization functions
Definition: user_dirdyn.c:28
user_equil_finish
void user_equil_finish(MbsData *MBSdata, MbsEquil *mbs_equil)
user own finishing functions
Definition: user_equil.c:53
mbs_aux.h
user_dirdyn_loop
void user_dirdyn_loop(MbsData *MBSdata, MbsDirdyn *mbs_dd)
user own loop functions
Definition: user_dirdyn.c:46
user_equil_init
void user_equil_init(MbsData *MBSdata, MbsEquil *mbs_equil)
user own initialization functions
Definition: user_equil.c:27