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; }