#include <stdio.h>
#include "mbs_part.h"
#include "mbs_dirdyn.h"
#include "mbs_buffer.h"
#include "mbs_data.h"
#include "mbs_aux.h"
Go to the source code of this file.
Classes | |
struct | MbsLmgcNode |
Structure defining the property of a node, i.e. More... | |
struct | MbsLmgcData |
Structure defining one multibody system and additional stuff for coupling it to LMGC90. More... | |
struct | MbsLmgcSystem |
Global data structure for a Robotran-LMGC90 project. More... | |
Functions | |
void | mbs_lmgc_set_nb_mbs (int nb) |
Set the number of mbs. More... | |
void | mbs_lmgc_set_mbsdata (int iMbs, MbsData *mbs_data) |
Set mbs_data (already loaded in memory) for the specified mbs. More... | |
void | mbs_lmgc_set_mbs_file (int iMbs, char *filename) |
Set the path to the mbs file for the specified mbs. More... | |
void | mbs_lmgc_set_res_file (int iMbs, char *filename) |
Set the path to the result files for the specified mbs. More... | |
void | mbs_lmgc_set_anim_file (int iMbs, char *filename) |
Set the path to the anim file for the specified mbs. More... | |
void | mbs_lmgc_set_node_sensor_ids (int iMbs, int nbNodes, int *sensorIds) |
Set the number of nodes for the specified mbs and set the index of sensors on which the nodes are attached. More... | |
int | mbs_lmgc_get_node_nb (int iMbs) |
Get the number of nodes attached to a mbs. More... | |
void | mbs_lmgc_initialize () |
Initialize the system. More... | |
void | mbs_lmgc_finalize () |
Save final configuration, free the memory and finalize the process. More... | |
void | mbs_lmgc_increment (double tsim) |
Increment time step. More... | |
void | mbs_lmgc_compute_free_vlocy (double h, double theta) |
Compute the free velocity of all mbs. More... | |
void | mbs_lmgc_update_nodes_3D (int i_mbs, double *coorTT, double *localFrameTT, int storage) |
Update the kinematics of 3D nodes belonging to the specified mbs. More... | |
void | mbs_lmgc_update_nodes_2D (int i_mbs, double *coorTT, int storage) |
Update the kinematics of 2D nodes belonging to the specified mbs. More... | |
void | mbs_lmgc_nullify_reac (int i_mbs) |
Reset the global reaction to zero for the specified mbs. More... | |
void | mbs_lmgc_nullify_rAux (int i_mbs) |
Reset the auxiliary reaction to zero for the specified mbs. More... | |
void | mbs_lmgc_add_reac (int i_mbs, int i_node, double *reacNode) |
Add the given node reaction contribution to the reaction of the global mbs. More... | |
void | mbs_lmgc_add_rAux (int i_mbs, int i_node, double *reacNode) |
Add the given node reaction to the reaction of the global mbs. More... | |
void | mbs_lmgc_nullify_vFree (int i_mbs) |
Reset the generalized velocity to zero for the specified mbs. More... | |
void | mbs_lmgc_nullify_vAux (int i_mbs) |
Reset the generalized velocity to zero for the specified mbs. More... | |
void | mbs_lmgc_comp_vAux_Iaux (int i_mbs) |
Compute the generalized velocity (vAux),. More... | |
void | mbs_lmgc_comp_vAux_Ireac (int i_mbs) |
Compute the generalized velocity (vAux) from value stored in Ireac,. More... | |
void | mbs_lmgc_get_vBeg (int i_mbs, int i_node, double *vlocyNode) |
Compute and set the velocity of a node at the beginning of the current time step. More... | |
void | mbs_lmgc_get_vFree (int i_mbs, int i_node, double *vlocyNode) |
Compute and set the free velocity of a node for the current time step. More... | |
void | mbs_lmgc_get_vAux (int i_mbs, int i_node, double *vlocyNode) |
Compute and set the auxiliary velocity of a node for the current time step. More... | |
void | mbs_lmgc_compute_dof (double h, double theta) |
Compute the new value of the generalized velocity and position for all mbs. More... | |
void | mbs_lmgc_update_dof () |
Finalize time step. More... | |
Variables | |
MbsLmgcSystem * | globalSystem |
Global pointer to the Robotran-LMGC90 system. More... | |
void mbs_lmgc_add_rAux | ( | int | i_mbs, |
int | i_node, | ||
double * | reacNode | ||
) |
Add the given node reaction to the reaction of the global mbs.
The given reaction is thus projected on the generalized coordinates space:
The array on which it is added corresponds to LMGC90 flag iRaux_ .
i_mbs | the index of the mbs for which the reaction must be applied |
i_node | the index of the node on which the reaction is applied |
reacNode | a 6 element (fx, fy, fz, tx, ty, tz) array containing the components of the force and torque to apply on the node. Components are given in the global frame. |
void mbs_lmgc_add_reac | ( | int | i_mbs, |
int | i_node, | ||
double * | reacNode | ||
) |
Add the given node reaction contribution to the reaction of the global mbs.
The given reaction is thus projected on the generalized coordinates space:
The array on which it is added corresponds to LMGC90 flag iReac_ .
i_mbs | the index of the mbs for which the reaction must be applied |
i_node | the index of the node on which the reaction is applied |
reacNode | a 6 element (fx, fy, fz, tx, ty, tz) array containing the components of the force and torque to apply on the node. Components are given in the global frame. |
void mbs_lmgc_comp_vAux_Iaux | ( | int | i_mbs | ) |
Compute the generalized velocity (vAux),.
It solve
The array for vAux and rAux corresponds to the LMGC90 flag iVaux_e_invM_t_Raux_
WARNING: this function assumes that the reduced mass matrix stored in the MbsAux structure (lds field of the MbsLmgcData of the specified mbs) has already been computed.
vAux will be computed simply by calling cholsl.
i_mbs | the index of the mbs for which the velocity must be reset |
void mbs_lmgc_comp_vAux_Ireac | ( | int | i_mbs | ) |
Compute the generalized velocity (vAux) from value stored in Ireac,.
It solve
The array for vAux and reac corresponds to the LMGC90 flag iVaux_e_invM_t_Ireac
WARNING: this function assumes that the reduced mass matrix stored in the MbsAux structure (lds field of the MbsLmgcData of the specified mbs) has already been computed.
vAux will be computed simply by calling cholsl.
i_mbs | the index of the mbs for which the velocity must be reset |
void mbs_lmgc_compute_dof | ( | double | h, |
double | theta | ||
) |
Compute the new value of the generalized velocity and position for all mbs.
The following operation are done:
Precondition:
WARNING:
h | the size of the time step |
theta | the value of the theta parameter in the theta scheme |
void mbs_lmgc_compute_free_vlocy | ( | double | h, |
double | theta | ||
) |
Compute the free velocity of all mbs.
After this function:
[in] | h | the size of the time step |
[in] | theta | the value of the theta parameter in the theta scheme |
void mbs_lmgc_finalize | ( | ) |
Save final configuration, free the memory and finalize the process.
If the MbsData structure was provided by function mbs_lmgc_set_mbsdata() , the structure is not freed.
int mbs_lmgc_get_node_nb | ( | int | iMbs | ) |
Get the number of nodes attached to a mbs.
[in] | iMbs | the ID of the Multibody system associated to the mbs_data. WARNING: ID's start from 0 ! |
void mbs_lmgc_get_vAux | ( | int | i_mbs, |
int | i_node, | ||
double * | vlocyNode | ||
) |
Compute and set the auxiliary velocity of a node for the current time step.
Compute the velocity of a node (linear and angular velocity) from a given value of the generalized velocities.
This is computed by
The array from which the velocity corresponds to the LMGC90 flag iVaux_.
The corresponding array is vAux stored in MbsLmgcData.
i_mbs | the index of the mbs from which the velocity is taken |
i_node | the index of the node for which the velocity is computed |
vlocyNode | a 6 element (vx, vy, vz, omx, omy, omz) array in which the components of the velocity will be stored. Components are computed and expressed in the global frame. |
void mbs_lmgc_get_vBeg | ( | int | i_mbs, |
int | i_node, | ||
double * | vlocyNode | ||
) |
Compute and set the velocity of a node at the beginning of the current time step.
Compute the velocity of a node (linear and angular velocity) from a given value of the generalized velocities.
This is computed by
The array from which the velocity corresponds to the LMGC90 flag iVbeg_
The corresponding array is vBeg stored in MbsLmgcData.
i_mbs | the index of the mbs from which the velocity is taken |
i_node | the index of the node for which the velocity is computed |
vlocyNode | a 6 element (vx, vy, vz, omx, omy, omz) array in which the components of the velocity will be stored. Components are computed and expressed in the global frame. |
void mbs_lmgc_get_vFree | ( | int | i_mbs, |
int | i_node, | ||
double * | vlocyNode | ||
) |
Compute and set the free velocity of a node for the current time step.
Compute the velocity of a node (linear and angular velocity) from a given value of the generalized velocities.
This is computed by
The array from which the velocity corresponds to the LMGC90 flag iVfree.
The corresponding array is vFree stored in MbsLmgcData.
i_mbs | the index of the mbs from which the velocity is taken |
i_node | the index of the node for which the velocity is computed |
vlocyNode | a 6 element (vx, vy, vz, omx, omy, omz) array in which the components of the velocity will be stored. Components are computed and expressed in the global frame. |
void mbs_lmgc_increment | ( | double | tsim | ) |
Increment time step.
Set the time at the beginning of the next iteration in the time simulation loop and save the system configuration.
tsim | the new value of time at the begining of the time step |
void mbs_lmgc_initialize | ( | ) |
Initialize the system.
void mbs_lmgc_nullify_rAux | ( | int | i_mbs | ) |
Reset the auxiliary reaction to zero for the specified mbs.
The vector to be reset corresponds to flag iRaux_ of LMGC90.
The corresponding array is rAux stored in MbsLmgcData.
i_mbs | the index of the mbs for which the reaction must be reset |
void mbs_lmgc_nullify_reac | ( | int | i_mbs | ) |
Reset the global reaction to zero for the specified mbs.
The vector to be reset corresponds to flag iReac_ of LMGC90.
The corresponding array is reac stored in MbsLmgcData.
i_mbs | the index of the mbs for which the reaction must be reset |
void mbs_lmgc_nullify_vAux | ( | int | i_mbs | ) |
Reset the generalized velocity to zero for the specified mbs.
The array to be reset corresponds to the LMGC90 flag iVaux_. The corresponding array is vAux stored in MbsLmgcData.
i_mbs | the index of the mbs for which the velocity must be reset |
void mbs_lmgc_nullify_vFree | ( | int | i_mbs | ) |
Reset the generalized velocity to zero for the specified mbs.
The array to be reset corresponds to the LMGC90 flag iVfree. The corresponding array is vFree stored in MbsLmgcData.
i_mbs | the index of the mbs for which the velocity must be reset |
void mbs_lmgc_set_anim_file | ( | int | iMbs, |
char * | filename | ||
) |
Set the path to the anim file for the specified mbs.
[in] | iMbs | the ID of the Multibody system. WARNING: ID's start from 0 ! |
[in] | filename | the path to the folder where results have to be saved including the prefix of the results files |
void mbs_lmgc_set_mbs_file | ( | int | iMbs, |
char * | filename | ||
) |
Set the path to the mbs file for the specified mbs.
[in] | iMbs | the ID of the Multibody system. WARNING: ID's start from 0 ! |
[in] | filename | the path to the mbs file including the mbs file name and extension (.mbs) |
void mbs_lmgc_set_mbsdata | ( | int | iMbs, |
MbsData * | mbs_data | ||
) |
Set mbs_data (already loaded in memory) for the specified mbs.
[in] | iMbs | the ID of the Multibody system associated to the mbs_data. WARNING: ID's start from 0 ! |
[in] | mbs_data | the pointer to the multibody system data structure to use |
void mbs_lmgc_set_nb_mbs | ( | int | nb | ) |
Set the number of mbs.
This operation can be run only once in the process
[in] | nb | the number of multibody system in the simulation |
void mbs_lmgc_set_node_sensor_ids | ( | int | iMbs, |
int | nbNodes, | ||
int * | sensorIds | ||
) |
Set the number of nodes for the specified mbs and set the index of sensors on which the nodes are attached.
the size of sensorIds must be equal to nbNodes.
[in] | iMbs | the ID of the Multibody system. WARNING: ID's start from 0 ! |
[in] | nbNodes | the number of nodes attached to the multibody system. |
[in] | sensorIds | pointer to the lists containing the sensors id's. |
void mbs_lmgc_set_res_file | ( | int | iMbs, |
char * | filename | ||
) |
Set the path to the result files for the specified mbs.
It specify the path + the name without extension. A suffix _q.res will be added for the position result file A suffix _qd.res will be added for the velocity result file
[in] | iMbs | the ID of the Multibody system. WARNING: ID's start from 0 ! |
[in] | filename | the path to the folder where results have to be saved including the prefix of the results files |
void mbs_lmgc_update_dof | ( | ) |
Finalize time step.
Update the vBeg value for next time step.
void mbs_lmgc_update_nodes_2D | ( | int | i_mbs, |
double * | coorTT, | ||
int | storage | ||
) |
Update the kinematics of 2D nodes belonging to the specified mbs.
For each sensor attached to a nodes of the mbs it:
The only axis considered in translation are X and Y; the only rotation is around Z axis.
Current value of q, Jv and are used (i.e. value stored in MbsData and MbsAux)
i_mbs | the index of the mbs for which the nodes must be updated |
coorTT | the array in which the position of nodes must be copied: being the coordinate of all nodes of the mbs |
storage | Flag to store the reduced Jacobian: 0: JRedBeg, computed in step initial configuration; 1: JRedQm, computed in step mid-configuration. |
void mbs_lmgc_update_nodes_3D | ( | int | i_mbs, |
double * | coorTT, | ||
double * | localFrameTT, | ||
int | storage | ||
) |
Update the kinematics of 3D nodes belonging to the specified mbs.
For each sensor attached to a nodes of the mbs it:
Current value of q, Jv and are used (i.e. value stored in MbsData and MbsAux)
i_mbs | the index of the mbs for which the nodes must be updated |
coorTT | the array in which the position of nodes must be copied: being the coordinate of all nodes of the mbs |
localFrameTT | the array in which the rotation matrix of nodes must be copied |
storage | Flag to store the reduced Jacobian: 0: JRedBeg, computed in step initial configuration; 1: JRedQm, computed in step mid-configuration. |
MbsLmgcSystem* globalSystem |
Global pointer to the Robotran-LMGC90 system.