void muonContained(double*nu,double*Nu,double rho, double*mu) { int i,k,l; double C; double NA=6.002141E23 /* mol-1*/; double Nrate[2]={0.5,0.5}; /* proton , neutron */ double*Sp[2]={nu,Nu}; nuSpectrum=tabNuSpectrum; mu[0]=0; for(i=1;i<NZ;i++) { Emu_stat=Mcdm*exp(Zi(i)); mu[i]=0; if(Emu_stat>0.01) for(k=0;k<2;k++) for(l=0;l<2;l++) { inNu=1-2*k; if(Sp[k]) { SpN_stat=Sp[k]; inPr=-1+2*l; mu[i]+=simpson(integrandEnuContained,1/Mcdm, 1/Emu_stat,1.E-4)*Nrate[l]; } } mu[i]*=rho*NA*Emu_stat*1E5; } }
int KinematicSolver::forward(btTransform& pose) const { TRACER_ENTER_SCOPE("KinematicSolver::forward()"); if (false && forwardKinTimestamp_ == arm_->timestamp()) { pose = forwardKinCached_; return 0; } Arm::IdType armId_new = arm_->id(); int armId = armIdFromSerial(arm_->id()); pose = actual_world_to_ik_world(armId) * Tw2b * Zs(THS_TO_IK(armId,arm_->getJointById(Joint::IdType::SHOULDER_)->position())) * Xu * Ze(THE_TO_IK(armId,arm_->getJointById(Joint::IdType::ELBOW_)->position())) * Xf * Zr(THR_TO_IK(armId,arm_->getJointById(Joint::IdType::ROTATION_)->position())) * Zi(D_TO_IK(armId,arm_->getJointById(Joint::IdType::INSERTION_)->position())) * Xip * Zp(THP_TO_IK(armId,arm_->getJointById(Joint::IdType::WRIST_)->position())) * Xpy * Zy(THY_TO_IK_FROM_FINGERS(armId,arm_->getJointById(Joint::IdType::FINGER1_)->position(),arm_->getJointById(Joint::IdType::FINGER2_)->position())) * Tg; //int grasp = MECH_GRASP_FROM_MECH_FINGERS(armId,arm_->getJointById(Joint::Type::GRIPPER1_)->position(),arm_->getJointById(Joint::Type::GRIPPER2_)->position()); const_cast<KinematicSolver*>(this)->forwardKinCached_ = pose; const_cast<KinematicSolver*>(this)->forwardKinTimestamp_ = arm_->timestamp(); return 0; }
int basicNuSpectra(int forSun, int pdgN, int outN, double * tab) { int inP,i,j; int N=abs(pdgN); double tab100[100]; double c=1; for(i=0;i<NZ;i++) tab[i]=0; // if(N==12 ||N==14 ||N==16) { if( pdgN*outN<0) return 0; else c=1;} switch(N) { case 1: case 2: case 3: inP=4; break; case 4: inP=3; break; case 5: inP=1; break; case 6: inP=5; break; case 15: inP=2; break; /*l */ case 23: inP=6; break; /*z */ case 24: inP=7; break; /*w */ case 12: case 14: case 16: if(forSun) inP=0; else { if((pdgN==14 && outN>0)||(pdgN==-14 && outN<0)) tab[0]=2/(Zi(0)-Zi(1)); return 0; } /*nu */ break; default: return 1; } if(forSun) mInterpSun(Mcdm,inP,-(outN-1)/2,tab100); else mInterpEarth(Mcdm,inP,-(outN-1)/2,tab100); for(i=0;i<NZ;i++) { double x=exp(Zi(i)); tab[i]+=c*x*polint3(x,100, xTab ,tab100); } return 0; }
void solarModulation(double PHI, double mass, double * inTab, double * outTab) { double buff[NZ]; int i; for(i=0;i<NZ;i++) { double X,E,P,Xs,Es,Ps; X=exp(Zi(i)); E=Mcdm*X; P=sqrt( E*(E+2*mass)); Es=E+PHI/1000.; if(Es>=Mcdm) buff[i]=0; else { Ps=sqrt( Es*(Es+2*mass)); Xs=Es/Mcdm; buff[i]= (X/Xs)*zInterp(log(Xs),inTab)*pow(P/Ps,2); } } for(i=0;i<NZ;i++) outTab[i]=buff[i]; }
void pbarFluxTab(double Emin, double sigmav, double *tab, double *tabOut) { int i; int N; double * Egrid,*Fgrid; double tab2[NZ]; double rho0; buildInterpolation(logPbarRate,log( Emin/Mcdm),log(0.9),0.01,&N,&Egrid,&Fgrid); /*printf("Npbar=%d\n",N);*/ rho0=rhoDM/hProfile_(Rsun)/Mcdm; for(i=0;i<NZ;i++) { double z=Zi(i); double E=Mcdm*exp(z); if(E<Emin*0.9) tab2[i]=0; else tab2[i]= sigmav*exp(polint4(z,N, Egrid, Fgrid) )*zInterp(z,tab); } for(i=0;i<NZ;i++) tabOut[i]=rho0*rho0*tab2[i]; free(Egrid); free(Fgrid); }
void posiFluxTab(double Emin, double sigmav, double *tab, double *tabOut) { double flu,rho0; int i; double buff[NZ]; tab_ =tab; rho0=rhoDM/hProfile_(Rsun)/Mcdm; flu = Tau_dif*sigmav/(4*M_PI)*CELERITY_LIGHT; buildInterpolation(integral_cal_In,0., dKt(Mcdm,Emin),-Eps,&N_,&xa_,&ya_); //printf("N_=%d\n",N_); for(i=0;i<NZ;i++) { Eobs=Mcdm*exp(Zi(i)); if(Eobs<Emin*0.9) buff[i]=0; else buff[i]= (flu/Eobs)* simpson(SpectIntegrand, Eobs, Mcdm, Eps); } for(i=0;i<NZ;i++) tabOut[i]=rho0*rho0*buff[i]; free(xa_); free(ya_); }
void ISVDMultiCD::makePass() { Epetra_LAPACK lapack; Epetra_BLAS blas; bool firstPass = (curRank_ == 0); const int numCols = A_->NumVectors(); TEUCHOS_TEST_FOR_EXCEPTION( !firstPass && (numProc_ != numCols), std::logic_error, "RBGen::ISVDMultiCD::makePass(): after first pass, numProc should be numCols"); // compute W = I - Z T Z^T from current V_ Teuchos::RCP<Epetra_MultiVector> lclAZT, lclZ; double *Z_A, *AZT_A; int Z_LDA, AZT_LDA; int oldRank = 0; double Rerr = 0.0; if (!firstPass) { // copy V_ into workZ_ lclAZT = Teuchos::rcp( new Epetra_MultiVector(::View,*workAZT_,0,curRank_) ); lclZ = Teuchos::rcp( new Epetra_MultiVector(::View,*workZ_,0,curRank_) ); { Epetra_MultiVector lclV(::View,*V_,0,curRank_); *lclZ = lclV; } // compute the Householder QR factorization of the current right basis // Vhat = W*R int info, lwork = curRank_; std::vector<double> tau(curRank_), work(lwork); info = lclZ->ExtractView(&Z_A,&Z_LDA); TEUCHOS_TEST_FOR_EXCEPTION(info != 0, std::logic_error, "RBGen::ISVDMultiCD::makePass(): error calling ExtractView on Epetra_MultiVector Z."); lapack.GEQRF(numCols,curRank_,Z_A,Z_LDA,&tau[0],&work[0],lwork,&info); TEUCHOS_TEST_FOR_EXCEPTION(info != 0, std::logic_error, "RBGen::ISVDMultiCD::makePass(): error calling GEQRF on current right basis while constructing next pass coefficients."); if (debug_) { // we just took the QR factorization of a set of orthonormal vectors // they should have an R factor which is diagonal, with unit elements (\pm 1) // check it Rerr = 0.0; for (int j=0; j<curRank_; j++) { for (int i=0; i<j; i++) { Rerr += abs(Z_A[j*Z_LDA+i]); } Rerr += abs(abs(Z_A[j*Z_LDA+j]) - 1.0); } } // compute the block representation // W = I - Z T Z^T lapack.LARFT('F','C',numCols,curRank_,Z_A,Z_LDA,&tau[0],workT_->A(),workT_->LDA()); // LARFT left upper tri block of Z unchanged // note: it should currently contain R factor of V_, which is very close to // diag(\pm 1, ..., \pm 1) // // we need to set it to: // [1 0 0 ... 0] // [ 1 0 ... 0] // [ .... ] // [ 1] // // see documentation for LARFT // for (int j=0; j<curRank_; j++) { Z_A[j*Z_LDA+j] = 1.0; for (int i=0; i<j; i++) { Z_A[j*Z_LDA+i] = 0.0; } } // compute part of A W: A Z T // put this in workAZT_ // first, A Z info = lclAZT->Multiply('N','N',1.0,*A_,*lclZ,0.0); TEUCHOS_TEST_FOR_EXCEPTION(info != 0,std::logic_error, "RBGen::ISVDMultiCD::makePass(): Error calling Epetra_MultiVector::Multiply() for A*Z"); // second, (A Z) T (in situ, as T is upper triangular) info = lclAZT->ExtractView(&AZT_A,&AZT_LDA); TEUCHOS_TEST_FOR_EXCEPTION(info != 0, std::logic_error, "RBGen::ISVDMultiCD::makePass(): error calling ExtractView on Epetra_MultiVector AZ."); blas.TRMM('R','U','N','N',numCols,curRank_,1.0,workT_->A(),workT_->LDA(),AZT_A,AZT_LDA); // save oldRank: it tells us the width of Z oldRank = curRank_; curRank_ = 0; numProc_ = 0; } else { // firstPass == true curRank_ = 0; numProc_ = 0; } while (numProc_ < numCols) { // // determine lup // // want lup >= lmin // lup <= lmax // need lup <= numCols - numProc // lup <= maxBasisSize - curRank // int lup; if (curRank_ == 0) { // first step uses startRank_ // this is not affected by lmin,lmax lup = startRank_; } else { // this value minimizes overall complexity, assuming fixed rank lup = (int)(curRank_ / Teuchos::ScalarTraits<double>::squareroot(2.0)); // contrain to [lmin,lmax] lup = (lup < lmin_ ? lmin_ : lup); lup = (lup > lmax_ ? lmax_ : lup); } // // now cap lup via maxBasisSize and the available data // these caps apply to all lup, as a result of memory and data constraints // // available data lup = (lup > numCols - numProc_ ? numCols - numProc_ : lup); // available memory lup = (lup > maxBasisSize_ - curRank_ ? maxBasisSize_ - curRank_ : lup); // get view of new vectors { const Epetra_MultiVector Aplus(::View,*A_,numProc_,lup); Epetra_MultiVector Unew(::View,*U_,curRank_,lup); // put them in U if (firstPass) { // new vectors are just Aplus Unew = Aplus; } else { // new vectors are Aplus - (A Z T) Z_i^T // specifically, Aplus - (A Z T) Z(numProc:numProc+lup-1,1:oldRank)^T Epetra_LocalMap lclmap(lup,0,A_->Comm()); Epetra_MultiVector Zi(::View,lclmap,&Z_A[numProc_],Z_LDA,oldRank); Unew = Aplus; int info = Unew.Multiply('N','T',-1.0,*lclAZT,Zi,1.0); TEUCHOS_TEST_FOR_EXCEPTION(info != 0,std::logic_error, "RBGen::ISVDMultiCD::makePass(): Error calling Epetra_MultiVector::Multiply() for A*Wi"); } } // perform the incremental step incStep(lup); } // compute W V = V - Z T Z^T V // Z^T V is oldRank x curRank // T Z^T V is oldRank x curRank // we need T Z^T V in a local Epetra_MultiVector if (!firstPass) { Teuchos::RCP<Epetra_MultiVector> lclV; double *TZTV_A; int TZTV_LDA; int info; Epetra_LocalMap lclmap(oldRank,0,A_->Comm()); // get pointer to current V lclV = Teuchos::rcp( new Epetra_MultiVector(::View,*V_,0,curRank_) ); // create space for T Z^T V Epetra_MultiVector TZTV(lclmap,curRank_,false); // multiply Z^T V info = TZTV.Multiply('T','N',1.0,*lclZ,*lclV,0.0); TEUCHOS_TEST_FOR_EXCEPTION(info != 0,std::logic_error, "RBGen::ISVDMultiCD::makePass(): Error calling Epetra_MultiVector::Multiply() for Z^T V."); // get pointer to data in Z^T V info = TZTV.ExtractView(&TZTV_A,&TZTV_LDA); TEUCHOS_TEST_FOR_EXCEPTION(info != 0, std::logic_error, "RBGen::ISVDMultiCD::makePass(): error calling ExtractView on Epetra_MultiVector TZTV."); // multiply T (Z^T V) blas.TRMM('L','U','N','N',oldRank,curRank_,1.0,workT_->A(),workT_->LDA(),TZTV_A,TZTV_LDA); // multiply V - Z (T Z^T V) info = lclV->Multiply('N','N',-1.0,*lclZ,TZTV,1.0); TEUCHOS_TEST_FOR_EXCEPTION(info != 0,std::logic_error, "RBGen::ISVDMultiCD::makePass(): Error calling Epetra_MultiVector::Multiply() for W V."); } // // compute the new residuals // we know that A V = U S // if, in addition, A^T U = V S, then have singular subspaces // check residuals A^T U - V S, scaling the i-th column by sigma[i] // { // make these static, because makePass() will be likely be called again static Epetra_LocalMap lclmap(A_->NumVectors(),0,A_->Comm()); static Epetra_MultiVector ATU(lclmap,maxBasisSize_,false); // we know that A V = U S // if, in addition, A^T U = V S, then have singular subspaces // check residuals A^T U - V S, scaling the i-th column by sigma[i] Epetra_MultiVector ATUlcl(::View,ATU,0,curRank_); Epetra_MultiVector Ulcl(::View,*U_,0,curRank_); Epetra_MultiVector Vlcl(::View,*V_,0,curRank_); // compute A^T U int info = ATUlcl.Multiply('T','N',1.0,*A_,Ulcl,0.0); TEUCHOS_TEST_FOR_EXCEPTION(info != 0, std::logic_error, "RBGen::ISVDMultiCD::makePass(): Error calling Epetra_MultiVector::Multiply for A^T U."); Epetra_LocalMap rankmap(curRank_,0,A_->Comm()); Epetra_MultiVector S(rankmap,curRank_,true); for (int i=0; i<curRank_; i++) { S[i][i] = sigma_[i]; } // subtract V S from A^T U info = ATUlcl.Multiply('N','N',-1.0,Vlcl,S,1.0); TEUCHOS_TEST_FOR_EXCEPTION(info != 0, std::logic_error, "RBGen::ISVDMultiCD::computeBasis(): Error calling Epetra_MultiVector::Multiply for V S."); resNorms_.resize(curRank_); ATUlcl.Norm2(&resNorms_[0]); // scale by sigmas for (int i=0; i<curRank_; i++) { if (sigma_[i] != 0.0) { resNorms_[i] /= sigma_[i]; } } } // debugging checks std::vector<double> errnorms(curRank_); if (debug_) { int info; // Check that A V = U Sigma // get pointers to current U and V, create workspace for A V - U Sigma Epetra_MultiVector work(U_->Map(),curRank_,false), curU(::View,*U_,0,curRank_), curV(::View,*V_,0,curRank_); // create local MV for sigmas Epetra_LocalMap lclmap(curRank_,0,A_->Comm()); Epetra_MultiVector curS(lclmap,curRank_,true); for (int i=0; i<curRank_; i++) { curS[i][i] = sigma_[i]; } info = work.Multiply('N','N',1.0,curU,curS,0.0); TEUCHOS_TEST_FOR_EXCEPTION(info != 0,std::logic_error, "RBGen::ISVDMultiCD::makePass(): Error calling Epetra_MultiVector::Multiply() for debugging U S."); info = work.Multiply('N','N',-1.0,*A_,curV,1.0); TEUCHOS_TEST_FOR_EXCEPTION(info != 0,std::logic_error, "RBGen::ISVDMultiCD::makePass(): Error calling Epetra_MultiVector::Multiply() for debugging U S - A V."); work.Norm2(&errnorms[0]); for (int i=0; i<curRank_; i++) { if (sigma_[i] != 0.0) { errnorms[i] /= sigma_[i]; } } } // update pass counter curNumPasses_++; // print out some info const Epetra_Comm *comm = &A_->Comm(); if (comm->MyPID() == 0 && verbLevel_ >= 1) { std::cout << "------------- ISVDMultiCD::makePass() -----------" << std::endl << "| Number of passes: " << curNumPasses_ << std::endl << "| Current rank: " << curRank_ << std::endl << "| Current sigmas: " << std::endl; for (int i=0; i<curRank_; i++) { std::cout << "| " << sigma_[i] << std::endl; } if (debug_) { std::cout << "|DBG US-AV norms: " << std::endl; for (int i=0; i<curRank_; i++) { std::cout << "|DBG " << errnorms[i] << std::endl; } if (!firstPass) { std::cout << "|DBG R-I norm: " << Rerr << std::endl; } } } return; }
extern SITE *get_sites( DATASET *dataset, /* the dataset */ MODEL *model, /* the model */ int *n, /* number of sites found */ int *best_site /* index of best site in array */ ) { int i, j; int nsites = 0; /* number of sites */ SITE *sites = NULL; /* list of sites */ int n_samples = dataset->n_samples; /* number of sequences */ SAMPLE **samples = dataset->samples; /* the sequences */ MOTYPE mtype = model->mtype; /* type of model */ int w = model->w; /* width of motif */ BOOLEAN invcomp = model->invcomp; /* using invcomp strand */ double best_z = -1; /* best overall zij */ if (mtype==Oops || mtype==Zoops) { /* (Z)OOPS model */ for (i=0; i<n_samples; i++) { /* sequence */ SAMPLE *s = samples[i]; /* sample */ int lseq = s->length; /* length of sequence */ double max_zij = -1; /* flag no z_ij found */ int best_j = 0; /* position of site (1..m) */ double *zi = s->z; // z_i[j], j in [-lseq...+lseq] if (lseq < w) continue; /* sequence to short for motif */ /* find maximum z_ij */ for (j=0; j<lseq-w+1; j++) { /* position */ int k = j+1; // Z_i = k double zij = invcomp ? MIN(1.0,Zi(-k)+Zi(k)) : Zi(k); if (zij > max_zij) { /* bigger found */ max_zij = zij; best_j = j; } } // Z_i = j /* record site */ if ((max_zij && mtype == Oops) || max_zij > 0.5) { /* site found */ if (nsites % 100 == 0) Resize(sites, nsites+101, SITE); sites[nsites].seqno = i; sites[nsites].pos = best_j; sites[nsites].zij = max_zij; int best_k = best_j + 1; // Z_i = best_k // on reverse complement strand? sites[nsites].invcomp = (invcomp & (Zi(-best_k)>Zi(best_k))); if (max_zij > best_z) { *best_site = nsites; best_z = max_zij; } nsites++; } /* site found */ } /* sequence */ } else { /* TCM model */ for (i=0; i<n_samples; i++) { /* sequence */ SAMPLE *s = samples[i]; /* sample */ int lseq = s->length; /* length of sequence */ double *zi = s->z; // z_i[j], j in [-lseq...+lseq] if (lseq < w) continue; /* sequence to short for motif */ /* find all z_ij > 0.5 */ for (j=0; j<lseq-w+1; j++) { /* position */ int k = j+1; // Z_i = k double zij = invcomp ? MIN(1.0,Zi(-k)+Zi(k)) : Zi(k); /* z_ij */ if (zij > 0.5) { /* record site */ if (nsites % 100 == 0) Resize(sites, nsites+101, SITE); sites[nsites].seqno = i; sites[nsites].pos = j; sites[nsites].zij = zij; /* on reverse complement strand? */ sites[nsites].invcomp = (invcomp && (Zi(-k)>Zi(k))); if (zij > best_z) { *best_site = nsites; best_z = zij; } nsites++; } /* record site */ } /* position */ } /* sequence */ } /* TCM model */ /* return results */ *n = nsites; return sites; } /* get_sites */
InverseKinematicsReportPtr KinematicSolver::internalInverseSoln(const btTransform& pose, Arm* arm,const InverseKinematicsOptions& options) const { TRACER_ENTER_SCOPE("KinematicSolver::internalInverseSoln(arm@%p)",arm); InverseKinematicsReportPtr report(new InverseKinematicsReport()); Arm::IdType armId_new = arm_->id(); int armId = armIdFromSerial(arm_->id()); // desired tip position // btVector3 currentPoint = btVector3(mech->pos.x,mech->pos.y,mech->pos.z) / MICRON_PER_M; // btVector3 actualPoint = btVector3(pos_d->x,pos_d->y,pos_d->z) / MICRON_PER_M; // btMatrix3x3 actualOrientation = toBt(ori_d->R); // btTransform actualPose(actualOrientation,actualPoint); // btTransform actualPose_fk = pose; // tb_angles currentPoseAngles = tb_angles(mech->ori.R); // tb_angles actualPoseAngles = tb_angles(ori_d->R); //float grasp = GRASP_TO_IK(armId,mech->ori_d.grasp); float grasp = arm->getJointById(Joint::IdType::GRASP_)->position(); // if (print) { // log_msg("j s % 2.1f e % 2.1f r % 2.1f i % 1.3f p % 2.1f y % 2.1f g % 2.1f g1 % 2.1f g2 % 2.1f", // arm->getJointById(Joint::Type::SHOULDER_)->position() RAD2DEG, // arm->getJointById(Joint::Type::ELBOW_)->position() RAD2DEG, // arm->getJointById(Joint::Type::TOOL_ROT_)->position() RAD2DEG, // arm->getJointById(Joint::Type::INSERTION__)->position(), // arm->getJointById(Joint::Type::WRIST_)->position() RAD2DEG, // fix_angle(arm->getJointById(Joint::Type::GRIPPER1_)->position() - arm->getJointById(Joint::Type::GRIPPER2_)->position()) / 2 RAD2DEG, // (arm->getJointById(Joint::Type::GRIPPER1_)->position() + arm->getJointById(Joint::Type::GRIPPER2_)->position()) RAD2DEG, // arm->getJointById(Joint::Type::GRIPPER1_)->position() RAD2DEG,arm->getJointById(Joint::Type::GRIPPER2_)->position() RAD2DEG); // log_msg("v s % 2.1f e % 2.1f r % 2.1f i % 1.3f p % 2.1f y % 2.1f g % 2.1f g1 % 2.1f g2 % 2.1f", // arm->getJointById(Joint::Type::SHOULDER].jvel RAD2DEG, // arm->getJointById(Joint::Type::ELBOW].jvel RAD2DEG, // arm->getJointById(Joint::Type::TOOL_ROT].jvel RAD2DEG, // arm->getJointById(Joint::Type::INSERTION_].jvel, // arm->getJointById(Joint::Type::WRIST].jvel RAD2DEG, // (arm->getJointById(Joint::Type::GRASP1].jvel - arm->getJointById(Joint::Type::GRASP2].jvel) / 2 RAD2DEG, // arm->getJointById(Joint::Type::GRASP1].jvel + arm->getJointById(Joint::Type::GRASP2].jvel RAD2DEG, // arm->getJointById(Joint::Type::GRASP1].jvel RAD2DEG,arm->getJointById(Joint::Type::GRASP2].jvel RAD2DEG); // log_msg("t s % 1.3f e % 1.3f r % 1.3f i % 1.3f p % 1.3f g1 % 1.3f g2 % 1.3f",arm->getJointById(Joint::Type::SHOULDER].tau_d, // arm->getJointById(Joint::Type::ELBOW].tau_d,arm->getJointById(Joint::Type::TOOL_ROT].tau_d,arm->getJointById(Joint::Type::INSERTION_].tau_d,arm->getJointById(Joint::Type::WRIST].tau_d, // arm->getJointById(Joint::Type::GRASP1].tau_d,arm->getJointById(Joint::Type::GRASP2].tau_d); // log_msg("d s %d e %d r %d i %d p %d g1 %d g2 %d",tToDACVal(&(arm->getJointById(Joint::Type::SHOULDER])), // tToDACVal(&(arm->getJointById(Joint::Type::ELBOW])),tToDACVal(&(arm->getJointById(Joint::Type::TOOL_ROT])),tToDACVal(&(arm->getJointById(Joint::Type::INSERTION_])),tToDACVal(&(arm->getJointById(Joint::Type::WRIST])), // tToDACVal(&(arm->getJointById(Joint::Type::GRASP1])),tToDACVal(&(arm->getJointById(Joint::Type::GRASP2]))); // log_msg("cp (% 1.3f,% 1.3f,% 1.3f\typr (% 1.3f,% 1.3f,% 1.3f))", // currentPoint.x(),currentPoint.y(),currentPoint.z(), // currentPoseAngles.yaw_deg,currentPoseAngles.pitch_deg,currentPoseAngles.roll_deg); // log_msg("pt (% 1.3f,% 1.3f,% 1.3f)\typr (% 1.3f,% 1.3f,% 1.3f)\tg % 1.3f", // actualPoint.x(),actualPoint.y(),actualPoint.z(), // actualPoseAngles.yaw_deg,actualPoseAngles.pitch_deg,actualPoseAngles.roll_deg, // grasp); // // btVector3 point = actualPose_fk.getOrigin(); // tb_angles angles = tb_angles(actualPose_fk.getBasis()); // log_msg("fp (% 1.3f,% 1.3f,% 1.3f)\typr (% 2.1f,% 2.1f,% 2.1f)\tg % 1.3f", // point.x(),point.y(),point.z(), // angles.yaw_deg,angles.pitch_deg,angles.roll_deg, // grasp); // // } /* * Actual pose is in the actual world frame, so we have <actual_world to gripper> * The ik world frame is the base frame. * Therefore, we need <base to actual_world> to get <base to gripper>. * <base to actual_world> is inverse of <actual_world to base> * * Since the ik is based on the yaw frame (to which the gripper is fixed), we * take the pose of the yaw frame, not the gripper frame */ btTransform ik_pose = ik_world_to_actual_world(armId) * pose * Tg.inverse(); btMatrix3x3 ik_orientation = ik_pose.getBasis(); btVector3 ik_point = ik_pose.getOrigin(); tb_angles ikPoseAngles = tb_angles(ik_pose); // if (print) { // log_msg("ik (%0.4f,%0.4f,%0.4f)\typr (%0.4f,%0.4f,%0.4f)", // ik_point.x(),ik_point.y(),ik_point.z(), // ikPoseAngles.yaw_deg,ikPoseAngles.pitch_deg,ikPoseAngles.roll_deg); // } float ths_offset, thr_offset, thp_offset; if (arm->isGold()) { ths_offset = SHOULDER_OFFSET_GOLD; thr_offset = TOOL_ROT_OFFSET_GOLD; thp_offset = WRIST_OFFSET_GOLD; } else { ths_offset = SHOULDER_OFFSET_GREEN; thr_offset = TOOL_ROT_OFFSET_GREEN; thp_offset = WRIST_OFFSET_GREEN; } const float th12 = THETA_12; const float th23 = THETA_23; const float ks12 = sin(th12); const float kc12 = cos(th12); const float ks23 = sin(th23); const float kc23 = cos(th23); const float dw = DW; btTransform Tworld_to_gripper = ik_pose; btTransform Tgripper_to_world = ik_pose.inverse(); // if (print) { // log_msg("Tw2g: %d",PRINT_EVERY); // for (int i=0;i<3;i++) { // log_msg(" %0.4f\t%0.4f\t%0.4f\t%0.4f",ik_pose.getBasis()[i][0],ik_pose.getBasis()[i][1],ik_pose.getBasis()[i][2],ik_pose.getOrigin()[i]); // } // } btVector3 origin_in_gripper_frame = Tgripper_to_world.getOrigin(); float px = origin_in_gripper_frame.x(); float py = origin_in_gripper_frame.y(); float pz = origin_in_gripper_frame.z(); float thy = atan2f(py,-px); float thp; if (fabs(thy) < 0.001) { thp = atan2f(-pz, -px/cos(thy) - dw); // if (print) { log_msg("zero thy: %0.4f, (%0.4f, %0.4f, %0.4f)",thp,px,py,pz); } } else { thp = atan2f(-pz, py/sin(thy) - dw); } float d = -pz / sin(thp); float d_act, thp_act, thy_act, g1_act, g2_act; d_act = D_FROM_IK(armId,d); thp_act = THP_FROM_IK(armId,thp); thy_act = THY_FROM_IK(armId,thy,grasp); g1_act = FINGER1_FROM_IK(armId,thy,grasp); g2_act = FINGER2_FROM_IK(armId,thy,grasp); //check angles int validity1[4]; bool valid1 = checkJointLimits1(d_act,thp_act,g1_act,g2_act,validity1); if (!valid1) { // if (_curr_rl == 3 && !(DISABLE_ALL_PRINTING)) { // printf("ik %d invalid --1-- d [%d] % 2.4f \tp [%d] % 3.1f\ty [%d %d] % 3.1f\n", // armId, // validity1[0], d_act, // validity1[1], thp_act RAD2DEG, // validity1[2],validity1[3], thy_act RAD2DEG); // } return report; } //set joints //setJointsWithLimits1(mech,d_act,thp_act,thy_act,grasp); btVector3 z_roll_in_world = btTransform(Zi(d) * Xip * Zp(thp) * Xpy * Zy(thy) * Tg * Tgripper_to_world).invXform(btVector3(0,0,1)); btVector3 x_roll_in_world = btTransform(Zi(d) * Xip * Zp(thp) * Xpy * Zy(thy) * Tg * Tgripper_to_world).invXform(btVector3(1,0,0)); float zx = z_roll_in_world.x(); float zy = z_roll_in_world.y(); float zz = z_roll_in_world.z(); float xx = x_roll_in_world.x(); float xy = x_roll_in_world.y(); float xz = x_roll_in_world.z(); float cthe = (zy + kc12*kc23) / (ks12*ks23); float the_1 = acos(cthe); float the_2 = -acos(cthe); float the_opt[2]; the_opt[0] = the_1; the_opt[1] = the_2; float ths_opt[2]; float thr_opt[2]; bool opts_valid[2]; float validity2[2][4]; float ths_act[2]; float the_act[2]; float thr_act[2]; for (int i=0;i<2;i++) { float sthe_tmp = sin(the_opt[i]); float C1 = ks12*kc23 + kc12*ks23*cthe; float C2 = ks23 * sthe_tmp; float C3 = C2 + C1*C1 / C2; ths_opt[i] = atan2( -sgn(C3)*(zx - C1 * zz / C2), sgn(C3)*(zz + C1 * zx / C2)); float sths_tmp = sin(ths_opt[i]); float cths_tmp = cos(ths_opt[i]); float C4 = ks12 * sin(the_opt[i]); float C5 = kc12 * ks23 + ks12 * kc23 * cos(the_opt[i]); float C6 = kc23*(sthe_tmp * sths_tmp - kc12*cthe*cths_tmp) + cths_tmp*ks12*ks23; float C7 = cthe*sths_tmp + kc12*cths_tmp*sthe_tmp; thr_opt[i] = atan2( (xx - C7 * xy / C4) / (C6 + C7*C5/C4), (xx + C6 * xy / C5) / (-C6*C4/C5 - C7)); ths_act[i] = THS_FROM_IK(armId,ths_opt[i]); the_act[i] = THE_FROM_IK(armId,the_opt[i]); thr_act[i] = THR_FROM_IK(armId,thr_opt[i]); // if (print) { // log_msg("j s % 3.1f e % 3.1f r % 3.1f i % 1.3f p % 3.1f y % 3.1f g % 3.1f g1 % 3.1f g2 % 3.1f", // arm->getJointById(Joint::Type::SHOULDER_)->position() RAD2DEG, // arm->getJointById(Joint::Type::ELBOW_)->position() RAD2DEG, // arm->getJointById(Joint::Type::TOOL_ROT_)->position() RAD2DEG, // arm->getJointById(Joint::Type::INSERTION__)->position(), // arm->getJointById(Joint::Type::WRIST_)->position() RAD2DEG, // THY_MECH_FROM_FINGERS(armIdFromMechType(mech->type),arm->getJointById(Joint::Type::GRIPPER1_)->position(), arm->getJointById(Joint::Type::GRIPPER2_)->position()) RAD2DEG,//fix_angle(arm->getJointById(Joint::Type::GRIPPER2_)->position() - arm->getJointById(Joint::Type::GRIPPER1_)->position(),0) / 2 RAD2DEG, // mech->ori.grasp * 1000. RAD2DEG, // fix_angle(arm->getJointById(Joint::Type::GRIPPER1_)->position() + arm->getJointById(Joint::Type::GRIPPER2_)->position(),0) RAD2DEG, // arm->getJointById(Joint::Type::GRIPPER1_)->position() RAD2DEG, arm->getJointById(Joint::Type::GRIPPER2_)->position() RAD2DEG); // log_msg("%d s % 3.1f e % 3.1f r % 3.1f i % 1.3f p % 3.1f y % 3.1f g % 3.1f g1 % 3.1f g2 % 3.1f",i, // ths_act[i] RAD2DEG, // the_act[i] RAD2DEG, // thr_act[i] RAD2DEG, // d_act, // thp_act RAD2DEG, // thy_act RAD2DEG, // grasp RAD2DEG, // g1_act RAD2DEG, // g2_act RAD2DEG); // // if (ths_act != ths_act) { // log_msg("C1 %0.4f\tC2 %0.4f\tC3 %0.4f\tC4 %0.4f\tC5 %0.4f\tC6 %0.4f\tC7 %0.4f\t",C1,C2,C3,C4,C5,C6,C7); // log_msg("ks23 %0.4f\tsthe_tmp %0.4f",ks23 , sthe_tmp); // log_msg("cthe %0.4f\tzy %0.4f\tkc12 %0.4f\tkc23 %0.4f\tks12 %0.4f\tks23 %0.4f",cthe,zy,kc12,kc23,ks12,ks23); // } // } bool valid2 = checkJointLimits2(ths_act[i],the_act[i],thr_act[i],validity2[i]); opts_valid[i] = valid2; if (valid2) { float ths_diff, the_diff, d_diff, thr_diff, thp_diff, thg1_diff, thg2_diff; //set joints setJointsWithLimits1(arm,d_act,thp_act,g1_act,g2_act); setJointsWithLimits2(arm,ths_act[i],the_act[i],thr_act[i]); ths_diff = arm->getJointById(Joint::IdType::SHOULDER_)->position() - ths_act[i]; the_diff = arm->getJointById(Joint::IdType::ELBOW_)->position() - the_act[i]; d_diff = arm->getJointById(Joint::IdType::INSERTION_)->position() - d_act; thr_diff = arm->getJointById(Joint::IdType::ROTATION_)->position() - thr_act[i]; thp_diff = arm->getJointById(Joint::IdType::WRIST_)->position() - thp_act; thg1_diff = arm->getJointById(Joint::IdType::FINGER1_)->position() - g1_act; thg2_diff = arm->getJointById(Joint::IdType::FINGER2_)->position() - g2_act; /* ths_diff = arm->getJointById(Joint::Type::SHOULDER_)->position()_d - ths_act; the_diff = arm->getJointById(Joint::Type::ELBOW_)->position()_d - the_act; d_diff = arm->getJointById(Joint::Type::INSERTION__)->position()_d - d_act; thr_diff = arm->getJointById(Joint::Type::TOOL_ROT_)->position()_d - thr_act; thp_diff = arm->getJointById(Joint::Type::WRIST_)->position()_d - thp_act; thg1_diff = arm->getJointById(Joint::Type::GRIPPER1_)->position()_d - g1_act; thg2_diff = arm->getJointById(Joint::Type::GRIPPER2_)->position()_d - g2_act; */ // if (print) { // log_msg("%d s % 2.1f e % 2.1f r % 2.1f i % 1.3f p % 2.1f g % 2.1f g1 % 2.1f g2 % 2.1f",i, // arm->getJointById(Joint::Type::SHOULDER_)->position()_d RAD2DEG, // arm->getJointById(Joint::Type::ELBOW_)->position()_d RAD2DEG, // arm->getJointById(Joint::Type::TOOL_ROT_)->position()_d RAD2DEG, // arm->getJointById(Joint::Type::INSERTION__)->position()_d, // arm->getJointById(Joint::Type::WRIST_)->position()_d RAD2DEG, // grasp RAD2DEG, // arm->getJointById(Joint::Type::GRIPPER1_)->position()_d RAD2DEG, // arm->getJointById(Joint::Type::GRIPPER2_)->position()_d RAD2DEG); // log_msg("diff:"); // log_msg("R s %0.4f e %0.4f r %0.4f d %0.4f p %0.4f g1 %0.4f g2 %0.4f", // ths_diff,the_diff,thr_diff,d_diff,thp_diff,thg1_diff,thg2_diff); // log_msg("D s %0.4f e %0.4f r %0.4f d %0.4f p %0.4f g1 %0.4f g2 %0.4f", // ths_diff*180/M_PI,the_diff*180/M_PI,thr_diff*180/M_PI,d_diff, // thp_diff*180/M_PI,thg1_diff*180/M_PI,thg2_diff*180/M_PI); // // } // // if (i==1 && _curr_rl == 3) { // //printf("ik ok! %d\n",_ik_counter); // } } } if (opts_valid[0]) { report->success_ = true; return report; } else if (opts_valid[1]) { // bool ENABLE_PARTIAL_INVALID_IK_PRINTING = false; // if (ENABLE_PARTIAL_INVALID_IK_PRINTING && (_curr_rl == 3 || print) && !(DISABLE_ALL_PRINTING)) { // printf("ik %d ok **2** s %1.4f %1.4f\te %1.4f %1.4f\tr %1.4f %1.4f\n", // armId, // ths_act[0] RAD2DEG,validity2[0][0], // the_act[0] RAD2DEG,validity2[0][1], // thr_act[0] RAD2DEG,validity2[0][2]); // /* // printf("%7d d %0.4f \tp %0.4f \ty %0.4f\n",_ik_counter, // d_act,thp_act RAD2DEG,thy_act RAD2DEG); // printf("x (%f, %f, %f) z (%f, %f, %f) %f\n",xx,xy,xz,zx,zy,zz,cthe); // printf("norms %f %f\n",x_roll_in_world.length(),z_roll_in_world.length()); // printf("(zy + kc12*kc23): %f (%f, %f)\n",(zy + kc12*kc23),zy, kc12*kc23); // printf("(ks12*ks23): %f\n",(ks12*ks23)); // */ // } report->success_ = true; return report; } else { const float maxValidDist = 3 DEG2RAD; float valid_dist[2]; for (int i=0;i<2;i++) { float sum = 0; for (int j=0;j<3;j++) { float v = fabs(validity2[i][j]); sum += v*v; } valid_dist[i] = sqrt(sum); } bool use0 = valid_dist[0] < maxValidDist && valid_dist[0] < valid_dist[1]; bool use1 = valid_dist[1] < maxValidDist && valid_dist[0] > valid_dist[1]; printf("ik validity distances: (%s | %s) % 1.3f\t%f\n",use0?"Y":" ",use1?"Y":" ",valid_dist[0],valid_dist[1]); if (valid_dist[0] < maxValidDist && valid_dist[0] < valid_dist[1]) { printf("setting joints to ik soln 1\n"); setJointsWithLimits1(arm,d_act,thp_act,g1_act,g2_act); setJointsWithLimits2(arm,ths_act[0],the_act[0],thr_act[0]); } else if (valid_dist[1] < maxValidDist) { printf("setting joints to ik soln 2\n"); setJointsWithLimits1(arm,d_act,thp_act,g1_act,g2_act); setJointsWithLimits2(arm,ths_act[1],the_act[1],thr_act[1]); } // if ((_curr_rl == 3 || print) && !(DISABLE_ALL_PRINTING)) { // printf("ik %d invalid **2** s %1.4f % 2.1f\te %1.4f % 2.1f\tr %1.4f % 2.1f\n", // armId, // ths_act[0] RAD2DEG,validity2[0][0] RAD2DEG, // the_act[0] RAD2DEG,validity2[0][1] RAD2DEG, // thr_act[0] RAD2DEG,validity2[0][2] RAD2DEG); // printf(" s %1.4f % 2.1f\te %1.4f % 2.1f\tr %1.4f % 2.1f\n", // ths_act[1] RAD2DEG,validity2[1][0] RAD2DEG, // the_act[1] RAD2DEG,validity2[1][1] RAD2DEG, // thr_act[1] RAD2DEG,validity2[1][2] RAD2DEG); // } return report; } return report; }