void KeySchedul(u16 mkey[2*NB],u16 rkey[NBROUND][2*NB]) { u16 i,j,temp,c[NB]; for(i=1;i<NB;i++) c[i]=0; for(i=0;i<2*NB;i++) rkey[0][i]=mkey[i]; for(i=1;i<=(NBROUND>>2);i++) { c[0]=i; //[KLi , KRi ] = FK (KLi−1 , KRi−1 , C(i)); fk(rkey[i-1],rkey[i-1]+3,rkey[i],rkey[i]+3,c); } for(j=0;j<NB;j++) { temp=rkey[NBROUND>>2][j]; rkey[NBROUND>>2][j]=rkey[NBROUND>>2][j+3]; rkey[NBROUND>>2][j+3]=temp; } for(;i<NBROUND;i++) { c[0]=NBROUND-i; //[KLi , KRi ] = FK (KLi−1 , KRi−1 , C(r − i)); fk(rkey[i-1],rkey[i-1]+3,rkey[i],rkey[i]+3,c); } return; }
double Clmbr::prden( double xi, double * err) // integrand for sl( theta, alpha ) generic formula { double den; if(variance_unknown) { if (th0ex) den= fk(m,xi); else den= fk(m-1,xi); } else den = Rf_dnorm4(lambda*xi ,0,1,0) ; double z_tilde; if( th0ex ) z_tilde = 0; else z_tilde = xi*c1 + c2; const double deltasq = lambdasq*(1-xi*xi) + z_tilde*z_tilde; if(variance_unknown) z= z_tilde/sqrt(deltasq); else z= z_tilde; double wsq; if(variance_unknown) wsq = 1 - omega/deltasq; else wsq = deltasq - omega; w = sqrt(max(wsq,0.)); double er= 0, *const per= &er; const double pr = sl_geo(per); if( err != 0 ) *err += (*per)*den; return pr*den; }
void encrypt(char *input, char *key) { initial_permutation(input); fk(output, k1); switch_halves(output); fk(output, k2); initial_permutation_inverse(output); }
double Astar::cost(configState* c1,configState* c2) { wsState* state1 = fk(c1); wsState* state2 = fk(c2); double workdist = distance(state1,state2); delete(state1); delete(state2); //TODO experiment with various angle penalties to determine the optimum path return workdist; }
//Determines how close a configuration is to the target by measuring Euclidean // distance in the workspace double Astar::heuristic(configState* c) { wsState* state = fk(c); double d = distance(state,target); delete(state); return d; }//end heurisitic
DLLIMPORT int get6AH01(double *qps, double *kinArray, double *arrayH01) { int ret = -1; FwdK6A fk(kinArray2Struct6A(kinArray)); Eigen::Matrix4d H01 = fk.qps2H01(qps); // non-static usage; //Eigen::Matrix4d H01; //H01 = FwdK::qps2H01(qps, params); //now static //arrayH01 = H01.data(); lv can't read it.. arrayH01[0] = H01(0, 0); arrayH01[1] = H01(1, 0); arrayH01[2] = H01(2, 0); arrayH01[3] = H01(0, 1); arrayH01[4] = H01(1, 1); arrayH01[5] = H01(2, 1); arrayH01[6] = H01(0, 2); arrayH01[7] = H01(1, 2); arrayH01[8] = H01(2, 2); arrayH01[9] = H01(0, 3); arrayH01[10] = H01(1, 3); arrayH01[11] = H01(2, 3); ret = 0; return ret; }
int Switch::eval(const double** arg, double** res, casadi_int* iw, double* w, void* mem) const { // Get the function to be evaluated casadi_int k = arg[0] ? static_cast<casadi_int>(*arg[0]) : 0; const Function& fk = k>=0 && k<f_.size() ? f_[k] : f_def_; // Project arguments with different sparsity const double** arg1; if (project_in_) { // Project one or more argument arg1 = arg + n_in_; for (casadi_int i=0; i<n_in_-1; ++i) { const Sparsity& f_sp = fk.sparsity_in(i); const Sparsity& sp = sparsity_in_[i+1]; arg1[i] = arg[i+1]; if (arg1[i] && f_sp!=sp) { casadi_project(arg1[i], sp, w, f_sp, w + f_sp.nnz()); arg1[i] = w; w += f_sp.nnz(); } } } else { // No inputs projected arg1 = arg + 1; } // Temporary memory for results with different sparsity double** res1; if (project_out_) { // Project one or more results res1 = res + n_out_; for (casadi_int i=0; i<n_out_; ++i) { const Sparsity& f_sp = fk.sparsity_out(i); const Sparsity& sp = sparsity_out_[i]; res1[i] = res[i]; if (res1[i] && f_sp!=sp) { res1[i] = w; w += f_sp.nnz(); } } } else { // No outputs projected res1 = res; } // Evaluate the corresponding function if (fk(arg1, res1, iw, w, 0)) return 1; // Project results with different sparsity if (project_out_) { for (casadi_int i=0; i<n_out_; ++i) { const Sparsity& f_sp = fk.sparsity_out(i); const Sparsity& sp = sparsity_out_[i]; if (res[i] && f_sp!=sp) { casadi_project(res1[i], f_sp, res[i], sp, w); } } } return 0; }
void Astar::compute_fk(int i, int j, int k){ double x; double z; double alpha; fk(&x, &z, &alpha, i, j, k,numticks); space[i][j][k].x = x; space[i][j][k].z = z; space[i][j][k].alpha = alpha; }
Polynomial<2> count_generator(int symmetries) { // Make the color generating function Polynomial<2> f; f.x[vec(0,0)]++; f.x[vec(1,0)]++; f.x[vec(0,1)]++; // List symmetries Array<symmetry_t> group; if (symmetries==1) group.append(symmetry_t(0)); else if (symmetries==8) for (int g=0;g<8;g++) group.append(symmetry_t(g,0)); else if (symmetries==2048) for (symmetry_t s : pentago::symmetries) group.append(s); else GEODE_ASSERT(false); // Generate the cycle index Polynomial<36> Z; for (symmetry_t s : group) { Vector<int,36> cycles; for (int i=0;i<4;i++) for (int j=0;j<9;j++) { side_t start = (side_t)1<<(16*i+j); side_t side = start; for (int o=1;;o++) { side = transform_side(s,side); if (side==start) { cycles[o-1]++; break; } } } for (int i=0;i<36;i++) { GEODE_ASSERT(cycles[i]%(i+1)==0); cycles[i] /= i+1; } Z.x[cycles]++; } // Compute f(b^k,w^k) for k = 1 to 36 vector<Polynomial<2>> fk(36); for (int k=1;k<=36;k++) fk[k-1] = compose(f,vec(monomial(vec(k,0)),monomial(vec(0,k)))); // Compose Zg and fk to get F auto F = compose(Z,fk); for (auto& m : F.x) { GEODE_ASSERT(m.second%symmetries==0); m.second /= symmetries; } return F; }
void RouterContext::SaveKeys () { std::ofstream fk (i2p::util::filesystem::GetFullPath (ROUTER_KEYS).c_str (), std::ofstream::binary | std::ofstream::out); i2p::data::Keys keys; memcpy (keys.privateKey, m_Keys.GetPrivateKey (), sizeof (keys.privateKey)); memcpy (keys.signingPrivateKey, m_Keys.GetSigningPrivateKey (), sizeof (keys.signingPrivateKey)); auto& ident = GetIdentity ().GetStandardIdentity (); memcpy (keys.publicKey, ident.publicKey, sizeof (keys.publicKey)); memcpy (keys.signingKey, ident.signingKey, sizeof (keys.signingKey)); fk.write ((char *)&keys, sizeof (keys)); }
int eGDP (int N, double *x, double *k, double *psi, double tol) { double max=-DBL_MAX,min; double optSigma; int i; for (i=0; i<N; i++) { if (x[i]>max) max = x[i]; } max=100*max; min=-max; optSigma = fmin_brent(min,max,N,x,fp,tol); //gss(&min,&max,N,x,fp,tol,&optSigma); *k = fk(N,x,optSigma); *psi = fk(N,x,optSigma)*optSigma; return 0; }
int BasicDebug::main( int argc, char** argv ) { (void)argc; (void)argv; BDEBUG("Esto es un mensaje"); DEBUGM; BDEBUG("Esto es otro mensaje"); DEBUGM; BDEBUG("Esto es un mensaje?"); DEBUGM; std::string fk(__FILE__); std::string kx(__FILE__); // std::string x (__LINE__); std::cout << fk + kx << std::endl; return 0; }
bool LogFileWriter::on_logLine(KPropertyMap& lmap) { char buf[64]; char buf2[64]; const char* tstr = lmap.get("time"); strcpy(buf, tstr); tstr = buf; lmap.erase("time"); const char* verb = lmap.get("verb"); strcpy(buf2, verb); verb = buf2; lmap.erase("verb"); time_t t = (time_t)str2int(tstr); KDatetime dt(t); KLocalDatetime ldt = dt.GetLocalTime(); int yyyy = ldt.year, mm = ldt.month, dd = ldt.day; const char* serverName = g_pApp->m_strPool.get(lmap.get("server")); int yyyymmdd = yyyy*10000 + mm*100 + dd; FileKey fk(serverName, yyyymmdd); if(m_currKey) { if(!FileKey::cmp::compare(*m_currKey, fk)) { return m_currFile->writeLine(dt, ldt, verb, lmap); } else { m_currFile->flush(); } } OutputFileMap::iterator it = m_fileMap.find(fk); if(it != m_fileMap.end()) { m_currKey = &it->first; m_currFile = it->second; return m_currFile->writeLine(dt, ldt, verb, lmap); } string_512 filePath = this->buildFilepath(serverName, yyyymmdd, verb); OutputLogFile* file = OutputLogFile::Alloc(); if(!file->open(filePath.c_str())) { Log(LOG_ERROR, "error: log file writer open %s", filePath.c_str()); return false; } it = m_fileMap.insert(fk, file); m_currKey = &it->first; m_currFile = it->second; return m_currFile->writeLine(dt, ldt, verb, lmap); }
SslParams::SslParams( const QString & certFile, const QString & keyFile, QObject * parent ) : QObject( parent ) { QFile fc( certFile, this ); fc.open( QFile::ReadOnly ); certificate = QSslCertificate( fc.readAll() ); fc.close(); ca << certificate; QFile fk( keyFile, this ); fk.open( QFile::ReadOnly ); privateKey = QSslKey( fk.readAll(), QSsl::Rsa, QSsl::Pem, QSsl::PrivateKey);//, passwd ); fk.close(); }
DLLIMPORT int get6AH05(double *qps, double *kinArray, double *arrayH05) { FwdK6A fk(kinArray2Struct6A(kinArray)); Eigen::Matrix4d H05 = fk.qps2H05(qps); arrayH05[0] = H05(0, 0); arrayH05[1] = H05(1, 0); arrayH05[2] = H05(2, 0); arrayH05[3] = H05(0, 1); arrayH05[4] = H05(1, 1); arrayH05[5] = H05(2, 1); arrayH05[6] = H05(0, 2); arrayH05[7] = H05(1, 2); arrayH05[8] = H05(2, 2); arrayH05[9] = H05(0, 3); arrayH05[10] = H05(1, 3); arrayH05[11] = H05(2, 3); return 0; }
DLLIMPORT int get11AH05(double *qps, double *kinArray, double *arrayH) { FwdK11A fk(kinArray2Struct11A(kinArray)); Eigen::Matrix4d H = fk.qps2H05(qps); arrayH[0] = H(0, 0); arrayH[1] = H(1, 0); arrayH[2] = H(2, 0); arrayH[3] = H(0, 1); arrayH[4] = H(1, 1); arrayH[5] = H(2, 1); arrayH[6] = H(0, 2); arrayH[7] = H(1, 2); arrayH[8] = H(2, 2); arrayH[9] = H(0, 3); arrayH[10] = H(1, 3); arrayH[11] = H(2, 3); return 0; }
bool RouterContext::Load () { std::ifstream fk (i2p::util::filesystem::GetFullPath (ROUTER_KEYS).c_str (), std::ifstream::binary | std::ofstream::in); if (!fk.is_open ()) return false; i2p::data::Keys keys; fk.read ((char *)&keys, sizeof (keys)); m_Keys = keys; i2p::data::RouterInfo routerInfo(i2p::util::filesystem::GetFullPath (ROUTER_INFO)); // TODO m_RouterInfo.Update (routerInfo.GetBuffer (), routerInfo.GetBufferLen ()); m_RouterInfo.SetProperty ("coreVersion", I2P_VERSION); m_RouterInfo.SetProperty ("router.version", I2P_VERSION); if (IsUnreachable ()) SetReachable (); // we assume reachable until we discover firewall through peer tests return true; }
// wrap forwardK DLLIMPORT int get5AH01(double *qps, double *kinArray, double *arrayH01) { int ret = -1; FwdK5A fk(kinArray2Struct5A(kinArray)); Eigen::Matrix4d H01 = fk.qps2H01(qps); arrayH01[0] = H01(0, 0); arrayH01[1] = H01(1, 0); arrayH01[2] = H01(2, 0); arrayH01[3] = H01(0, 1); arrayH01[4] = H01(1, 1); arrayH01[5] = H01(2, 1); arrayH01[6] = H01(0, 2); arrayH01[7] = H01(1, 2); arrayH01[8] = H01(2, 2); arrayH01[9] = H01(0, 3); arrayH01[10] = H01(1, 3); arrayH01[11] = H01(2, 3); ret = 0; return ret; }
void RouterContext::Save (bool infoOnly) { std::string dataDir = i2p::util::filesystem::GetDataDir ().string (); #ifndef _WIN32 dataDir.append ("/"); #else dataDir.append ("\\"); #endif std::string router_keys = dataDir; router_keys.append (ROUTER_KEYS); std::string router_info = dataDir; router_info.append (ROUTER_INFO); if (!infoOnly) { std::ofstream fk (router_keys.c_str (), std::ofstream::binary | std::ofstream::out); fk.write ((char *)&m_Keys, sizeof (m_Keys)); } std::ofstream fi (router_info.c_str (), std::ofstream::binary | std::ofstream::out); fi.write ((char *)m_RouterInfo.GetBuffer (), m_RouterInfo.GetBufferLen ()); }
int main() { initComm(); Scene scene; SceneConfig::enableIK = false; PR2Manager pr2m(scene); KinectTransformer kinectTrans(pr2m.pr2->robot); kinectTrans.calibrate(btTransform::getIdentity()); CoordinateTransformer CT(kinectTrans.getWFC()); FakeKinect fk(scene.env->osg, CT.worldFromCamEigen); scene.startViewer(); scene.step(0); while (true) { fk.sendMessage(); } }
DLLIMPORT int get5AH03(double *qps, double *kinArray, double *arrayH03) { int ret = -1; FwdK5A fk(kinArray2Struct5A(kinArray)); Eigen::Matrix4d H03 = fk.qps2H03(qps); arrayH03[0] = H03(0, 0); arrayH03[1] = H03(1, 0); arrayH03[2] = H03(2, 0); arrayH03[3] = H03(0, 1); arrayH03[4] = H03(1, 1); arrayH03[5] = H03(2, 1); arrayH03[6] = H03(0, 2); arrayH03[7] = H03(1, 2); arrayH03[8] = H03(2, 2); arrayH03[9] = H03(0, 3); arrayH03[10] = H03(1, 3); arrayH03[11] = H03(2, 3); ret = 0; return ret; }
DLLIMPORT int get5AH04(double *qps, double *kinArray, double *arrayH04) { int ret = -1; FwdK5A fk(kinArray2Struct5A(kinArray)); Eigen::Matrix4d H04 = fk.qps2H04(qps); arrayH04[0] = H04(0, 0); arrayH04[1] = H04(1, 0); arrayH04[2] = H04(2, 0); arrayH04[3] = H04(0, 1); arrayH04[4] = H04(1, 1); arrayH04[5] = H04(2, 1); arrayH04[6] = H04(0, 2); arrayH04[7] = H04(1, 2); arrayH04[8] = H04(2, 2); arrayH04[9] = H04(0, 3); arrayH04[10] = H04(1, 3); arrayH04[11] = H04(2, 3); ret = 0; return ret; }
DLLIMPORT int get5AH02(double *qps, double *kinArray, double *arrayH02) { int ret = -1; FwdK5A fk(kinArray2Struct5A(kinArray)); Eigen::Matrix4d H02 = fk.qps2H02(qps); arrayH02[0] = H02(0, 0); arrayH02[1] = H02(1, 0); arrayH02[2] = H02(2, 0); arrayH02[3] = H02(0, 1); arrayH02[4] = H02(1, 1); arrayH02[5] = H02(2, 1); arrayH02[6] = H02(0, 2); arrayH02[7] = H02(1, 2); arrayH02[8] = H02(2, 2); arrayH02[9] = H02(0, 3); arrayH02[10] = H02(1, 3); arrayH02[11] = H02(2, 3); ret = 0; return ret; }
bool RouterContext::Load () { std::string dataDir = i2p::util::filesystem::GetDataDir ().string (); #ifndef _WIN32 dataDir.append ("/"); #else dataDir.append ("\\"); #endif std::string router_keys = dataDir; router_keys.append (ROUTER_KEYS); std::string router_info = dataDir; router_info.append (ROUTER_INFO); std::ifstream fk (router_keys.c_str (), std::ifstream::binary | std::ofstream::in); if (!fk.is_open ()) return false; fk.read ((char *)&m_Keys, sizeof (m_Keys)); m_SigningPrivateKey.Initialize (i2p::crypto::dsap, i2p::crypto::dsaq, i2p::crypto::dsag, CryptoPP::Integer (m_Keys.signingPrivateKey, 20)); m_RouterInfo = i2p::data::RouterInfo (router_info.c_str ()); // TODO return true; }
float find_k() { float h=1,x=0,y=0,c,a,b; while(fk(a)*fk(b)>0){ y++; h=1/y; a=x; b=a+h; while((a!=y)&&(fk(a)*fk(b)>=0)){ a=a+h; b=b+h; } } while(fabs(b-a)/2>=eps){ c=(b+a)/2; if(fk(a)*fk(c)<0)b=c; else a=c; } return (b+a)/2; }
double fp (int N, double *x, double sigma) { return N*(log(fk(N,x,sigma)*sigma)-fk(N,x,sigma)+1); }
std::vector<configState*> Astar::run(configState* s, wsState* t){ std::cout << "Initializing Start and Target\n"; target = t; start = s; std::cout << "Target: "; wsstate_tostring(target); std::cout << "Initialize start data structures\n"; /* Runtime checks */ float avg_frontier_pop = 0; int pop_count = 0; float avg_vis_add = 0; int vis_add_count = 0; float avg_expansion = 0; int expansion_count = 0; /* Init */ visData* startv = create_visdata(); startv->current = start; startv->prev = 0; startv->value = heuristic(start); frontier.push(startv); visData* next = 0; wsState* wscurrent = fk(start); while(distance(target, wscurrent) > target_threshold){ clock_t pqstart = clock(); next = frontier.top(); frontier.pop(); float secsElapsed = (float)(clock() - pqstart)/CLOCKS_PER_SEC; avg_frontier_pop += secsElapsed; pop_count++; delete(wscurrent); wscurrent = fk(next->current); if(next != 0){ clock_t visaddstart = clock(); add_visited(next->current, next); secsElapsed= (float)(clock() - visaddstart)/CLOCKS_PER_SEC; avg_vis_add += secsElapsed; vis_add_count ++; clock_t expstart = clock(); expand_frontier(next->current); secsElapsed = (float)(clock() - expstart)/CLOCKS_PER_SEC; avg_expansion += secsElapsed; expansion_count++; } else { std::vector<configState*> path; return path; } } std::cout << "Visited Set: "<< visited_set.size() << '\n'; std::cout << "Queue: "<< frontier.size() << '\n'; std::cout << "Completed Search\n"; std::cout << "Runtimes: \n\n\n\n"; std::cout << "Frontier Popping :" << avg_frontier_pop << '\n'; std::cout << "Expansion: " << avg_expansion << '\n'; std::cout << " Vis Gets: " << get_visdata_time << '\n'; std::cout << " Value Comp Time: " << value_comp_time << '\n'; std::cout << " Frontier Push Time: " << frontier_insert_time << '\n'; std::cout << " Mem_Time: " << mem_time << '\n'; std::cout << " VisAdd :" << avg_vis_add*3 << '\n'; std::cout << "Counts: " << pop_count << '\n'; visData* current = next; std::vector<configState*> path; std::cout << "Backtracing path\n"; while(current != 0){ path.push_back(current->current); if(current -> prev == 0){ current = 0; continue; } current = get_visdata(current->prev); } std::cout << "Back trace completed\n"; return path; }
int Switch::eval_sx(const SXElem** arg, SXElem** res, casadi_int* iw, SXElem* w, void* mem) const { // Input and output buffers const SXElem** arg1 = arg + n_in_; SXElem** res1 = res + n_out_; // Extra memory needed for chaining if_else calls std::vector<SXElem> w_extra(nnz_out()); std::vector<SXElem*> res_tempv(n_out_); SXElem** res_temp = get_ptr(res_tempv); for (casadi_int k=0; k<f_.size()+1; ++k) { // Local work vector SXElem* wl = w; // Local work vector SXElem* wll = get_ptr(w_extra); if (k==0) { // For the default case, redirect the temporary results to res copy_n(res, n_out_, res_temp); } else { // For the other cases, store the temporary results for (casadi_int i=0; i<n_out_; ++i) { res_temp[i] = wll; wll += nnz_out(i); } } copy_n(arg+1, n_in_-1, arg1); copy_n(res_temp, n_out_, res1); const Function& fk = k==0 ? f_def_ : f_[k-1]; // Project arguments with different sparsity for (casadi_int i=0; i<n_in_-1; ++i) { if (arg1[i]) { const Sparsity& f_sp = fk.sparsity_in(i); const Sparsity& sp = sparsity_in_[i+1]; if (f_sp!=sp) { SXElem *t = wl; wl += f_sp.nnz(); // t is non-const casadi_project(arg1[i], sp, t, f_sp, wl); arg1[i] = t; } } } // Temporary memory for results with different sparsity for (casadi_int i=0; i<n_out_; ++i) { if (res1[i]) { const Sparsity& f_sp = fk.sparsity_out(i); const Sparsity& sp = sparsity_out_[i]; if (f_sp!=sp) { res1[i] = wl; wl += f_sp.nnz();} } } // Evaluate the corresponding function if (fk(arg1, res1, iw, wl, 0)) return 1; // Project results with different sparsity for (casadi_int i=0; i<n_out_; ++i) { if (res1[i]) { const Sparsity& f_sp = fk.sparsity_out(i); const Sparsity& sp = sparsity_out_[i]; if (f_sp!=sp) casadi_project(res1[i], f_sp, res_temp[i], sp, wl); } } if (k>0) { // output the temporary results via an if_else SXElem cond = k-1==arg[0][0]; for (casadi_int i=0; i<n_out_; ++i) { if (res[i]) { for (casadi_int j=0; j<nnz_out(i); ++j) { res[i][j] = if_else(cond, res_temp[i][j], res[i][j]); } } } } } return 0; }
int main(int argc, char** argv) { IKREAL_TYPE eerot[9],eetrans[3]; #if IK_VERSION > 54 // for IKFast 56,61 unsigned int num_of_joints = GetNumJoints(); unsigned int num_free_parameters = GetNumFreeParameters(); #else // for IKFast 54 unsigned int num_of_joints = getNumJoints(); unsigned int num_free_parameters = getNumFreeParameters(); #endif std::string cmd; if (argv[1]) cmd = argv[1]; //printf("command: %s \n\n", cmd.c_str() ); if (cmd.compare("ik") == 0) // ik mode { if( argc == 1+7+num_free_parameters+1 ) // ik, given translation vector and quaternion pose { #if IK_VERSION > 54 // for IKFast 56,61 IkSolutionList<IKREAL_TYPE> solutions; #else // for IKFast 54 std::vector<IKSolution> vsolutions; #endif std::vector<IKREAL_TYPE> vfree(num_free_parameters); eetrans[0] = atof(argv[2]); eetrans[1] = atof(argv[3]); eetrans[2] = atof(argv[4]); // Convert input effector pose, in w x y z quaternion notation, to rotation matrix. // Must use doubles, else lose precision compared to directly inputting the rotation matrix. double qw = atof(argv[5]); double qx = atof(argv[6]); double qy = atof(argv[7]); double qz = atof(argv[8]); const double n = 1.0f/sqrt(qx*qx+qy*qy+qz*qz+qw*qw); qw *= n; qx *= n; qy *= n; qz *= n; eerot[0] = 1.0f - 2.0f*qy*qy - 2.0f*qz*qz; eerot[1] = 2.0f*qx*qy - 2.0f*qz*qw; eerot[2] = 2.0f*qx*qz + 2.0f*qy*qw; eerot[3] = 2.0f*qx*qy + 2.0f*qz*qw; eerot[4] = 1.0f - 2.0f*qx*qx - 2.0f*qz*qz; eerot[5] = 2.0f*qy*qz - 2.0f*qx*qw; eerot[6] = 2.0f*qx*qz - 2.0f*qy*qw; eerot[7] = 2.0f*qy*qz + 2.0f*qx*qw; eerot[8] = 1.0f - 2.0f*qx*qx - 2.0f*qy*qy; // For debugging, output the matrix for (unsigned char i=0; i<=8; i++) { // detect -0.0 and replace with 0.0 if ( ((int&)(eerot[i]) & 0xFFFFFFFF) == 0) eerot[i] = 0.0; } printf(" Rotation %f %f %f \n", eerot[0], eerot[1], eerot[2] ); printf(" %f %f %f \n", eerot[3], eerot[4], eerot[5] ); printf(" %f %f %f \n", eerot[6], eerot[7], eerot[8] ); printf("\n"); for(std::size_t i = 0; i < vfree.size(); ++i) vfree[i] = atof(argv[13+i]); #if IK_VERSION > 54 // for IKFast 56,61 bool bSuccess = ComputeIk(eetrans, eerot, vfree.size() > 0 ? &vfree[0] : NULL, solutions); #else // for IKFast 54 bool bSuccess = ik(eetrans, eerot, vfree.size() > 0 ? &vfree[0] : NULL, vsolutions); #endif if( !bSuccess ) { fprintf(stderr,"Failed to get ik solution\n"); //return -1; } #if IK_VERSION > 54 // for IKFast 56,61 unsigned int num_of_solutions = (int)solutions.GetNumSolutions(); #else // for IKFast 54 unsigned int num_of_solutions = (int)vsolutions.size(); #endif printf("Found %d ik solutions:\n", num_of_solutions ); std::vector<IKREAL_TYPE> solvalues(num_of_joints); for(std::size_t i = 0; i < num_of_solutions; ++i) { #if IK_VERSION > 54 // for IKFast 56,61 const IkSolutionBase<IKREAL_TYPE>& sol = solutions.GetSolution(i); int this_sol_free_params = (int)sol.GetFree().size(); #else // for IKFast 54 int this_sol_free_params = (int)vsolutions[i].GetFree().size(); #endif printf("sol%d (free=%d): ", (int)i, this_sol_free_params ); std::vector<IKREAL_TYPE> vsolfree(this_sol_free_params); #if IK_VERSION > 54 // for IKFast 56,61 sol.GetSolution(&solvalues[0],vsolfree.size()>0?&vsolfree[0]:NULL); #else // for IKFast 54 vsolutions[i].GetSolution(&solvalues[0],vsolfree.size()>0?&vsolfree[0]:NULL); #endif for( std::size_t j = 0; j < solvalues.size(); ++j) printf("%.15f, ", solvalues[j]); printf("\n"); } } else if( argc == 1+12+num_free_parameters+1 ) // ik, given rotation-translation matrix { #if IK_VERSION > 54 // for IKFast 56,61 IkSolutionList<IKREAL_TYPE> solutions; #else // for IKFast 54 std::vector<IKSolution> vsolutions; #endif std::vector<IKREAL_TYPE> vfree(num_free_parameters); eerot[0] = atof(argv[2]); eerot[1] = atof(argv[3]); eerot[2] = atof(argv[4]); eetrans[0] = atof(argv[5]); eerot[3] = atof(argv[6]); eerot[4] = atof(argv[7]); eerot[5] = atof(argv[8]); eetrans[1] = atof(argv[9]); eerot[6] = atof(argv[10]); eerot[7] = atof(argv[11]); eerot[8] = atof(argv[12]); eetrans[2] = atof(argv[13]); for(std::size_t i = 0; i < vfree.size(); ++i) vfree[i] = atof(argv[14+i]); printf("translation: \n"); for (unsigned int i = 0; i < 3; i++) { printf("%lf ", eetrans[i]); } printf("\n"); printf("rotation matix: \n"); for (unsigned int i = 0; i < 9; i++) { printf("%lf%s", eerot[i], (i % 3 == 2)?"\n":" "); } printf("\n"); printf("vfree:\n"); for(std::size_t i = 0; i < vfree.size(); ++i) printf("%lf ", vfree[i]); printf("\n"); #if IK_VERSION > 54 // for IKFast 56,61 bool bSuccess = ComputeIk(eetrans, eerot, vfree.size() > 0 ? &vfree[0] : NULL, solutions); #else // for IKFast 54 bool bSuccess = ik(eetrans, eerot, vfree.size() > 0 ? &vfree[0] : NULL, vsolutions); #endif if( !bSuccess ) { fprintf(stderr,"Failed to get ik solution\n"); return -1; } #if IK_VERSION > 54 // for IKFast 56,61 unsigned int num_of_solutions = (int)solutions.GetNumSolutions(); #else // for IKFast 54 unsigned int num_of_solutions = (int)vsolutions.size(); #endif printf("Found %d ik solutions:\n", num_of_solutions ); std::vector<IKREAL_TYPE> solvalues(num_of_joints); for(std::size_t i = 0; i < num_of_solutions; ++i) { #if IK_VERSION > 54 // for IKFast 56,61 const IkSolutionBase<IKREAL_TYPE>& sol = solutions.GetSolution(i); int this_sol_free_params = (int)sol.GetFree().size(); #else // for IKFast 54 int this_sol_free_params = (int)vsolutions[i].GetFree().size(); #endif printf("sol%d (free=%d): ", (int)i, this_sol_free_params ); std::vector<IKREAL_TYPE> vsolfree(this_sol_free_params); #if IK_VERSION > 54 // for IKFast 56,61 sol.GetSolution(&solvalues[0],vsolfree.size()>0?&vsolfree[0]:NULL); #else // for IKFast 54 vsolutions[i].GetSolution(&solvalues[0],vsolfree.size()>0?&vsolfree[0]:NULL); #endif for( std::size_t j = 0; j < solvalues.size(); ++j) printf("%.15f, ", solvalues[j]); printf("\n"); } } else { printf("\n " "Usage: \n\n " " ./compute ik t0 t1 t2 qw qi qj qk free0 ...\n\n " " Returns the ik solutions given the transformation of the end effector specified by \n " " a 3x1 translation (tX), and a 1x4 quaternion (w + i + j + k). \n " " There are %d free parameters that have to be specified.\n\n", num_free_parameters ); printf(" \n " " ./compute ik r00 r01 r02 t0 r10 r11 r12 t1 r20 r21 r22 t2 free0 ...\n\n " " Returns the ik solutions given the transformation of the end effector specified by \n " " a 3x3 rotation R (rXX), and a 3x1 translation (tX). \n " " There are %d free parameters that have to be specified.\n\n", num_free_parameters ); return 1; } } // endif ik mode else if (cmd.compare("fk") == 0) // fk mode { if( argc != num_of_joints+2 ) { printf("\n " "Usage: \n\n " " ./compute fk j0 j1 ... j%d \n\n" " Returns the forward kinematic solution given the joint angles (in radians). \n\n", num_of_joints-1 ); return 1; } printf("\n\n"); // Put input joint values into array IKREAL_TYPE joints[num_of_joints]; for (unsigned int i=0; i<num_of_joints; i++) { joints[i] = atof(argv[i+2]); } #if IK_VERSION > 54 // for IKFast 56,61 ComputeFk(joints, eetrans, eerot); // void return #else // for IKFast 54 fk(joints, eetrans, eerot); // void return #endif printf("Found fk solution for end frame: \n\n"); printf(" Translation: x: %f y: %f z: %f \n", eetrans[0], eetrans[1], eetrans[2] ); printf("\n"); printf(" Rotation %f %f %f \n", eerot[0], eerot[1], eerot[2] ); printf(" Matrix: %f %f %f \n", eerot[3], eerot[4], eerot[5] ); printf(" %f %f %f \n", eerot[6], eerot[7], eerot[8] ); printf("\n"); // Display equivalent Euler angles float yaw; float pitch; float roll; if ( eerot[5] > 0.998 || eerot[5] < -0.998 ) { // singularity yaw = IKatan2( -eerot[6], eerot[0] ); pitch = 0; } else { yaw = IKatan2( eerot[2], eerot[8] ); pitch = IKatan2( eerot[3], eerot[4] ); } roll = IKasin( eerot[5] ); printf(" Euler angles: \n"); printf(" Yaw: %f ", yaw ); printf("(1st: rotation around vertical blue Z-axis in ROS Rviz) \n"); printf(" Pitch: %f \n", pitch ); printf(" Roll: %f \n", roll ); printf("\n"); // Convert rotation matrix to quaternion (Daisuke Miyazaki) float q0 = ( eerot[0] + eerot[4] + eerot[8] + 1.0f) / 4.0f; float q1 = ( eerot[0] - eerot[4] - eerot[8] + 1.0f) / 4.0f; float q2 = (-eerot[0] + eerot[4] - eerot[8] + 1.0f) / 4.0f; float q3 = (-eerot[0] - eerot[4] + eerot[8] + 1.0f) / 4.0f; if(q0 < 0.0f) q0 = 0.0f; if(q1 < 0.0f) q1 = 0.0f; if(q2 < 0.0f) q2 = 0.0f; if(q3 < 0.0f) q3 = 0.0f; q0 = sqrt(q0); q1 = sqrt(q1); q2 = sqrt(q2); q3 = sqrt(q3); if(q0 >= q1 && q0 >= q2 && q0 >= q3) { q0 *= +1.0f; q1 *= SIGN(eerot[7] - eerot[5]); q2 *= SIGN(eerot[2] - eerot[6]); q3 *= SIGN(eerot[3] - eerot[1]); } else if(q1 >= q0 && q1 >= q2 && q1 >= q3) { q0 *= SIGN(eerot[7] - eerot[5]); q1 *= +1.0f; q2 *= SIGN(eerot[3] + eerot[1]); q3 *= SIGN(eerot[2] + eerot[6]); } else if(q2 >= q0 && q2 >= q1 && q2 >= q3) { q0 *= SIGN(eerot[2] - eerot[6]); q1 *= SIGN(eerot[3] + eerot[1]); q2 *= +1.0f; q3 *= SIGN(eerot[7] + eerot[5]); } else if(q3 >= q0 && q3 >= q1 && q3 >= q2) { q0 *= SIGN(eerot[3] - eerot[1]); q1 *= SIGN(eerot[6] + eerot[2]); q2 *= SIGN(eerot[7] + eerot[5]); q3 *= +1.0f; } else { printf("Error while converting to quaternion! \n"); } float r = NORM(q0, q1, q2, q3); q0 /= r; q1 /= r; q2 /= r; q3 /= r; printf(" Quaternion: %f %f %f %f \n", q0, q1, q2, q3 ); printf(" "); // print quaternion with convention and +/- signs such that it can be copy-pasted into WolframAlpha.com printf("%f ", q0); if (q1 > 0) printf("+ %fi ", q1); else if (q1 < 0) printf("- %fi ", -q1); else printf("+ 0.00000i "); if (q2 > 0) printf("+ %fj ", q2); else if (q2 < 0) printf("- %fj ", -q2); else printf("+ 0.00000j "); if (q3 > 0) printf("+ %fk ", q3); else if (q3 < 0) printf("- %fk ", -q3); else printf("+ 0.00000k "); printf(" (alternate convention) \n"); printf("\n\n"); } else if (cmd.compare("iktiming") == 0) // generate random ik and check time performance { if( argc != 2 ) { printf("\n " "Usage: \n\n " " ./compute iktiming \n\n" " For fixed number of iterations, generates random joint angles, then \n" " calculates fk, calculates ik, measures average time taken. \n\n", num_of_joints-1 ); return 1; } printf("\n\n"); #if IK_VERSION > 54 // for IKFast 56,61 IkSolutionList<IKREAL_TYPE> solutions; #else // for IKFast 54 std::vector<IKSolution> vsolutions; #endif std::vector<IKREAL_TYPE> vfree(num_free_parameters); //for(std::size_t i = 0; i < vfree.size(); ++i) // vfree[i] = atof(argv[13+i]); srand( (unsigned)time(0) ); // seed random number generator float min = -3.14; float max = 3.14; float rnd; IKREAL_TYPE joints[num_of_joints]; timespec start_time, end_time; unsigned int elapsed_time = 0; unsigned int sum_time = 0; unsigned int fail_count = 0; #if IK_VERSION > 54 // for IKFast 56,61 unsigned int num_of_tests = 10; #else // for IKFast 54 unsigned int num_of_tests = 100000; #endif for (unsigned int i=0; i < num_of_tests; i++) { // Measure avg time for whole process //clock_gettime(CLOCK_REALTIME, &start_time); // Put random joint values into array for (unsigned int i=0; i<num_of_joints; i++) { float rnd = (float)rand() / (float)RAND_MAX; joints[i] = min + rnd * (max - min); } /* printf("Joint angles: "); for (unsigned int i=0; i<num_of_joints; i++) { printf("%f ", joints[i] ); } printf("\n"); */ #if IK_VERSION > 54 // for IKFast 56,61 ComputeFk(joints, eetrans, eerot); // void return #else // for IKFast 54 fk(joints, eetrans, eerot); // void return #endif // Measure avg time for IK clock_gettime(CLOCK_REALTIME, &start_time); #if IK_VERSION > 54 // for IKFast 56,61 vfree[0] = 5; if (!ComputeIk(eetrans, eerot, vfree.size() > 0 ? &vfree[0] : NULL, solutions)) { fail_count++; } else { printf("joint angles: \n"); for (unsigned int i=0; i<num_of_joints; i++) { printf("%lf ", joints[i]); } printf("\n"); printf("translation: \n"); for (unsigned int i = 0; i < 3; i++) { printf("%lf ", eetrans[i]); } printf("\n"); printf("rotation matix: \n"); for (unsigned int i = 0; i < 9; i++) { printf("%lf%s", eerot[i], (i % 3 == 2)?"\n":" "); } printf("\n"); /*unsigned int num_of_solutions = (int)solutions.GetNumSolutions(); printf("Found %d ik solutions:\n", num_of_solutions ); std::vector<IKREAL_TYPE> solvalues(num_of_joints); for(std::size_t i = 0; i < num_of_solutions; ++i) { const IkSolutionBase<IKREAL_TYPE>& sol = solutions.GetSolution(i); int this_sol_free_params = (int)sol.GetFree().size(); printf("sol%d (free=%d): ", (int)i, this_sol_free_params ); std::vector<IKREAL_TYPE> vsolfree(this_sol_free_params); sol.GetSolution(&solvalues[0],vsolfree.size()>0?&vsolfree[0]:NULL); for( std::size_t j = 0; j < solvalues.size(); ++j) printf("%.15f, ", solvalues[j]); printf("\n"); }*/ } #else // for IKFast 54 ik(eetrans, eerot, vfree.size() > 0 ? &vfree[0] : NULL, vsolutions); #endif /* #if IK_VERSION > 54 // for IKFast 56,61 unsigned int num_of_solutions = (int)solutions.GetNumSolutions(); #else // for IKFast 54 unsigned int num_of_solutions = (int)vsolutions.size(); #endif printf("Found %d ik solutions:\n", num_of_solutions ); */ clock_gettime(CLOCK_REALTIME, &end_time); elapsed_time = (unsigned int)(end_time.tv_nsec - start_time.tv_nsec); sum_time += elapsed_time; } // endfor unsigned int avg_time = (unsigned int)sum_time / (unsigned int)num_of_tests; printf("avg time: %f ms over %d tests \n", (float)avg_time/1000.0, num_of_tests ); printf("Failed of %d tests\n", fail_count); return 1; } else if (cmd.compare("iktiming2") == 0) // for fixed joint values, check time performance of ik { if( argc != 2 ) { printf("\n " "Usage: \n\n " " ./compute iktiming2 \n\n" " For fixed number of iterations, with one set of joint variables, this \n" " finds the ik solutions and measures the average time taken. \n\n", num_of_joints-1 ); return 1; } printf("\n\n"); #if IK_VERSION > 54 // for IKFast 56,61 IkSolutionList<IKREAL_TYPE> solutions; #else // for IKFast 54 std::vector<IKSolution> vsolutions; #endif std::vector<IKREAL_TYPE> vfree(num_free_parameters); //for(std::size_t i = 0; i < vfree.size(); ++i) // vfree[i] = atof(argv[13+i]); IKREAL_TYPE joints[num_of_joints]; timespec start_time, end_time; unsigned int elapsed_time = 0; unsigned int sum_time = 0; #if IK_VERSION > 54 // for IKFast 56,61 unsigned int num_of_tests = 1000000; #else // for IKFast 54 unsigned int num_of_tests = 100000; #endif // fixed rotation-translation matrix corresponding to an unusual robot pose eerot[0] = 0.002569; eerot[1] = -0.658044; eerot[2] = -0.752975; eetrans[0] = 0.121937; eerot[3] = 0.001347; eerot[4] = -0.752975; eerot[5] = 0.658048; eetrans[1] = -0.276022; eerot[6] = -0.999996; eerot[7] = -0.002705; eerot[8] = -0.001047; eetrans[2] = 0.005685; for (unsigned int i=0; i < num_of_tests; i++) { clock_gettime(CLOCK_REALTIME, &start_time); #if IK_VERSION > 54 // for IKFast 56,61 ComputeIk(eetrans, eerot, vfree.size() > 0 ? &vfree[0] : NULL, solutions); #else // for IKFast 54 ik(eetrans, eerot, vfree.size() > 0 ? &vfree[0] : NULL, vsolutions); #endif /* #if IK_VERSION > 54 // for IKFast 56,61 unsigned int num_of_solutions = (int)solutions.GetNumSolutions(); #else // for IKFast 54 unsigned int num_of_solutions = (int)vsolutions.size(); #endif printf("Found %d ik solutions:\n", num_of_solutions ); */ clock_gettime(CLOCK_REALTIME, &end_time); elapsed_time = (unsigned int)(end_time.tv_nsec - start_time.tv_nsec); sum_time += elapsed_time; } // endfor unsigned int avg_time = (unsigned int)sum_time / (unsigned int)num_of_tests; printf("avg time: %f ms over %d tests \n", (float)avg_time/1000.0, num_of_tests ); return 1; } else { printf("\n" "Usage: \n\n"); printf(" ./compute fk j0 j1 ... j%d \n\n" " Returns the forward kinematic solution given the joint angles (in radians). \n\n", num_of_joints-1 ); printf("\n" " ./compute ik t0 t1 t2 qw qi qj qk free0 ... \n\n" " Returns the ik solutions given the transformation of the end effector specified by \n" " a 3x1 translation (tX), and a 1x4 quaternion (w + i + j + k). \n" " There are %d free parameters that have to be specified. \n\n", num_free_parameters ); printf(" \n" " ./compute ik r00 r01 r02 t0 r10 r11 r12 t1 r20 r21 r22 t2 free0 ...\n\n" " Returns the ik solutions given the transformation of the end effector specified by \n" " a 3x3 rotation R (rXX), and a 3x1 translation (tX). \n" " There are %d free parameters that have to be specified. \n\n", num_free_parameters ); printf("\n" " ./compute iktiming \n\n" " For fixed number of iterations, generates random joint angles, then \n" " calculates fk, calculates ik, measures average time taken. \n\n", num_of_joints-1 ); printf("\n" " ./compute iktiming2 \n\n" " For fixed number of iterations, with one set of joint variables, this \n" " finds the ik solutions and measures the average time taken. \n\n", num_of_joints-1 ); return 1; } return 0; }
DLLIMPORT int getTask11A_xyz(double *qps, double *kinArray, double *xyz) { FwdK11A fk(kinArray2Struct11A(kinArray)); TaskXYZ<FwdK11A> task(fk); task.qps2task(qps, xyz); return 0; }