int main(int argc, char** argv) {
if (argc != 2) {
printf("Usage: %s <calibrationFile>\n", argv[0]);
printf(" <calibrationFile> File containing the gain-matrix for the attached Force-Torque Sensor\n");
return 1;
}
ProductManager pm;
if ( !pm.foundForceTorqueSensor() ) {
printf("ERROR: No Force-Torque Sensor found!\n");
return 1;
}
ForceTorqueSensor& fts = *pm.getForceTorqueSensor();
int calValue = 0;
ifstream cal(argv[1]);
for (int i = 0; i < GM_SIZE; ++i) {
if (cal.good()) {
cal >> calValue;
if (calValue < -32768 || calValue > 32767) {
printf("Calibration file is poorly formated: value out of range: %d.\n", calValue);
return 1;
}
// TODO(dc): Fix once F/T has working ROLE
//fts.setProperty(Puck::GM, calValue);
Puck::setProperty(pm.getBus(), fts.getPuck()->getId(), Puck::getPropertyId(Puck::GM, Puck::PT_ForceTorque, 152), calValue);
printf(".");
fflush(stdout);
usleep(1000000);
} else {
void takeAccelSample(ProductManager& pm, int duration_us, const char* fileName)
{
BARRETT_UNITS_FIXED_SIZE_TYPEDEFS;
if ( !pm.foundForceTorqueSensor() ) {
throw std::runtime_error("Couldn't find an FTS!");
}
pm.startExecutionManager();
char tmpFile[] = "/tmp/btXXXXXX";
if (mkstemp(tmpFile) == -1) {
throw std::runtime_error("Couldn't create temporary file!");
}
systems::Ramp time(pm.getExecutionManager(), 1.0);
FTSAccel ftsa(pm.getForceTorqueSensor());
systems::TupleGrouper<double, ca_type> tg;
connect(time.output, tg.getInput<0>());
connect(ftsa.output, tg.getInput<1>());
typedef boost::tuple<double, ca_type> tuple_type;
const size_t PERIOD_MULTIPLIER = 1;
systems::PeriodicDataLogger<tuple_type> logger(
pm.getExecutionManager(),
new log::RealTimeWriter<tuple_type>(tmpFile, PERIOD_MULTIPLIER * pm.getExecutionManager()->getPeriod()),
PERIOD_MULTIPLIER);
time.start();
connect(tg.output, logger.input);
printf("Logging started.\n");
usleep(duration_us);
logger.closeLog();
printf("Logging stopped.\n");
log::Reader<tuple_type> lr(tmpFile);
lr.exportCSV(fileName);
printf("Output written to %s.\n", fileName);
std::remove(tmpFile);
}
int wam_main(int argc, char** argv, ProductManager& pm,
systems::Wam<DOF>& wam) {
BARRETT_UNITS_TEMPLATE_TYPEDEFS(DOF);
typedef Hand::jp_type hjp_t;
typedef boost::tuple<double, hjp_t> tuple_type;
typedef systems::TupleGrouper<double, hjp_t> tg_type;
tg_type tg;
char tmpFile[] = "btXXXXXX";
if (mkstemp(tmpFile) == -1) {
printf("ERROR: Couldn't create temporary file!\n");
return 1;
}
const double TRANSITION_DURATION = 0.5;
wam.gravityCompensate();
// printf("Press [Enter] to go to given position");
// waitForEnter();
// jp_type startpos(0.0); // TODO : change here
// wam.moveTo(startpos);
// Is an FTS attached?
ForceTorqueSensor* fts = NULL;
if (pm.foundForceTorqueSensor()) {
fts = pm.getForceTorqueSensor();
fts->tare();
}
// Is a Hand attached?
Hand* hand = NULL;
std::vector<TactilePuck*> tps;
if (pm.foundHand()) {
hand = pm.getHand();
printf(
">>> Press [Enter] to initialize Hand. (Make sure it has room!)");
waitForEnter();
hand->initialize();
hand->trapezoidalMove(Hand::jp_type((1.0 / 3.0) * M_PI), Hand::SPREAD);
hand->trapezoidalMove(Hand::jp_type((1.0 / 3.0) * M_PI), Hand::GRASP);
hand->trapezoidalMove(Hand::jp_type((0.0) * M_PI), Hand::GRASP);
}
printf("Error 1 \n");
tps = hand->getTactilePucks();
// TODO write some error statement
bool Release_Mode = 0;
double delta_step = 0.002; //pm.getExecutionManager()->getPeriod();
std::string input_angle_string;
input_angle_string = argv[1];
double input_angle = atoi(input_angle_string.c_str());
double spread_angle = (input_angle / 180.0) * M_PI;
// std::string threshold_impulse_str;
// std::cout << "Enter the inpulse threshold limit: ";
// std::cin >> threshold_impulse_str;
// std::cout << "\n" << std::endl;
std::string threshold_impulse_string;
threshold_impulse_string = argv[2];
double threshold_impulse = atof(threshold_impulse_string.c_str());
printf("Press [Enter] to turn on the system");
waitForEnter();
printf("Error 2 \n");
systems::Ramp time(pm.getExecutionManager(), 1.0);
// const size_t PERIOD_MULTIPLIER = 1;
const size_t PERIOD_MULTIPLIER = 1;
systems::PeriodicDataLogger<tuple_type> logger(pm.getExecutionManager(),
new log::RealTimeWriter<tuple_type>(tmpFile,
PERIOD_MULTIPLIER * pm.getExecutionManager()->getPeriod()),
PERIOD_MULTIPLIER);
printf("Error 3 \n");
// Hand_forcetorque_sense<DOF> hand_ft(hand, fts);
Hand_tactile_sense hand_tact(hand, tps);
main_processor<DOF> brain(hand, delta_step, spread_angle, threshold_impulse,
Release_Mode);
Hand_full_move hand_move(hand);
systems::connect(tg.output, logger.input);
systems::connect(time.output, brain.Current_time);
// systems::connect(hand_ft.Force_hand, brain.Force_hand);
// systems::connect(hand_ft.Torque_hand, brain.Torque_hand);
// systems::connect(hand_ft.Acceleration_hand, brain.Acceleration_hand);
systems::connect(hand_tact.Finger_Tactile_1, brain.Finger_Tactile_1);
systems::connect(hand_tact.Finger_Tactile_2, brain.Finger_Tactile_2);
systems::connect(hand_tact.Finger_Tactile_3, brain.Finger_Tactile_3);
systems::connect(hand_tact.Finger_Tactile_4, brain.Finger_Tactile_4);
systems::connect(brain.Desired_Finger_Angles, hand_move.Finger_Angles);
systems::connect(time.output, tg.template getInput<0>());
systems::connect(brain.Desired_Finger_Angles, tg.template getInput<1>());
// systems::connect(hand_ft.Force_hand_cf, tg.template getInput<1>());
printf("Error 4 \n");
time.smoothStart(TRANSITION_DURATION);
printf("Press [Enter] to stop.");
waitForEnter();
printf("Error 5 \n");
logger.closeLog();
time.smoothStop(TRANSITION_DURATION);
wam.idle();
printf("Error 6 \n");
pm.getSafetyModule()->waitForMode(SafetyModule::IDLE);
log::Reader<boost::tuple<tuple_type> > lr(tmpFile);
lr.exportCSV(argv[3]);
printf("Error 7 \n");
printf("Output written to %s.\n", argv[3]);
std::remove(tmpFile);
return 0;
}
