struct state simulate_fixed_control(struct state init_state, struct command U, double time){
struct state state_x;
state_x = init_state;
printf("comm u1:%lf u2:%lf\n", U.u1, U.u2);
printf("sim1 init: elevation: %lf, pitch: %lf, travel: %lf, d_elevation: %lf, d_pitch: %lf, d_travel: %lf\n", init_state.elevation, init_state.pitch, init_state.travel, init_state.d_elevation, init_state.d_pitch, init_state.d_travel);
int steps = time/SIM_STEP;
int k = 0;
for (k = 0; k <steps; k++){
state_x = eval_state(state_x, U);
if(check_safety(state_x) == 0)
{
state_x.safe = 0;
printf("sim1 nsafe: elevation: %lf, pitch: %lf, travel: %lf, d_elevation: %lf, d_pitch: %lf, d_travel: %lf, step: %d, tot_steps: %d\n", state_x.elevation, state_x.pitch, state_x.travel, state_x.d_elevation, state_x.d_pitch, state_x.d_travel, k , steps);
return state_x;
}
}
state_x.safe = 1;
printf("sim1 safe: elevation: %lf, pitch: %lf, travel: %lf, d_elevation: %lf, d_pitch: %lf, d_travel: %lf\n", state_x.elevation, state_x.pitch, state_x.travel, state_x.d_elevation, state_x.d_pitch, state_x.d_travel);
return state_x;
}
struct state simulate_with_controller(struct state init_state, double time){
struct state state_x;
state_x = init_state;
int steps = time/SIM_STEP;
struct controller_storage cs;
cs.int_elevation = 0;
cs.int_pitch = 0;
cs.int_travel = 0;
int k = 0;
for (k = 0; k <steps; k++){
struct command U = controller_safety(sps, state_x, &cs);
state_x = eval_state(state_x, U);
if ( check_safety(state_x) == 0){
state_x.safe = 0;
printf("sim2 nsafe: elevation: %lf, pitch: %lf, travel: %lf, d_elevation: %lf, d_pitch: %lf, d_travel: %lf\n", state_x.elevation, state_x.pitch, state_x.travel, state_x.d_elevation, state_x.d_pitch, state_x.d_travel);
return state_x;
}
}
state_x.safe = 1;
printf("sim2 safe: elevation: %lf, pitch: %lf, travel: %lf, d_elevation: %lf, d_pitch: %lf, d_travel: %lf\n", state_x.elevation, state_x.pitch, state_x.travel, state_x.d_elevation, state_x.d_pitch, state_x.d_travel);
return state_x;
}
/*
* Check the safety of the desired joint states explicitly
* and then calculate the u_ff with inverse dynamics
*
* If friction compensation is turned on, then add some compensation on top of u_ff
*
*/
static void control_arm() {
check_safety();
// control the robot
// calculate the feedforward commands with inverse dynamics
SL_InvDyn(NULL, joint_des_state, endeff, &base_state, &base_orient);
if (friction_comp) {
addFrictionModel(joint_des_state);
}
}
ssize_t CondorFileTable::write( int fd, const void *data, size_t nbyte )
{
if( resume(fd)<0 ) return -1;
if( !pointers[fd]->file->is_writeable() ) {
errno = EBADF;
return -1;
}
if (!data) {
errno = EINVAL;
return -1;
}
if( nbyte==0 ) return 0;
CondorFilePointer *fp = pointers[fd];
CondorFile *f = fp->file;
// Write to the object at the current offset
// XXX Don't fix the warning on this line. There is a semantic breakdown
// between char* and const void* in the file table API itself, and
// between that and the UNIX system API. It will take a pretty serious
// rewrite of the function signatures to make everything conform, and
// there is a chance there is no such conforming API.
int actual = f->write( fp->offset, (char*)data, nbyte );
// If there is an error, don't touch the offset.
if(actual<0) return -1;
// Special case: always flush data on stderr
if(fd==2) f->flush();
// Update the offset by the amount written
fp->offset += actual;
// Record that a write was done
fp->info->write_count++;
fp->info->write_bytes+=actual;
// Perhaps send a warning message
check_safety(fp);
// Return the number of bytes written
return actual;
}
ssize_t CondorFileTable::read( int fd, void *data, size_t nbyte )
{
if( resume(fd)<0 ) return -1;
if( !pointers[fd]->file->is_readable() ) {
errno = EBADF;
return -1;
}
if (!data) {
errno = EINVAL;
return -1;
}
CondorFilePointer *fp = pointers[fd];
CondorFile *f = fp->file;
if( nbyte==0 ) return 0;
// get the data from the object
int actual = f->read( fp->offset, (char*) data, nbyte );
// If there is an error, don't touch the offset.
if(actual<0) return -1;
// Update the offset
fp->offset += actual;
// Record that a read was done
fp->info->read_count++;
fp->info->read_bytes+=actual;
// Perhaps send a warning message
check_safety(fp);
// Return the number of bytes read
return actual;
}
