/* * 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); }