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;
}
