/*
* Arguments : const mxArray *prhs[5]
* const mxArray *plhs[1]
* Return Type : void
*/
void SpringForce_api(const mxArray *prhs[5], const mxArray *plhs[1])
{
emxArray_real_T *particleDist;
emxArray_real_T *springConst;
emxArray_boolean_T *connectivityMap;
emxArray_real_T *minParticleDist;
emxArray_real_T *fixedParticleNum;
emxArray_real_T *force;
emlrtStack st = { NULL, NULL, NULL };
st.tls = emlrtRootTLSGlobal;
emlrtHeapReferenceStackEnterFcnR2012b(&st);
emxInit_real_T(&st, &particleDist, 2, true);
emxInit_real_T(&st, &springConst, 2, true);
emxInit_boolean_T(&st, &connectivityMap, 2, true);
emxInit_real_T(&st, &minParticleDist, 2, true);
emxInit_real_T(&st, &fixedParticleNum, 2, true);
emxInit_real_T(&st, &force, 2, true);
prhs[0] = emlrtProtectR2012b(prhs[0], 0, false, -1);
prhs[1] = emlrtProtectR2012b(prhs[1], 1, false, -1);
prhs[2] = emlrtProtectR2012b(prhs[2], 2, false, -1);
prhs[3] = emlrtProtectR2012b(prhs[3], 3, false, -1);
prhs[4] = emlrtProtectR2012b(prhs[4], 4, false, -1);
/* Marshall function inputs */
emlrt_marshallIn(&st, emlrtAlias(prhs[0]), "particleDist", particleDist);
emlrt_marshallIn(&st, emlrtAlias(prhs[1]), "springConst", springConst);
c_emlrt_marshallIn(&st, emlrtAlias(prhs[2]), "connectivityMap",
connectivityMap);
emlrt_marshallIn(&st, emlrtAlias(prhs[3]), "minParticleDist", minParticleDist);
e_emlrt_marshallIn(&st, emlrtAlias(prhs[4]), "fixedParticleNum",
fixedParticleNum);
/* Invoke the target function */
SpringForce(particleDist, springConst, connectivityMap, minParticleDist,
fixedParticleNum, force);
/* Marshall function outputs */
plhs[0] = emlrt_marshallOut(force);
force->canFreeData = false;
emxFree_real_T(&force);
fixedParticleNum->canFreeData = false;
emxFree_real_T(&fixedParticleNum);
minParticleDist->canFreeData = false;
emxFree_real_T(&minParticleDist);
connectivityMap->canFreeData = false;
emxFree_boolean_T(&connectivityMap);
springConst->canFreeData = false;
emxFree_real_T(&springConst);
particleDist->canFreeData = false;
emxFree_real_T(&particleDist);
emlrtHeapReferenceStackLeaveFcnR2012b(&st);
}
SICONOS_EXPORT void U(double time, unsigned int sizeOfq, const double *q, const double *velocity, double *U, unsigned int sizeZ, double* z)
{
int ndof = sizeOfq;
int ncont = 2;
int z_size = 0;
double *state = 0;
double *myz = 0;
double *zdot = 0;
double q_local[sizeOfq];
double v_local[sizeOfq];
for (unsigned int i = 0; i < sizeOfq; i++)
{
q_local[i] = q[i];
v_local[i] = velocity[i];
}
actuationDynamics(&time, q_local, v_local, myz, state, &ndof, &ncont, &z_size, zdot, U);
SpringForce(v_local, q_local);
for (unsigned int i = 0; i < sizeOfq; i++)
U[i] = -U[i] + v_local[i];
}
SICONOS_EXPORT void controlLaw(double * t, double * q, double * qdot, int * NDOF, int * NCONT, double * torques)
{
int ndof;
int contacts;
char lagrangianModelName[20] = "";
getLagrangianModelName(lagrangianModelName);
getActiveDOFsize(&ndof);
vector<int> activeDOF(ndof);
if (ndof > 0)
getActiveDOF(&(activeDOF[0]));
else
ndof = *NDOF;
///////////////////////////////////////////
// Computation of the Lagrangian model
///////////////////////////////////////////
matrix<double, column_major> M(*NDOF, *NDOF);
vector<double> N(*NDOF);
if (strcmp(lagrangianModelName, "Human36") == 0)
{
double L[31];
double addl[84];
double mass;
GetModelMass(&mass);
GetAnatomicalLengths(L);
GetTag2JointLengths(addl);
InertiaH36(&(M(0, 0)), q, L, addl, mass);
NLEffectsH36(&(N[0]), q, qdot, L, addl, mass);
}
else
{
Inertia(&(M(0, 0)), q);
NLEffects(&(N[0]), q, qdot);
}
//////////////////////////////////////////////////
//Additionnal forces
/////////////////////////////////////////////////
vector<double> fAdditionnal(*NDOF);
if (strcmp(lagrangianModelName, "PA10") == 0)
Friction(&(fAdditionnal[0]), q, qdot);
else if (strcmp(lagrangianModelName, "RX90") == 0)
SpringForce(&(fAdditionnal[0]), q);
else
fAdditionnal.clear();
///////////////////////////////////////////////////
// Limitation to the selected degrees of freedom
///////////////////////////////////////////////////
reduceMatrix(M, activeDOF, activeDOF);
N = reduceVector(N, activeDOF);
fAdditionnal = reduceVector(fAdditionnal, activeDOF);
//////////////////////////////////////////////////
// Proportionnel-derivee in the task space
//////////////////////////////////////////////////
vector<double> sDesired(*NDOF);
vector<double> sdotDesired(*NDOF);
matrix<double, column_major> sddotDesiredMATRIX(*NDOF, 1);
matrix_column<matrix<double, column_major> > sddotDesired(sddotDesiredMATRIX, 1);
//vector<double> sddotDesired(*NDOF);
trajectory(t, &(sDesired[0]), &(sdotDesired[0]), &(sddotDesired[0]), &contacts);
//v = Kp*(s_desiree-TaskFunction(q))+Kv*(sdot_desiree-H*qdot)-h+sddot_desiree;
vector<double> TF(*NDOF);
vector<double> h(*NDOF);
matrix<double, column_major> H(*NDOF, *NDOF);
TaskFunction(&(TF[0]), q);
TaskJacobian(&(H(0, 0)), q);
TaskNLEffects(&(h[0]), q, qdot);
double Kp = 100.0;
double Kv = 30.0;
vector<double, array_adaptor<double> > tmp_cast(*NDOF, array_adaptor<double> (*NDOF, qdot));
sddotDesired += Kp * (sDesired - TF) + Kv * (sdotDesired - prod(H, tmp_cast)) - h;
/////////////////////////////////////////////////////////////
// Generalized Torques to make the realisation of the command
/////////////////////////////////////////////////////////////
//qddot = inv(H)*v;
vector<int> ipiv(*NDOF);
// lapack::getrf(H, ipiv);
// sddotDesired = prod(H, sddotDesired);
lapack::gesv(H, ipiv, sddotDesiredMATRIX);
//D = M*qddot(ActiveDOF) + N + FAdditionnal;
sddotDesired = reduceVector(sddotDesired, activeDOF);
N += prod(M, sddotDesired) + fAdditionnal;
//nmot = sum(ActiveDOF<=size(q, 1)); en fait ici nmot = ndof
//Torques = zeros(q);
//Torques(ActiveDOF(1:nmot)) = D(1:nmot);
for (int i = 0; i < *NDOF; i++)
torques[i] = 0;
expandVector(torques, N, activeDOF);
}
