btString* btString_create_buf( btString *s, char *initbuf, int buflen) { s=btString_create(s); char *buf = btmalloc( buflen); memcpy( buf, initbuf, buflen); btString_setbuf(s,buf,buflen); return s; }
/** \brief The benc_get_string function deserializes a btSring from the stream 'bts' using the BEncoding scheme. \param bts a parameter of type struct btStream* \param s a parameter of type btString * \return int 0 - success, 1 - some error */ int benc_get_string( struct btStream* bts, btString *s) { int res; char ibuf[10]; char *sbuf; int slen; res = bts_scanbreak( bts, "0123456789", ":", ibuf, sizeof(ibuf)); if (res) return res; slen= atoi(ibuf); bt_assert(slen<MAXSTRING); sbuf = btmalloc(slen+1); bt_assert(sbuf); if (slen > 0) { res = bts_read( bts, sbuf, slen); } else if (slen < 0) { errno = BTERR_NEGATIVE_STRING_LENGTH; res = -1; } else { *sbuf = 0; res = 0; } if (res) { btfree(sbuf); return res; } sbuf[slen]=0; btString_setbuf(s, sbuf, slen); return 0; }
btObject *btObject_val( btObject *o, char *index) { char *cpos = index; char *buf; int len; for(;;) { while( *cpos != '/' && *cpos) cpos++; len = cpos-index; buf=btmalloc(len+1); memcpy(buf, index, len); buf[len]=0; if (o->t == BT_LIST) { int idx = atoi(buf); btList *l=BTLIST(o); o=btList_index(l,idx); } else if (o->t == BT_DICT) { btString k; btDict *d=BTDICT(o); btString_create_str(&k, buf); o=btDict_find(d,&k); btString_destroy(&k); } else { btfree(buf); return NULL; } btfree(buf); if (!*cpos) break; index=++cpos; } return o; }
btString* btString_create( btString *s) { int allocated = 0; if (s == NULL) { s=btmalloc(sizeof( btString)); allocated = 1; } memset( s, 0, sizeof(btString)); s->allocated = allocated; s->parent.t = BT_STRING; return s; }
btInteger* btInteger_create( btInteger *i) { int allocated = 0; if (i == NULL) { i=btmalloc(sizeof( btInteger)); allocated = 1;} memset( i, 0, sizeof(btInteger)); i->allocated = allocated; i->parent.t = BT_INTEGER; return i; }
btDict* btDict_create( btDict *buf) { int allocated = 0; if (buf == NULL) { buf=btmalloc(sizeof( btDict)); allocated = 1; } memset( buf, 0, sizeof(btDict)); buf->allocated = allocated; buf->parent.t = BT_DICT; return buf; }
btDictIt* btDictIt_create( btDictIt *buf, btDict *d) { if (!buf) { buf = btmalloc( sizeof(btDictIt)); bt_assert(buf); } memset(buf, 0, sizeof(btDictIt)); buf->d = d; return buf; }
/** \internal chk'd TH 051103 Internal function to allocate memory for a vta object. */ via_trj_array* malloc_vta(int num_columns) { void *vmem; via_trj_array* vt; vmem = btmalloc(sizeof(via_trj_array) + (size_t)num_columns * sizeof(via_trj)); vt = (via_trj_array*)vmem; vt->trj = vmem + sizeof(via_trj_array); vt->elements = num_columns; return vt; }
int alloc_perf_buffer(int size) { int i; printk("Perfmon: allocate buffer\n"); if(perfmon_base.state == PMC_STATE_INITIAL) { perfmon_base.profile_length = (((unsigned long) &_etext - (unsigned long) &_stext) >> 2) * sizeof(int); perfmon_base.profile_buffer = (unsigned long)btmalloc(perfmon_base.profile_length); perfmon_base.trace_length = 1024*1024*16; perfmon_base.trace_buffer = (unsigned long)btmalloc(perfmon_base.trace_length); perfmon_base.timeslice_length = PMC_SLICES_MAX*PMC_MAX_COUNTERS*PMC_MAX_CPUS; perfmon_base.timeslice_buffer = (unsigned long)pmc_timeslice_data; if(perfmon_base.profile_buffer && perfmon_base.trace_buffer) { memset((char *)perfmon_base.profile_buffer, 0, perfmon_base.profile_length); printk("Profile buffer created at address 0x%lx of length 0x%lx\n", perfmon_base.profile_buffer, perfmon_base.profile_length); } else { printk("Profile buffer creation failed\n"); return 0; } /* Fault in the first bolted segment - it then remains in the stab for all time */ pmc_touch_bolted(NULL); smp_call_function(pmc_touch_bolted, (void *)NULL, 0, 1); for (i=0; i<MAX_PACAS; ++i) { paca[i].prof_shift = 2; paca[i].prof_len = perfmon_base.profile_length; paca[i].prof_buffer = (unsigned *)(perfmon_base.profile_buffer); paca[i].prof_stext = (unsigned *)&_stext; paca[i].prof_etext = (unsigned *)&_etext; mb(); } perfmon_base.state = PMC_STATE_READY; }
kStringBuffer* kStringBuffer_create( kStringBuffer* _sb) { kStringBuffer *sb; if (_sb) { sb = _sb; } else { sb = btmalloc( sizeof( kStringBuffer)); } memset( sb, 0, sizeof( kStringBuffer)); return sb; }
/************************ pararray_vn **************************************/ pararray_vn* new_pavn(int max, int elements) { void *vmem; int cnt; pararray_vn *pavn; vmem = btmalloc(sizeof(pararray_vn) + sizeof(pararray*) * elements); pavn = (pararray_vn*)vmem; pavn->pa = (pararray**)(vmem + sizeof(pararray_vn)); pavn->elements = elements; for (cnt = 0; cnt < elements; cnt++) pavn->pa[cnt] = new_pa(max); pavn->result = new_vn(elements); return pavn; }
btFileSet* btFileSet_create( btFileSet *fs, int npieces, int blocksize, const char *hashbuf) { if (!fs) { fs = btcalloc( 1, sizeof(btFileSet)); bt_assert(fs); } fs->npieces = npieces; fs->blocksize = blocksize; fs->dl = 0; fs->ul = 0; fs->hashes = btmalloc( npieces * SHA_DIGEST_LENGTH); memcpy(fs->hashes, hashbuf, npieces * SHA_DIGEST_LENGTH); kBitSet_create(&fs->completed, npieces); return fs; }
int seg_review( btFileSet *fs, kBitSet *writeCache, int piece) { char *buf; int rs = 0; int len = seg_piecelen( fs, piece); buf = btmalloc(len); /* Trick readbuf into reading the block */ bs_set(writeCache, piece); if (seg_readbuf(fs, writeCache, piece, 0, buf, len)) { rs = -1; } else if (_checkhash( fs, piece, buf)) { rs = 1; } if(rs!=1) { bs_clr(writeCache, piece); } btfree(buf); return rs; }
/************** Segment array functions *****************/ pararray* new_pa(int max) { void* mem; pararray* pa; /*malloc_pa*/ mem = btmalloc(sizeof(parabolic) * max + sizeof(pararray)); /*initptr_pa*/ pa = (pararray*)mem; pa->pb = (parabolic*)(mem + sizeof(pararray)); /*initval_pa*/ pa->max = max; pa->cnt = 0; pa->state = BTTRAJ_STOPPED; pa->t = 0.0; return pa; }
int do_mode_mu() { int err; /* Generic error variable for function calls */ int busCount; /* Number of WAMs defined in the configuration file */ int j; /* Stuff that's filled in once */ vect_n ** poses; int num_poses; int pose; vect_n * angle_diff; /* Stuff that's used once for each pose in measure mode */ vect_n * torques_top; vect_n * positions_top; vect_n * torques_bottom; vect_n * positions_bottom; /* For storing the results from measure mode, one vector per pose */ vect_n ** torques; vect_n ** positions; /* For storing the results from computation mode, one vector per joint */ vect_3 ** mus; /* Make a list of phases for each pose*/ enum { MU_P_START, MU_P_TO_TOP, MU_P_FROM_TOP, MU_P_MEAS_TOP, MU_P_TO_BOT, MU_P_FROM_BOT, MU_P_MEAS_BOT, MU_P_DONE } phase; char * phasenm[] = { "START", "TO_TOP", "FROM_TOP", "MEAS_TOP", "TO_BOT", "FROM_BOT", "MEAS_BOT", "DONE" }; clear(); mvprintw(0,0,"Starting Gravity Calibration Mode"); /* Ensure the WAM is set up, power cycled, and in the home position */ mvprintw(2,0,"To begin the calibration, follow the following steps:"); mvprintw(3,0," a) Ensure that all WAM power and signal cables are securely fastened."); mvprintw(4,0," b) Ensure that the WAM is powered on (receiving power)."); mvprintw(5,0," c) Ensure that all E-STOPs are released."); mvprintw(6,0," d) Place the WAM in Shift+Idle mode."); mvprintw(7,0," e) Carefully ensure that the WAM is in its home (folded) position."); mvprintw(8,0,"Press [Enter] to continue."); refresh(); while (btkey_get()!=BTKEY_ENTER) usleep(10000); /* Initialize system buses */ err = ReadSystemFromConfig("../../wam.conf", &busCount); if(err) return -1; /* Spin off the CAN thread */ startDone = 0; btrt_thread_create(&can_thd,"can",45,(void*)can_thd_function,NULL); while (!startDone) usleep(10000); /* Allocate the global variables */ mu_jts = (double **) malloc( wam->dof * sizeof(double *) ); mu_jps = (double **) malloc( wam->dof * sizeof(double *) ); for (j=0; j<wam->dof; j++) { mu_jts[j] = (double *) malloc( NUM_POINTS * sizeof(double) ); mu_jps[j] = (double *) malloc( NUM_POINTS * sizeof(double) ); } /* Spin off the WAM thread */ mu_n = NUM_POINTS; wam_thd.period = 0.002; /* Control loop period in seconds */ registerWAMcallback(wam, mu_callback); btrt_thread_create(&wam_thd,"ctrl",90,(void*)WAMControlThread,(void*)wam); /* Grab poses from the configuration file */ do { btparser parser; char key[256]; err = btParseFile(&parser,"cal.conf"); if (err) { syslog(LOG_ERR,"Calibration configuration file cal.conf not found."); printf("Calibration configuration file cal.conf not found.\n"); break; } sprintf(key,"calibration-poses-wam%d.poseCount",wam->dof); err = btParseGetVal(&parser,INT, key, &num_poses); if (err || num_poses < 4) { syslog(LOG_ERR,"Configuration group calibration-poses-wam%d not found,",wam->dof); syslog(LOG_ERR,"numPoses not present, or numPoses not at least 4."); printf("Configuration group calibration-poses-wam%d not found,\n",wam->dof); printf("numPoses not present, or numPoses not at least 4."); btParseClose(&parser); break; } poses = (vect_n **) btmalloc( num_poses * sizeof(vect_n *) ); for (pose=0; pose<num_poses; pose++) { poses[pose] = new_vn(wam->dof); sprintf(key, "calibration-poses-wam%d.pose[%d]", wam->dof, pose); err = btParseGetVal(&parser, VECTOR, key, (void*)poses[pose]); if (err) { syslog(LOG_ERR,"Not enough poses found! (%d expected, found only %d)", num_poses, pose); printf("Not enough poses found! (%d expected, found only %d)\n", num_poses, pose); break; } } btParseClose(&parser); } while (0); if (err) { /* Free the variables */ for (j=0; j<wam->dof; j++) { free(mu_jts[j]); free(mu_jps[j]); } free(mu_jts); free(mu_jps); /* Stop threads ... */ wam_thd.done = 1; usleep(10000); can_thd.done = 1; usleep(50000); /* End ncurses */ endwin(); return 0; } angle_diff = new_vn(wam->dof); fill_vn(angle_diff,ANGLE_DIFF); torques_top = new_vn(wam->dof); positions_top = new_vn(wam->dof); torques_bottom = new_vn(wam->dof); positions_bottom = new_vn(wam->dof); torques = (vect_n **) malloc( num_poses * sizeof(vect_n *) ); positions = (vect_n **) malloc( num_poses * sizeof(vect_n *) ); for (pose=0; pose<num_poses; pose++) { torques[pose] = new_vn(wam->dof); positions[pose] = new_vn(wam->dof); } mus = (vect_3 **) malloc( wam->dof * sizeof(vect_3 *) ); for (j=0; j<wam->dof; j++) mus[j] = new_v3(); /* Allow the user to shift-activate */ mvprintw(10,0,"IMPORTANT: Once gravity compensation begins, the WAM will begin"); mvprintw(11,0,"to move to a set of %d predefined poses (defined in cal.conf).",num_poses); mvprintw(13,0,"DO NOT TOUCH the WAM during the measurement process, or the"); mvprintw(14,0,"calibration computations will be sigificantly wrong, and"); mvprintw(15,0,"any subsequent gravity compensation will fail spectacularly!"); mvprintw(17,0,"Place the WAM in Shift+Activate mode,"); mvprintw(18,0,"and press [Enter] to start."); refresh(); while (btkey_get()!=BTKEY_ENTER) usleep(10000); /* Start the GUI! */ pose = 0; phase = MU_P_START; clear(); mvprintw(1,0,"Note: Press [Control+C] at any time to cancel the calibration."); mvprintw(2,0,"DO NOT TOUCH the WAM during the calibration process!"); done = 0; signal(SIGINT, sigint); while (!done) { /* Print current state */ mvprintw(0, 0,"Current Pose: %d of %d. ",pose+1,num_poses); mvprintw(0,30,"Current Phase: %s ",phasenm[phase]); /* Print current joint position and torque */ mvprintw(4,0, " Joint:"); mvprintw(5,0, " Position:"); mvprintw(6,0, " Torque:"); for (j=0; j<wam->dof; j++) { mvprintw(4, 13 + 9*j, " Joint %d ",j+1); mvprintw(5, 13 + 9*j, "% 08.5f ",wam->Jpos->q[j]); mvprintw(6, 13 + 9*j, "% 08.4f ",wam->Jtrq->q[j]); } /* Line 9 - Status Updates */ mvprintw(8,0,"Current Status:"); /* Note - lines 12-?? reserved for printing statistics */ mvprintw(11,0,"Recent Statistics:"); refresh(); usleep(5E4); /* Move through the state machine */ switch (phase) { case MU_P_START: mvprintw(9,3,"Moving to above position ... "); MoveSetup(wam, 1.0, 1.0); MoveWAM(wam,add_vn(poses[pose],angle_diff)); phase++; break; case MU_P_TO_TOP: if (!MoveIsDone(wam)) break; mvprintw(9,3,"Moving to position (from above) ... "); MoveSetup(wam, 0.05, 0.05); MoveWAM(wam,poses[pose]); phase++; case MU_P_FROM_TOP: if (!MoveIsDone(wam)) break; mvprintw(9,3,"Starting a measurement ... "); mu_n = 0; phase++; break; case MU_P_MEAS_TOP: if (mu_n < NUM_POINTS) break; mu_stats( torques_top, positions_top ); mvprintw(9,3,"Moving to below position ... "); MoveSetup(wam, 1.0, 1.0); MoveWAM(wam,sub_vn(poses[pose],angle_diff)); phase++; break; case MU_P_TO_BOT: if (!MoveIsDone(wam)) break; mvprintw(9,3,"Moving to position (from below) ... "); MoveSetup(wam, 0.05, 0.05); MoveWAM(wam,poses[pose]); phase++; break; case MU_P_FROM_BOT: if (!MoveIsDone(wam)) break; mvprintw(9,3,"Starting a measurement ... "); mu_n = 0; phase++; break; case MU_P_MEAS_BOT: if (mu_n < NUM_POINTS) break; mu_stats( torques_bottom, positions_bottom ); phase++; break; case MU_P_DONE: /* Get the midpoint position and torque ... */ set_vn(torques[pose],scale_vn(0.5,add_vn(torques_top,torques_bottom))); set_vn(positions[pose],scale_vn(0.5,add_vn(positions_top,positions_bottom))); pose++; phase = MU_P_START; if (pose == num_poses) done = 1; break; } } /* Re-fold, print, and exit */ clear(); mvprintw(0,0,"Moving back to the park location ..."); refresh(); MoveSetup(wam, 1.0, 1.0); MoveWAM(wam,wam->park_location); while (!MoveIsDone(wam)) usleep(10000); mvprintw(1,0,"Shift+Idle, and press [Enter] to continue."); refresh(); while (btkey_get()!=BTKEY_ENTER) usleep(10000); /* Stop threads ... */ wam_thd.done = 1; usleep(10000); can_thd.done = 1; usleep(50000); /* Free unneeded variables */ for (j=0; j<wam->dof; j++) { free(mu_jts[j]); free(mu_jps[j]); } free(mu_jts); free(mu_jps); destroy_vn(&angle_diff); destroy_vn(&torques_top); destroy_vn(&positions_top); destroy_vn(&torques_bottom); destroy_vn(&positions_bottom); /* End ncurses */ endwin(); if (done == 1) { /* Here we have the "Iterative Algorithm" * described in the Chris Dellin document entitled * "Newton-Euler First-Moment Gravity Compensation" */ /* Here we have the matrix composed of L matrices (negative) */ matr_mn * nLL; /* We have a GT matrix and a Y vector for each link */ matr_mn ** GT; vect_n ** Y; /* We have a solution vector P for each link */ vect_n ** P; vect_n * grav_torques; /* Start calculating ... * We have vectors of torque and position * in torques[] and positions[] */ printf("\n"); printf("Calculating ...\n"); /* Make the LL matrix */ nLL = new_mn( 3*num_poses, 3+2*num_poses ); zero_mn(nLL); for (pose=0; pose<num_poses; pose++) { setval_mn(nLL, 3*pose+0, 3+2*pose+0, -1.0 ); setval_mn(nLL, 3*pose+1, 3+2*pose+1, -1.0 ); } /* Make each link's GT matrix */ /* Make each link's Y torque solution matrix */ Y = (vect_n **) btmalloc( wam->dof * sizeof(vect_n *) ); GT = (matr_mn **) btmalloc( wam->dof * sizeof(matr_mn *) ); for (j=0; j<wam->dof; j++) { Y[j] = new_vn( 3*num_poses ); GT[j] = new_mn( 3*num_poses, 3+2*num_poses ); zero_mn(GT[j]); } grav_torques = new_vn(wam->dof); for (pose=0; pose<num_poses; pose++) { /* Put the robot in the pose (by position) */ set_vn(wam->robot.q,positions[pose]); eval_fk_bot(&wam->robot); get_gravity_torques(&wam->robot,grav_torques); for (j=0; j<wam->dof; j++) { /* GT: Gravity skew matrix */ setval_mn(GT[j], 3*pose+0, 1, -getval_v3(wam->robot.links[j].g,2) ); setval_mn(GT[j], 3*pose+0, 2, getval_v3(wam->robot.links[j].g,1) ); setval_mn(GT[j], 3*pose+1, 0, getval_v3(wam->robot.links[j].g,2) ); setval_mn(GT[j], 3*pose+1, 2, -getval_v3(wam->robot.links[j].g,0) ); setval_mn(GT[j], 3*pose+2, 0, -getval_v3(wam->robot.links[j].g,1) ); setval_mn(GT[j], 3*pose+2, 1, getval_v3(wam->robot.links[j].g,0) ); /* GT: -R*L */ setval_mn(GT[j], 3*pose+0, 3+2*pose+0, -getval_mh(wam->robot.links[j].trans,0,0) ); setval_mn(GT[j], 3*pose+0, 3+2*pose+1, -getval_mh(wam->robot.links[j].trans,1,0) ); setval_mn(GT[j], 3*pose+1, 3+2*pose+0, -getval_mh(wam->robot.links[j].trans,0,1) ); setval_mn(GT[j], 3*pose+1, 3+2*pose+1, -getval_mh(wam->robot.links[j].trans,1,1) ); setval_mn(GT[j], 3*pose+2, 3+2*pose+0, -getval_mh(wam->robot.links[j].trans,0,2) ); setval_mn(GT[j], 3*pose+2, 3+2*pose+1, -getval_mh(wam->robot.links[j].trans,1,2) ); /* Y */ setval_vn(Y[j], 3*pose+0, getval_vn(torques[pose],j) * getval_mh(wam->robot.links[j].trans,2,0) ); setval_vn(Y[j], 3*pose+1, getval_vn(torques[pose],j) * getval_mh(wam->robot.links[j].trans,2,1) ); setval_vn(Y[j], 3*pose+2, getval_vn(torques[pose],j) * getval_mh(wam->robot.links[j].trans,2,2) ); if (j < wam->dof-1) setval_vn(Y[j], 3*pose+2, getval_vn(Y[j],3*pose+2) - getval_vn(torques[pose],j+1) ); } } destroy_vn(&grav_torques); /* Make a space for each link's P solution vector */ P = (vect_n **) btmalloc( wam->dof * sizeof(vect_n *) ); for (j=0; j<wam->dof; j++) P[j] = new_vn( 3+2*num_poses ); /* Do the regression for each link */ { int i; matr_mn * m2x2; matr_mn * m2x3; vect_n * Yi; vect_n * index; vect_n * col; m2x2 = new_mn( 3+2*num_poses, 3+2*num_poses ); m2x3 = new_mn( 3+2*num_poses, 3*num_poses ); Yi = new_vn( 3*num_poses ); index = new_vn( 3+2*num_poses ); col = new_vn( 3+2*num_poses ); for (j=wam->dof-1; j>=0; j--) { mul_mn( m2x2, T_mn(GT[j]), GT[j] ); for (i=0; i<3+2*num_poses; i++) setval_mn(m2x2,i,i, getval_mn(m2x2,i,i) + CALC_LAMBDA ); mul_mn( m2x3, inv_mn(m2x2,index,col), T_mn(GT[j]) ); if (j < wam->dof-1) matXvec_mn(nLL,P[j+1],Yi); else fill_vn(Yi,0.0); set_vn(Yi, add_vn(Yi,Y[j]) ); matXvec_mn( m2x3, Yi, P[j] ); } destroy_mn(&m2x2); destroy_mn(&m2x3); destroy_vn(&Yi); destroy_vn(&index); destroy_vn(&col); /* Copy the results */ for (j=0; j<wam->dof; j++) { setval_v3(mus[j],0, getval_vn(P[j],0) ); setval_v3(mus[j],1, getval_vn(P[j],1) ); setval_v3(mus[j],2, getval_vn(P[j],2) ); } /* Clean up */ destroy_mn(&nLL); for (j=0; j<wam->dof; j++) { destroy_mn(>[j]); destroy_vn(&Y[j]); destroy_vn(&P[j]); } btfree((void **)>); btfree((void **)&Y); btfree((void **)&P); } /* Print results */ printf("\n"); printf("Gravity calibration ended.\n"); printf("\n"); printf("Copy the following lines into your wam.conf file, in the %s{} group.\n",wam->name); printf("It is usually placed above the safety{} group.\n"); printf("--------\n"); printf(" # Calibrated gravity vectors\n"); printf(" calibrated-gravity{\n"); for (j=0; j<wam->dof; j++) { printf(" mu%d = < % .6f, % .6f, % .6f >\n", j+1, getval_v3(mus[j],0), getval_v3(mus[j],1), getval_v3(mus[j],2)); } printf(" }\n"); printf("--------\n"); { FILE * logfile; logfile = fopen("cal-gravity.log","w"); if (logfile) { fprintf(logfile," # Calibrated gravity vectors\n"); fprintf(logfile," calibrated-gravity{\n"); for (j=0; j<wam->dof; j++) fprintf(logfile," mu%d = < % .6f, % .6f, % .6f >\n", j+1, getval_v3(mus[j],0), getval_v3(mus[j],1), getval_v3(mus[j],2)); fprintf(logfile," }\n"); fclose(logfile); printf("This text has been saved to cal-gravity.log.\n"); } else { syslog(LOG_ERR,"Could not write to cal-gravity.log."); printf("Error: Could not write to cal-gravity.log.\n"); } } printf("\n"); } /* Free the variables */ for (pose=0; pose<num_poses; pose++) { destroy_vn(&torques[pose]); destroy_vn(&positions[pose]); } free(torques); free(positions); for (j=0; j<wam->dof; j++) destroy_vn((vect_n **)&mus[j]); free(mus); return 0; }