Skip to content

Commit

Permalink
Merge remote-tracking branch 'pyomeca/master' into paper
Browse files Browse the repository at this point in the history
  • Loading branch information
@pariterre
pariterre committed Jun 16, 2020
2 parents 30b2738 + 5cb8205 commit 22edc06
Show file tree
Hide file tree
Showing 91 changed files with 1,930 additions and 838 deletions.
4 changes: 2 additions & 2 deletions CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3,7 +3,7 @@ if (${CMAKE_VERSION} VERSION_GREATER_EQUAL "3.0")
cmake_policy(SET CMP0042 NEW)
endif()

project(biorbd VERSION 1.3.0)
project(biorbd VERSION 1.3.1)
set (CMAKE_CXX_STANDARD 11)
set (BIORBD_NAME ${PROJECT_NAME})

Expand Down Expand Up @@ -236,7 +236,7 @@ add_subdirectory("binding")
# Add the example if asked
option(BUILD_EXAMPLE "Build a C++ example" ON)
if (BUILD_EXAMPLE)
add_subdirectory("example")
add_subdirectory("examples")
endif()

# Doc
Expand Down
4 changes: 2 additions & 2 deletions binding/biorbd_muscles.i.in
View file
Original file line number Diff line number Diff line change
Expand Up @@ -27,8 +27,8 @@ namespace std {
%template(VecBiorbdMuscleGroup) std::vector<biorbd::muscles::MuscleGroup>;
%template(SharedBiorbdMuscle) std::shared_ptr<biorbd::muscles::Muscle>;
%template(VecSharedBiorbdMuscle) std::vector<std::shared_ptr<biorbd::muscles::Muscle>>;
%template(VecBiorbdMuscleStateDynamics) std::vector<biorbd::muscles::StateDynamics>;
%template(MatBiorbdMuscleStateDynamics) std::vector<std::vector<biorbd::muscles::StateDynamics>>;
%template(VecBiorbdMuscleState) std::vector<biorbd::muscles::State>;
%template(MatBiorbdMuscleState) std::vector<std::vector<biorbd::muscles::State>>;
%template(SharedBiorbdMuscleFatigueState) std::shared_ptr<biorbd::muscles::FatigueState>;
}

Expand Down
4 changes: 4 additions & 0 deletions binding/biorbd_rigidbody.i.in
View file
Original file line number Diff line number Diff line change
Expand Up @@ -29,6 +29,10 @@ namespace std {
%template(MatBiorbdGeneralizedTorque) std::vector<std::vector<biorbd::rigidbody::GeneralizedTorque>>;
%template(VecBiorbdGeneralizedCoordinates) std::vector<biorbd::rigidbody::GeneralizedCoordinates>;
%template(MatBiorbdGeneralizedCoordinates) std::vector<std::vector<biorbd::rigidbody::GeneralizedCoordinates>>;
%template(VecBiorbdGeneralizedVelocity) std::vector<biorbd::rigidbody::GeneralizedVelocity>;
%template(MatBiorbdGeneralizedVelocity) std::vector<std::vector<biorbd::rigidbody::GeneralizedVelocity>>;
%template(VecBiorbdGeneralizedAcceleration) std::vector<biorbd::rigidbody::GeneralizedAcceleration>;
%template(MatBiorbdGeneralizedAcceleration) std::vector<std::vector<biorbd::rigidbody::GeneralizedAcceleration>>;

%template(VecBiorbdNodeSegment) std::vector<biorbd::rigidbody::NodeSegment>;
%template(MatBiorbdNodeSegment) std::vector<std::vector<biorbd::rigidbody::NodeSegment>>;
Expand Down
2 changes: 1 addition & 1 deletion binding/c/biorbd_c.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -203,7 +203,7 @@ biorbd::rigidbody::KalmanReconsIMU* c_BiorbdKalmanReconsIMU(
{
// Créer un pointeur sur un filtre de kalman
biorbd::rigidbody::KalmanReconsIMU* kalman = new biorbd::rigidbody::KalmanReconsIMU(
*model, biorbd::rigidbody::KalmanReconsIMU::KalmanParam(freq, noiseF, errorF));
*model, biorbd::rigidbody::KalmanParam(freq, noiseF, errorF));

// Mettre le initial guess
biorbd::rigidbody::GeneralizedCoordinates e_QinitialGuess(*model);
Expand Down
7 changes: 5 additions & 2 deletions binding/matlab/Matlab_MusclesActivationDot.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -12,12 +12,15 @@ void Matlab_MusclesActivationDot( int, mxArray *plhs[],
int nrhs, const mxArray*prhs[] ){

// Verifier les arguments d'entrée
checkNombreInputParametres(nrhs, 5, 5, "4 arguments are required [+1 optional] where the 2nd is the handler on the model, 3rd is the excitation, 4th is the activation and 5th is a bool to express if excitation is already normalized");
checkNombreInputParametres(nrhs, 5, 5, "4 arguments are required [+1 optional] where the 2nd is the handler on the model, "
"3rd is the excitation, 4th is the activation and 5th is a bool to express if "
"excitation is already normalized");
// Recevoir le model
biorbd::Model * model = convertMat2Ptr<biorbd::Model>(prhs[1]);

// Recevoir les états musculaires
std::vector<std::vector<std::shared_ptr<biorbd::muscles::StateDynamics>>> state = getParameterMuscleState(prhs, 2, 3, model->nbMuscleTotal());
std::vector<std::vector<std::shared_ptr<biorbd::muscles::State>>> state
= getParameterMuscleState(prhs, 2, 3, model->nbMuscleTotal());

// Already normalized
bool areadyNormalized(false);
Expand Down
78 changes: 46 additions & 32 deletions binding/matlab/Matlab_MusclesForce.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -8,52 +8,66 @@

void Matlab_MusclesForce( int, mxArray *plhs[],
int nrhs, const mxArray*prhs[] ){

// Verifier les arguments d'entrée
checkNombreInputParametres(nrhs, 5, 6, "5 arguments are required [+1 optional] where the 2nd is the handler on the model, 3rd is the Q, 4th is Qdot, 5th is muscles states and optional 6th is if update [true] must be done. Note that if update is set to [false], the user MUST update it by himself before calling this function");
checkNombreInputParametres(nrhs, 3, 5, "3 arguments are required (+2 optional) where the 2nd is the handler on the model, "
"3rd is the muscles states and optional 4th and 5th are the Q and QDot, respectively."
"WARNING: if the function is called without Q and Qdot, the user MUST update by himself "
"before calling this function (using updateMuscle).");
// Recevoir le model
biorbd::Model * model = convertMat2Ptr<biorbd::Model>(prhs[1]);
unsigned int nQ = model->nbQ(); // Get the number of DoF
unsigned int nQdot = model->nbQdot(); // Get the number of DoF
unsigned int nMus = model->nbMuscleTotal();

// Recevoir Q
std::vector<biorbd::rigidbody::GeneralizedCoordinates> Q = getParameterQ(prhs, 2, nQ);
unsigned int nTau = model->nbGeneralizedTorque(); // Get the number of DoF
unsigned int nMuscleTotal = model->nbMuscles();

// Recevoir Q
std::vector<biorbd::rigidbody::GeneralizedVelocity> Qdot = getParameterQdot(prhs, 3, nQdot);
// Recevoir muscleStates
std::vector<std::vector<std::shared_ptr<biorbd::muscles::State>>> s = getParameterMuscleStateActivation(prhs,2,nMuscleTotal);
unsigned int nFrame(static_cast<unsigned int>(s.size()));

// S'assurer que Q, Qdot et Qddot (et Forces s'il y a lieu) sont de la bonne dimension
unsigned int nFrame(static_cast<unsigned int>(Q.size()));
if (Qdot.size() != nFrame)
mexErrMsgIdAndTxt( "MATLAB:dim:WrongDimension", "QDot must have the same number of frames than Q");
bool updateKin(false); // Par défaut c'est false, mais ce comportement est celui le moins attendu
if (nrhs >= 4){ // Si on doit récupérer la cinématique
if (nrhs != 5)
mexErrMsgIdAndTxt("MATLAB:dim:WrongArguments", "Q and Qdot must be sent to the function");
updateKin = true;
} else { // Si on n'update pas, s'assurer qu'un seul frame est envoyé
if (nFrame > 1)
mexErrMsgIdAndTxt("MATLAB:dim:WrongDimension", "Update == false is incompatible with more than one frame.");
}

// Recevoir les états musculaires
std::vector<std::vector<std::shared_ptr<biorbd::muscles::StateDynamics>>> state = getParameterMuscleStateActivation(prhs, 4, model->nbMuscleTotal());
// Recueillir la cinématique
std::vector<biorbd::rigidbody::GeneralizedCoordinates> Q;
std::vector<biorbd::rigidbody::GeneralizedVelocity> QDot;
if (updateKin){ // Si on update pas la cinématique Q et Qdot ne sont pas nécessaire
// Recevoir Q
Q = getParameterQ(prhs, 3, nQ);
// Recevoir Qdot
QDot = getParameterQdot(prhs, 4, nQdot);

bool updateKin(true);
if (nrhs >= 6){ // Si on doit récupérer si on update ou pas
updateKin = getBool(prhs, 5);
if (!updateKin && nFrame > 1) // Si on n'update pas, s'assurer qu'un seul frame est envoyé
mexErrMsgIdAndTxt("MATLAB:dim:WrongDimension", "No update is incompatible with more than one frame.");
// S'assurer que Q, Qdot et activations sont de la bonne dimension
if (Q.size() != nFrame)
mexErrMsgIdAndTxt( "MATLAB:dim:WrongDimension", "Q must have the same number of frames than muscles states");
if (QDot.size() != nFrame)
mexErrMsgIdAndTxt( "MATLAB:dim:WrongDimension", "QDot must have the same number of frames than muscles states");
}

// Cellules de sortie
mwSize dims[2];
dims[0] = nMus;
dims[1] = nFrame;
plhs[0] = mxCreateNumericArray(2, dims, mxDOUBLE_CLASS, mxREAL);
double *muscleForce = mxGetPr(plhs[0]);
// Create a matrix for the return argument
mwSize dims2[2];
dims2[0] = nMuscleTotal;
dims2[1] = nFrame;
plhs[0] = mxCreateNumericArray(2, dims2, mxDOUBLE_CLASS, mxREAL);
double * Mus = mxGetPr(plhs[0]);

// Aller chercher les valeurs
for (unsigned int iF=0; iF<nFrame; ++iF){
biorbd::utils::Vector Force;
// Remplir le output
biorbd::rigidbody::GeneralizedTorque muscleTorque;
biorbd::utils::Vector muscleForces;
for (unsigned int i=0; i<nFrame; ++i){
if (updateKin)
Force = model->musclesForces(state[iF], updateKin, &Q[iF], &Qdot[iF]);
muscleForces = model->muscleForces(s[i], Q[i], QDot[i]);
else
Force = model->musclesForces(state[iF], updateKin);
for (unsigned int i=0; i<nMus; ++i){
muscleForce[nFrame*nMus + i] = Force[i];
muscleForces = model->muscleForces(s[i]);

for (unsigned int j=0; j<nMuscleTotal; ++j){
Mus[i*nMuscleTotal+j] = muscleForces(j);
}
}

Expand Down
15 changes: 10 additions & 5 deletions binding/matlab/Matlab_inverseKinematicsEKF.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -29,7 +29,7 @@ void Matlab_setEKF(int nlhs, mxArray *plhs[],
noiseF = getDouble(prhs,3,"Noise Factor");
if (nrhs >= 3)
freq = getDouble(prhs,2,"Acquisition Frequency");
biorbd::rigidbody::KalmanRecons::KalmanParam kParams(freq, noiseF, errorF);
biorbd::rigidbody::KalmanParam kParams(freq, noiseF, errorF);

// Créer un filtre de Kalman
try{
Expand Down Expand Up @@ -121,7 +121,12 @@ void Matlab_inverseKinematicsEKFallInOneCall( int, mxArray *plhs[],
int nrhs, const mxArray*prhs[] ){

// Verifier les arguments d'entrée
checkNombreInputParametres(nrhs, 3, 8, "4 arguments are required (+5 optional) where the 2nd is the handler on the model, 3th is the 3xNxT marker positions hypermatrix, the optional 4th is the initial guess for Q [default 0], 5th is acquisition frequency [default 100Hz], 6th is noise factor [default 1e-10] and 7th is error factor [default 1e-5] and 7th if you want to remove axes as specified in the model file [default = true]");
checkNombreInputParametres(nrhs, 3, 8, "4 arguments are required (+5 optional) where the 2nd is the handler on the model, "
"3th is the 3xNxT marker positions hypermatrix, "
"the optional 4th is the initial guess for Q [default 0], "
"5th is acquisition frequency [default 100Hz], "
"6th is noise factor [default 1e-10] and "
"7th is error factor [default 1e-5] and 7th if you want to remove axes as specified in the model file [default = true]");

// Recevoir le model
biorbd::Model * model = convertMat2Ptr<biorbd::Model>(prhs[1]);
Expand All @@ -139,7 +144,7 @@ void Matlab_inverseKinematicsEKFallInOneCall( int, mxArray *plhs[],
freq = getDouble(prhs,4,"Acquisition Frequency");

// Créer un filtre de Kalman
biorbd::rigidbody::KalmanReconsMarkers kalman(*model, biorbd::rigidbody::KalmanReconsMarkers::KalmanParam(freq, noiseF, errorF));
biorbd::rigidbody::KalmanReconsMarkers kalman(*model, biorbd::rigidbody::KalmanParam(freq, noiseF, errorF));

bool removeAxes(true);
if (nrhs >= 8)
Expand All @@ -153,7 +158,7 @@ void Matlab_inverseKinematicsEKFallInOneCall( int, mxArray *plhs[],

// Recevoir Qinit
if (kalman.first() && nrhs >= 4){
biorbd::rigidbody::GeneralizedCoordinates Qinit(*getParameterQ(prhs, 3, nQ).begin());
biorbd::rigidbody::GeneralizedCoordinates Qinit(getParameterQ(prhs, 3, nQ)[0]);
kalman.setInitState(&Qinit);
}

Expand All @@ -173,7 +178,7 @@ void Matlab_inverseKinematicsEKFallInOneCall( int, mxArray *plhs[],
biorbd::rigidbody::GeneralizedAcceleration QDDot(nQddot);

// Faire la cinématique inverse
kalman.reconstructFrame(*model, *(markersOverTime.begin()+i), &Q, &QDot, &QDDot, removeAxes);
kalman.reconstructFrame(*model, markersOverTime[i], &Q, &QDot, &QDDot, removeAxes);

// Remplir la variable de sortie
for (unsigned int j=0; j<nQ; ++j){
Expand Down
4 changes: 2 additions & 2 deletions binding/matlab/Matlab_inverseKinematicsEKF_IMU.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -28,7 +28,7 @@ void Matlab_setEKF_IMU(int nlhs, mxArray *plhs[],
noiseF = getDouble(prhs,3,"Noise Factor");
if (nrhs >= 3)
freq = getDouble(prhs,2,"Acquisition Frequency");
biorbd::rigidbody::KalmanRecons::KalmanParam kParams(freq, noiseF, errorF);
biorbd::rigidbody::KalmanParam kParams(freq, noiseF, errorF);

// Créer un filtre de Kalman
try{
Expand Down Expand Up @@ -130,7 +130,7 @@ void Matlab_inverseKinematicsEKF_IMUallInOneCall( int, mxArray *plhs[],
freq = getDouble(prhs,4,"Acquisition Frequency");

// Créer un filtre de Kalman
biorbd::rigidbody::KalmanReconsIMU kalman(*model, biorbd::rigidbody::KalmanRecons::KalmanParam(freq, noiseF, errorF));
biorbd::rigidbody::KalmanReconsIMU kalman(*model, biorbd::rigidbody::KalmanParam(freq, noiseF, errorF));

// Recevoir la matrice des imus
std::vector<std::vector<biorbd::rigidbody::IMU>> imusOverTime = getParameterAllIMUs(prhs,2);
Expand Down
35 changes: 15 additions & 20 deletions binding/matlab/Matlab_muscleJointTorqueFromActivation.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -12,16 +12,17 @@ void Matlab_muscleJointTorqueFromActivation( int nlhs, mxArray *plhs[],
// Verifier les arguments d'entrée
checkNombreInputParametres(nrhs, 3, 5, "3 arguments are required (+2 optional) where the 2nd is the handler on the model, "
"3rd is the muscles states and optional 4th and 5th are the Q and QDot, respectively."
"WARNING: if the function is called without Q and Qdot, the user MUST update by himself before calling this function (using updateMuscle).");
"WARNING: if the function is called without Q and Qdot, the user MUST update by himself "
"before calling this function (using updateMuscle).");
// Recevoir le model
biorbd::Model * model = convertMat2Ptr<biorbd::Model>(prhs[1]);
unsigned int nQ = model->nbQ(); // Get the number of DoF
unsigned int nQdot = model->nbQdot(); // Get the number of DoF
unsigned int nTau = model->nbGeneralizedTorque(); // Get the number of DoF
unsigned int nMuscleTotal = model->nbMuscleTotal();
unsigned int nMuscleTotal = model->nbMuscles();

// Recevoir muscleStates
std::vector<std::vector<std::shared_ptr<biorbd::muscles::StateDynamics>>> s = getParameterMuscleStateActivation(prhs,2,nMuscleTotal);
std::vector<std::vector<std::shared_ptr<biorbd::muscles::State>>> s = getParameterMuscleStateActivation(prhs,2,nMuscleTotal);
unsigned int nFrame(static_cast<unsigned int>(s.size()));

bool updateKin(false); // Par défaut c'est false, mais ce comportement est celui le moins attendu
Expand Down Expand Up @@ -69,31 +70,25 @@ void Matlab_muscleJointTorqueFromActivation( int nlhs, mxArray *plhs[],

// Remplir le output
biorbd::rigidbody::GeneralizedTorque muscleTorque;
biorbd::utils::Vector force;
biorbd::utils::Vector muscleForces;
for (unsigned int i=0; i<nFrame; ++i){
force.setZero();
if (nlhs >= 2) // Si on doit récupérer les forces
if (updateKin)
muscleTorque = model->muscularJointTorque(s[i], force, updateKin, &Q[i], &QDot[i]);
else
muscleTorque = model->muscularJointTorque(s[i], force, updateKin);

else // Si non
if (updateKin)
muscleTorque = model->muscularJointTorque(s[i], updateKin, &Q[i], &QDot[i]);
else
muscleTorque = model->muscularJointTorque(s[i], updateKin);

if (updateKin)
muscleForces = model->muscleForces(s[i], Q[i], QDot[i]);
else
muscleForces = model->muscleForces(s[i]);
muscleTorque = model->muscularJointTorque(muscleForces);

// distribuer les Tau
for (unsigned int j=0; j<nTau; ++j){
tau[i*nTau+j] = muscleTorque(j);
}

// Distribuer les forces
if (nlhs >= 2) // Si on doit récupérer les forces
for (unsigned int j=0; j<nMuscleTotal; ++j)
Mus[i*nMuscleTotal+j] = force(j);
if (nlhs >= 2){ // Si on doit récupérer les forces
for (unsigned int j=0; j<nMuscleTotal; ++j){
Mus[i*nMuscleTotal+j] = muscleForces(j);
}
}
}

return;
Expand Down
Loading

0 comments on commit 22edc06

Please sign in to comment.