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);
}
void Begin(int argc, char* argv[]) {
char task[64],
buffer[1024];
int i,
cmdLength=0,
cmdBegin=1,
function;
Task* workingtask = NULL;
// vérification arguments
function = CheckArgument(argv[1]);
int AcceptNull = (function==_H||function==_CF||function==_R||function==_I||function==_LT||function==_LD);
// début de traitement
switch (AcceptNull)
{
// cas des commandes qui n'ont pas besoin d'une tache valide
case true :
{
switch (function)
{
case _R : printf("-- number of running task"); TaskFunction = Running; break;
case _CF : printf("-- open configuration file\n"); TaskFunction = OpenConfig; break;
case _H : printf("-- help of the program\n"); TaskFunction = Help; break;
case _I : printf("-- number of running instances"); TaskFunction = Instances; break;
case _LT : printf("-- list all tasks\n"); TaskFunction = ListAllTasks; break;
case _LD : printf("-- list loaded tasks\n"); TaskFunction = ListLoadedTasks; break;
}
break;
}
// cas des commandes nécessitant une tache valide
default :
{
// nom de la tache
if (function != _A && argc > 2) sprintf(task, "%s", argv[2]);
else sprintf(task, "%s", DEFAULT_TASK);
// pointeur sur celle-ci
workingtask=GetTask(task);
if (workingtask==NULL) {
workingtask=LoadTask(task);
if (workingtask==NULL) {
workingtask=AddTask(task);
if (workingtask==NULL) {
// tache introuvable, plus de place
fprintf(stderr, "*** unable to work on task %s, exit\n", task);
exit(2);
}
}
}
// tache valide
switch (function)
{
case _L : printf("-- list %s task\n", task); TaskFunction = ListTask; break;
case _S : printf("-- start %s task\n", task); TaskFunction = StartTask; break;
case _D : printf("-- delete last command in %s task\n", task); TaskFunction = DeleteLastCommand; break;
case _X : printf("-- stop %s task\n", task); TaskFunction = StopTask; break;
case _C : printf("-- clean %s task\n", task); TaskFunction = CleanTask; break;
case _M : {
if (argc > 3) sprintf(DEFAULT_EDITOR, "%s", argv[3]);
printf("-- modify %s task\n", task);
TaskFunction = ModifyTask;
break;
}
case _T : cmdBegin+=2;
case _A : {
GetEnv("$PWD", addedCommand.wd);
sprintf(addedCommand.cmd, "%s", argv[cmdBegin]);
cmdLength = strlen(addedCommand.cmd);
for (i=cmdBegin+1; i<argc; i++) {
cmdLength += strlen(argv[i]);
if (cmdLength >= 1023) {
fprintf(stderr, "*** attention : command is too long (max. size 1024)\n");
fprintf(stderr, " possible unexpected behavior\n");
break;
}
sprintf(buffer, " %s", argv[i]);
strncat(addedCommand.cmd, buffer, 1024);
}
printf("-- add '%s' to %s task\n", addedCommand.cmd, task);
TaskFunction = AddToTask;
break;
}
default : fprintf(stderr, "*** error while interpreting arguments : returning value %d\n", function); break;
}
break;
}
}
TaskFunction(workingtask);
if (workingtask!=NULL) {
if ((!workingtask->exec) && (!workingtask->demand))
workingtask->inuse=0;
}
}
