int huboLoop(int mode, bool compliance_mode, bool pause_feature) {
double newRef[2] = {1.0, 0.0};
// get initial values for hubo
struct hubo_ref H_ref;
memset( &H_ref, 0, sizeof(H_ref));
hubo_virtual_t H_virtual;
memset( &H_virtual, 0, sizeof(H_virtual));
size_t fs;
int r = ach_get( &chan_hubo_ref, &H_ref, sizeof(H_ref), &fs, NULL, ACH_O_COPY );
if(ACH_OK != r) {
if(hubo_debug) {
// printf("Ref ini r = %s\n",ach_result_to_string(r));
}
}
else{ assert( sizeof(H_ref) == fs ); }
struct hubo_state H_state;
memset( &H_state, 0, sizeof(H_state) );
r = ach_get( &chan_hubo_state, &H_state, sizeof(H_state), &fs, NULL, ACH_O_COPY );
if (ACH_OK != r) {
if (hubo_debug) {
//printf("State ini r = %s\n", ach_result_to_string(r));
}
} else {
assert( sizeof(H_state) == fs );
}
// time info
struct timespec t;
/* Sampling Period */
double T = (double)interval/(double)NSEC_PER_SEC; // (sec)
clock_gettime( 0,&t);
// ------------------------------------------------------------------------------
// ---------------[ DO NOT EDIT AVBOE THIS LINE]---------------------------------
// ------------------------------------------------------------------------------
// char* fileName = "valve0.traj";
runTraj(fileName,mode, &H_ref, &t, &H_state, compliance_mode, pause_feature);
// ------------------------------------------------------------------------------
// ---------------[ DO NOT EDIT BELOW THIS LINE]---------------------------------
// ------------------------------------------------------------------------------
printf("Trajectory Finished\n");
return 0;
}
int main(int argc, char* argv[] )
{
// Copy to char pointers
char outputChanChar[1024];
strcpy(outputChanChar, PERCEPTION_CHANNEL.c_str());
char debugChanChar[1024];
strcpy(debugChanChar, DEBUG_CHANNEL.c_str());
// open the channels
int r = ach_open( &channel, outputChanChar, NULL );
int rdebug = ach_open( &debug_channel, debugChanChar, NULL );
assert(ACH_OK == r && ACH_OK == rdebug);
r = ach_flush(&channel);
r = ach_flush(&debug_channel);
// test receive
double rtraj[NUM_OBJECTS][NDIM];
memset(rtraj, 0, NUM_OBJECTS*NDIM*sizeof(double));
size_t frame_size;
while( true ) {
r = ach_get( &channel, &rtraj,
sizeof(rtraj),
&frame_size,
NULL,
ACH_O_WAIT );
std::cout << "Received traj (visible, x, y, angle): " << std::endl;
print_arr_2d( rtraj, NUM_OBJECTS );
// DEBUG CHANNEL
r = ach_get( &debug_channel, &rtraj,
sizeof(rtraj),
&frame_size,
NULL,
ACH_O_WAIT );
std::cout << "[DEBUG] Received traj (visible, x, y, angle): " << std::endl;
print_arr_2d( rtraj, NUM_OBJECTS );
// Read every second
usleep(1.0*1e6);
}
}
int main( int argc, char **argv ) {
ach_channel_t chan;
sns_start();
// open channel
sns_chan_open( &chan, "pir-state", NULL );
{
ach_channel_t *chans[] = {&chan, NULL};
sns_sigcancel( chans, sns_sig_term_default );
}
// state
/* -- RUN -- */
while (!sns_cx.shutdown) {
struct pir_state state;
size_t frame_size;
ach_status_t r = ach_get( &chan, &state, sizeof(state), &frame_size,
NULL, ACH_O_LAST );
switch(r) {
case ACH_OK:
case ACH_MISSED_FRAME:
dump(&state);
case ACH_CANCELED:
case ACH_TIMEOUT:
case ACH_STALE_FRAMES:
break;
default:
SNS_LOG(LOG_ERR, "Failed to get frame: %s\n", ach_result_to_string(r) );
}
}
}
void receiver_ach(int rt) {
fprintf(stderr,"receiver\n");
make_realtime(99);
/* flush some initial delayed messages */
size_t i;
for( i = 0; i < 5; i ++ ) {
ticks_t ticks;
size_t fs;
enum ach_status r = ach_get(&chan, &ticks, sizeof(ticks), &fs, NULL,
ACH_O_LAST | ACH_O_WAIT);
switch (r) {
case ACH_OK: case ACH_MISSED_FRAME: break;
default:
fprintf(stderr, "bad ach_get result: %s\n", ach_result_to_string(r));
exit(EXIT_FAILURE);
}
}
/* now the good stuff */
ticks_t ticks = get_ticks();
while(1) {
size_t fs;
ticks_t then;
if (KERNDEV) {
then.tv_sec = 1;
then.tv_nsec = 0;
} else {
then = ticks;
then.tv_sec += 1;
}
int r = ach_get(&chan, &ticks, sizeof(ticks), &fs, &then,
ACH_O_LAST | ACH_O_WAIT);
ticks_t now = get_ticks();
if( ACH_TIMEOUT == r ) break;
assert(ACH_OK == r || sizeof(ticks) == fs);
/* only print real-time latencies */
if (rt) {
send_time((float)ticks_delta(ticks,now));
}
}
}
int huboLoop() {
double newRef[2] = {1.0, 0.0};
// get initial values for hubo
struct hubo_ref H_ref;
memset( &H_ref, 0, sizeof(H_ref));
size_t fs;
int r = ach_get( &chan_hubo_ref, &H_ref, sizeof(H_ref), &fs, NULL, ACH_O_COPY );
if(ACH_OK != r) {
if(hubo_debug) {
printf("Ref ini r = %s\n",ach_result_to_string(r));}
}
else{ assert( sizeof(H_ref) == fs ); }
// time info
struct timespec t;
/* Sampling Period */
double T = (double)interval/(double)NSEC_PER_SEC; // (sec)
clock_gettime( 0,&t);
// ------------------------------------------------------------------------------
// ---------------[ DO NOT EDIT AVBOE THIS LINE]---------------------------------
// ------------------------------------------------------------------------------
// char* fileName = "valve0.traj";
runTraj(fileName, &H_ref, &t);
// runTraj("ybTest1.traj",&H_ref_filter, &t);
// runTraj("valve0.traj", &H_ref_filter, &t);
// runTraj("valve1.traj", &H_ref_filter, &t);
// runTraj("valve2.traj", &H_ref_filter, &t);
// runTraj("valve1.traj", &H_ref_filter, &t);
// runTraj("valve2.traj", &H_ref_filter, &t);
// runTraj("valve1.traj", &H_ref_filter, &t);
// runTraj("valve2.traj", &H_ref_filter, &t);
// runTraj("valve1.traj", &H_ref_filter, &t);
// runTraj("valve2.traj", &H_ref_filter, &t);
// runTraj("valve1.traj", &H_ref_filter, &t);
// runTraj("valve2.traj", &H_ref_filter, &t);
// runTraj("valve3.traj", &H_ref_filter, &t);
// ------------------------------------------------------------------------------
// ---------------[ DO NOT EDIT BELOW THIS LINE]---------------------------------
// ------------------------------------------------------------------------------
printf("Trajectory Finished\n");
return 0;
}
// Function to get encoder value from the Ach channel
double* getEncoderValues(){
static double encoderValues[40];
int t2 = ach_open(&chan_hubo_state, HUBO_CHAN_STATE_NAME, NULL);
assert( ACH_OK == t2);
// open to sim chan
int t1 = ach_open(&chan_hubo_from_sim, HUBO_CHAN_VIRTUAL_FROM_SIM_NAME, NULL);
assert( ACH_OK == t1);
struct hubo_state H_state;
size_t fs;
int r= ach_get( &chan_hubo_state, &H_state, sizeof(H_state), &fs, NULL, ACH_O_LAST );
if(hubo_debug) {
// printf("State ini r = %s\n",ach_result_to_string(r));
}
else{
assert( sizeof(H_state) == fs );
}
encoderValues[0]=H_state.joint[RHY].pos;
encoderValues[1]=H_state.joint[RHR].pos;
encoderValues[2]=H_state.joint[RHP].pos;
encoderValues[3]=H_state.joint[RKN].pos;
encoderValues[4]=H_state.joint[RAP].pos;
encoderValues[5]=H_state.joint[RAR].pos;
encoderValues[6]=H_state.joint[LHY].pos;
encoderValues[7]=H_state.joint[LHR].pos;
encoderValues[8]=H_state.joint[LHP].pos;
encoderValues[9]=H_state.joint[LKN].pos;
encoderValues[10]=H_state.joint[LAP].pos;
encoderValues[11]=H_state.joint[LAR].pos;
encoderValues[12]=H_state.joint[RSP].pos;
encoderValues[13]=H_state.joint[RSR].pos;
encoderValues[14]=H_state.joint[RSY].pos;
encoderValues[15]=H_state.joint[REB].pos;
encoderValues[16]=H_state.joint[RWY].pos;
encoderValues[17]=H_state.joint[RWR].pos;
encoderValues[18]=H_state.joint[RWP].pos;
encoderValues[19]=H_state.joint[LSP].pos;
encoderValues[20]=H_state.joint[LSR].pos;
encoderValues[21]=H_state.joint[LSY].pos;
encoderValues[22]=H_state.joint[LEB].pos;
encoderValues[23]=H_state.joint[LWY].pos;
encoderValues[24]=H_state.joint[LWR].pos;
encoderValues[25]=H_state.joint[LWP].pos;
encoderValues[26]=H_state.joint[NKY].pos;
encoderValues[27]=H_state.joint[NK1].pos;
encoderValues[28]=H_state.joint[NK2].pos;
encoderValues[29]=H_state.joint[WST].pos;
return encoderValues;
}
HuboCan::error_result_t receive_data(double timeout_seconds = 0)
{
if(!_check_initialized("receive_data"))
return HuboCan::ACH_ERROR;
size_t fs = 0;
struct timespec wait_time;
clock_gettime( ACH_DEFAULT_CLOCK, &wait_time );
long long nano_wait = wait_time.tv_nsec + (long long)(timeout_seconds*1E9);
wait_time.tv_sec += (int)(nano_wait/1E9);
wait_time.tv_nsec = (int)(nano_wait%((int)1E9));
ach_status_t r = ach_get(&_channel, _raw_data,
get_data_size<DataClass>(_raw_data),
&fs, &wait_time, ACH_O_LAST | ACH_O_WAIT);
if( ACH_TIMEOUT == r )
{
if(verbose)
{
std::cout << "[HuboData::receive_data] Ach channel '" << _channel_name
<< "' timed out!" << std::endl;
}
return HuboCan::TIMEOUT;
}
if(fs != get_data_size<DataClass>(_raw_data))
{
std::cout << "[HuboData::receive_data] Framesize mismatch for '" << _channel_name
<< "': " << fs << " received, " << get_data_size<DataClass>(_raw_data)
<< " expected!" << std::endl;
}
if( ACH_OK == r || ACH_STALE_FRAMES == r || ACH_MISSED_FRAME == r )
{
if(verbose)
{
std::cout << "[HuboData::receive_data] Ach result for channel '"
<< _channel_name << "': " << ach_result_to_string(r) << std::endl;
}
return HuboCan::OKAY;
}
else
{
std::cout << "[HuboData::receive_data] Unexpected ach result for channel '"
<< _channel_name << "'. Check error log for more info" << std::endl;
report_ach_errors(r, "HuboData::receive_data", "ach_get", _channel_name.c_str());
return HuboCan::ACH_ERROR;
}
return HuboCan::UNDEFINED_ERROR;
}
/* print samples periodically */
void periodic_logger(void) {
enum ach_status r = ach_open(&chan_feedback, "feedback", NULL);
if(ACH_OK != r) abort();
while(1) {
x_t X;
size_t fs;
r = ach_get( &chan_feedback, X, sizeof(X), &fs, NULL, ACH_O_WAIT|ACH_O_LAST );
if( ach_status_match(r, ACH_MASK_OK | ACH_MASK_MISSED_FRAME) && sizeof(X) == fs ) {
printf("%f\t%f\t%f\n", X[0], X[1], X[2]);
usleep((int) (1e6 * 0.1)); /* 10 Hertz */
} else abort();
}
exit(0);
}
void Aggregator::_aggregator_loop()
{
size_t max_expected_size = hubo_cmd_data_get_size(_input_data);
while(_rt.good())
{
size_t fs;
double quit_check = 1;
struct timespec wait_time;
clock_gettime( ACH_DEFAULT_CLOCK, &wait_time);
long nano_wait = wait_time.tv_nsec + (long)(quit_check*1E9);
wait_time.tv_sec += (long)(nano_wait/1E9);
wait_time.tv_nsec = (long)(nano_wait%((long)1E9));
ach_status_t result = ach_get(&_cmd_chan, _input_data, max_expected_size,
&fs, &wait_time, ACH_O_WAIT);
if(ACH_TIMEOUT == result)
{
continue;
}
if( ACH_OK != result )
{
std::cout << "Ach error: (" << (int)result << ")" << ach_result_to_string(result) << std::endl;
// TODO: Broadcast the fact that we had an ach error?
continue;
}
if(hubo_cmd_header_check(_input_data) != HUBO_DATA_OKAY)
{
std::cout << "Malformed command header!" << std::endl;
// TODO: Broadcast the fact that the header was malformed?
continue;
}
if( hubo_cmd_data_get_size(_input_data) != fs )
{
std::cout << "Data size error! Expected size:" << hubo_cmd_data_get_size(_input_data)
<< ", Frame size:" << fs <<", Max size:" << max_expected_size << std::endl;
// TODO: Broadcast the fact that we had a sizing error?
continue;
}
_check_hubocan_state();
_collate_input();
_send_output();
}
}
bool Walker::checkForNewTrajectory(zmp_traj_t &newTrajectory, bool haveNewTrajAlready)
{
size_t fs;
ach_status_t r = ach_get( &zmp_chan, &newTrajectory, sizeof(newTrajectory), &fs, NULL, ACH_O_LAST );
if( ACH_OK==r || ACH_MISSED_FRAME==r )
{
fprintf(stdout, "Noticed new trajectory: ID #%d\n", (int)newTrajectory.trajNumber);
return true;
}
else
return haveNewTrajAlready || false;
}
/* log all samples to a file */
void full_logger(void) {
enum ach_status r = ach_open(&chan_feedback, "feedback", NULL);
if(ACH_OK != r) abort();
FILE *fp = fopen("ach-example.dat", "w");
if(NULL == fp) abort();
while(1) {
x_t X;
size_t fs;
r = ach_get( &chan_feedback, X, sizeof(X), &fs, NULL, ACH_O_WAIT );
if( ach_status_match(r, ACH_MASK_OK | ACH_MASK_MISSED_FRAME) ) {
fprintf(fp,"%f\t%f\t%f\n", X[0], X[1], X[2]);
} else abort();
}
fclose(fp);
exit(0);
}
const JointCmdArray& Aggregator::update()
{
if(!_memory_set)
{
std::cout << "Trying to update commands before a valid HuboDescription has been loaded! Don't do that!!" << std::endl;
return _aggregated_cmds;
}
size_t fs;
ach_status_t result = ach_get(&_agg_chan, _final_data, hubo_cmd_data_get_size(_final_data), &fs, NULL, ACH_O_LAST);
if( ACH_OK != result && ACH_STALE_FRAMES != result && ACH_MISSED_FRAME != result )
{ // TODO: Is ACH_STALE_FRAMES really okay? It might imply that the upstream pipeline has been frozen
std::cout << "Unexpected Ach result: " << ach_result_to_string(result) << " (" << (int)result << ")" << std::endl;
return _aggregated_cmds;
}
_copy_final_data_to_array();
return _aggregated_cmds;
}
int main( int argc, char **argv ) {
(void)argc;
(void) argv;
ach_channel_t channel;
ach_status_t r;
/* unlink */
r = ach_unlink(OPT_CHAN);
test( ach_status_match(r, ACH_MASK_OK | ACH_MASK_ENOENT),
r, "ach_unlink");
/* create */
r = ach_create(OPT_CHAN, 32ul, 64ul, NULL );
test(ACH_OK == r, r, "ach_create");
/* open */
r = ach_open(&channel, OPT_CHAN, NULL);
test(ACH_OK == r, r, "ach_open");
/* first test */
r = ach_get( &channel, NULL, 0, NULL, NULL, ACH_O_LAST );
test( ACH_STALE_FRAMES == r, r, "get stale");
/* read test */
make_locked();
r = ach_get( &channel, NULL, 0, NULL, NULL, ACH_O_LAST );
test( ACH_STALE_FRAMES == r, r, "get stale");
/* corrupt test */
make_locked();
channel.shm->sync.dirty = 1;
r = ach_get( &channel, NULL, 0, NULL, NULL, ACH_O_LAST );
test( ACH_CORRUPT == r, r, "get corrupt");
/* and again */
r = ach_get( &channel, NULL, 0, NULL, NULL, ACH_O_LAST );
test( ACH_CORRUPT == r, r, "get corrupt");
/* another read test */
channel.shm->sync.dirty = 0;
r = ach_get( &channel, NULL, 0, NULL, NULL, ACH_O_LAST );
r = ach_get( &channel, NULL, 0, NULL, NULL, ACH_O_LAST );
test( ACH_STALE_FRAMES == r, r, "get stale");
return 0;
}
/* simple integrator, x = dt * dx */
void robot(void) {
enum ach_status r = ach_open(&chan_feedback, "feedback", NULL);
if(ACH_OK != r) abort();
r = ach_open(&chan_control, "control", NULL);
if(ACH_OK != r) abort();
x_t X = {now(),0,0};
while(1) {
u_t U;
size_t fs;
r = ach_get( &chan_control, U, sizeof(U), &fs, NULL, ACH_O_WAIT|ACH_O_LAST );
if( ach_status_match(r, ACH_MASK_OK | ACH_MASK_MISSED_FRAME) && sizeof(U) == fs ) {
double tm = now();
X[2] = U[0]; /* dx = u */
X[1] = (tm - X[0]) * X[2]; /* x = dt * dx */
X[0] = tm;
ach_put(&chan_feedback, X, sizeof(X));
} else abort();
}
exit(0);
}
/**
* Load the reference channel
*/
void ReferenceChannel::load(){
if (errored) return;
Reference temp;
memset( &temp, 0, sizeof(temp));
size_t fs;
int r = ach_get(&huboReferenceChannel, &temp, sizeof(temp), &fs, NULL, ACH_O_LAST);
if(ACH_OK != r && ACH_MISSED_FRAME != r && ACH_STALE_FRAMES != r) {
cout << "Error! Reference Channel failed with state " << r << endl;
errored = true;
} else if (ACH_STALE_FRAMES != r){
if (sizeof(temp) != fs) {
cout << "Error! File size inconsistent with state struct! fs = " << fs << " sizeof currentReference: " << sizeof(temp) << endl;
errored = true;
return;
}
} else
return;
currentReference = temp;
}
void print_times(void) {
while(1) {
float tm;
size_t fs;
struct timespec then;
if (KERNDEV) {
then.tv_sec = 3;
then.tv_nsec = 0;
} else {
then = get_ticks();
then.tv_sec += 3;
}
int r = ach_get(&time_chan, &tm, sizeof(tm), &fs, &then,
ACH_O_WAIT);
if( ACH_TIMEOUT == r ) break;
assert(ACH_OK == r);
assert(fs == sizeof(tm));
printf("%f\n", tm*1e6);
}
fflush(stdout);
}
void hubo_cb(const hubo_ros::HuboCommand &msg)
{
printf("Received command message\n");
//Send the commands from the HuboCommand message onto ACH to the robot
//Make the necessary hubo struct for ACH
struct hubo_ref H_ref_filter;
memset( &H_ref_filter, 0, sizeof(H_ref_filter));
size_t fs;
//First, get the current state of the Hubo from ACH
int r = ach_get(&chan_hubo_ref_filter, &H_ref_filter, sizeof(H_ref_filter), &fs, NULL, ACH_O_LAST);
if(ACH_OK != r)
{
if(hubo_debug)
{
printf("State ini r = %i\n",r);
}
}
else
{
assert(sizeof(H_ref_filter) == fs);
}
printf("Converting ROS message containing [%d] joint commands to hubo-ach\n", msg.num_joints);
//Add the joint values one at a time into the hubo struct
//for each joint command, we lookup the best matching
//joint in the header to set the index
for (int i = 0; i < msg.num_joints; i++)
{
int index = IndexLookup(msg.joints[i].name);
if (index != -1)
{
printf("Mapped URDF joint name [%s] to hubo joint index [%d]\n", msg.joints[i].name.c_str(), index);
H_ref_filter.ref[index] = msg.joints[i].position;
}
}
//If there are any joint values not assigned in the message, don't change them in the struct!
printf("Sending a new state on ACH\n");
//Put the new message into the ACH channel
ach_put(&chan_hubo_ref_filter, &H_ref_filter, sizeof(H_ref_filter));
}
static ssize_t ach_ch_read(struct file *file, char *buffer, size_t len,
loff_t * offset)
{
enum ach_status stat;
ssize_t retlen = -1;
struct ach_ch_file *ch_file = (struct ach_ch_file *)file->private_data;
/* KDEBUG1("In ach_ch_read (minor=%d)\n", ch_file->dev->minor); */
stat = ach_get(ch_file, buffer, len, &retlen);
switch (stat) {
case ACH_MISSED_FRAME:
/* TODO: how can we return this this? */
case ACH_OK:
break;
case ACH_EINTR:
return -ERESTARTSYS;
default:
return -get_errno(stat);
}
/* KDEBUG2("read@%d %d bytes\n", ch_file->dev->minor, retlen); */
return retlen;
}
void Walker::checkCommands()
{
size_t fs;
ach_get( &bal_cmd_chan, &cmd, sizeof(cmd), &fs, NULL, ACH_O_LAST );
}
int main(int argc, char** argv) {
if (argc < 2) {
std::cerr << "usage: " << argv[0] << " OUTPUTFILE.data\n";
return 1;
}
ach_channel_t chan_hubo_state;
ach_status_t r;
r = ach_open(&chan_hubo_state, HUBO_CHAN_STATE_NAME, NULL);
if (r != ACH_OK) {
std::cerr << "error opening state channel: " << ach_result_to_string(r) << "\n";
return 1;
}
LogWriter writer(argv[1], 200);
struct hubo_state H_state;
JointInfoArray jinfo;
buildJointTable(jinfo);
writer.add(&H_state.time, "state.time", "s");
for (size_t i=0; i<jinfo.size(); ++i) {
const std::string& name = jinfo[i].second;
const size_t index = jinfo[i].first;
writer.add(&H_state.joint[index].ref, "state.joint." + name + ".ref", "rad");
writer.add(&H_state.joint[index].pos, "state.joint." + name + ".pos", "rad");
writer.add(&H_state.joint[index].cur, "state.joint." + name + ".cur", "amp");
writer.add(&H_state.joint[index].vel, "state.joint." + name + ".vel", "rad/s");
writer.add(&H_state.joint[index].duty, "state.joint." + name + ".duty");
writer.add(&H_state.joint[index].heat, "state.joint." + name + ".heat", "J");
writer.add(&H_state.joint[index].active, "state.joint." + name + ".active");
writer.add(&H_state.joint[index].zeroed, "state.joint." + name + ".zeroed");
}
for (int i=0; i<4; ++i) {
std::ostringstream ostr;
ostr << "state.imu" << i;
std::string imustr = ostr.str();
writer.add(&H_state.imu[i].a_x, imustr+".a_x");
writer.add(&H_state.imu[i].a_y, imustr+".a_y");
writer.add(&H_state.imu[i].a_z, imustr+".a_z");
writer.add(&H_state.imu[i].w_x, imustr+".w_x");
writer.add(&H_state.imu[i].w_y, imustr+".w_y");
writer.add(&H_state.imu[i].w_z, imustr+".w_z");
}
for (int i=0; i<4; ++i) {
std::ostringstream ostr;
ostr << "state.ft" << i;
std::string ftstr = ostr.str();
writer.add(&H_state.ft[i].m_x, ftstr+".m_x");
writer.add(&H_state.ft[i].m_y, ftstr+".m_y");
writer.add(&H_state.ft[i].f_z, ftstr+".f_z");
}
writer.sortChannels();
writer.writeHeader();
std::cout << "Opened " << argv[1] << " for output, Ctrl+C to halt logging.\n";
signal(SIGINT, interruptHandler);
while (!interrupted) {
bool write = false;
size_t fs;
r = ach_get( &chan_hubo_state, &H_state, sizeof(H_state), &fs, NULL, ACH_O_WAIT );
if (useable(r)) {
write = true;
} else {
std::cout << "warning: ach returned " << ach_result_to_string(r) << " for state\n";
}
if (write) {
writer.writeSample();
}
}
std::cout << "Successfully wrote " << argv[1] << "\n";
return 0;
}
int runTrajFunction(char* s, int mode, bool compliance_mode, bool pause_feature) {
/* open ach channel */
int t1 = ach_open(&chan_hubo_ref, HUBO_CHAN_REF_NAME , NULL);
assert( ACH_OK == t1);
int t2 = ach_open(&chan_hubo_state, HUBO_CHAN_STATE_NAME, NULL);
assert( ACH_OK == t2);
// open to sim chan
t1 = ach_open(&chan_hubo_from_sim, HUBO_CHAN_VIRTUAL_FROM_SIM_NAME, NULL);
assert( ACH_OK == t1);
printf("\n ======================================= \n");
printf(" Hubo-Read-Trajectory-Function-Mode \n");
printf(" ======================================= \n");
// fflush(stdout);
// printf("into the func \n");
double newRef[2] = {1.0, 0.0};
// get initial values for hubo
struct hubo_ref H_ref;
memset( &H_ref, 0, sizeof(H_ref));
hubo_virtual_t H_virtual;
memset( &H_virtual, 0, sizeof(H_virtual));
// printf("after creating \n");
// fflush(stdout);
size_t fs;
int r = ach_get( &chan_hubo_ref, &H_ref, sizeof(H_ref), &fs, NULL, ACH_O_COPY );
// printf("after r \n");
// fflush(stdout);
if(ACH_OK != r) {
if(hubo_debug) {
//printf("Ref ini r = %s\n",ach_result_to_string(r));
}
}
else{ assert( sizeof(H_ref) == fs ); }
//printf("after achget \n");
// fflush(stdout);
struct hubo_state H_state;
memset( &H_state, 0, sizeof(H_state) );
r = ach_get( &chan_hubo_state, &H_state, sizeof(H_state), &fs, NULL, ACH_O_COPY );
if (ACH_OK != r) {
if (hubo_debug) {
//printf("State ini r = %s\n", ach_result_to_string(r));
}
} else {
assert( sizeof(H_state) == fs );
}
//printf("after ach \n");
// time info
struct timespec t;
/* Sampling Period */
double T = (double)interval/(double)NSEC_PER_SEC; // (sec)
clock_gettime( 0,&t);
// ------------------------------------------------------------------------------
// ---------------[ DO NOT EDIT AVBOE THIS LINE]---------------------------------
// ------------------------------------------------------------------------------
// char* fileName = "valve0.traj";
printf("Getting into trajectory \n");
fflush(stdout);
runTraj(s,mode, &H_ref, &t, &H_state, compliance_mode, pause_feature);
// ------------------------------------------------------------------------------
// ---------------[ DO NOT EDIT BELOW THIS LINE]---------------------------------
// ------------------------------------------------------------------------------
printf("Trajectory Finished \n");
fflush(stdout);
return 0;
}
// Function to get reference value from the Ach channel - store into ARMA vec format
vec getRefValues_new(int mode){
vec RefValues = zeros<vec>(40,1);
/* open ach channel */
// int t1 = ach_open(&chan_hubo_ref, HUBO_CHAN_REF_NAME , NULL);
// assert( ACH_OK == t1);
int t2 = ach_open(&chan_hubo_state, HUBO_CHAN_STATE_NAME, NULL);
assert( ACH_OK == t2);
// open to sim chan
int t1 = ach_open(&chan_hubo_from_sim, HUBO_CHAN_VIRTUAL_FROM_SIM_NAME, NULL);
assert( ACH_OK == t1);
struct hubo_state H_state;
// hubo_virtual_t H_virtual;
// memset( &H_virtual, 0, sizeof(H_virtual));
size_t fs;
if( HUBO_VIRTUAL_MODE_OPENHUBO == mode ){
int r = ach_get( &chan_hubo_from_sim, &H_state, sizeof(H_state), &fs, NULL, ACH_O_LAST );
}
else{
int r = ach_get( &chan_hubo_state, &H_state, sizeof(H_state), &fs, NULL, ACH_O_LAST );
// if(hubo_debug) {
// // printf("State ini r = %s\n",ach_result_to_string(r));
// }
// else{
// assert( sizeof(H_state) == fs );
// }
}
RefValues(0)=H_state.joint[RHY].ref;
RefValues(1)=H_state.joint[RHR].ref;
RefValues(2)=H_state.joint[RHP].ref;
RefValues(3)=H_state.joint[RKN].ref;
RefValues(4)=H_state.joint[RAP].ref;
RefValues(5)=H_state.joint[RAR].ref;
RefValues(6)=H_state.joint[LHY].ref;
RefValues(7)=H_state.joint[LHR].ref;
RefValues(8)=H_state.joint[LHP].ref;
RefValues(9)=H_state.joint[LKN].ref;
RefValues(10)=H_state.joint[LAP].ref;
RefValues(11)=H_state.joint[LAR].ref;
RefValues(12)=H_state.joint[RSP].ref;
RefValues(13)=H_state.joint[RSR].ref;
RefValues(14)=H_state.joint[RSY].ref;
RefValues(15)=H_state.joint[REB].ref;
RefValues(16)=H_state.joint[RWY].ref;
RefValues(17)=H_state.joint[RWR].ref;
RefValues(18)=H_state.joint[RWP].ref;
RefValues(19)=H_state.joint[LSP].ref;
RefValues(20)=H_state.joint[LSR].ref;
RefValues(21)=H_state.joint[LSY].ref;
RefValues(22)=H_state.joint[LEB].ref;
RefValues(23)=H_state.joint[LWY].ref;
RefValues(24)=H_state.joint[LWR].ref;
RefValues(25)=H_state.joint[LWP].ref;
RefValues(26)=H_state.joint[NKY].ref;
RefValues(27)=H_state.joint[NK1].ref;
RefValues(28)=H_state.joint[NK2].ref;
RefValues(29)=H_state.joint[WST].ref;
return RefValues;
}
int runTraj(char* s, int mode, struct hubo_ref *r, struct timespec *t, struct hubo_state* H_state, bool compliance_mode, bool pause_feature) {
int i = 0;
// int interval = 10000000; // 100 hz (0.01 sec)
hubo_virtual_t H_virtual;
memset( &H_virtual, 0, sizeof(H_virtual));
size_t fs;
int rr = 0;
char str[1000];
FILE *fp; // file pointer
fp = fopen(s,"r");
if(!fp) {
printf("No Trajectory File!!!\n");
return 1; // exit if not file
}
char c;
bool paused=false;
// tweak_init();
int line_counter=0;
double T = (double)interval/(double)NSEC_PER_SEC;
// printf("Reading %s\n",s);
double id = 0.0;
while(fgets(str,sizeof(str),fp) != NULL) {
// printf("i = %d\n",i);
// i = i+1;
// wait until next shot
clock_nanosleep(0,TIMER_ABSTIME,t, NULL);
if( HUBO_VIRTUAL_MODE_OPENHUBO == mode ){
for( id = 0 ; id < T; id = id + HUBO_LOOP_PERIOD ){
rr = ach_get( &chan_hubo_from_sim, &H_virtual, sizeof(H_virtual), &fs, NULL, ACH_O_WAIT );
}
}
// ------------------------------------------------------------------------------
// ---------------[ DO NOT EDIT AVBOE THIS LINE]---------------------------------
// ------------------------------------------------------------------------------
line_counter++;
//printf("line is %d \n", line_counter);
// if ( read(STDIN_FILENO, &c, 1) == 1) {
// if (c=='p' && pause_feature==true) {
// paused=!paused;
// printf("paused is now %s \n", paused ? "true" : "false");
// }
// }
// while (paused==true){
// usleep(1000000);//1 second
// t->tv_sec+=1; // for the 1 sec delay in line above
// if ( read(STDIN_FILENO, &c, 1) == 1) {
// if (c=='p'&& pause_feature==true) {
// paused=!paused;
// printf("paused is now %s \n", paused ? "true" : "false");
// }
// }
// }
int len = strlen(str)-1;
if(str[len] == '\n') {
str[len] = 0;
}
// Read motion from the text file
getArg(str, r);
// Compliance Mode
int joint;
if (compliance_mode==true){
for (joint=4; joint<18; joint++){//hard coded for arms only
r->comply[joint]=1;
}
}
else{
for (joint=4; joint<18; joint++){//hard coded for arms only
r->comply[joint]=0;
}
}
if (goto_init_flag) {
struct hubo_ref tmpref;
int i;
for (i=0; i<200; ++i) {
double u = (double)(i+1)/200;
for (joint=0; joint<HUBO_JOINT_COUNT; ++joint) {
tmpref.ref[joint] = u * r->ref[joint] + (1-u) * H_state->joint[joint].ref;
}
ach_put( &chan_hubo_ref, &tmpref, sizeof(tmpref));
usleep(0.01 * 1e6);
}
goto_init_flag = 0;
clock_gettime(0, t);
// reset clock
}
// ------------------------------------------------------------------------------
// ---------------[ DO NOT EDIT BELOW THIS LINE]---------------------------------
// ------------------------------------------------------------------------------
// Cheeting No more RAP or LAP
/*
r->ref[RHP] = 0.0;
r->ref[LHP] = 0.0;
r->ref[RAP] = 0.0;
r->ref[LAP] = 0.0;
r->ref[RKN] = 0.0;
r->ref[LKN] = 0.0;
r->ref[RAR] = 0.0;
r->ref[LAR] = 0.0;
r->ref[RHR] = 0.0;
r->ref[LHR] = 0.0;
*/
/*
for( i = 0 ; i < HUBO_JOINT_COUNT; i++){
r->mode[i] = HUBO_REF_MODE_REF;
}
*/
ach_put( &chan_hubo_ref, r, sizeof(*r));
//printf("Ref r = %s\n",ach_result_to_string(r));
t->tv_nsec+=interval;
tsnorm(t);
}
}
int main( int argc, char* argv[] ) {
ach_channel_t left_arm_chan;
ach_channel_t right_arm_chan;
ach_channel_t upper_body_chan;
double left_arm_q[3][3];
double right_arm_q[3][3];
double upper_body_q[3][3];
// Open ach channels
enum ach_status r;
r = ach_open( &left_arm_chan, "subject_larm_state", NULL );
if( r != ACH_OK ) {
printf("Could not open left arm correctly \n");
}
r = ach_open( &right_arm_chan, "subject_rarm_state", NULL );
if( r != ACH_OK ) {
printf("Could not open right arm correctly \n");
}
r = ach_open( &upper_body_chan, "subject_upper_state", NULL );
if( r != ACH_OK ) {
printf("Could not open upper body correctly \n");
}
// Read constantly
size_t frame_size;
while( true ) {
// Left Arm
ach_get( &left_arm_chan, &left_arm_q,sizeof( left_arm_q ),
&frame_size, NULL, 0 );
if( ACH_MISSED_FRAME == r ) {
printf("Missed some messages \n");
} else if( ACH_STALE_FRAMES == r ) {
printf("No new data \n");
} else if( ACH_OK != r ) {
printf("Unable to get a message. \n");
} else if( frame_size != sizeof(left_arm_q) ) {
printf("Unexpected message of size %d , we were expecting size %d \n",
frame_size, sizeof(left_arm_q) );
}
printf("*********************\n");
printf("Left shoulder: %f %f %f \n", left_arm_q[0][0], left_arm_q[0][1], left_arm_q[0][2] );
printf("Left elbow: %f %f %f \n", left_arm_q[1][0], left_arm_q[1][1], left_arm_q[1][2] );
printf("Left wrist: %f %f %f \n", left_arm_q[2][0], left_arm_q[2][1], left_arm_q[2][2] );
printf(" -- ** -- \n");
// Right Arm
ach_get( &right_arm_chan, &right_arm_q,sizeof( right_arm_q ),
&frame_size, NULL, 0 );
if( ACH_MISSED_FRAME == r ) {
printf("Missed some messages \n");
} else if( ACH_STALE_FRAMES == r ) {
printf("No new data \n");
} else if( ACH_OK != r ) {
printf("Unable to get a message. \n");
} else if( frame_size != sizeof(right_arm_q) ) {
printf("Unexpected message of size %d , we were expecting size %d \n",
frame_size, sizeof(right_arm_q) );
}
printf("Right shoulder: %f %f %f \n", right_arm_q[0][0], right_arm_q[0][1], right_arm_q[0][2] );
printf("Right elbow: %f %f %f \n", right_arm_q[1][0], right_arm_q[1][1], right_arm_q[1][2] );
printf("Right wrist: %f %f %f \n", right_arm_q[2][0], right_arm_q[2][1], right_arm_q[2][2] );
printf(" -- ** -- \n");
// Upper body
ach_get( &upper_body_chan, &upper_body_q,sizeof( upper_body_q ),
&frame_size, NULL, 0 );
if( ACH_MISSED_FRAME == r ) {
printf("Missed some messages \n");
} else if( ACH_STALE_FRAMES == r ) {
printf("No new data \n");
} else if( ACH_OK != r ) {
printf("Unable to get a message. \n");
} else if( frame_size != sizeof(upper_body_q) ) {
printf("Unexpected message of size %d , we were expecting size %d \n",
frame_size, sizeof(upper_body_q) );
}
printf("Head: %f %f %f \n", upper_body_q[0][0], upper_body_q[0][1], upper_body_q[0][2] );
printf("Neck: %f %f %f \n", upper_body_q[1][0], upper_body_q[1][1], upper_body_q[1][2] );
printf("Torso: %f %f %f \n", upper_body_q[2][0], upper_body_q[2][1], upper_body_q[2][2] );
printf("*********************\n");
usleep(200*1000);
}
return 0;
}
static PyObject *
get_buf( PyObject *self, PyObject *args ) {
(void)self;
PyObject *py_chan, *b;
int wait, last;
// get arg objects
if( !PyArg_ParseTuple(args, "OOii", &py_chan, &b, &wait, &last) ) {
return NULL;
}
// parse channel
ach_channel_t *c = parse_channel_pointer(py_chan);
if( NULL == c ) {
return NULL;
}
// parse buffer
if( ! PyObject_CheckBuffer(b) ) {
PyErr_SetString( PyExc_TypeError, "invalid buffer" );
return NULL;
}
// view buffer
Py_buffer buf;
if( PyObject_GetBuffer( b, &buf, PyBUF_WRITABLE ) ) {
PyErr_SetString( PyExc_BufferError, "couldn't view writable buffer" );
return NULL;
}
// parse opts
int opts = 0;
if (wait) opts |= ACH_O_WAIT;
if (last) opts |= ACH_O_LAST;
// make the damn call
size_t frame_size;
ach_status_t r = ach_get( c, buf.buf, (size_t)buf.len,
&frame_size, NULL, opts );
// cleanup
PyBuffer_Release(&buf);
// return
switch(r) {
case ACH_OK:
case ACH_STALE_FRAMES:
case ACH_MISSED_FRAME:
case ACH_TIMEOUT:
case ACH_CANCELED:
return Py_BuildValue("ik", r, frame_size);
case ACH_OVERFLOW:
case ACH_INVALID_NAME:
case ACH_BAD_SHM_FILE:
case ACH_FAILED_SYSCALL:
case ACH_CLOSED:
case ACH_EEXIST:
case ACH_ENOENT:
case ACH_BUG:
case ACH_EINVAL:
case ACH_CORRUPT:
case ACH_BAD_HEADER:
case ACH_EACCES:
case ACH_EINTR:
case ACH_EFAULT:
case ACH_ENOTSUP:
return raise_error(r);
}
return NULL;
}
//NEW MAIN LOOP
int main(int argc, char **argv)
{
printf("Initializing ACH-to-ROS bridge\n");
//initialize HUBO-ACH feedback channel
int r = ach_open(&chan_hubo_state, HUBO_CHAN_STATE_NAME , NULL);
assert(ACH_OK == r);
//initialize HUBO-ACH reference channel
r = ach_open(&chan_hubo_ref_filter, HUBO_CHAN_REF_NAME , NULL);
assert(ACH_OK == r);
//initialize HUBO-ACH message structs
struct hubo_state H_state;
memset(&H_state, 0, sizeof(H_state));
struct hubo_ref H_ref_filter;
memset( &H_ref_filter, 0, sizeof(H_ref_filter));
size_t fs;
printf("HUBO-ACH channels loaded\n");
//initialize ROS node
ros::init(argc, argv, "hubo_ros_feedback");
ros::NodeHandle nh;
//construct ROS publisher
ros::Publisher hubo_state_pub = nh.advertise<hubo_ros::HuboState>("Hubo/HuboState", 1);
printf("ROS publisher loaded\n");
//Loop
while (ros::ok())
{
//Get latest state from HUBO-ACH
r = ach_get(&chan_hubo_state, &H_state, sizeof(H_state), &fs, NULL, ACH_O_WAIT);
if(ACH_OK != r)
{
if(hubo_debug)
{
printf("State ini r = %i\n",r);
}
}
else
{
assert(sizeof(H_state) == fs);
}
//Get latest reference from HUBO-ACH
r = ach_get(&chan_hubo_ref_filter, &H_ref_filter, sizeof(H_ref_filter), &fs, NULL, ACH_O_LAST);
if(ACH_OK != r)
{
if(hubo_debug)
{
printf("State ini r = %i\n",r);
}
}
else
{
assert(sizeof(H_ref_filter) == fs);
}
//Assemble new HuboState message
hubo_ros::HuboState msg = hubo_ros::HuboState();
msg.joints.resize(HUBO_JOINT_COUNT);
if(ACHtoHuboState(&H_state, &H_ref_filter, &msg))
{
//Publish HuboState
hubo_state_pub.publish(msg);
}
else
{
printf("*** Invalid state recieved from HUBO! ***\n");
}
ros::spinOnce();
}
//Satisfy the compiler
return 0;
}
void mainLoop(void){
int r = ach_open(&chan_num, "can", NULL);
assert( ACH_OK == r );
struct timespec t;
int interval = 200000000;
// open can
int skt = open_can();
// get current time
clock_gettime(0,&t);
// start one second after
t.tv_sec++;
d D = {0};
while(1){
// wait until next shot
clock_nanosleep(0,TIMER_ABSTIME,&t, NULL);
//------------------------------
//------[ do sutff start ]------
//------------------------------
// get info from ach
size_t fs;
r = ach_get( &chan_num, D, sizeof(D), &fs, NULL, ACH_O_WAIT );
// send can
struct can_frame frame;
memset(&frame,0,sizeof(frame));
//frame.can_id = 0x123;
//strcpy( frame.data, "xxxxxxxx" );
//frame.can_dlc = strlen( frame.data );
//int bytes_sent = write (skt, &frame, sizeof(frame) );
int bytes_read = read( skt, &frame, sizeof(frame) );
//printf("num2 = %f\n", (float)D[0]);
if ( bytes_read < 0 )
{
perror("write failed");
}
printf("Bytes sent are %d - %s\n",bytes_read, frame.data);
/*
if( fflush(skt) )i
{
perror("something failed");
}
*/
//------------------------------
//-----[ do stuff stop ]--------
//------------------------------
// calculate next shot
t.tv_nsec+=interval;
tsnorm(&t);
}
}
static void *worker( void *arg ) {
struct log_desc *desc = (struct log_desc*)arg;
/* write header */
fprintf( desc->fout,
"ACHLOG\n"
"channel-name: %s\n"
"log-version: 0\n"
"log-time-ach: %lu.%09lu\n"
"log-time-real: %lu.%09lu # %s"
"local-host: %s\n",
desc->name,
now_ach.tv_sec, now_ach.tv_nsec,
now_real.tv_sec, now_real.tv_nsec, now_real_str,
host );
if( passwd ) {
fprintf( desc->fout,
"user: %s # %s\n",
passwd->pw_name, passwd->pw_gecos
);
}
fputs( ".\n", desc->fout );
if( fflush(desc->fout) ) {
ACH_LOG( LOG_ERR, "Could not flush file %s: %s\n",
desc->name, strerror(errno) );
}
size_t max = 512;
ach_pipe_frame_t *frame = ach_pipe_alloc( max );
/* get frames */
int canceled = 0;
while( ! canceled ) {
/* push the data */
size_t frame_size;
ach_status_t r = ach_get( &desc->chan, frame->data, max, &frame_size, NULL,
ACH_O_WAIT | ((opt_last ) ? ACH_O_LAST : 0) );
switch(r) {
case ACH_OVERFLOW:
/* enlarge buffer and retry on overflow */
assert(frame_size > max );
max = frame_size;
free(frame);
frame = ach_pipe_alloc( max );
continue;
case ACH_MISSED_FRAME:
case ACH_OK:
{
ach_pipe_set_size( frame, frame_size );
size_t size = sizeof(ach_pipe_frame_t) - 1 + frame_size;
size_t s = fwrite( frame, 1, size, desc->fout );
if( s != size ) {
ACH_LOG( LOG_ERR, "Could not write frame to %s, %"PRIuPTR" written instead of %"PRIuPTR"\n",
desc->name, s, size );
canceled = 1;
}
}
break;
case ACH_CANCELED:
canceled = 1;
break;
default:
ACH_LOG( LOG_ERR, "Could not get frame from %s: %s\n",
desc->name, strerror(errno) );
canceled = 1;
break;
}
}
/* sync */
if( fflush(desc->fout) ) {
ACH_LOG( LOG_ERR, "Could not flush file %s: %s\n",
desc->name, strerror(errno) );
}
return arg;
}
void Walker::commenceWalking(balance_state_t &parent_state, nudge_state_t &state, balance_gains_t &gains)
{
int timeIndex=0, nextTimeIndex=0, prevTimeIndex=0;
keepWalking = true;
size_t fs;
zmp_traj_t *prevTrajectory, *currentTrajectory, *nextTrajectory;
prevTrajectory = new zmp_traj_t;
currentTrajectory = new zmp_traj_t;
nextTrajectory = new zmp_traj_t;
memset( prevTrajectory, 0, sizeof(*prevTrajectory) );
memset( currentTrajectory, 0, sizeof(*currentTrajectory) );
memset( nextTrajectory, 0, sizeof(*nextTrajectory) );
// TODO: Consider making these values persistent
memset( &state, 0, sizeof(state) );
memcpy( &bal_state, &parent_state, sizeof(bal_state) );
bal_state.m_balance_mode = BAL_ZMP_WALKING;
bal_state.m_walk_mode = WALK_WAITING;
bal_state.m_walk_error = NO_WALK_ERROR;
sendState();
currentTrajectory->reuse = true;
fprintf(stdout, "Waiting for first trajectory\n"); fflush(stdout);
ach_status_t r;
do {
struct timespec t;
clock_gettime( ACH_DEFAULT_CLOCK, &t );
t.tv_sec += 1;
r = ach_get( &zmp_chan, currentTrajectory, sizeof(*currentTrajectory), &fs,
&t, ACH_O_WAIT | ACH_O_LAST );
checkCommands();
if( cmd.cmd_request != BAL_ZMP_WALKING )
keepWalking = false;
} while(!daemon_sig_quit && keepWalking && (r==ACH_TIMEOUT
|| !currentTrajectory->reuse) ); // TODO: Replace this with something more intelligent
if(!keepWalking || !currentTrajectory->reuse) // TODO: Take out the reuse condition here
{
bal_state.m_walk_mode = WALK_INACTIVE;
sendState();
return;
}
if(!daemon_sig_quit)
fprintf(stdout, "First trajectory acquired\n");
daemon_assert( !daemon_sig_quit, __LINE__ );
bal_state.m_walk_mode = WALK_INITIALIZING;
sendState();
// Get the balancing gains from the ach channel
ach_get( ¶m_chan, &gains, sizeof(gains), &fs, NULL, ACH_O_LAST );
hubo.update(true);
// Set all the joints to the initial posiiton in the trajectory
// using the control daemon to interpolate in joint space.
for(int i=0; i<HUBO_JOINT_COUNT; i++)
{
// Don't worry about where these joint are
if( LF1!=i && LF2!=i && LF3!=i && LF4!=i && LF5!=i
&& RF1!=i && RF2!=i && RF3!=i && RF4!=i && RF5!=i
&& NK1!=i && NK2!=i && NKY!=i && LWR!=i && RWR!=i && RWY!=i && RWP!=i) //FIXME
{
hubo.setJointAngle( i, currentTrajectory->traj[0].angles[i] + bal_state.jointOffset[i] );
hubo.setJointNominalSpeed( i, 0.4 );
hubo.setJointNominalAcceleration( i, 0.4 );
}
//std::cout << jointNames[i] << " = " << currentTrajectory->traj[0].angles[i] + bal_state.jointOffset[i] << "\n";
}
hubo.setJointNominalSpeed( RKN, 0.8 );
hubo.setJointNominalAcceleration( RKN, 0.8 );
hubo.setJointNominalSpeed( LKN, 0.8 );
hubo.setJointNominalAcceleration( LKN, 0.8 );
// hubo.setJointAngle( RSR, currentTrajectory->traj[0].angles[RSR] + hubo.getJointAngleMax(RSR) );
// hubo.setJointAngle( LSR, currentTrajectory->traj[0].angles[LSR] + hubo.getJointAngleMin(LSR) );
hubo.sendControls();
// Wait specified time for joints to get into initial configuration,
// otherwise time out and alert user.
double m_maxInitTime = 10;
double biggestErr = 0;
int worstJoint=-1;
double dt, time, stime; stime=hubo.getTime(); time=hubo.getTime();
double norm = m_jointSpaceTolerance+1; // make sure this fails initially
while( !daemon_sig_quit && (norm > m_jointSpaceTolerance && time-stime < m_maxInitTime)) {
// while(false) { // FIXME TODO: SWITCH THIS BACK!!!
hubo.update(true);
norm = 0;
for(int i=0; i<HUBO_JOINT_COUNT; i++)
{
double err=0;
// Don't worry about waiting for these joints to get into position.
if( LF1!=i && LF2!=i && LF3!=i && LF4!=i && LF5!=i
&& RF1!=i && RF2!=i && RF3!=i && RF4!=i && RF5!=i
&& NK1!=i && NK2!=i && NKY!=i && LWR!=i && RWR!=i && RWY!=i && RWP!=i) //FIXME
err = (hubo.getJointAngleState( i )-currentTrajectory->traj[0].angles[i] + bal_state.jointOffset[i]);
// if( LSR == i )
// err -= hubo.getJointAngleMin(i);
// if( RSR == i )
// err -= hubo.getJointAngleMax(i);
norm += err*err;
if( fabs(err) > fabs(biggestErr) )
{
biggestErr = err;
worstJoint = i;
}
}
time = hubo.getTime();
}
// Print timeout error if joints don't get to initial positions in time
if( time-stime >= m_maxInitTime )
{
fprintf(stderr, "Warning: could not reach the starting Trajectory within %f seconds\n"
" -- Biggest error was %f radians in joint %s\n",
m_maxInitTime, biggestErr, jointNames[worstJoint] );
keepWalking = false;
bal_state.m_walk_error = WALK_INIT_FAILED;
}
timeIndex = 1;
bool haveNewTrajectory = false;
if( keepWalking )
fprintf(stdout, "Beginning main walking loop\n"); fflush(stdout);
while(keepWalking && !daemon_sig_quit)
{
haveNewTrajectory = checkForNewTrajectory(*nextTrajectory, haveNewTrajectory);
ach_get( ¶m_chan, &gains, sizeof(gains), &fs, NULL, ACH_O_LAST );
hubo.update(true);
bal_state.m_walk_mode = WALK_IN_PROGRESS;
dt = hubo.getTime() - time;
time = hubo.getTime();
if( dt <= 0 )
{
fprintf(stderr, "Something unnatural has happened... %f\n", dt);
continue;
}
if( timeIndex==0 )
{
bal_state.m_walk_error = NO_WALK_ERROR;
nextTimeIndex = timeIndex+1;
executeTimeStep( hubo, prevTrajectory->traj[prevTimeIndex],
currentTrajectory->traj[timeIndex],
currentTrajectory->traj[nextTimeIndex],
state, gains, dt );
}
else if( timeIndex == currentTrajectory->periodEndTick && haveNewTrajectory )
{
if( validateNextTrajectory( currentTrajectory->traj[timeIndex],
nextTrajectory->traj[0], dt ) )
{
nextTimeIndex = 0;
executeTimeStep( hubo, currentTrajectory->traj[prevTimeIndex],
currentTrajectory->traj[timeIndex],
nextTrajectory->traj[nextTimeIndex],
state, gains, dt );
memcpy( prevTrajectory, currentTrajectory, sizeof(*prevTrajectory) );
memcpy( currentTrajectory, nextTrajectory, sizeof(*nextTrajectory) );
fprintf(stderr, "Notice: Swapping in new trajectory\n");
}
else
{
fprintf(stderr, "WARNING: Discontinuous trajectory passed in. Walking to a stop.\n");
bal_state.m_walk_error = WALK_FAILED_SWAP;
nextTimeIndex = timeIndex+1;
executeTimeStep( hubo, currentTrajectory->traj[prevTimeIndex],
currentTrajectory->traj[timeIndex],
currentTrajectory->traj[nextTimeIndex],
state, gains, dt );
}
haveNewTrajectory = false;
}
else if( timeIndex == currentTrajectory->periodEndTick && currentTrajectory->reuse )
{
checkCommands();
if( cmd.cmd_request != BAL_ZMP_WALKING )
currentTrajectory->reuse = false;
if( currentTrajectory->reuse == true )
nextTimeIndex = currentTrajectory->periodStartTick;
else
nextTimeIndex = timeIndex+1;
executeTimeStep( hubo, currentTrajectory->traj[prevTimeIndex],
currentTrajectory->traj[timeIndex],
currentTrajectory->traj[nextTimeIndex],
state, gains, dt );
}
else if( timeIndex < currentTrajectory->count-1 )
{
nextTimeIndex = timeIndex+1;
executeTimeStep( hubo, currentTrajectory->traj[prevTimeIndex],
currentTrajectory->traj[timeIndex],
currentTrajectory->traj[nextTimeIndex],
state, gains, dt );
}
else if( timeIndex == currentTrajectory->count-1 && haveNewTrajectory )
{
checkCommands();
if( cmd.cmd_request != BAL_ZMP_WALKING )
keepWalking = false;
if( keepWalking )
{
if( validateNextTrajectory( currentTrajectory->traj[timeIndex],
nextTrajectory->traj[0], dt ) )
{
bal_state.m_walk_error = NO_WALK_ERROR;
nextTimeIndex = 0;
executeTimeStep( hubo, currentTrajectory->traj[prevTimeIndex],
currentTrajectory->traj[timeIndex],
nextTrajectory->traj[nextTimeIndex],
state, gains, dt );
memcpy( prevTrajectory, currentTrajectory, sizeof(*prevTrajectory) );
memcpy( currentTrajectory, nextTrajectory, sizeof(*nextTrajectory) );
}
else
{
bal_state.m_walk_mode = WALK_WAITING;
bal_state.m_walk_error = WALK_FAILED_SWAP;
fprintf(stderr, "WARNING: Discontinuous trajectory passed in. Discarding it.\n");
}
haveNewTrajectory = false;
}
}
else
{
checkCommands();
if( cmd.cmd_request != BAL_ZMP_WALKING )
keepWalking = false;
}
prevTimeIndex = timeIndex;
timeIndex = nextTimeIndex;
sendState();
}
bal_state.m_walk_mode = WALK_INACTIVE;
sendState();
}
void huboLoop(struct hubo_param *H_param) {
// get initial values for hubo
struct hubo_ref H_ref;
struct hubo_state H_state;
memset( &H_ref, 0, sizeof(H_ref));
memset( &H_state, 0, sizeof(H_state));
size_t fs;
//int r = ach_get( &chan_hubo_ref, &H, sizeof(H), &fs, NULL, ACH_O_LAST );
//assert( sizeof(H) == fs );
int r = ach_get( &chan_hubo_ref, &H_ref, sizeof(H_ref), &fs, NULL, ACH_O_LAST );
if(ACH_OK != r) {
if(hubo_debug) {
printf("Ref ini r = %s\n",ach_result_to_string(r));}
}
else{ assert( sizeof(H_ref) == fs ); }
r = ach_get( &chan_hubo_state, &H_state, sizeof(H_state), &fs, NULL, ACH_O_LAST );
if(ACH_OK != r) {
if(hubo_debug) {
printf("State ini r = %s\n",ach_result_to_string(r));}
}
else{
assert( sizeof(H_state) == fs );
}
// time info
struct timespec t;
//int interval = 500000000; // 2hz (0.5 sec)
int interval = 10000000; // 100 hz (0.01 sec)
//int interval = 5000000; // 200 hz (0.005 sec)
//int interval = 2000000; // 500 hz (0.002 sec)
/* Sampling Period */
double T = (double)interval/(double)NSEC_PER_SEC; // (sec)
// get current time
//clock_gettime( CLOCK_MONOTONIC,&t);
clock_gettime( 0,&t);
while(1) {
// wait until next shot
clock_nanosleep(0,TIMER_ABSTIME,&t, NULL);
/* Get latest ACH message */
r = ach_get( &chan_hubo_ref, &H_ref, sizeof(H_ref), &fs, NULL, ACH_O_LAST );
if(ACH_OK != r) {
if(hubo_debug) {
printf("Ref r = %s\n",ach_result_to_string(r));}
}
else{ assert( sizeof(H_ref) == fs ); }
r = ach_get( &chan_hubo_state, &H_state, sizeof(H_state), &fs, NULL, ACH_O_LAST );
if(ACH_OK != r) {
if(hubo_debug) {
printf("State r = %s\n",ach_result_to_string(r));}
}
else{ assert( sizeof(H_state) == fs ); }
// ------------------------------------------------------------------------------
// ---------------[ DO NOT EDIT AVBOE THIS LINE]---------------------------------
// ------------------------------------------------------------------------------
H_ref.ref[RHY] = 0.3;
H_ref.ref[LEB] = -0.4;
H_ref.ref[RSP] = 0.3;
double encRSP = H_state.joint[RSP].pos;
// ------------------------------------------------------------------------------
// ---------------[ DO NOT EDIT BELOW THIS LINE]---------------------------------
// ------------------------------------------------------------------------------
ach_put( &chan_hubo_ref, &H_ref, sizeof(H_ref));
t.tv_nsec+=interval;
tsnorm(&t);
}
}
