/* return the amount of a read */
static int max_input_read(struct telnet_session_s *t)
{
int max_in;
int max_out;
daemon_assert(invariant(t));
/* figure out how much space is available in the input buffer */
if (t->in_buflen < BUF_LEN) {
max_in = BUF_LEN - t->in_buflen;
} else {
max_in = 0;
}
/* worry about space in the output buffer (for DONT/WONT replies) */
if (t->out_buflen < BUF_LEN) {
max_out = BUF_LEN - t->out_buflen;
} else {
max_out = 0;
}
daemon_assert(invariant(t));
/* return the minimum of the two values */
return (max_in < max_out) ? max_in : max_out;
}
int watchdog_init(watchdog_t *w, int inactivity_timeout, error_t *err)
{
pthread_t thread_id;
int error_code;
daemon_assert(w != NULL);
daemon_assert(inactivity_timeout > 0);
daemon_assert(err != NULL);
pthread_mutex_init(&w->mutex, NULL);
w->inactivity_timeout = inactivity_timeout;
w->oldest = NULL;
w->newest = NULL;
error_code = pthread_create(&thread_id, NULL, watcher, w);
if (error_code != 0) {
error_init(err, error_code, "error %d from pthread_create()",
error_code);
return 0;
}
pthread_detach(thread_id);
daemon_assert(invariant(w));
return 1;
}
int watchdog_init(struct watchdog_s *w, int inactivity_timeout)
{
pthread_t thread_id;
int error_code;
daemon_assert(w != NULL);
daemon_assert(inactivity_timeout > 0);
_MUTEX_INIT(w->mutex);
w->inactivity_timeout = inactivity_timeout;
w->oldest = NULL;
w->newest = NULL;
error_code = pthread_create(&thread_id, DEFAULT_THREAD_ATTR, watcher, w);
if (error_code != 0) {
errno = error_code;
ERROR_CONNECT("Watchdog thread create problem\n");
return 0;
}
pthread_detach(thread_id);
daemon_assert(invariant(w));
return 1;
}
static const char *parse_host_port(struct sockaddr_in *addr, const char *s)
{
int i;
int octets[6];
char addr_str[16];
int port;
struct in_addr in_addr;
daemon_assert(addr != NULL);
daemon_assert(s != NULL);
/* scan in 5 pairs of "#," */
for (i=0; i<5; i++) {
s = parse_number(&octets[i], s, 255);
if (s == NULL) {
return NULL;
}
if (*s != ',') {
return NULL;
}
s++;
}
/* scan in ending "#" */
s = parse_number(&octets[5], s, 255);
if (s == NULL) {
return NULL;
}
daemon_assert(octets[0] >= 0);
daemon_assert(octets[0] <= 255);
daemon_assert(octets[1] >= 0);
daemon_assert(octets[1] <= 255);
daemon_assert(octets[2] >= 0);
daemon_assert(octets[2] <= 255);
daemon_assert(octets[3] >= 0);
daemon_assert(octets[3] <= 255);
/* convert our number to a IP/port */
sprintf(addr_str, "%d.%d.%d.%d",
octets[0], octets[1], octets[2], octets[3]);
port = (octets[4] << 8) | octets[5];
#ifdef HAVE_INET_ATON
if (inet_aton(addr_str, &in_addr) == 0) {
return NULL;
}
#else
in_addr.s_addr = inet_addr(addr_str);
if (in_addr.s_addr == -1) {
return NULL;
}
#endif
addr->sin_family = AF_INET;
addr->sin_addr = in_addr;
addr->sin_port = htons(port);
/* return success */
return s;
}
static const char *parse_offset(off_t *ofs, const char *s)
{
off_t tmp_ofs;
int cur_digit;
off_t max_ofs;
#if SIZEOF_UNSIGNED_LONG >= SIZEOF_OFF_T
unsigned long cutoff, cutlim;
#elif SIZEOF_UNSIGNED_LONG_LONG && SIZEOF_UNSIGNED_LONG_LONG >= SIZEOF_OFF_T
unsigned long long cutoff, cutlim;
#else
# error cannot figure out a type larger or equal to off_t
#endif
daemon_assert(ofs != NULL);
daemon_assert(s != NULL);
/* calculate maximum allowable offset based on size of off_t */
#if SIZEOF_OFF_T == 8
max_ofs = (off_t)9223372036854775807LL;
#elif SIZEOF_OFF_T == 4
max_ofs = (off_t)2147483647LL;
#elif SIZEOF_OFF_T == 2
max_ofs = (off_t)32767LL;
#else
# error sizeof(off_t) unknown
#endif
cutoff = max_ofs / 10;
cutlim = max_ofs % 10;
/* handle first character */
if (!isdigit(*s)) {
return NULL;
}
tmp_ofs = (*s - '0');
s++;
/* handle subsequent characters */
while (isdigit(*s)) {
cur_digit = (*s - '0');
if (tmp_ofs > cutoff || (tmp_ofs == cutoff && cur_digit > cutlim))
return NULL; /*overflow*/
tmp_ofs *= 10;
tmp_ofs += cur_digit;
s++;
}
/* return */
*ofs = tmp_ofs;
return s;
}
/* read a line */
int telnet_session_readln(struct telnet_session_s *t, char *buf, int buflen)
{
int amt_read;
daemon_assert(invariant(t));
amt_read = 0;
for (;;) {
if ((t->in_errno != 0) || (t->in_eof != 0)) {
daemon_assert(invariant(t));
return 0;
}
while (t->in_buflen > 0) {
if (amt_read == buflen - 1) {
buf[amt_read] = '\0';
daemon_assert(invariant(t));
return 1;
}
daemon_assert(amt_read >= 0);
daemon_assert(amt_read < buflen);
buf[amt_read] = use_incoming_char(t);
if (buf[amt_read] == '\012') {
daemon_assert(amt_read + 1 >= 0);
daemon_assert(amt_read + 1 < buflen);
buf[amt_read + 1] = '\0';
daemon_assert(invariant(t));
return 1;
}
amt_read++;
}
process_data(t, 1);
}
}
/* print a line output */
int telnet_session_println(struct telnet_session_s *t, const char *s)
{
daemon_assert(invariant(t));
if (!telnet_session_print(t, s)) {
daemon_assert(invariant(t));
return 0;
}
if (!telnet_session_print(t, "\015\012")) {
daemon_assert(invariant(t));
return 0;
}
daemon_assert(invariant(t));
return 1;
}
void watchdog_add_watched(struct watchdog_s *w, struct watched_s *watched)
{
daemon_assert(invariant(w));
_MUTEX_LOCK(w->mutex);
watched->watched_thread = pthread_self();
watched->watchdog = w;
insert(w, watched);
_MUTEX_UNLOCK(w->mutex);
daemon_assert(invariant(w));
}
void watchdog_remove_watched(struct watched_s *watched)
{
struct watchdog_s *w;
daemon_assert(invariant(watched->watchdog));
w = watched->watchdog;
_MUTEX_LOCK(w->mutex);
delete(w, watched);
_MUTEX_UNLOCK(w->mutex);
daemon_assert(invariant(w));
}
/* copy a string terminated with a newline */
static const char *copy_string(char *dst, const char *src)
{
int i;
daemon_assert(dst != NULL);
daemon_assert(src != NULL);
for (i=0; (i<=MAX_STRING_LEN) && (src[i]!='\0') && (src[i]!='\n'); i++) {
dst[i] = src[i];
}
dst[i] = '\0';
return src+i;
}
void watchdog_remove_watched(watched_t *watched)
{
watchdog_t *w;
daemon_assert(invariant(watched->watchdog));
w = watched->watchdog;
pthread_mutex_lock(&w->mutex);
delete(w, watched);
pthread_mutex_unlock(&w->mutex);
daemon_assert(invariant(w));
}
void watchdog_add_watched(watchdog_t *w, watched_t *watched)
{
daemon_assert(invariant(w));
pthread_mutex_lock(&w->mutex);
watched->watched_thread = pthread_self();
watched->watchdog = w;
insert(w, watched);
pthread_mutex_unlock(&w->mutex);
daemon_assert(invariant(w));
}
/* remove a single character */
static int use_incoming_char(struct telnet_session_s *t)
{
int c;
daemon_assert(t->in_take >= 0);
daemon_assert(t->in_take < BUF_LEN);
c = t->in_buf[t->in_take];
t->in_take++;
if (t->in_take == BUF_LEN) {
t->in_take = 0;
}
t->in_buflen--;
return c;
}
void error_init(error_t *err, int error_code, const char *desc_fmt, ...)
{
va_list args;
daemon_assert(err != NULL);
daemon_assert(error_code >= 0);
daemon_assert(desc_fmt != NULL);
err->error_code = error_code;
va_start(args, desc_fmt);
vsnprintf(err->desc, sizeof(err->desc), desc_fmt, args);
va_end(args);
daemon_assert(invariant(err));
}
void telnet_session_destroy(struct telnet_session_s *t)
{
daemon_assert(invariant(t));
Test_and_Close( & t->in_fd);
Test_and_Close( & t->out_fd);
}
static void write_outgoing_data(struct telnet_session_s *t)
{
ssize_t write_ret;
if (t->out_take < t->out_add) {
/* handle a buffer that looks like this: */
/* |-- empty --|-- data --|-- empty --| */
daemon_assert(t->out_take >= 0);
daemon_assert(t->out_take < BUF_LEN);
daemon_assert(t->out_buflen > 0);
daemon_assert(t->out_buflen + t->out_take <= BUF_LEN);
write_ret = write(t->out_fd, t->out_buf + t->out_take, t->out_buflen);
} else {
/* handle a buffer that looks like this: */
/* |-- data --|-- empty --|-- data --| */
daemon_assert(t->out_take >= 0);
daemon_assert(t->out_take < BUF_LEN);
daemon_assert((BUF_LEN - t->out_take) > 0);
write_ret = write(t->out_fd, t->out_buf + t->out_take, BUF_LEN - t->out_take);
}
/* handle three possible write results */
if (write_ret == -1) {
t->out_errno = errno;
} else if (write_ret == 0) {
t->out_eof = 1;
} else {
t->out_buflen -= write_ret;
t->out_take += write_ret;
if (t->out_take >= BUF_LEN) {
t->out_take -= BUF_LEN;
}
}
}
/* add a single character, hopefully will never happen :) */
static void add_outgoing_char(struct telnet_session_s *t, int c)
{
daemon_assert(t->out_add >= 0);
daemon_assert(t->out_add < BUF_LEN);
t->out_buf[t->out_add] = c;
t->out_add++;
if (t->out_add == BUF_LEN) {
t->out_add = 0;
}
if (t->out_buflen == BUF_LEN) {
t->out_take++;
if (t->out_take == BUF_LEN) {
t->out_take = 0;
}
} else {
t->out_buflen++;
}
}
/* returns NULL if not at least one digit */
static const char *parse_number(int *num, const char *s, int max_num)
{
int tmp;
int cur_digit;
daemon_assert(s != NULL);
daemon_assert(num != NULL);
/* handle first character */
if (!isdigit(*s)) {
return NULL;
}
tmp = (*s - '0');
s++;
/* handle subsequent characters */
while (isdigit(*s)) {
cur_digit = (*s - '0');
/* check for overflow */
if ((max_num - cur_digit) < (tmp*10)) {
return NULL;
}
tmp *= 10;
tmp += cur_digit;
s++;
}
daemon_assert(tmp >= 0);
daemon_assert(tmp <= max_num);
/* return the result */
*num = tmp;
return s;
}
/* initialize a telnet session */
void telnet_session_init(struct telnet_session_s *t, FILE_DESCRIPTOR_OR_ERROR in, FILE_DESCRIPTOR_OR_ERROR out)
{
daemon_assert(t != NULL);
daemon_assert(in >= 0);
daemon_assert(out >= 0);
t->in_fd = in;
t->in_errno = 0;
t->in_eof = 0;
t->in_take = t->in_add = 0;
t->in_buflen = 0;
t->in_status = NORMAL;
t->out_fd = out;
t->out_errno = 0;
t->out_eof = 0;
t->out_take = t->out_add = 0;
t->out_buflen = 0;
process_data(t, 0);
daemon_assert(invariant(t));
}
int main( int argc, char **argv ) {
cx_t cx;
memset(&cx, 0, sizeof(cx));
/* static cx_t cx;
memset(&cx, 0, sizeof(cx));
cx.d_opts.ident = "fastrakd";
cx.d_opts.sched_rt = SOMATIC_D_SCHED_UI; // logger not realtime
cx.opt_chan_name = "fastrak";
cx.opt_freq = -1;
cx.opt_peekabot = 0;
cx.opt_peekabot_host = "localhost";
cx.opt_device = "/dev/ttyS0";
cx.opt_kalman = 0;
cx.opt_diag_E = 1;
cx.opt_diag_Q = 250;
cx.opt_diag_R = 1;
*/
// parse options
// argp_parse (&argp, argc, argv, 0, NULL, &cx);
// somatic_verbprintf_prefix="fastrak";
// somatic_verbprintf( 1, "Frequency of %d Hz\n", cx.opt_freq );
// init(&cx);
daemonize("fastrakd");
int r = fastrak_init(&(cx.fk), "/dev/ttyS0" );
r = ach_open( &chan, "fastrak", NULL );
daemon_assert( ACH_OK == r, __LINE__ );
//printf("About to start\n");
run(&cx);
// destroy(&cx);
daemon_close();
return 0;
}
static void read_incoming_data(struct telnet_session_s *t)
{
ssize_t read_ret;
char buf[BUF_LEN];
ssize_t i;
/* read as much data as we have buffer space for */
daemon_assert(max_input_read(t) <= BUF_LEN);
read_ret = read(t->in_fd, buf, max_input_read(t));
/* handle three possible read results */
if (read_ret == -1) {
t->in_errno = errno;
} else if (read_ret == 0) {
t->in_eof = 1;
} else {
for (i = 0; i < read_ret; i++) {
process_incoming_char(t, (unsigned char) buf[i]);
}
}
}
/* print output */
int telnet_session_print(struct telnet_session_s *t, const char *s)
{
int len;
int amt_printed;
daemon_assert(invariant(t));
len = strlen(s);
if (len == 0) {
daemon_assert(invariant(t));
return 1;
}
amt_printed = 0;
do {
if ((t->out_errno != 0) || (t->out_eof != 0)) {
daemon_assert(invariant(t));
return 0;
}
while ((amt_printed < len) && (t->out_buflen < BUF_LEN)) {
daemon_assert(s[amt_printed] != '\0');
add_outgoing_char(t, s[amt_printed]);
amt_printed++;
}
process_data(t, 1);
} while (amt_printed < len);
while (t->out_buflen > 0) {
if ((t->out_errno != 0) || (t->out_eof != 0)) {
daemon_assert(invariant(t));
return 0;
}
process_data(t, 1);
}
daemon_assert(invariant(t));
return 1;
}
const char *error_get_desc(const error_t *err)
{
daemon_assert(invariant(err));
return err->desc;
}
int error_get_error_code(const error_t *err)
{
daemon_assert(invariant(err));
return err->error_code;
}
/* this is okay, as long as subsequent uses VERIFY THE FAMILY first */
static const char *parse_host_port_long(sockaddr_storage_t *sa, const char *s)
{
int i;
int family;
int tmp;
int addr_len;
unsigned char addr[255];
int port_len;
unsigned char port[255];
/* we are family */
s = parse_number(&family, s, 255);
if (s == NULL) {
return NULL;
}
if (*s != ',') {
return NULL;
}
s++;
/* parse host length */
s = parse_number(&addr_len, s, 255);
if (s == NULL) {
return NULL;
}
if (*s != ',') {
return NULL;
}
s++;
/* parse address */
for (i=0; i<addr_len; i++) {
daemon_assert(i < sizeof(addr)/sizeof(addr[0]));
s = parse_number(&tmp, s, 255);
addr[i] = tmp;
if (s == NULL) {
return NULL;
}
if (*s != ',') {
return NULL;
}
s++;
}
/* parse port length */
s = parse_number(&port_len, s, 255);
if (s == NULL) {
return NULL;
}
/* parse port */
for (i=0; i<port_len; i++) {
if (*s != ',') {
return NULL;
}
s++;
daemon_assert(i < sizeof(port)/sizeof(port[0]));
s = parse_number(&tmp, s, 255);
port[i] = tmp;
}
/* okay, everything parses, load the address if possible */
if (family == 4) {
SAFAM(sa) = AF_INET;
if (addr_len != sizeof(struct in_addr)) {
return NULL;
}
if (port_len != 2) {
return NULL;
}
memcpy(&SINADDR(sa), addr, addr_len);
SINPORT(sa) = htons((port[0] << 8) + port[1]);
}
#ifdef INET6
else if (family == 6) {
SAFAM(sa) = AF_INET6;
if (addr_len != sizeof(struct in6_addr)) {
return NULL;
}
if (port_len != 2) {
return NULL;
}
memcpy(&SIN6ADDR(sa), addr, addr_len);
SINPORT(sa) = htons((port[0] << 8) + port[1]);
}
#endif
else {
SAFAM(sa) = -1;
}
/* return new pointer */
return s;
}
int ftp_command_parse(const char *input, ftp_command_t *cmd)
{
int i;
int len;
int match;
ftp_command_t tmp;
int c;
const char *optional_number;
daemon_assert(input != NULL);
daemon_assert(cmd != NULL);
/* see if our input starts with a valid command */
match = -1;
for (i=0; (i<NUM_COMMAND) && (match == -1); i++) {
len = strlen(command_def[i].name);
if (strncasecmp(input, command_def[i].name, len) == 0) {
match = i;
}
}
/* if we didn't find a match, return error */
if (match == -1) {
return 0;
}
daemon_assert(match >= 0);
daemon_assert(match < NUM_COMMAND);
/* copy our command */
strcpy(tmp.command, command_def[match].name);
/* advance input past the command */
input += strlen(command_def[match].name);
/* now act based on the command */
switch (command_def[match].arg_type) {
case ARG_NONE:
tmp.num_arg = 0;
break;
case ARG_STRING:
if (*input != ' ') {
return 0;
}
++input;
input = copy_string(tmp.arg[0].string, input);
tmp.num_arg = 1;
break;
case ARG_OPTIONAL_STRING:
if (*input == ' ') {
++input;
input = copy_string(tmp.arg[0].string, input);
tmp.num_arg = 1;
} else {
tmp.num_arg = 0;
}
break;
case ARG_HOST_PORT:
if (*input != ' ') {
return 0;
}
input++;
/* parse the host & port information (if any) */
input = parse_host_port(&tmp.arg[0].host_port, input);
if (input == NULL) {
return 0;
}
tmp.num_arg = 1;
break;
case ARG_HOST_PORT_LONG:
if (*input != ' ') {
return 0;
}
input++;
/* parse the host & port information (if any) */
input = parse_host_port_long(&tmp.arg[0].host_port, input);
if (input == NULL) {
return 0;
}
tmp.num_arg = 1;
break;
case ARG_HOST_PORT_EXT:
if (*input != ' ') {
return 0;
}
input++;
/* parse the host & port information (if any) */
input = parse_host_port_ext(&tmp.arg[0].host_port, input);
if (input == NULL) {
return 0;
}
tmp.num_arg = 1;
break;
/* the optional number may also be "ALL" */
case ARG_OPTIONAL_NUMBER:
if (*input == ' ') {
++input;
optional_number = parse_number(&tmp.arg[0].num, input, 255);
if (optional_number != NULL) {
input = optional_number;
} else {
if ((tolower(input[0]) == 'a') &&
(tolower(input[1]) == 'l') &&
(tolower(input[2]) == 'l'))
{
tmp.arg[0].num = EPSV_ALL;
input += 3;
} else {
return 0;
}
}
tmp.num_arg = 1;
} else {
tmp.num_arg = 0;
}
break;
case ARG_TYPE:
if (*input != ' ') {
return 0;
}
input++;
c = toupper(*input);
if ((c == 'A') || (c == 'E')) {
tmp.arg[0].string[0] = c;
tmp.arg[0].string[1] = '\0';
input++;
if (*input == ' ') {
input++;
c = toupper(*input);
if ((c != 'N') && (c != 'T') && (c != 'C')) {
return 0;
}
tmp.arg[1].string[0] = c;
tmp.arg[1].string[1] = '\0';
input++;
tmp.num_arg = 2;
} else {
tmp.num_arg = 1;
}
} else if (c == 'I') {
tmp.arg[0].string[0] = 'I';
tmp.arg[0].string[1] = '\0';
input++;
tmp.num_arg = 1;
} else if (c == 'L') {
tmp.arg[0].string[0] = 'L';
tmp.arg[0].string[1] = '\0';
input++;
input = parse_number(&tmp.arg[1].num, input, 255);
if (input == NULL) {
return 0;
}
tmp.num_arg = 2;
} else {
return 0;
}
break;
case ARG_STRUCTURE:
if (*input != ' ') {
return 0;
}
input++;
c = toupper(*input);
if ((c != 'F') && (c != 'R') && (c != 'P')) {
return 0;
}
input++;
tmp.arg[0].string[0] = c;
tmp.arg[0].string[1] = '\0';
tmp.num_arg = 1;
break;
case ARG_MODE:
if (*input != ' ') {
return 0;
}
input++;
c = toupper(*input);
if ((c != 'S') && (c != 'B') && (c != 'C')) {
return 0;
}
input++;
tmp.arg[0].string[0] = c;
tmp.arg[0].string[1] = '\0';
tmp.num_arg = 1;
break;
case ARG_OFFSET:
if (*input != ' ') {
return 0;
}
input++;
input = parse_offset(&tmp.arg[0].offset, input);
if (input == NULL) {
return 0;
}
tmp.num_arg = 1;
break;
default:
daemon_assert(0);
}
/* check for our ending newline */
if (*input != '\n') {
return 0;
}
/* return our result */
*cmd = tmp;
return 1;
}
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();
}
int main( int argc, char **argv )
{
Hubo_Control hubo("proto-manip-daemon");
ach_channel_t chan_manip_cmd;
int r = ach_open( &chan_manip_cmd, CHAN_HUBO_MANIP, NULL );
daemon_assert( r==ACH_OK, __LINE__ );
hubo_manip_cmd manip;
memset( &manip, 0, sizeof(manip) );
hubo.update();
Eigen::Isometry3d Br, Bl;
Vector6d right, left, zeros; zeros.setZero();
Vector3d rtrans, ltrans, langles, rangles;
hubo.getRightArmAngles(right);
hubo.getLeftArmAngles(left);
hubo.huboArmFK( Br, right, RIGHT );
hubo.huboArmFK( Bl, left, LEFT );
std::cout << "Performed initial FK" << std::endl;
for(int i=0; i<3; i++)
{
manip.translation[RIGHT][i] = Br(i,3);
manip.translation[LEFT][i] = Bl(i,3);
}
std::cout << "Putting first transformation" << std::endl;
ach_put( &chan_manip_cmd, &manip, sizeof(manip) );
size_t fs;
std::cout << "About to start loop" << std::endl;
while( !daemon_sig_quit )
{
hubo.update();
ach_get( &chan_manip_cmd, &manip, sizeof(manip), &fs, NULL, ACH_O_LAST );
for(int i=0; i<3; i++)
{
rtrans(i) = manip.translation[RIGHT][i];
ltrans(i) = manip.translation[LEFT][i];
rangles(i) = manip.eulerAngles[RIGHT][i];
langles(i) = manip.eulerAngles[LEFT][i];
}
// Handle the right arm
Br = Eigen::Matrix4d::Identity();
Br.translate(rtrans);
Br.rotate( Eigen::AngleAxisd(rangles(0), Vector3d(1,0,0)) );
Br.rotate( Eigen::AngleAxisd(rangles(1), Vector3d(0,1,0)) );
Br.rotate( Eigen::AngleAxisd(rangles(2), Vector3d(0,0,1)) );
hubo.huboArmIK( right, Br, zeros, RIGHT );
hubo.setRightArmAngles( right );
// Handle the left arm
Bl = Eigen::Matrix4d::Identity();
Bl.translate(ltrans);
Bl.rotate( Eigen::AngleAxisd(langles(0), Vector3d(1,0,0)) );
Bl.rotate( Eigen::AngleAxisd(langles(1), Vector3d(0,1,0)) );
Bl.rotate( Eigen::AngleAxisd(langles(2), Vector3d(0,0,1)) );
hubo.huboArmIK( left, Bl, zeros, LEFT );
hubo.setLeftArmAngles( left );
// Send commands off to the control daemon
hubo.sendControls();
}
ach_close( &chan_manip_cmd );
}
