JNIEXPORT void JNICALL Java_us_ihmc_commonWalkingControlModules_controlModules_nativeOptimization_CVXMomentumOptimizerWithGRFSmootherNative_initialize
(JNIEnv * env, jclass jClass)
{
set_defaults();
setup_indexing();
settings.verbose = 0;
AByteBuffer = (*env)->NewGlobalRef(env, (*env)->NewDirectByteBuffer(env, params.A, sizeof(double) * ASize));
bByteBuffer = (*env)->NewGlobalRef(env, (*env)->NewDirectByteBuffer(env, params.b, sizeof(double) * bSize));
CByteBuffer = (*env)->NewGlobalRef(env, (*env)->NewDirectByteBuffer(env, params.C, sizeof(double) * CSize));
JsByteBuffer = (*env)->NewGlobalRef(env, (*env)->NewDirectByteBuffer(env, params.Js, sizeof(double) * JsSize));
psByteBuffer = (*env)->NewGlobalRef(env, (*env)->NewDirectByteBuffer(env, params.ps, sizeof(double) * psSize));
WsByteBuffer = (*env)->NewGlobalRef(env, (*env)->NewDirectByteBuffer(env, params.Ws, sizeof(double) * WsSize));
LambdaByteBuffer = (*env)->NewGlobalRef(env, (*env)->NewDirectByteBuffer(env, params.Lambda, sizeof(double) * LambdaSize));
QrhoByteBuffer = (*env)->NewGlobalRef(env, (*env)->NewDirectByteBuffer(env, params.Qrho, sizeof(double) * QrhoSize));
QphiByteBuffer = (*env)->NewGlobalRef(env, (*env)->NewDirectByteBuffer(env, params.Qphi, sizeof(double) * QphiSize));
cByteBuffer = (*env)->NewGlobalRef(env, (*env)->NewDirectByteBuffer(env, params.c, sizeof(double) * cSize));
rhoMinByteBuffer = (*env)->NewGlobalRef(env, (*env)->NewDirectByteBuffer(env, params.rhoMin, sizeof(double) * rhoMinSize));
phiMinByteBuffer = (*env)->NewGlobalRef(env, (*env)->NewDirectByteBuffer(env, params.phiMin, sizeof(double) * phiMinSize));
phiMaxByteBuffer = (*env)->NewGlobalRef(env, (*env)->NewDirectByteBuffer(env, params.phiMax, sizeof(double) * phiMaxSize));
WRhoByteBuffer = (*env)->NewGlobalRef(env, (*env)->NewDirectByteBuffer(env, params.WRho, sizeof(double) * WRhoSize));
WPhiByteBuffer = (*env)->NewGlobalRef(env, (*env)->NewDirectByteBuffer(env, params.WPhi, sizeof(double) * WPhiSize));
rhoPreviousByteBuffer = (*env)->NewGlobalRef(env, (*env)->NewDirectByteBuffer(env, params.rhoPrevious, sizeof(double) * rhoPreviousSize));
WRhoSmootherByteBuffer = (*env)->NewGlobalRef(env, (*env)->NewDirectByteBuffer(env, params.WRhoSmoother, sizeof(double) * WRhoSmootherSize));
rhoByteBuffer = (*env)->NewGlobalRef(env, (*env)->NewDirectByteBuffer(env, vars.rho, sizeof(double) * rhoSize));
phiByteBuffer = (*env)->NewGlobalRef(env, (*env)->NewDirectByteBuffer(env, vars.phi, sizeof(double) * phiSize));
vdByteBuffer = (*env)->NewGlobalRef(env, (*env)->NewDirectByteBuffer(env, vars.vd, sizeof(double) * vdSize));
}
int main(int argc, char **argv) {
int num_iters;
#if (NUMTESTS > 0)
int i;
double time;
double time_per;
#endif
set_defaults();
setup_indexing();
load_default_data();
/* Solve problem instance for the record. */
settings.verbose = 1;
num_iters = solve();
#ifndef ZERO_LIBRARY_MODE
#if (NUMTESTS > 0)
/* Now solve multiple problem instances for timing purposes. */
settings.verbose = 0;
tic();
for (i = 0; i < NUMTESTS; i++) {
solve();
}
time = tocq();
printf("Timed %d solves over %.3f seconds.\n", NUMTESTS, time);
time_per = time / NUMTESTS;
if (time_per > 1) {
printf("Actual time taken per solve: %.3g s.\n", time_per);
} else if (time_per > 1e-3) {
printf("Actual time taken per solve: %.3g ms.\n", 1e3*time_per);
} else {
printf("Actual time taken per solve: %.3g us.\n", 1e6*time_per);
}
#endif
#endif
return 0;
}
int flowmin(double delta[N], double k[L*2], double hmns[L],
double hpls[L], double bmin, double flow[L]) {
int num_iters;
set_defaults();
setup_indexing();
load_data(delta, k, hmns, hpls, bmin);
/* Solve problem instance for the record. */
settings.verbose = 0;
num_iters = solve();
if (work.converged != 1)
printf("Flow minimization failed to converge!\n");
int i=0;
for (i=0; i<L; i++)
flow[i]=vars.F[i]; // return flow by reference
return 0;
}
double balmin(double delta[N], double k[L*2], double hmns[L],
double hpls[L]) {
int num_iters;
//printf("%f, %f\n",hmns[5],hpls[5]);
set_defaults();
setup_indexing();
//settings.eps=1.e-2;
//settings.resid_tol=1.e-2;
//settings.max_iters=100;
//settings.kkt_reg=1.e-3;
load_data(delta, k, hmns, hpls);
/* Solve problem instance for the record. */
settings.verbose = 0;
num_iters = solve();
if (work.converged != 1)
printf("Balancing minimization failed to converge!\n");
return work.optval;
}
void cvxgen_ldp(double *Y, double *v) {
set_defaults();
setup_indexing();
settings.verbose = 0;
double *src;
double *dest;
int i;
src = Y;
dest = params.Y;
for (i = 0; i < 24; i++) {
*dest++ = *src++;
}
int num_iters = solve();
dest = v;
src = vars.v;
for (i = 0; i < 3; i++) {
*dest++ = *src++;
}
}
JNIEXPORT jdoubleArray JNICALL Java_CVXGENController_controlSignalCVXGEN
(JNIEnv * env, jobject thisObj, jdoubleArray inJNIArray){
int num_iters;
#if (NUMTESTS > 0)
int i;
double time;
double time_per;
#endif
set_defaults();
setup_indexing();
load_default_data();
/* Solve problem instance for the record. */
settings.verbose = 1;
//Convert the Array from JNI data type to C data type
jdouble *inCArray = (*env)->GetDoubleArrayElements(env, inJNIArray, NULL);
if (NULL == inCArray) return NULL;
jsize length = (*env)->GetArrayLength(env, inJNIArray);
params.xe_0[0] = inCArray[0];
params.xe_0[1] = inCArray[1];
params.xe_0[2] = 0;
params.xe_0[3] = 0;
params.xe_0[4] = inCArray[4];
params.xe_0[5] = inCArray[5];
params.r[0] = inCArray[6];
params.r[1] = inCArray[7];
//printf("%f\n", inCArray[4]); TODO remove maybe
//printf("%f\n", inCArray[5]); TODO remove maybe
//Solve the QP
num_iters = solve();
//gettting the data after solving the QP
//jdouble x = *vars.x[1];
//jdouble x1 = *vars.x[2];
//jdouble u0 = *vars.u_0;
//jdouble u1 = *(vars.u_0+1);
jdouble u1 = vars.u_0[0];
jdouble u2= vars.u_0[1];
jdouble controlArray[] ={u1 ,u2};
//printf("%f\n", x);
//printf("Hello");
/*
#ifndef ZERO_LIBRARY_MODE
#if (NUMTESTS > 0)
/* Now solve multiple problem instances for timing purposes. */
/*
settings.verbose = 0;
tic();
for (i = 0; i < NUMTESTS; i++) {
solve();
}
time = tocq();
printf("Timed %d solves over %.3f seconds.\n", NUMTESTS, time);
time_per = time / NUMTESTS;
if (time_per > 1) {
printf("Actual time taken per solve: %.3g s.\n", time_per);
} else if (time_per > 1e-3) {
printf("Actual time taken per solve: %.3g ms.\n", 1e3*time_per);
} else {
printf("Actual time taken per solve: %.3g us.\n", 1e6*time_per);
}
#endif
#endif
*/
jdoubleArray outJNIArray = (*env)->NewDoubleArray(env, 2); // allocate
if (NULL == outJNIArray) return NULL;
(*env)->SetDoubleArrayRegion(env, outJNIArray, 0 , 2, controlArray); // copy
return outJNIArray;
}