bool CalibrationData::saveSLCALIB(const QString& filename){
FILE * fp = fopen(qPrintable(filename), "w");
if (!fp)
return false;
fprintf(fp, "#V1.0 SLStudio calibration\n");
fprintf(fp, "#Calibration time: %s\n\n", calibrationDateTime.c_str());
fprintf(fp, "Kc\n%f %f %f\n%f %f %f\n%f %f %f\n\n", Kc(0,0), Kc(0,1), Kc(0,2), Kc(1,0), Kc(1,1), Kc(1,2), Kc(2,0), Kc(2,1), Kc(2,2));
fprintf(fp, "kc\n%f %f %f %f %f\n\n", kc(0), kc(1), kc(2), kc(3), kc(4));
fprintf(fp, "Kp\n%f %f %f\n%f %f %f\n%f %f %f\n\n", Kp(0,0), Kp(0,1), Kp(0,2), Kp(1,0), Kp(1,1), Kp(1,2), Kp(2,0), Kp(2,1), Kp(2,2));
fprintf(fp, "kp\n%f %f %f %f %f\n\n", kp(0), kp(1), kp(2), kp(3), kp(4));
fprintf(fp, "Rp\n%f %f %f\n%f %f %f\n%f %f %f\n\n", Rp(0,0), Rp(0,1), Rp(0,2), Rp(1,0), Rp(1,1), Rp(1,2), Rp(2,0), Rp(2,1), Rp(2,2));
fprintf(fp, "Tp\n%f %f %f\n\n", Tp(0), Tp(1), Tp(2));
fprintf(fp, "cam_error: %f\n\n", cam_error);
fprintf(fp, "proj_error: %f\n\n", proj_error);
fprintf(fp, "stereo_error: %f\n\n", stereo_error);
fclose(fp);
return true;
}
const Matrix &
SSPquadUP::getMass(void)
{
mMass.Zero();
// compute compressibility matrix term
double oneOverQ = -0.25*J0*mThickness*mPorosity/fBulk;
// get mass density from the material
double density = theMaterial->getRho();
// transpose the shape function derivative array
Matrix dNp(2,4);
dNp(0,0) = dN(0,0); dNp(0,1) = dN(1,0); dNp(0,2) = dN(2,0); dNp(0,3) = dN(3,0);
dNp(1,0) = dN(0,1); dNp(1,1) = dN(1,1); dNp(1,2) = dN(2,1); dNp(1,3) = dN(3,1);
// compute stabilization matrix for incompressible problems
Matrix Kp(4,4);
Kp = -4.0*mAlpha*J0*mThickness*dN*dNp;
// return zero matrix if density is zero
if (density == 0.0) {
return mMass;
}
// full mass matrix for the element [ M 0 ]
// includes M and S submatrices [ 0 -S ]
for (int i = 0; i < 4; i++) {
int I = 2*i;
int Ip1 = 2*i+1;
int II = 3*i;
int IIp1 = 3*i+1;
int IIp2 = 3*i+2;
for (int j = 0; j < 4; j++) {
int J = 2*j;
int Jp1 = 2*j+1;
int JJ = 3*j;
int JJp1 = 3*j+1;
int JJp2 = 3*j+2;
mMass(II,JJ) = mSolidM(I,J);
mMass(IIp1,JJ) = mSolidM(Ip1,J);
mMass(IIp1,JJp1) = mSolidM(Ip1,Jp1);
mMass(II,JJp1) = mSolidM(I,Jp1);
// contribution of compressibility matrix
mMass(IIp2,JJp2) = Kp(i,j) + oneOverQ;
}
}
return mMass;
}
void Solver::CreateBlocksKPHH(){
vec identifiers = zeros<vec>(0); Nkphh = 0;
for (int k1=0; k1<NKp; k1++){
for (int k2=0; k2<NKp3; k2++){
if (Kp(k1,1) == Kphh(k2,3) ) { // Block found
bool IDExist = any( identifiers == Kp(k1,1) );
if ( ! IDExist){
identifiers.insert_rows(Nkphh,1);
identifiers(Nkphh) = Kp(k1,1);
Nkphh ++;
}
}
}
}
blockskphh = new Block*[Nkphh];
for (int n=0; n<Nkphh; n++) blockskphh[n] = new Block(basis,Nholes,Nparticles);
for (int k1=0; k1<NKp; k1++){
for (int k2=0; k2<NKp3; k2++){
if (Kp(k1,1) == Kphh(k2,3) ) { // Block found
uvec indices = find( identifiers == Kp(k1,1));
int indice = indices(0);
blockskphh[indice]->AddTripleStates( Kp.row(k1), Kphh.row(k2) );
}
}
}
for (int n=0; n<Nkphh; n++) blockskphh[n]->FinishBlock();
#pragma omp parallel
{
int id = omp_get_thread_num();
int threads = omp_get_num_threads();
for (int n=id; n<Nkphh; n+=threads) blockskphh[n]->Epsilonkphh();
}
}
int BlockPCGSolver::Solve(const Epetra_MultiVector &X, Epetra_MultiVector &Y) const {
int info = 0;
int localVerbose = verbose*(MyComm.MyPID() == 0);
int xr = X.MyLength();
int wSize = 3*xr;
if (lWorkSpace < wSize) {
if (workSpace)
delete[] workSpace;
workSpace = new (std::nothrow) double[wSize];
if (workSpace == 0) {
info = -1;
return info;
}
lWorkSpace = wSize;
} // if (lWorkSpace < wSize)
double *pointer = workSpace;
Epetra_Vector r(View, X.Map(), pointer);
pointer = pointer + xr;
Epetra_Vector p(View, X.Map(), pointer);
pointer = pointer + xr;
// Note: Kp and z uses the same memory space
Epetra_Vector Kp(View, X.Map(), pointer);
Epetra_Vector z(View, X.Map(), pointer);
double tmp;
double initNorm = 0.0, rNorm = 0.0, newRZ = 0.0, oldRZ = 0.0, alpha = 0.0;
double tolSquare = tolCG*tolCG;
memcpy(r.Values(), X.Values(), xr*sizeof(double));
tmp = callBLAS.DOT(xr, r.Values(), 1, r.Values(), 1);
MyComm.SumAll(&tmp, &initNorm, 1);
Y.PutScalar(0.0);
if (localVerbose > 1) {
std::cout << std::endl;
std::cout << " --- PCG Iterations --- " << std::endl;
}
int iter;
for (iter = 1; iter <= iterMax; ++iter) {
if (Prec) {
Prec->ApplyInverse(r, z);
}
else {
memcpy(z.Values(), r.Values(), xr*sizeof(double));
}
if (iter == 1) {
tmp = callBLAS.DOT(xr, r.Values(), 1, z.Values(), 1);
MyComm.SumAll(&tmp, &newRZ, 1);
memcpy(p.Values(), z.Values(), xr*sizeof(double));
}
else {
oldRZ = newRZ;
tmp = callBLAS.DOT(xr, r.Values(), 1, z.Values(), 1);
MyComm.SumAll(&tmp, &newRZ, 1);
p.Update(1.0, z, newRZ/oldRZ);
}
K->Apply(p, Kp);
tmp = callBLAS.DOT(xr, p.Values(), 1, Kp.Values(), 1);
MyComm.SumAll(&tmp, &alpha, 1);
alpha = newRZ/alpha;
TEUCHOS_TEST_FOR_EXCEPTION(alpha <= 0.0, std::runtime_error,
" !!! Non-positive value for p^TKp (" << alpha << ") !!!");
callBLAS.AXPY(xr, alpha, p.Values(), 1, Y.Values(), 1);
alpha *= -1.0;
callBLAS.AXPY(xr, alpha, Kp.Values(), 1, r.Values(), 1);
// Check convergence
tmp = callBLAS.DOT(xr, r.Values(), 1, r.Values(), 1);
MyComm.SumAll(&tmp, &rNorm, 1);
if (localVerbose > 1) {
std::cout << " Iter. " << iter;
std::cout.precision(4);
std::cout.setf(std::ios::scientific, std::ios::floatfield);
std::cout << " Residual reduction " << std::sqrt(rNorm/initNorm) << std::endl;
}
if (rNorm <= tolSquare*initNorm)
break;
} // for (iter = 1; iter <= iterMax; ++iter)
if (localVerbose == 1) {
std::cout << std::endl;
std::cout << " --- End of PCG solve ---" << std::endl;
std::cout << " Iter. " << iter;
std::cout.precision(4);
std::cout.setf(std::ios::scientific, std::ios::floatfield);
std::cout << " Residual reduction " << std::sqrt(rNorm/initNorm) << std::endl;
std::cout << std::endl;
}
if (localVerbose > 1) {
std::cout << std::endl;
}
numSolve += 1;
minIter = (iter < minIter) ? iter : minIter;
maxIter = (iter > maxIter) ? iter : maxIter;
sumIter += iter;
return info;
}
int main( int argc, char** argv ){
#if (CISST_OS == CISST_LINUX_XENOMAI)
mlockall(MCL_CURRENT | MCL_FUTURE);
RT_TASK task;
rt_task_shadow( &task, "GroupTest", 99, 0 );
#endif
cmnLogger::SetMask( CMN_LOG_ALLOW_ALL );
cmnLogger::SetMaskFunction( CMN_LOG_ALLOW_ALL );
cmnLogger::SetMaskDefaultLog( CMN_LOG_ALLOW_ALL );
if( argc != 2 ){
std::cout << "Usage: " << argv[0] << " can[0-1]" << std::endl;
return -1;
}
#if (CISST_OS == CISST_LINUX_XENOMAI)
osaRTSocketCAN can( argv[1], osaCANBus::RATE_1000 );
#else
osaSocketCAN can( argv[1], osaCANBus::RATE_1000 );
#endif
if( can.Open() != osaCANBus::ESUCCESS ){
CMN_LOG_RUN_ERROR << argv[0] << "Failed to open " << argv[1] << std::endl;
return -1;
}
osaWAM WAM( &can );
if( WAM.Initialize() != osaWAM::ESUCCESS ){
CMN_LOG_RUN_ERROR << "Failed to initialize WAM" << std::endl;
return -1;
}
vctDynamicVector<double> qinit( 7, 0.0 );
qinit[1] = -cmnPI_2;
qinit[3] = cmnPI;
if( WAM.SetPositions( qinit ) != osaWAM::ESUCCESS ){
CMN_LOG_RUN_ERROR << "Failed to set position: " << qinit << std::endl;
return -1;
}
cmnPath path;
path.AddRelativeToCisstShare("/models/WAM");
std::string fname = path.Find("wam7.rob", cmnPath::READ);
// Rotate the base
vctMatrixRotation3<double> Rw0( 0.0, 0.0, -1.0,
0.0, 1.0, 0.0,
1.0, 0.0, 0.0 );
vctFixedSizeVector<double,3> tw0(0.0);
vctFrame4x4<double> Rtw0( Rw0, tw0 );
// Gain matrices
vctDynamicMatrix<double> Kp(7, 7, 0.0), Kd(7, 7, 0.0);
Kp[0][0] = 250; Kd[0][0] = 3.0;
Kp[1][1] = 250; Kd[1][1] = 3.0;
Kp[2][2] = 250; Kd[2][2] = 3.0;
Kp[3][3] = 200; Kd[3][3] = 3;
Kp[4][4] = 50; Kd[4][4] = 0.8;
Kp[5][5] = 50; Kd[5][5] = 0.8;
Kp[6][6] = 10; Kd[6][6] = .1;
osaPDGC PDGC( fname, Rtw0, Kp, Kd, qinit );
std::cout << "Activate the WAM" << std::endl;
bool activated = false;
double t1 = osaGetTime();
while( 1 ){
// Get the positions
vctDynamicVector<double> q;
if( WAM.GetPositions( q ) != osaWAM::ESUCCESS ){
CMN_LOG_RUN_ERROR << "Failed to get positions" << std::endl;
return -1;
}
// Check if the pucks are activated
if( !activated ) {
osaWAM::Mode mode;
if( WAM.GetMode( mode ) != osaWAM::ESUCCESS ){
CMN_LOG_RUN_ERROR << "Failed to get mode" << std::endl;
return -1;
}
if( mode == osaWAM::MODE_ACTIVATED )
{ activated = true; }
}
// if pucks are activated, run the controller
vctDynamicVector<double> tau( q.size(), 0.0 );
double t2 = osaGetTime();
if( activated ){
if( PDGC.Evaluate( qinit, q, tau, t2-t1 ) != osaPDGC::ESUCCESS ){
CMN_LOG_RUN_ERROR << "Failed to evaluate controller" << std::endl;
return -1;
}
}
t1 = t2;
// apply torques
if( WAM.SetTorques( tau ) != osaWAM::ESUCCESS ){
CMN_LOG_RUN_ERROR << "Failed to set torques" << std::endl;
return -1;
}
std::cout << "q: " << q << std::endl;
std::cout << "tau: " << tau << std::endl;
}
return 0;
}
int main( int argc, char** argv ){
#if (CISST_OS == CISST_LINUX_XENOMAI)
mlockall(MCL_CURRENT | MCL_FUTURE);
RT_TASK task;
rt_task_shadow( &task, "mtsWAMPDGCExample", 40, 0 );
#endif
cmnLogger::SetMask( CMN_LOG_ALLOW_ALL );
cmnLogger::SetMaskFunction( CMN_LOG_ALLOW_ALL );
cmnLogger::SetMaskDefaultLog( CMN_LOG_ALLOW_ALL );
if( argc != 3 ){
std::cout << "Usage: " << argv[0] << " can[0-1] GCMIP" << std::endl;
return -1;
}
std::string process( "Slave" );
mtsManagerLocal* taskManager = NULL;
try{ taskManager = mtsTaskManager::GetInstance( argv[2], process ); }
catch( ... ){
std::cerr << "Failed to connect to GCM: " << argv[2] << std::endl;
taskManager = mtsManagerLocal::GetInstance();
}
mtsKeyboard kb;
kb.SetQuitKey( 'q' );
kb.AddKeyWriteFunction( 'C', "PDGCEnable", "Enable", true );
kb.AddKeyWriteFunction( 'C', "TrajEnable", "Enable", true );
kb.AddKeyWriteFunction( 'M', "MoveEnable", "Enable", true );
taskManager->AddComponent( &kb );
// Initial configuration
vctDynamicVector<double> qinit( 7, 0.0 );
qinit[1] = -cmnPI_2;
qinit[3] = cmnPI-0.01;
qinit[5] = -cmnPI_2;
#if (CISST_OS == CISST_LINUX_XENOMAI)
osaRTSocketCAN can( argv[1], osaCANBus::RATE_1000 );
#else
osaSocketCAN can( argv[1], osaCANBus::RATE_1000 );
#endif
if( can.Open() != osaCANBus::ESUCCESS ){
CMN_LOG_RUN_ERROR << argv[0] << "Failed to open " << argv[1] << std::endl;
return -1;
}
mtsWAM WAM( "WAM", &can, osaWAM::WAM_7DOF, OSA_CPU4, 80 );
WAM.Configure();
WAM.SetPositions( qinit );
taskManager->AddComponent( &WAM );
cmnPath path;
path.AddRelativeToCisstShare("models/WAM");
// Rotate the base
vctMatrixRotation3<double> Rw0( 0.0, 0.0, -1.0,
0.0, 1.0, 0.0,
1.0, 0.0, 0.0 );
vctFixedSizeVector<double,3> tw0(0.0);
vctFrame4x4<double> Rtw0( Rw0, tw0 );
// Gain matrices
vctDynamicMatrix<double> Kp(7, 7, 0.0), Kd(7, 7, 0.0);
Kp[0][0] = 250; Kd[0][0] = 3.0;
Kp[1][1] = 250; Kd[1][1] = 3.0;
Kp[2][2] = 250; Kd[2][2] = 3.0;
Kp[3][3] = 200; Kd[3][3] = 3;
Kp[4][4] = 50; Kd[4][4] = 0.8;
Kp[5][5] = 50; Kd[5][5] = 0.8;
Kp[6][6] = 10; Kd[6][6] = .1;
mtsPDGC PDGC( "PDGC",
0.00125,
path.Find( "wam7cutter.rob" ),
Rtw0,
Kp,
Kd,
qinit,
OSA_CPU3 );
taskManager->AddComponent( &PDGC );
mtsTrajectory traj( "trajectory",
0.002,
path.Find( "wam7cutter.rob" ),
Rtw0,
qinit );
taskManager->AddComponent( &traj );
SetPoints setpoints( path.Find( "wam7cutter.rob" ), Rtw0, qinit );
taskManager->AddComponent( &setpoints );
// Connect the keyboard
if( !taskManager->Connect( kb.GetName(), "PDGCEnable",
PDGC.GetName(),"Control") ){
std::cout << "Failed to connect: "
<< kb.GetName() << "::PDGCEnable to "
<< PDGC.GetName() << "::Control" << std::endl;
return -1;
}
if( !taskManager->Connect( kb.GetName(), "TrajEnable",
traj.GetName(),"Control") ){
std::cout << "Failed to connect: "
<< kb.GetName() << "::TrajEnable to "
<< traj.GetName() << "::Control" << std::endl;
return -1;
}
if( !taskManager->Connect( kb.GetName(), "MoveEnable",
setpoints.GetName(),"Control") ){
std::cout << "Failed to connect: "
<< kb.GetName() << "::TrajEnable to "
<< setpoints.GetName() << "::Control" << std::endl;
return -1;
}
// connect the trajectory with setpoints
if( !taskManager->Connect( traj.GetName(), "Input",
setpoints.GetName(), "Output" ) ){
std::cout << "Failed to connect: "
<< traj.GetName() << "::Input to "
<< setpoints.GetName() << "::Output" << std::endl;
return -1;
}
// connect the trajectory to the controller
if( !taskManager->Connect( traj.GetName(), "Output",
PDGC.GetName(), "Input") ){
std::cout << "Failed to connect: "
<< traj.GetName() << "::Output to "
<< PDGC.GetName() << "::Input" << std::endl;
return -1;
}
// connect the controller to the WAM
if( !taskManager->Connect( WAM.GetName(), "Input",
PDGC.GetName(), "Output" ) ){
std::cout << "Failed to connect: "
<< WAM.GetName() << "::Input to "
<< PDGC.GetName() << "::Output" << std::endl;
return -1;
}
if( !taskManager->Connect( WAM.GetName(), "Output",
PDGC.GetName(), "Feedback" ) ){
std::cout << "Failed to connect: "
<< WAM.GetName() << "::Output to "
<< PDGC.GetName() << "::Feedback" << std::endl;
return -1;
}
taskManager->CreateAll();
taskManager->StartAll();
std::cout << "Press 'q' to exit." << std::endl;
pause();
return 0;
}
Localizer::Localizer(exchangeData *_commData, const unsigned int _period) :
RateThread(_period), commData(_commData), period(_period)
{
iCubHeadCenter eyeC(commData->head_version>1.0?"right_v2":"right");
eyeL=new iCubEye(commData->head_version>1.0?"left_v2":"left");
eyeR=new iCubEye(commData->head_version>1.0?"right_v2":"right");
// remove constraints on the links
// we use the chains for logging purpose
eyeL->setAllConstraints(false);
eyeR->setAllConstraints(false);
// release links
eyeL->releaseLink(0); eyeC.releaseLink(0); eyeR->releaseLink(0);
eyeL->releaseLink(1); eyeC.releaseLink(1); eyeR->releaseLink(1);
eyeL->releaseLink(2); eyeC.releaseLink(2); eyeR->releaseLink(2);
// add aligning matrices read from configuration file
getAlignHN(commData->rf_cameras,"ALIGN_KIN_LEFT",eyeL->asChain(),true);
getAlignHN(commData->rf_cameras,"ALIGN_KIN_RIGHT",eyeR->asChain(),true);
// overwrite aligning matrices iff specified through tweak values
if (commData->tweakOverwrite)
{
getAlignHN(commData->rf_tweak,"ALIGN_KIN_LEFT",eyeL->asChain(),true);
getAlignHN(commData->rf_tweak,"ALIGN_KIN_RIGHT",eyeR->asChain(),true);
}
// get the absolute reference frame of the head
Vector q(eyeC.getDOF(),0.0);
eyeCAbsFrame=eyeC.getH(q);
// ... and its inverse
invEyeCAbsFrame=SE3inv(eyeCAbsFrame);
// get the length of the half of the eyes baseline
eyesHalfBaseline=0.5*norm(eyeL->EndEffPose().subVector(0,2)-eyeR->EndEffPose().subVector(0,2));
bool ret;
// get camera projection matrix
ret=getCamPrj(commData->rf_cameras,"CAMERA_CALIBRATION_LEFT",&PrjL,true);
if (commData->tweakOverwrite)
{
Matrix *Prj;
if (getCamPrj(commData->rf_tweak,"CAMERA_CALIBRATION_LEFT",&Prj,true))
{
delete PrjL;
PrjL=Prj;
}
}
if (ret)
{
cxl=(*PrjL)(0,2);
cyl=(*PrjL)(1,2);
invPrjL=new Matrix(pinv(PrjL->transposed()).transposed());
}
else
PrjL=invPrjL=NULL;
// get camera projection matrix
ret=getCamPrj(commData->rf_cameras,"CAMERA_CALIBRATION_RIGHT",&PrjR,true);
if (commData->tweakOverwrite)
{
Matrix *Prj;
if (getCamPrj(commData->rf_tweak,"CAMERA_CALIBRATION_RIGHT",&Prj,true))
{
delete PrjR;
PrjR=Prj;
}
}
if (ret)
{
cxr=(*PrjR)(0,2);
cyr=(*PrjR)(1,2);
invPrjR=new Matrix(pinv(PrjR->transposed()).transposed());
}
else
PrjR=invPrjR=NULL;
Vector Kp(1,0.001), Ki(1,0.001), Kd(1,0.0);
Vector Wp(1,1.0), Wi(1,1.0), Wd(1,1.0);
Vector N(1,10.0), Tt(1,1.0);
Matrix satLim(1,2);
satLim(0,0)=0.05;
satLim(0,1)=10.0;
pid=new parallelPID(0.05,Kp,Ki,Kd,Wp,Wi,Wd,N,Tt,satLim);
Vector z0(1,0.5);
pid->reset(z0);
dominantEye="left";
port_xd=NULL;
}
//------------------------------------------------------------------------------
//void CKonst(int lmax, double *cma, double *cpa, double *cza,
// double *KmLm, double *KoLo, double *KpLM,
// double *KmlM, double *Kolo, double *Kplm){
// int i=0;
// int l, m;
// for(l=0;l<=lmax;l++){
// for(m=-l;m<=l;m++){
// cma[i]=cm(l,m);
// cza[i]=m;
// cpa[i]=cp(l,m);
// KmLm[i]=Km(l+1,m,m-1);
// KoLo[i]=Ko(l+1,m,m );
// KpLM[i]=Kp(l+1,m,m+1);
// KmlM[i]=Km(l ,m,m+1);
// Kolo[i]=Ko(l ,m,m );
// Kplm[i]=Kp(l ,m,m-1);
// i++;
// }
// }
//}
////------------------------------------------------------------------------------
//// CALCULATION OF CONSTANTS - ONE MORE LOOP - POSITION INDEPENDENT
////------------------------------------------------------------------------------
//// L DEPENDENT
//double CL[LMAX];
//double LL[LMAX];
//// CONSTANTS FOR X
//double CM[LMAX];
//double CZ[LMAX];
//double CP[LMAX];
//// CONSTANTS FOR Y AND V
//KmLm[LMAX];
//KoLo[LMAX];
//KpLM[LMAX];
//KmlM[LMAX];
//Kolo[LMAX];
//Kplm[LMAX];
//// SINGLE LOOP
//for(int l=1;l<=lmax;l++){
// for(int m=-l; m<=l; m++){
// CL[jlm(l,m)]=l;
// LL[jlm(l,m)]=2*l+1;
// CM[jlm(l,m)]=cm(l,m);
// CZ[jlm(l,m)]=m;
// CP[jlm(l,m)]=cp(l,m);
// KmLm[jlm(l,m)]=Km(l+1,m,m-1);
// KoLo[jlm(l,m)]=Ko(l+1,m,m );
// KpLM[jlm(l,m)]=Kp(l+1,m,m+1);
// KmlM[jlm(l,m)]=Km(l ,m,m+1);
// Kolo[jlm(l,m)]=Ko(l ,m,m );
// Kplm[jlm(l,m)]=Kp(l ,m,m-1);
// }
//}
//------------------------------------------------------------------------------
// POSITION DEPENDENT CALCULATIONS - MULTIPLES LOOPS - COMPLETE CALCULATIONS
//------------------------------------------------------------------------------
void VectorSphericalWaveFunctions(double *k,double *x, double *y, double *z,int *lmax,
double complex *GTE, double complex *GTM,
double complex *Em, double complex *Ez, double complex *Ep,
double complex *Hm, double complex *Hz, double complex *Hp
){
// printf("%d\t%E\t%E\t%E\t%E\n",*lmax+1,*k,*x,*y,*z);
int l,m;
int LMAX=*lmax*(*lmax+2);
int LMAXE=(*lmax+1)*(*lmax+3);
double cph;
double sph;
double rho=sqrt(*x*(*x)+*y*(*y));
double r=sqrt(rho*rho+*z*(*z));
double sth=rho/r;
double cth=*z/r;
if((*x==0)&&(*y==0)){
cph=1;
sph=0;
}else{
cph=*x/rho;
sph=*y/rho;
}
// Spherical Bessel Funtions
double JLM[*lmax+2];
// double *JLM=&JLM0[0];
gsl_sf_bessel_jl_steed_array(*lmax+1,*k*r,JLM);
/* CALCULATIONS OK
for(l=0;l<(*lmax+2);l++){
printf("%d\t%f\t%E\n",l,*k*r,JLM[l]);
}
*/
// Qlm - primeiros 4 termos
double Qlm[LMAXE];
Qlm[jlm(0, 0)]=1/sqrt(4*M_PI);
Qlm[jlm(1, 1)]=-gammaQ(1)*sth*Qlm[jlm(0,0)]; // Q11
Qlm[jlm(1, 0)]=sqrt(3.0)*cth*Qlm[jlm(0,0)]; // Q10
Qlm[jlm(1,-1)]=-Qlm[jlm(1,1)]; // Q11*(-1)
// Complex Exponencial for m=-1,0,1
double complex Eim[2*(*lmax)+3];
Eim[*lmax-1]=(cph-I*sph);
Eim[*lmax ]=1+I*0;
Eim[*lmax+1]=(cph+I*sph);
// Ylm - primeiros 4 termos
double complex Ylm[LMAXE];
Ylm[jlm(0, 0)]=Qlm[jlm(0, 0)];
Ylm[jlm(1,-1)]=Qlm[jlm(1,-1)]*Eim[*lmax-1];
Ylm[jlm(1, 0)]=Qlm[jlm(1, 0)];
Ylm[jlm(1, 1)]=Qlm[jlm(1, 1)]*Eim[*lmax+1];
/* OK jl, Qlm, Ylm
for(l=0;l<2;l++){
for(m=-l;m<=l;m++){
printf("%d\t%d\t%d\t%f\t%f\t%f+%fi\n",l,m,jlm(l,m),JLM[jlm(l,m)],Qlm[jlm(l,m)],creal(Ylm[jlm(l,m)]),cimag(Ylm[jlm(l,m)]));
}
}
printf("======================================================================\n");
*/
// VECTOR SPHERICAL HARMONICS
double complex XM; //[LMAX];
double complex XZ; //[LMAX];
double complex XP; //[LMAX];
double complex YM; //[LMAX];
double complex YZ; //[LMAX];
double complex YP; //[LMAX];
double complex VM; //[LMAX];
double complex VZ; //[LMAX];
double complex VP; //[LMAX];
// HANSEN MULTIPOLES
double complex MM; //[LMAX];
double complex MZ; //[LMAX];
double complex MP; //[LMAX];
double complex NM; //[LMAX];
double complex NZ; //[LMAX];
double complex NP; //[LMAX];
// OTHERS
double kl;
// MAIN LOOP
for(l=1;l<=(*lmax);l++){
//------------------------------------------------------------------------------
//Qlm extremos positivos um passo a frente
Qlm[jlm(l+1, l+1)]=-gammaQ(l+1)*sth*Qlm[jlm(l,l)];
Qlm[jlm(l+1, l )]= deltaQ(l+1)*cth*Qlm[jlm(l,l)];
//Qlm extremos negativos um passo a frente
Qlm[jlm(l+1,-l-1)]=pow(-1,l+1)*Qlm[jlm(l+1, l+1)];
Qlm[jlm(l+1,-l )]=pow(-1,l )*Qlm[jlm(l+1, l )];
// Exponenciais um passo a frente
Eim[*lmax+l+1]=Eim[*lmax+l]*(cph+I*sph);
Eim[*lmax-l-1]=Eim[*lmax-l]*(cph-I*sph);
// Harmonicos esfericos extremos um passo a frente
Ylm[jlm(l+1, l+1)]=Qlm[jlm(l+1, l+1)]*Eim[*lmax+l+1];
Ylm[jlm(l+1, l )]=Qlm[jlm(l+1, l )]*Eim[*lmax+l ];
Ylm[jlm(l+1,-l-1)]=Qlm[jlm(l+1,-l-1)]*Eim[*lmax-l-1];
Ylm[jlm(l+1,-l )]=Qlm[jlm(l+1,-l )]*Eim[*lmax-l ];
// others
kl=1/(sqrt(l*(l+1)));
for(m=0; m<l; m++){
// Demais valores de Qlm e Ylm
Qlm[jlm(l+1, m)]=alfaQ(l+1,m)*cth*Qlm[jlm(l,m)]-betaQ(l+1,m)*Qlm[jlm(l-1,m)];
Qlm[jlm(l+1,-m)]=pow(-1,m)*Qlm[jlm(l+1, m)];
Ylm[jlm(l+1, m)]=Qlm[jlm(l+1, m)]*Eim[*lmax+m];
Ylm[jlm(l+1,-m)]=Qlm[jlm(l+1,-m)]*Eim[*lmax-m];
}
//------------------------------------------------------------------------------
// for(m=-(l+1); m<=(l+1); m++){
// Ylm[jlm(l+1,m)]=Qlm[jlm(l+1,m)]*Eim[*lmax+m];
// }
// for(m=-l; m<=l; m++){
// printf("%d\t%d\t%d\t%f\t%f+%fi\t%f+%fi\n",l,m,jlm(l,m)+1,Qlm[jlm(l,m)],creal(Eim[*lmax+m]),cimag(Eim[*lmax+m]),creal(Ylm[jlm(l,m)]),cimag(Ylm[jlm(l,m)]));
// }
//------------------------------------------------------------------------------
for(int m=-l; m<=l; m++){
XM=kl*cm(l,m)*Ylm[jlm(l,m-1)]/sqrt(2);
XZ=kl*m*Ylm[jlm(l,m )];
XP=kl*cp(l,m)*Ylm[jlm(l,m+1)]/sqrt(2);
// printf("--------X--------------------------------\n");
printf("%d\t%d\t%d\t%f+%fi\n",l,m,jlm(l,m),creal(XM),cimag(XM));
// printf("%d\t%d\t%d\t%f+%fi\n",l,m,jlm(l,m),creal(XZ),cimag(XZ));
// printf("%d\t%d\t%d\t%f+%fi\n",l,m,jlm(l,m),creal(XP),cimag(XP));
YM=(-Kp(l,m,m-1)*Ylm[jlm(l,m-1)]+Km(l+1,m,m-1)*Ylm[jlm(l+1,m-1)])/sqrt(2);
YZ= Ko(l,m,m )*Ylm[jlm(l,m )]+Ko(l+1,m,m )*Ylm[jlm(l+1,m )];
YP=( Km(l,m,m+1)*Ylm[jlm(l,m+1)]-Kp(l+1,m,m+1)*Ylm[jlm(l+1,m+1)])/sqrt(2);
// printf("--------Y--------------------------------\n");
// printf("%d\t%d\t%d\t%f+%fi\n",l,m,jlm(l,m),creal(YM),cimag(YM));
// printf("%d\t%d\t%d\t%f+%fi\n",l,m,jlm(l,m),creal(YZ),cimag(YZ));
// printf("%d\t%d\t%d\t%f+%fi\n",l,m,jlm(l,m),creal(YP),cimag(YP));
VM=kl*(-(l+1)*Kp(l,m,m-1)*Ylm[jlm(l,m-1)]-l*Km(l+1,m,m-1)*Ylm[jlm(l+1,m-1)])/sqrt(2);
VZ=kl*( (l+1)*Ko(l,m,m )*Ylm[jlm(l,m )]-l*Ko(l+1,m,m )*Ylm[jlm(l+1,m )]);
VP=kl*( (l+1)*Km(l,m,m+1)*Ylm[jlm(l,m+1)]+l*Kp(l+1,m,m+1)*Ylm[jlm(l+1,m+1)])/sqrt(2);
// printf("--------V--------------------------------\n");
// printf("%d\t%d\t%d\t%f+%fi\n",l,m,jlm(l,m),creal(VM),cimag(VM));
// printf("%d\t%d\t%d\t%f+%fi\n",l,m,jlm(l,m),creal(VZ),cimag(VZ));
// printf("%d\t%d\t%d\t%f+%fi\n",l,m,jlm(l,m),creal(VP),cimag(VP));
// CALCULATION OF HANSEM MULTIPOLES
MM=JLM[l]*XM;
MZ=JLM[l]*XZ;
MP=JLM[l]*XP;
// printf("--------M--------------------------------\n");
// printf("%d\t%d\t%d\t%f+%fi\n",l,m,jlm(l,m),creal(MM),cimag(MM));
// printf("%d\t%d\t%d\t%f+%fi\n",l,m,jlm(l,m),creal(MZ),cimag(MZ));
// printf("%d\t%d\t%d\t%f+%fi\n",l,m,jlm(l,m),creal(MP),cimag(MP));
NM=((l+1)*JLM[l-1]-l*JLM[l+1])*VM/(2*l+1)+sqrt(l*(l+1))*JLM[l]*YM;
NZ=((l+1)*JLM[l-1]-l*JLM[l+1])*VZ/(2*l+1)+sqrt(l*(l+1))*JLM[l]*YZ;
NP=((l+1)*JLM[l-1]-l*JLM[l+1])*VP/(2*l+1)+sqrt(l*(l+1))*JLM[l]*YP;
// printf("--------N--------------------------------\n");
// printf("%d\t%d\t%d\t%f+%fi\n",l,m,jlm(l,m),creal(NM),cimag(NM));
// printf("%d\t%d\t%d\t%f+%fi\n",l,m,jlm(l,m),creal(NZ),cimag(NZ));
// printf("%d\t%d\t%d\t%f+%fi\n",l,m,jlm(l,m),creal(NP),cimag(NP));
// CALCULATION OF THE ELECTROMAGNETIC FIELDS
*Em=*Em+MM*GTE[jlm(l,m)]-NM*GTM[jlm(l,m)];
*Ez=*Ez+MZ*GTE[jlm(l,m)]-NZ*GTM[jlm(l,m)];
*Ep=*Ep+MP*GTE[jlm(l,m)]-NP*GTM[jlm(l,m)];
*Hm=*Hm+MM*GTM[jlm(l,m)]+NM*GTE[jlm(l,m)];
*Hz=*Hz+MZ*GTM[jlm(l,m)]+NZ*GTE[jlm(l,m)];
*Hp=*Hp+MP*GTM[jlm(l,m)]+NP*GTE[jlm(l,m)];
}
}
}
Eigen::VectorXd controlEnv::mobile_manip_inverse_kinematics_siciliano(void){
// Siciliano; modelling, planning and control; pag 139
// --------------------------------------------------------------------------------
// std::cout << "AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA: " << n_iteration_ << std::endl;
// Position
VectorXd p_err(3);
for (unsigned int i=0; i<3; i++) p_err(i) = pose_error_.p()(i);
MatrixXd Kp(3,3);
Kp = MatrixXd::Zero(3,3);
Kp(0,0) = 1.0;
Kp(1,1) = 1.0;
Kp(2,2) = 1.0;
VectorXd vd(3);
desired_pose_velocity_ = compute_pose_velocity(desired_pose_, past_desired_pose_, time_increment_);
vd = desired_pose_velocity_.p();
VectorXd v_p(3);
v_p = vd + Kp * p_err;
// Orientation
Quaternion<double> rot_current, rot_desired;
rot_current = mobile_manip_.tcp_pose().o();
rot_desired = desired_pose_.o();
MatrixXd L(3,3);
L = Lmat(rot_current, rot_desired);
MatrixXd Sne(3,3), Sse(3,3), Sae(3,3);
Sne = cross_product_matrix_S(rot_current.toRotationMatrix().col(0));
Sse = cross_product_matrix_S(rot_current.toRotationMatrix().col(1));
Sae = cross_product_matrix_S(rot_current.toRotationMatrix().col(2));
VectorXd nd(3), sd(3), ad(3);
nd = rot_desired.toRotationMatrix().col(0);
sd = rot_desired.toRotationMatrix().col(1);
ad = rot_desired.toRotationMatrix().col(2);
VectorXd o_err(3);
o_err = 0.5 * ( Sne*nd + Sse*sd + Sae*ad );
MatrixXd Ko(3,3);
Ko = MatrixXd::Zero(3,3);
Ko(0,0) = 1.0;
Ko(1,1) = 1.0;
Ko(2,2) = 1.0;
VectorXd wd(3);
wd = desired_pose_velocity_.o_angaxis_r3();
VectorXd v_o(3);
v_o = L.inverse() * (L.transpose() * wd + Ko * o_err );
// Put all together
VectorXd v(6);
for (unsigned int i=0; i<3; i++){
v(i) = v_p(i);
v(i+3) = v_o(i);
}
return pinv(jacobian(mobile_manip_), 0.001)*v;
}
void Solver::TripleStates_Parallel(){
// %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
// %%%%%%%%%%%%%%% SETTING UP K_h AND K_pph %%%%%%%%%%%%%%%%%%%%%%%
// %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
Kh = zeros<mat>(0,2); Khpp = zeros<mat>(0,4);
int n=0;
for (int i=0; i<Nholes; i++){
int Nx = basis.States(i,1); // Combining x-momentum
int Ny = basis.States(i,2); // Combining y-momentum
int Nz = basis.States(i,3); // Combining z-momentum
int Sz = basis.States(i,4); // Combining spin
// Adding a new two-hole-state configuration to matrix. (i, j, Identifier)
Kh.insert_rows(n,1);
Kh(n,0) = i; Kh(n,1) = Identifier(Nx,Ny,Nz,Sz);
n++;
}
#pragma omp parallel
{
int id = omp_get_thread_num();
int nthreads = omp_get_num_threads();
// Setting up size for the partial Holes matrix. This size is more deeply explained in the thesis.
int size = floor( Nparticles/nthreads) * Nholes*(Nparticles-1);
if ( id < Nparticles%nthreads) size += Nholes*(Nparticles-1);
mat partialStates = zeros<mat>(size,4);
int n=0;
for (int aa=id; aa<Nparticles; aa += nthreads){
for (int i=0; i<Nholes; i++){
for (int bb=0; bb<Nparticles; bb++){
if (aa != bb){
int a=aa+Nholes; int b=bb+Nholes;
int Nx = basis.States(a,1) + basis.States(b,1) - basis.States(i,1);
int Ny = basis.States(a,2) + basis.States(b,2) - basis.States(i,2);
int Nz = basis.States(a,3) + basis.States(b,3) - basis.States(i,3);
int Sz = basis.States(a,4) + basis.States(b,4) - basis.States(i,4);
partialStates(n,0) = i; partialStates(n,1) = a; partialStates(n,2) = b; partialStates(n,3) = Identifier(Nx,Ny,Nz,Sz);
n++;
}
}
}
}
#pragma omp critical
{
Khpp.insert_rows(0,partialStates);
}
}
NKh3 = Khpp.n_rows; NKh = Kh.n_rows;
// %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
// %%%%%%%%%%%%%%%%% SETTING UP K_p AND K_phh STATES %%%%%%%%%%%%%%%%%%%%%%
// %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
Kp = zeros<mat>(0,2); Kphh = zeros<mat>(0,4);
n=0;
for (int aa=0; aa<Nparticles; aa++){
int a = aa+Nholes;
int Nx = basis.States(a,1); // Combining x-momentum
int Ny = basis.States(a,2); // Combining y-momentum
int Nz = basis.States(a,3); // Combining z-momentum
int Sz = basis.States(a,4); // Combining spin
// Adding a new two-hole-state configuration to matrix. (i, j, Identifier)
Kp.insert_rows(n,1);
Kp(n,0) = a; Kp(n,1) = Identifier(Nx,Ny,Nz,Sz);
n++;
}
#pragma omp parallel
{
int id = omp_get_thread_num();
int nthreads = omp_get_num_threads();
// Setting up size for the partial Holes matrix. This size is more deeply explained in the thesis.
int size = floor( Nholes/nthreads) * Nparticles*(Nholes-1);
if ( id < Nholes%nthreads) size += Nparticles*(Nholes-1);
mat partialStates = zeros<mat>(size,4);
int n=0;
for (int i=id; i<Nholes; i+=nthreads){
for (int j=0; j<Nholes; j++){
for (int aa=0; aa<Nparticles; aa++){
if (i != j){
int a=aa+Nholes;
int Nx = basis.States(i,1) + basis.States(j,1) - basis.States(a,1);
int Ny = basis.States(i,2) + basis.States(j,2) - basis.States(a,2);
int Nz = basis.States(i,3) + basis.States(j,3) - basis.States(a,3);
int Sz = basis.States(i,4) + basis.States(j,4) - basis.States(a,4);
partialStates(n,0) = a; partialStates(n,1) = i; partialStates(n,2) = j; partialStates(n,3) = Identifier(Nx,Ny,Nz,Sz);
n++;
}
}
}
}
#pragma omp critical
{
Kphh.insert_rows(0,partialStates);
}
}
NKp3 = Kphh.n_rows; NKp = Kp.n_rows;
}
