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;
}
void btString_destroy( btString *s) {
if (s) {
if (s->buf) {
btfree(s->buf);
s->buf = NULL;
}
if (s->allocated) btfree(s);
}
}
void bts_strstream_destroy( btStream *bts) {
btStrStream *ss = (btStrStream *)bts;
if (!ss) return;
if (ss->bts.type != BTSTRSTREAM) return;
if (ss->buf) {
btfree(ss->buf);
ss->buf=NULL;
ss->len=0;
ss->max=0;
}
btfree( ss);
}
void btList_destroy( btList *buf) {
int i;
if (buf) {
if (buf->list) {
for (i=0; i < buf->len; i++) {
btObject_destroy( buf->list[i]);
}
btfree(buf->list);
buf->list = NULL;
}
if (buf->allocated) btfree (buf);
}
}
/* takes ownership of the string k, and object v */
int btDict_add( btDict *_this, btString* k, btObject* v) {
int hi, lo, mid, res, ipos;
int i;
int idx = _this->len++;
if (idx >= _this->dictsize) {
int newsize = QUANTIZE(idx+1);
_this->key = btrealloc( _this->key, sizeof(*_this->key)*newsize);
_this->value = btrealloc( _this->value, sizeof(*_this->value)*newsize);
_this->dictsize = newsize;
}
/* binary search */
ipos = hi = idx;
lo = 0;
while (lo < hi) {
mid = (lo + hi) / 2;
res = btString_cmp(k, &_this->key[mid]);
if (res == 0) {
return -1; /* key is already in the dictionary */
}
if (res < 0) ipos = hi = mid;
if (res > 0) lo = mid+1;
}
for (i=idx-1; i>=ipos; i--) {
_this->key[i+1] = _this->key[i];
_this->value[i+1] = _this->value[i];
}
btString_create( &_this->key[ipos]);
btString_setbuf( &_this->key[ipos], k->buf, k->len);
if (k->allocated) btfree(k);
_this->value[ipos] = v;
return 0;
}
/** \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;
}
/** Free memory allocated during new_vr()
Note that we are expecting the address of your pointer. As a matter of convention,
your pointer will be be set to NULL to indicate that it is no longer pointing to
a valid via_trj_array object.
\internal chk'd TH 051103
*/
void destroy_vta(via_trj_array** vt)
{
if (*vt != NULL) {
destroy_vr(&(*vt)->vr);
btfree((void**)vt);
}
}
void bts_filestream_destroy( btStream *bts) {
btFileStream *fs = (btFileStream *)bts;
if (!fs) return;
if (fs->bts.type != BTFILESTREAM) return;
if (fs->fp == NULL) return;
fclose(fs->fp);
fs->fp=NULL;
btfree( fs);
}
/*
* Return 0 if continue downloading
* Return 1 if block complete
*/
int
seg_writebuf( btFileSet *fs, int piece, int offset, char *buf, int len) {
int res=0;
int blocksize;
btPartialPiece *p=fs->partial;
while(p && p->piecenumber!=piece) {
p=p->next;
}
if (!p) {
//printf("got extra packet %d, offset %d\n", piece, offset);
return 0;
}
blocksize = seg_piecelen( fs, piece);
bt_assert(offset>=0 && offset<blocksize &&
offset+len<=blocksize && len>0);
memcpy(p->buffer + offset, buf, len);
bs_setRange( &p->filled, offset, offset+len);
#if 0
printf("packet %d of %d, offset %d, blocksize %d\n",
piece, fs->npieces, offset, blocksize);
#endif
if ( p->isdone || bs_isFull( &p->filled))
{
/* last data for this piece */
p->isdone=1;
if (_checkhash( fs, piece, p->buffer)) {
/* Remove from partial list */
btPartialPiece *pp=fs->partial;
if(pp==p) {
/* First in list */
fs->partial=p->next;
} else {
while(pp->next != p)
pp=pp->next;
pp->next=p->next;
}
fs->dl += blocksize;
fs->left -= blocksize;
#if 0
printf("hash ok for block %d\n", piece);
#endif
if( seg_write( fs, p) < 0 ) return -1;
kBitSet_finit( &p->filled);
btfree(p);
res = 1;
} else {
//printf("hash bad for block %d\n", piece);
}
}
return res;
}
void
kStringBuffer_finit( kStringBuffer* sb) {
if (sb) {
if ( sb->buf)
btfree( sb->buf);
sb->buf = NULL;
sb->bufsize = 0;
sb->cpos = 0;
}
}
void btFileSet_destroy( btFileSet *fs) {
btPartialPiece *p, *oldp;
int i;
for (i=0; i< fs->nfiles; i++) {
btfree( fs->file[i]->path);
btfree( fs->file[i]);
}
btfree( fs->file);
btfree( fs->hashes);
kBitSet_finit(&fs->completed);
p=fs->partial;
while(p) {
kBitSet_finit(&p->filled);
oldp=p;
p=p->next;
btfree(oldp);
}
memset( fs, 0, sizeof(*fs));
}
void btDict_destroy( btDict *_this) {
int i;
if (_this) {
for (i = 0; i < _this->len; i++) {
btString_destroy( &_this->key[i]);
if (_this->value[i]) {
btObject_destroy( _this->value[i]);
_this->value[i] = NULL;
}
}
if (_this->key) {
btfree(_this->key);
_this->key = NULL;
}
if (_this->value) {
btfree(_this->value);
_this->value = NULL;
}
}
if (_this->allocated) btfree (_this);
}
void destroy_pavn(pararray_vn** pavn)
{
int cnt;
if (*pavn != NULL) {
destroy_vn(&(*pavn)->result);
for (cnt = 0; cnt < (*pavn)->elements; cnt ++)
destroy_pa(&(*pavn)->pa[cnt]);
btfree((void**)pavn);
}
}
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;
}
void CTorrent::clientError(const char *activity, int cs)
{
int i, j;
char buf[128];
btContext *ctx = &m_Context;
const char *err = "connection closed";
btPeer *p = ctx->sockpeer[cs];
if (p->ios.error != 0) err = bts_strerror( p->ios.error);
/* errors on a socket */
//sprintf( buf, "%d: Peer %s shutdown %s (%s)", cs, inet_ntoa(ctx->sockpeer[cs]->sa.sin_addr), activity, err);
//m_pListener->notify( buf );
int dl=p->download;
peer_shutdown( ctx, p, err);
/* find where peer is */
if(dl!=INT_MAX) {
btPeerset *pset=&ctx->downloads[dl]->peerset;
for (i=0; i < pset->len; i++) {
if (pset->peer[i] == p) {
j = i;
pset->peer[i] = NULL;
break;
}
}
/* shift down the rest */
for (i=j; i < pset->len-1; i++) {
pset->peer[i] = pset->peer[i+1];
}
pset->len--;
}
btfree(p);
close(cs);
/* clear the execute bit if it was set */
if (ctx->x_set[cs]) {
ctx->xsock--;
ctx->x_set[cs] = 0;
}
}
void destroy_pa(pararray** pa)
{
btfree((void**)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;
}
void btInteger_destroy( btInteger *i) {
if (i && i->allocated) btfree(i);
}
