int main(int argc, char** argv)
{
ros::init(argc, argv, "bhand_node");
ProductManager pm;
if (!pm.foundHand())
{
printf("ERROR: No Hand found on bus!\n");
return 1;
}
Hand* hand = pm.getHand();
BHandNode bhand_node(hand);
bhand_node.init();
ros::Rate pub_rate(PUBLISH_FREQ);
while (bhand_node.n_.ok())
{
ros::spinOnce();
bhand_node.publish();
pub_rate.sleep();
}
hand->idle();
return 0;
}
int main() {
ProductManager pm;
if ( !pm.foundGimbalsHandController() ) {
printf("ERROR: No Gimbals Hand Controller found!\n");
return 1;
}
GimbalsHandController& ghc = *pm.getGimbalsHandController();
int loopCount = 0;
while (true) {
usleep(10000);
ghc.update();
loopCount = (loopCount+1) % 25;
if (loopCount == 0) {
printf("%d,%d %d,%d %d,%d %d,%d %f\n",
ghc.getThumbOpen(), ghc.getThumbClose(),
ghc.getPointerOpen(), ghc.getPointerClose(),
ghc.getMiddleOpen(), ghc.getMiddleClose(),
ghc.getRockerUp(), ghc.getRockerDown(),
ghc.getKnob());
}
}
}
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 {
boost::thread startWam(ProductManager& pm,
boost::function<void (ProductManager&, systems::Wam<4>&)> wt4,
boost::function<void (ProductManager&, systems::Wam<7>&)> wt7)
{
pm.waitForWam();
pm.wakeAllPucks();
if (pm.foundWam4()) {
return boost::thread(wt4, boost::ref(pm), boost::ref(*pm.getWam4()));
} else {
return boost::thread(wt7, boost::ref(pm), boost::ref(*pm.getWam7()));
}
}
int wam_main(int argc, char** argv, ProductManager& pm, systems::Wam<DOF>& wam) {
BARRETT_UNITS_TEMPLATE_TYPEDEFS(DOF);
const char* path = pm.getWamDefaultConfigPath();
std::cout<<"got configuration "<<std::endl;
size_t i=0;
while(path[i]!='\0'){
std::cout<<path[i];
i++;
}
std::cout<<"configuration should be printed"<<std::endl;
pm.getSafetyModule()->waitForMode(SafetyModule::IDLE);
return 0;
}
int wam_main(int argc, char** argv, ProductManager& pm,
systems::Wam<DOF>& wam) {
BARRETT_UNITS_TEMPLATE_TYPEDEFS(DOF);
std::string filename(argv[1]);
printf("\nMoving to start configuration \n");
jp_type jp(0.0);
jp[1] = -1.967;
jp[3] = 2.5;
jp[5] = -0.5;
wam.moveTo(jp);
printf("Opening hands\n");
// Open hands.
barrett::Hand& hand = *pm.getHand();
hand.initialize();
// wam.idle();
Teach<DOF> teach(wam, pm, filename);
teach.init();
printf("\nPress [Enter] to start teaching.\n");
waitForEnter();
teach.record();
//boost::thread t(&Teach<DOF>::display, &teach);
printf("Press [Enter] to stop teaching.\n");
waitForEnter();
teach.createSpline();
// Move to start and close hands.
wam.moveTo(jp);
hand.close();
hand.idle();
wam.idle();
pm.getSafetyModule()->waitForMode(SafetyModule::IDLE);
return 0;
}
int wam_main(int argc, char** argv, ProductManager& pm, systems::Wam<DOF>& wam) {
BARRETT_UNITS_TEMPLATE_TYPEDEFS(DOF);
/* Read configuration file */
libconfig::Config config;
try {
config.readFile(CAL_CONFIG_FILE.c_str());
} catch (const libconfig::FileIOException &fioex) {
printf("EXITING: I/O error while reading %s\n", CAL_CONFIG_FILE.c_str());
btsleep(5.0);
return (false);
} catch (const libconfig::ParseException &pex) {
printf("EXITING: Parse error at %s: %d - %s\n", pex.getFile(), pex.getLine(), pex.getError());
btsleep(5.0);
return (false);
}
const libconfig::Setting& setting = config.lookup("autotension")[pm.getWamDefaultConfigPath()];
std::vector<int> arg_list = validate_args<DOF>(argc, argv);
if (arg_list.size() == 0) {
pm.getSafetyModule()->waitForMode(SafetyModule::IDLE);
return 1;
}
printf("Press <Enter> to begin autotensioning.\n");
detail::waitForEnter();
AutoTension<DOF> autotension(wam, setting);
autotension.init(pm, arg_list);
/* Autotension Specified Joints*/
while (arg_list.size() != 0)
arg_list = autotension.tensionJoint(arg_list);
/* Re-fold and exit */
if (autotension.hand != 0) {
autotension.hand->open(Hand::GRASP);
autotension.hand->close(Hand::SPREAD);
autotension.hand->trapezoidalMove(Hand::jp_type(M_PI / 2.0), Hand::GRASP);
}
wam.moveHome();
printf("\n**************************\n");
printf("Autotensioning Routine Complete\n");
pm.getSafetyModule()->waitForMode(SafetyModule::IDLE);
return 0;
}
int main() {
ProductManager pm;
SafetyModule& sm = *pm.getSafetyModule();
SafetyModule::PendantState ps;
// Optional: Instantiate a Wam object and start the realtime control loop
if (pm.foundWam4()) {
pm.getWam4(false);
} else {
pm.getWam7(false);
}
while (true) {
sm.getPendantState(&ps);
std::cout << ps.toString() << " " << ps.allSafe() << " " << ps.hasFaults() << "\n";
usleep(1000000);
}
return 0;
}
int main(int argc, char** argv) {
// Message to user
printf("\n"
" *** Barrett WAM Autotensioning Utility ***\n"
"\n"
"This utility will autotension the specified cables of your WAM Arm.\n"
"Cable tensioning is necessary after signs of the WAM cables becoming\n"
"loose after extended use, or after replacing any cables on your WAM Arm.\n"
"After completion of this routine, you must zero calibrate and gravity\n"
"calibrate the WAM, as pulling tension from the cables will introduce\n"
"offsets to your previous calibrations.\n"
"\n"
"WAMs with serial numbers < 5 and WAM wrists with serial numbers < 9 are not\n"
"eligible for autotensioning.\n"
"\n"
"This program assumes the WAM is mounted such that the base is horizontal.\n"
"\n"
"\n");
// For clean stack traces
barrett::installExceptionHandler();
// Create our product manager
ProductManager pm;
pm.waitForWam();
if (pm.foundWam4()) {
return wam_main<4>(argc, argv, pm, *pm.getWam4(true, NULL));
} else if (pm.foundWam7()) {
return wam_main<7>(argc, argv, pm, *pm.getWam7(true, NULL));
}
}
template <size_t DOF> void wamThread0(ProductManager& pm0, systems::Wam<DOF>& wam0) {
BARRETT_UNITS_TEMPLATE_TYPEDEFS(DOF);
wam0.gravityCompensate();
jp_type jp(0.0);
while (pm0.getSafetyModule()->getMode() == SafetyModule::ACTIVE) {
wam0.moveTo(jp);
sleep(1);
wam0.moveHome();
sleep(1);
}
}
int wam_main(int argc, char** argv, ProductManager& pm, systems::Wam<DOF>& wam) {
BARRETT_UNITS_TEMPLATE_TYPEDEFS(DOF);
typedef boost::tuple<double, jp_type> jp_sample_type;
char tmpFile[] = "/tmp/btXXXXXX";
if (mkstemp(tmpFile) == -1) {
printf("ERROR: Couldn't create temporary file!\n");
return 1;
}
const double T_s = pm.getExecutionManager()->getPeriod();
//wam.gravityCompensate();
boost::thread displayThread(displayEntryPoint, pm.getHand());
std::remove(tmpFile);
pm.getSafetyModule()->waitForMode(SafetyModule::IDLE);
return 0;
}
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);
std::string filename(argv[1]);
Teach<DOF> teach(wam, pm, filename);
teach.init();
printf("\nPress [Enter] to start teaching.\n");
waitForEnter();
teach.record();
//boost::thread t(&Teach<DOF>::display, &teach);
printf("Press [Enter] to stop teaching.\n");
waitForEnter();
teach.createSpline();
pm.getSafetyModule()->waitForMode(SafetyModule::IDLE);
return 0;
}
int wam_main(int argc, char** argv, ProductManager& pm, systems::Wam<DOF>& wam) {
BARRETT_UNITS_TEMPLATE_TYPEDEFS(DOF);
typedef boost::tuple<double, jp_type> jp_sample_type;
char tmpFile[] = "/tmp/btXXXXXX";
if (mkstemp(tmpFile) == -1) {
printf("ERROR: Couldn't create temporary file!\n");
return 1;
}
const double T_s = pm.getExecutionManager()->getPeriod();
wam.gravityCompensate();
systems::Ramp time(pm.getExecutionManager());
systems::TupleGrouper<double, jp_type> jpLogTg;
// Record at 1/10th of the loop rate
systems::PeriodicDataLogger<jp_sample_type> jpLogger(pm.getExecutionManager(),
new barrett::log::RealTimeWriter<jp_sample_type>(tmpFile, 10*T_s), 10);
printf("Press [Enter] to start teaching.\n");
waitForEnter();
{
// Make sure the Systems are connected on the same execution cycle
// that the time is started. Otherwise we might record a bunch of
// samples all having t=0; this is bad because the Spline requires time
// to be monotonic.
BARRETT_SCOPED_LOCK(pm.getExecutionManager()->getMutex());
connect(time.output, jpLogTg.template getInput<0>());
connect(wam.jpOutput, jpLogTg.template getInput<1>());
connect(jpLogTg.output, jpLogger.input);
time.start();
}
printf("Press [Enter] to stop teaching.\n");
waitForEnter();
jpLogger.closeLog();
disconnect(jpLogger.input);
// Build spline between recorded points
log::Reader<jp_sample_type> lr(tmpFile);
std::vector<jp_sample_type> vec;
for (size_t i = 0; i < lr.numRecords(); ++i) {
vec.push_back(lr.getRecord());
}
math::Spline<jp_type> spline(vec);
printf("Press [Enter] to play back the recorded trajectory.\n");
waitForEnter();
// First, move to the starting position
wam.moveTo(spline.eval(spline.initialS()));
// Then play back the recorded motion
time.stop();
time.setOutput(spline.initialS());
systems::Callback<double, jp_type> trajectory(boost::ref(spline));
connect(time.output, trajectory.input);
wam.trackReferenceSignal(trajectory.output);
time.start();
while (trajectory.input.getValue() < spline.finalS()) {
usleep(100000);
}
printf("Press [Enter] to idle the WAM.\n");
waitForEnter();
wam.idle();
std::remove(tmpFile);
pm.getSafetyModule()->waitForMode(SafetyModule::IDLE);
return 0;
}
int main() {
typedef Hand::jp_type hjp_t;
ProductManager pm;
if ( !pm.foundHand() ) {
printf("ERROR: No Hand found on bus!\n");
return 1;
}
Hand& hand = *pm.getHand();
hand.initialize();
double O = 0.0;
double C = 2.4;
double SC = M_PI;
hjp_t open(O);
hjp_t closed(C);
closed[3] = SC;
double OR = -0.75;
double CR = 0.75;
Hand::jv_type opening(OR);
Hand::jv_type closing(CR);
{
assertPosition(hand, open);
hand.close();
assertPosition(hand, closed);
hand.open();
assertPosition(hand, open);
hand.close(Hand::SPREAD);
assertPosition(hand, hjp_t(O,O,O,SC));
hand.close(Hand::GRASP);
assertPosition(hand, closed);
hand.open(Hand::GRASP, false);
btsleep(0.5);
assertPosition(hand, hjp_t(1.6,1.6,1.6,SC));
hand.open();
assertPosition(hand, open);
}
{
// Original interface preserved? Should move all 4 motors.
hand.trapezoidalMove(closed);
assertPosition(hand, closed);
hand.trapezoidalMove(open, false);
hand.waitUntilDoneMoving();
assertPosition(hand, open);
// New interface
hand.trapezoidalMove(closed, Hand::SPREAD);
assertPosition(hand, hjp_t(O,O,O,SC));
hand.trapezoidalMove(closed, Hand::F1);
assertPosition(hand, hjp_t(C,O,O,SC));
hand.trapezoidalMove(closed, Hand::F2);
assertPosition(hand, hjp_t(C,C,O,SC));
hand.trapezoidalMove(closed, Hand::F3);
assertPosition(hand, closed);
hand.trapezoidalMove(open, Hand::GRASP);
assertPosition(hand, hjp_t(O,O,O,SC));
hand.trapezoidalMove(open, Hand::SPREAD);
assertPosition(hand, open);
hand.trapezoidalMove(closed, Hand::F3 | Hand::SPREAD);
assertPosition(hand, hjp_t(O,O,C,SC));
hand.trapezoidalMove(open, Hand::WHOLE_HAND);
assertPosition(hand, open);
}
{
double t = 0.0;
// Original interface preserved? Should move all 4 motors.
hand.velocityMove(closing);
btsleep(1);
t = 1.0;
assertPosition(hand, hjp_t(CR*t), 0.2);
// New interface
hand.velocityMove(opening, Hand::GRASP);
btsleep(1);
t = 2.0;
assertPosition(hand, hjp_t(O,O,O,CR*t), 0.4);
hand.velocityMove(opening, Hand::WHOLE_HAND);
hand.waitUntilDoneMoving();
assertPosition(hand, open);
}
{
double t = 0.0;
// Original interface preserved? Should move all 4 motors.
hand.velocityMove(closing);
btsleep(1);
t = 1.0;
assertPosition(hand, hjp_t(CR*t), 0.2);
// SPREAD should continue its velocity move
hand.trapezoidalMove(open, Hand::F1); // Only blocks for F1
t = 1.6;
assertPosition(hand, hjp_t(O,CR*t,CR*t,CR*t), 0.4);
hand.open(Hand::GRASP); // Only blocks for GRASP
t = 2.4;
assertPosition(hand, hjp_t(O,O,O,CR*t), 0.4);
btsleep(1);
t = 3.4;
assertPosition(hand, hjp_t(O,O,O,CR*t), 0.6);
hand.open();
assertPosition(hand, open);
}
hand.idle();
return 0;
}
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;
}
bool isWamActivated(ProductManager& pm) {
return pm.getSafetyModule()->getMode() == SafetyModule::ACTIVE;
}
int wam_main(int argc, char** argv, ProductManager& pm, systems::Wam<DOF>& wam)
{
BARRETT_UNITS_TEMPLATE_TYPEDEFS(DOF);
cp_type cartSetPoint;
wam.gravityCompensate();
/////////////////////////////////////////////////////////////////////////////////////
// Add the following to our system to make our Cartesian controller much more robust.
/////////////////////////////////////////////////////////////////////////////////////
// Set up singularity avoidance springs
jp_type joint_center(0.0); // Initialize our joint centers to 0.0 on all joints
std::vector<double> spring_constants(DOF); // create spring constants
std::vector<double> damping_constants(DOF); // create damping constants
joint_center[0] = 0.0;
joint_center[1] = 0.0;
joint_center[2] = 0.0;
joint_center[3] = 1.1; // J4 Joint Range Center at 1.1 Radians
spring_constants[0] = 15.0;
spring_constants[1] = 5.0;
spring_constants[2] = 5.0;
spring_constants[3] = 5.0;
damping_constants[0] = 10.0;
damping_constants[1] = 20.0;
damping_constants[2] = 10.0;
damping_constants[3] = 6.0;
if (DOF == 7)
{
joint_center[4] = -1.76; // J5 Joint Range Center at -1.76 Radians
joint_center[5] = 0.0;
joint_center[6] = 0.0;
spring_constants[4] = 0.5;
spring_constants[5] = 0.25;
spring_constants[6] = 0.25;
damping_constants[4] = 0.05;
damping_constants[5] = 0.05;
damping_constants[6] = 0.0;
}
printf("Press Enter to Turn on Haptic Singularity Avoidance.\n");
detail::waitForEnter();
//Initialization Move
jp_type wam_init = wam.getHomePosition();
wam_init[3] -= .35;
wam.moveTo(wam_init); // Adjust the elbow, moving the end-effector out of the haptic boundary and hold position for haptic force initialization.
// Change decrease our tool position controller gains slightly
cp_type cp_kp, cp_kd;
for (size_t i = 0; i < 3; i++)
{
cp_kp[i] = 1500;
cp_kd[i] = 5.0;
}
wam.tpController.setKp(cp_kp);
wam.tpController.setKd(cp_kd);
//Torque Summer from our three systems
systems::Summer<jt_type, 4> singJTSum(true);
// Our singularity avoidance system
SingularityAvoid<DOF> singularityAvoid(joint_center);
systems::connect(wam.jpOutput, singularityAvoid.input);
systems::connect(singularityAvoid.output, singJTSum.getInput(0));
// Attach our joint stop springs
JointStopSprings<DOF> jointStopSprings(joint_center, spring_constants);
systems::connect(wam.jpOutput, jointStopSprings.input);
systems::connect(jointStopSprings.output, singJTSum.getInput(1));
// Joint velocity damper for kinematic solutions causing velocity faults
JVDamper<DOF> jvDamper(damping_constants);
systems::connect(wam.jvOutput, jvDamper.input);
systems::connect(jvDamper.output, singJTSum.getInput(2));
// Haptic collision avoidance boundary portion
HapticCollisionAvoid<DOF> hapticCollisionAvoid(2000);
systems::ToolForceToJointTorques<DOF> tf2jt;
systems::connect(wam.kinematicsBase.kinOutput, tf2jt.kinInput);
systems::connect(wam.toolPosition.output, hapticCollisionAvoid.input);
systems::connect(hapticCollisionAvoid.output, tf2jt.input);
systems::connect(tf2jt.output, singJTSum.getInput(3));
systems::connect(singJTSum.output, wam.input);
// New system watchdogs to monitor and stop trajectories if expecting a fault
TorqueWatchdog<DOF> torqueWatchdog;
VelocityWatchdog<DOF> velocityWatchdog;
pm.getExecutionManager()->startManaging(torqueWatchdog);
pm.getExecutionManager()->startManaging(velocityWatchdog);
systems::connect(wam.jtSum.output, torqueWatchdog.input);
systems::connect(wam.jvOutput, velocityWatchdog.input);
/////////////////////////////////////////////////////////////////////////////////////
// End of robust cartesian setup
/////////////////////////////////////////////////////////////////////////////////////
printf("Press Enter to start test.\n");
detail::waitForEnter();
int i;
for (i = 1; i < 31; i++) //from random.cc
{
srand(time(NULL));
double x = -1 + 2 * ((double)rand()) / RAND_MAX;
double y = -1 + 2 * (((double)rand()) / ((double)RAND_MAX));
double z = ((double)rand()) / ((double)RAND_MAX);
cartSetPoint[0] = x;
cartSetPoint[1] = y;
cartSetPoint[2] = z;
double reach = sqrt(x * x + y * y + z * z);
if (reach < 0.95)
{
std::cout << i << ": Moving to coordinate " << x << ", " << y << ", " << z << ".\n";
std::cout << reach << "\n";
wam.moveTo(cartSetPoint, false);
torqueWatchdog.activate();
velocityWatchdog.activate(); // The situation here is interesting. A velocity fault can present itself from Cartesian end-effector or elbow velocities,
// However, in its current capacity, we can only monitor joint velocities, or end-effector velocities.
bool faulted = false;
while (!wam.moveIsDone() && !faulted)
{
//Check our velocity and torque output to see if crossing threshold (approaching a fault situation)
jt_type curJT = torqueWatchdog.getCurrentTorque();
jv_type curJV = velocityWatchdog.getCurrentVelocity();
for (size_t i = 0; i < DOF; i++)
{
if (curJT[i] > 30.0 || curJV[i] > 0.9)
{
wam.idle();
printf("Stopping Move - Fault presented on Joint %zu - Torque: %f, Velocity: %f\n", i, curJT[i],
curJV[i]);
faulted = true;
wam.moveTo(wam.getJointPositions());
}
}
btsleep(0.01);
}
torqueWatchdog.deactivate();
velocityWatchdog.deactivate();
continue;
}
else
i--;
continue;
}
systems::disconnect(wam.input);
wam.moveHome();
pm.getSafetyModule()->waitForMode(SafetyModule::IDLE);
return 0;
}
int wam_main(int argc, char** argv, ProductManager& pm, systems::Wam<DOF>& wam) {
BARRETT_UNITS_TEMPLATE_TYPEDEFS(DOF);
// These vectors are fixed sized, stack allocated, and zero-initialized.
jp_type jp; // jp is a DOFx1 column vector of joint positions
cp_type cp; // cp is a 3x1 vector representing a Cartesian position
wam.gravityCompensate();
printMenu();
Hand* hand = pm.getHand();
// Write a socket program here. It is a server socket that would close only when the program closes
// This client socket would connect to a server socket on a external machine a recieve data from it
//---------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
int status;
struct addrinfo host_info;
struct addrinfo *host_info_list;
memset(&host_info, 0, sizeof host_info);
host_info.ai_family = AF_UNSPEC;
host_info.ai_socktype = SOCK_STREAM;
host_info.ai_flags = AI_PASSIVE;
status = getaddrinfo("192.168.0.110", "5555", &host_info, &host_info_list); //192.168.0.110 this is the wam(server) address
int socketfd ;
socketfd = socket(host_info_list->ai_family, host_info_list->ai_socktype, host_info_list->ai_protocol);
int yes = 1;
status = setsockopt(socketfd, SOL_SOCKET, SO_REUSEADDR, &yes, sizeof(int));
status = bind(socketfd, host_info_list->ai_addr, host_info_list->ai_addrlen);
status = listen(socketfd, 5);
int new_sd;
struct sockaddr_storage their_addr;
socklen_t addr_size = sizeof(their_addr);
new_sd = accept(socketfd, (struct sockaddr *)&their_addr, &addr_size);
std::string line;
bool going = true;
while (going) {
std::cout << "Whiel loop starts" << std::endl;
//-----------------------------------------------------------------------------------
ssize_t bytes_recieved;
char incomming_data_buffer[1000];
bytes_recieved = recv(new_sd, incomming_data_buffer,1000, 0);
std::cout << bytes_recieved << std::endl;
// if (bytes_recieved == 0) std::cout << "host shut down." << std::endl ;
// if (bytes_recieved == -1)std::cout << "recieve error!" << std::endl ;
incomming_data_buffer[bytes_recieved] = '\0';
std::cout << incomming_data_buffer << std::endl;
const char *msg = "Niladri Das I am the Server";
const char *msg1 = "Press f exactly four times";
int len1;
int len;
ssize_t bytes_sent;
ssize_t bytes_sent1;
len = strlen(msg);
len1 = strlen(msg1);
//-----------------------------------------------------------------------------------
// printf(">>> ");
// std::cin.getline(incomming_data_buffer, line);
line = std::string(incomming_data_buffer);
std::cout << incomming_data_buffer << std::endl;
switch (line[0]) {
case 'j':
moveToStr(wam, &jp, "joint positions", line.substr(1));
std::cout << "sending back a message..." << std::endl;
bytes_sent = send(new_sd, msg, len, 0);
break;
case 'f':
std::getline(file_jp,line_jp);
{
moveToStr(wam, &jp, "joint positions", line_jp);
}
std::cout << "sending back a message..." << std::endl;
bytes_sent1 = send(new_sd, msg1, len1, 0);
break;
case 'p':
moveToStr(wam, &cp, "tool position", line.substr(1));
std::cout << "sending back a message..." << std::endl;
bytes_sent = send(new_sd, msg, len, 0);
break;
case 'M':
std::cout << "send()ing back a message..." << std::endl;
bytes_sent = send(new_sd, msg, len, 0);
break;
case 'h':
std::cout << "Moving to home position: "
<< wam.getHomePosition() << std::endl;
wam.moveHome();
std::cout << "sending back a message" << std::endl;
bytes_sent = send(new_sd, msg, len,0);
break;
case 'i':
printf("WAM idled.\n");
wam.idle();
std::cout << "sending back a message" << std::endl;
bytes_sent = send(new_sd, msg, len,0);
break;
case 'o':
hand->initialize();
// hand->close(Hand::GRASP);
// hand->open(Hand::GRASP);
// hand->close(Hand::SPREAD);
// hand->trapezoidalMove(Hand::jp_type(M_PI/2.0),1, Hand::GRASP);
// hand->trapezoidalMove(Hand::jp_type(M_PI/2.0), Hand::GRASP);
std::cout << "sending back a message..." << std::endl;
bytes_sent = send(new_sd, msg, len, 0);
break;
case 'a':
// hand->initialize();
hand->close(Hand::SPREAD);
// hand->close(Hand::GRASP);
std::cout << "sending back a message..." << std::endl;
bytes_sent = send(new_sd, msg, len, 0);
break;
case 'c':
// hand->initialize();
hand->close(Hand::GRASP);
std::cout << "sending back a message..." << std::endl;
bytes_sent = send(new_sd, msg, len, 0);
break;
case 'd':
// hand->initialize();
hand->open(Hand::SPREAD);
std::cout << "sending back a message..." << std::endl;
bytes_sent = send(new_sd, msg, len, 0);
break;
case 'e':
// hand->initialize();
hand->open(Hand::GRASP);
std::cout << "sending back a message..." << std::endl;
bytes_sent = send(new_sd, msg, len, 0);
break;
case 'b':
hand->trapezoidalMove(Hand::jp_type(M_PI/2.0), Hand::GRASP);
std::cout << "sending back a message..." << std::endl;
bytes_sent = send(new_sd, msg, len, 0);
break;
case 't':
hand->trapezoidalMove(Hand::jp_type((M_PI)/6.0), Hand::GRASP);
std::cout << "sending back a message..." << std::endl;
bytes_sent = send(new_sd, msg, len, 0);
break;
case 'm':
// temp_str = line.substr(2);
iss2.clear();
iss2.str("");
iss2.str(line.substr(2)) ;
iss2 >> angle_degree;
// std::cin >> angle_degree;
angle_radian = angle_degree*(M_PI/180);
hand->trapezoidalMove(Hand::jp_type(angle_radian), Hand::GRASP);
std::cout << angle_degree << std::endl;
bytes_sent = send(new_sd, msg, len, 0);
break;
case 'q':
case 'x':
printf("Quitting.\n");
going = false;
std::cout << "sending back a message..." << std::endl;
bytes_sent = send(new_sd, msg, len, 0);
break;
default:
if (line.size() != 0) {
printf("Unrecognized option.\n");
std::cout << "sending back a message..." << std::endl;
bytes_sent = send(new_sd, msg, len, 0);
printMenu();
}
break;
}
std::cout << "While loop complete" << std::endl;
}
wam.idle();
pm.getSafetyModule()->waitForMode(SafetyModule::IDLE);
//-----------------------------------------------------------------------------
freeaddrinfo(host_info_list);
close(new_sd);
close(socketfd);
//-----------------------------------------------------------------------------
return 0;
}
int wam_main(int argc, char** argv, ProductManager& pm, systems::Wam<DOF>& wam) {
BARRETT_UNITS_TEMPLATE_TYPEDEFS(DOF);
// instantiate Systems
NetworkHaptics nh(pm.getExecutionManager(), remoteHost);
cp_type center;
center << 0.4, -.3, 0.0;
systems::HapticBall ball(center, 0.2);
center << 0.35, 0.4, 0.0;
math::Vector<3>::type size;
size << 0.3, 0.3, 0.3;
systems::HapticBox box(center, size);
systems::Summer<cf_type> dirSum;
systems::Summer<double> depthSum;
systems::PIDController<double, double> comp;
systems::Constant<double> zero(0.0);
systems::TupleGrouper<cf_type, double> tg;
systems::Callback<boost::tuple<cf_type, double>, cf_type> mult(scale);
systems::ToolForceToJointTorques<DOF> tf2jt;
jt_type jtLimits(35.0);
systems::Callback<jt_type> jtSat(boost::bind(saturateJt<DOF>, _1, jtLimits));
// configure Systems
comp.setKp(kp);
comp.setKd(kd);
// connect Systems
connect(wam.toolPosition.output, nh.input);
connect(wam.toolPosition.output, ball.input);
connect(ball.directionOutput, dirSum.getInput(0));
connect(ball.depthOutput, depthSum.getInput(0));
connect(wam.toolPosition.output, box.input);
connect(box.directionOutput, dirSum.getInput(1));
connect(box.depthOutput, depthSum.getInput(1));
connect(wam.kinematicsBase.kinOutput, tf2jt.kinInput);
connect(dirSum.output, tg.getInput<0>());
connect(depthSum.output, comp.referenceInput);
connect(zero.output, comp.feedbackInput);
connect(comp.controlOutput, tg.getInput<1>());
connect(tg.output, mult.input);
connect(mult.output, tf2jt.input);
connect(tf2jt.output, jtSat.input);
// adjust velocity fault limit
pm.getSafetyModule()->setVelocityLimit(1.5);
while (true) {
wam.gravityCompensate();
connect(jtSat.output, wam.input);
// block until the user Shift-idles
pm.getSafetyModule()->waitForMode(SafetyModule::IDLE);
systems::disconnect(wam.input);
wam.gravityCompensate(false);
// block until the user Shift-activates
pm.getSafetyModule()->waitForMode(SafetyModule::ACTIVE);
}
return 0;
}
int wam_main(int argc, char** argv, ProductManager& pm, systems::Wam<DOF>& wam) {
BARRETT_UNITS_TEMPLATE_TYPEDEFS(DOF);
// These vectors are fixed sized, stack allocated, and zero-initialized.
jp_type jp; // jp is a DOFx1 column vector of joint positions
cp_type cp; // cp is a 3x1 vector representing a Cartesian position
wam.gravityCompensate();
printMenu();
Hand* hand = pm.getHand();
std::string line;
bool going = true;
while (going) {
printf(">>> ");
std::getline(std::cin, line);
switch (line[0]) {
case 'p':
moveToStr(wam, &cp, "tool position", line.substr(1));
break;
case 'j':
moveToStr(wam, &jp, "joint positions", line.substr(1));
break;
case 'h':
std::cout << "Moving to home position: "
<< wam.getHomePosition() << std::endl;
wam.moveHome();
break;
case 'i':
printf("WAM idled.\n");
wam.idle();
break;
case 'g':
hand->trapezoidalMove(Hand::jp_type(M_PI/3.0), Hand::SPREAD);
break;
case 'o':
hand->initialize();
// hand->close(Hand::GRASP);
// hand->open(Hand::GRASP);
// hand->close(Hand::SPREAD);
// hand->trapezoidalMove(Hand::jp_type(M_PI/2.0),1, Hand::GRASP);
// hand->trapezoidalMove(Hand::jp_type(M_PI/2.0), Hand::GRASP);
break;
case 'a':
// hand->initialize();
hand->close(Hand::SPREAD);
// hand->close(Hand::GRASP);
break;
case 'c':
// hand->initialize();
hand->close(Hand::GRASP);
break;
// case 'f':
// while (std::getline(file_jp,line_jp))
// {
// moveToStr(wam, &jp, "joint positions", line_jp);
// waitForEnter();
// }
// break;
case 'd':
// hand->initialize();
hand->open(Hand::SPREAD);
break;
case 'e':
// hand->initialize();
hand->open(Hand::GRASP);
break;
case 'b':
hand->trapezoidalMove(Hand::jp_type(M_PI/2.0), Hand::GRASP);
break;
case 't':
hand->trapezoidalMove(Hand::jp_type((M_PI)/6.0), Hand::GRASP);
// std::cout << "sending back a message..." << std::endl;
break;
case 'm':
// temp_str = line.substr(2);
iss2.clear();
iss2.str("");
iss2.str(line.substr(2)) ;
iss2 >> angle_degree;
// std::cin >> angle_degree;
angle_radian = angle_degree*(M_PI/180);
hand->trapezoidalMove(Hand::jp_type(angle_radian), Hand::GRASP);
std::cout << angle_degree << std::endl;
break;
case 'q':
case 'x':
printf("Quitting.\n");
going = false;
break;
default:
if (line.size() != 0) {
printf("Unrecognized option.\n");
printMenu();
}
break;
}
}
wam.idle();
pm.getSafetyModule()->waitForMode(SafetyModule::IDLE);
return 0;
}
int wam_main(int argc, char** argv, ProductManager& pm,
systems::Wam<DOF>& wam) {
BARRETT_UNITS_TEMPLATE_TYPEDEFS(DOF);
Eigen::MatrixXd Coeff;
Eigen::VectorXd Delta;
Eigen::VectorXd Amp;
Eigen::VectorXd Freq;
Eigen::VectorXd StartPos;
Eigen::MatrixXd A;
Eigen::MatrixXd B;
Eigen::VectorXd P;
Eigen::VectorXd Q;
Sam::initEigenMat<double>(A, Sam::readFile<double>("A.txt"));
Sam::initEigenMat<double>(B, Sam::readFile<double>("B.txt"));
Sam::initEigenVec<double>(P, Sam::readFile<double>("P.txt"));
Sam::initEigenVec<double>(Q, Sam::readFile<double>("Q.txt"));
Sam::initEigenMat<double>(Coeff, Sam::readFile<double>("coeff.txt"));
Sam::initEigenVec<double>(Delta, Sam::readFile<double>("delta.txt"));
Sam::initEigenVec<double>(Amp, Sam::readFile<double>("amp.txt"));
Sam::initEigenVec<double>(Freq, Sam::readFile<double>("freq.txt"));
Sam::initEigenVec<double>(StartPos, Sam::readFile<double>("start.txt"));
typedef boost::tuple<double, jp_type, jv_type, jp_type, jv_type, jt_type> tuple_type;
typedef systems::TupleGrouper<double, jp_type, jv_type, jp_type, jv_type,
jt_type> 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;
double amplitude1, omega1;
const Eigen::Matrix4d coeff = Coeff;
const Eigen::Vector4d delta = Delta;
jp_type startpos(0.0);
startpos[0] = StartPos[0];
startpos[1] = StartPos[1];
startpos[2] = StartPos[2];
startpos[3] = StartPos[3];
const Eigen::Vector4d JP_AMPLITUDE = Amp;
const Eigen::Vector4d OMEGA = Freq;
bool status = true;
const Eigen::Matrix4d A1 = A;
const Eigen::Matrix4d B1 = B;
const float P1 = P[0];
const float Q1 = Q[0];
J_ref<DOF> joint_ref(JP_AMPLITUDE, OMEGA, startpos);
SMC_higher_order<DOF> slide(status, coeff, delta , A1, B1,P1, Q1 );
Dynamics<DOF> nilu_dynamics;
wam.gravityCompensate();
printf("Press [Enter] to turn on torque control to go to zero position");
waitForEnter();
wam.moveTo(startpos);
printf("Press [Enter] to turn on torque control to joint 2.");
waitForEnter();
printf("Error 1 \n");
systems::Ramp time(pm.getExecutionManager(), 1.0);
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 2 \n");
systems::connect(tg.output, logger.input);
systems::connect(time.output, joint_ref.timef);
systems::connect(wam.jpOutput, slide.feedbackjpInput);
systems::connect(wam.jvOutput, slide.feedbackjvInput);
systems::connect(wam.jpOutput, nilu_dynamics.jpInputDynamics);
systems::connect(wam.jvOutput, nilu_dynamics.jvInputDynamics);
systems::connect(nilu_dynamics.MassMAtrixOutput, slide.M);
systems::connect(nilu_dynamics.CVectorOutput, slide.C);
systems::connect(joint_ref.referencejpTrack, slide.referencejpInput);
systems::connect(joint_ref.referencejvTrack, slide.referencejvInput);
systems::connect(joint_ref.referencejaTrack, slide.referencejaInput);
wam.trackReferenceSignal(slide.controlOutput);
printf("Error 3 \n");
systems::connect(time.output, tg.template getInput<0>());
systems::connect(joint_ref.referencejpTrack, tg.template getInput<1>());
systems::connect(joint_ref.referencejvTrack, tg.template getInput<2>());
systems::connect(wam.jpOutput, tg.template getInput<3>());
systems::connect(wam.jvOutput, tg.template getInput<4>());
systems::connect(slide.controlOutput, tg.template getInput<5>());
printf("Error 4 \n");
time.smoothStart(TRANSITION_DURATION);
printf("Press [Enter] to stop.");
waitForEnter();
logger.closeLog();
time.smoothStop(TRANSITION_DURATION);
wam.idle();
printf("Error 5 \n");
pm.getSafetyModule()->waitForMode(SafetyModule::IDLE);
log::Reader<boost::tuple<tuple_type> > lr(tmpFile);
lr.exportCSV(argv[1]);
printf("Error 6 \n");
printf("Output written to %s.\n", argv[1]);
std::remove(tmpFile);
return 0;
}
int wam_main(int argc, char** argv, ProductManager& pm,
systems::Wam<DOF>& wam) {
BARRETT_UNITS_TEMPLATE_TYPEDEFS(DOF);
Eigen::MatrixXd Lamda;
Eigen::MatrixXd Coeff;
Eigen::VectorXd Delta;
Eigen::VectorXd Amp;
Eigen::VectorXd Freq;
Eigen::VectorXd StartPos;
Eigen::VectorXd dist_K_tmp;
Eigen::VectorXd state_intg;
Eigen::VectorXd A_tmp;
Sam::initEigenMat<double>(Lamda, Sam::readFile<double>("lamda.txt"));
Sam::initEigenMat<double>(Coeff, Sam::readFile<double>("coeff.txt"));
Sam::initEigenVec<double>(Delta, Sam::readFile<double>("delta.txt"));
Sam::initEigenVec<double>(Amp, Sam::readFile<double>("amp.txt"));
Sam::initEigenVec<double>(Freq, Sam::readFile<double>("freq.txt"));
Sam::initEigenVec<double>(StartPos, Sam::readFile<double>("start.txt"));
Sam::initEigenVec<double>(dist_K_tmp, Sam::readFile<double>("dist_K.txt"));
Sam::initEigenVec<double>(state_intg, Sam::readFile<double>("integrator_state.txt"));
Sam::initEigenVec<double>(A_tmp, Sam::readFile<double>("A.txt"));
std::cout << "Lamda is" << Lamda << "\n" << "Coeff is" << Coeff << "\n"
<< "Delta is" << Delta << "\n" << std::endl;
typedef boost::tuple<double, jp_type, jv_type, jp_type, jv_type, jt_type,
jt_type, double, jp_type> tuple_type;
typedef systems::TupleGrouper<double, jp_type, jv_type, jp_type, jv_type,
jt_type, jt_type, double, jp_type> 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;
// double amplitude1, omega1;
const Eigen::Matrix4d lamda = Lamda;
const Eigen::Matrix4d coeff = Coeff;
const Eigen::Vector4d delta = Delta;
jp_type startpos(0.0);
startpos[0] = StartPos[0];
startpos[1] = StartPos[1];
startpos[2] = StartPos[2];
startpos[3] = StartPos[3];
const Eigen::Vector4d JP_AMPLITUDE = Amp;
const Eigen::Vector4d OMEGA = Freq;
Eigen::Vector4d tmp_velocity;
tmp_velocity[0] = Amp[0]*Freq[0];
tmp_velocity[1] = Amp[1]*Freq[1];
tmp_velocity[2] = Amp[2]*Freq[2];
tmp_velocity[3] = Amp[3]*Freq[3];
bool status = true;
const double dist_K = dist_K_tmp[0];
const int state = state_intg[0];
const Eigen::Vector4d initial_velocity = tmp_velocity;
const float A = A_tmp[0];
J_ref<DOF> joint_ref(JP_AMPLITUDE, OMEGA, startpos);
Slidingmode_Controller<DOF> slide(status, lamda, coeff, delta);
Dynamics<DOF> nilu_dynamics;
disturbance_observer<DOF> nilu_disturbance(dist_K,state, initial_velocity,A);
Dummy<DOF> nilu_dummy;
wam.gravityCompensate();
printf("Press [Enter] to turn on torque control to go to zero position");
waitForEnter();
wam.moveTo(startpos);
printf("Press [Enter] to turn on torque control to joint 2.");
waitForEnter();
printf("Error 1 \n");
systems::Ramp time(pm.getExecutionManager(), 1.0);
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 2 \n");
systems::connect(tg.output, logger.input);
systems::connect(time.output, joint_ref.timef);
systems::connect(wam.jpOutput, slide.feedbackjpInput);
systems::connect(wam.jvOutput, slide.feedbackjvInput);
systems::connect(wam.jpOutput, nilu_dynamics.jpInputDynamics);
systems::connect(wam.jvOutput, nilu_dynamics.jvInputDynamics);
systems::connect(nilu_dynamics.MassMAtrixOutput, slide.M);
systems::connect(nilu_dynamics.CVectorOutput, slide.C);
systems::connect(joint_ref.referencejpTrack, slide.referencejpInput);
systems::connect(joint_ref.referencejvTrack, slide.referencejvInput);
systems::connect(joint_ref.referencejaTrack, slide.referencejaInput);
systems::connect(time.output, nilu_disturbance.time);
systems::connect(nilu_dynamics.MassMAtrixOutput, nilu_disturbance.M);
systems::connect(nilu_dynamics.CVectorOutput, nilu_disturbance.C);
systems::connect(wam.jpOutput, nilu_disturbance.inputactual_pos);
systems::connect(wam.jvOutput, nilu_disturbance.inputactual_vel);
systems::connect(slide.controlOutput, nilu_disturbance.SMC);
systems::connect(slide.controlOutput, nilu_dummy.ControllerInput);
systems::connect(nilu_disturbance.disturbance_torque_etimate,
nilu_dummy.CompensatorInput);
wam.trackReferenceSignal(nilu_dummy.Total_torque);
printf("Error 3 \n");
systems::connect(time.output, tg.template getInput<0>());
systems::connect(joint_ref.referencejpTrack, tg.template getInput<1>());
systems::connect(joint_ref.referencejvTrack, tg.template getInput<2>());
systems::connect(wam.jpOutput, tg.template getInput<3>());
systems::connect(wam.jvOutput, tg.template getInput<4>());
systems::connect(nilu_disturbance.disturbance_torque_etimate,
tg.template getInput<5>());
systems::connect(slide.controlOutput, tg.template getInput<6>());
systems::connect(nilu_disturbance.Test, tg.template getInput<7>());
systems::connect(nilu_disturbance.InverseTest, tg.template getInput<8>());
printf("Error 4 \n");
time.smoothStart(TRANSITION_DURATION);
printf("Press [Enter] to stop.");
waitForEnter();
logger.closeLog();
time.smoothStop(TRANSITION_DURATION);
wam.idle();
printf("Error 5 \n");
pm.getSafetyModule()->waitForMode(SafetyModule::IDLE);
log::Reader<boost::tuple<tuple_type> > lr(tmpFile);
lr.exportCSV(argv[1]);
printf("Error 6 \n");
printf("Output written to %s.\n", argv[1]);
std::remove(tmpFile);
return 0;
}
int wam_main(int argc, char** argv, ProductManager& pm,
systems::Wam& wam_) {
BARRETT_UNITS_TEMPLATE_TYPEDEFS(DIMENSION);
systems::Wam<DIMENSION>* wam = (systems::Wam<DIMENSION>*)(&wam_);
Robot robot(wam, pm, filename, config.getRoot());
if (!robot.init())
return 1;
robot.loop = true;
curState = PLAYING;
float sleep_s = 0.002;
//while (roboting) {
while(pm.getSafetyModule()->getMode() == SafetyModule::ACTIVE){
if(robot.problem){
//cout << "contact!" << endl;
//cout << "hd: " << robot.hand_debug.str() << endl;
cout << "uh-oh" << endl;
robot.hand_debug.str("");
robot.problem = false;
}
switch (curState) {
case QUIT:
//roboting = false;
break;
case PLAYING:
switch (lastState) {
case STOPPED:
robot.moveToStart();
robot.init_data_logger();
robot.reconnectSystems();
robot.startplayback();
lastState = PLAYING;
break;
case PAUSED:
robot.startplayback();
lastState = PLAYING;
break;
case PLAYING:
if (robot.playbackActive()) {
btsleep(sleep_s);
break;
} else if (robot.loop) {
loop_count++;
robot.disconnectSystems();
lastState = STOPPED;
curState = PLAYING;
break;
} else {
curState = STOPPED;
break;
}
default: break;
}
break;
case PAUSED:
switch (lastState) {
case PLAYING:
robot.pauseplayback();
lastState = PAUSED;
break;
case PAUSED:
btsleep(sleep_s);
break;
case STOPPED:
break;
default: break;
}
break;
case STOPPED:
switch (lastState) {
case PLAYING:
robot.disconnectSystems();
lastState = STOPPED;
break;
case PAUSED:
robot.disconnectSystems();
lastState = STOPPED;
break;
case STOPPED:
btsleep(sleep_s);
break;
default: break;
}
break;
}
/*if(!collecting_data){
robot.collect_data_stream();
collecting_data = true;
}*/
}
robot.disconnectSystems();
robot.output_data_stream();
//wam->moveHome();
//printf("\n\n");
//pm.getSafetyModule()->waitForMode(SafetyModule::IDLE);
return 0;
}
void AutoTension<DOF>::init(ProductManager& pm, std::vector<int> args) {
wam.gravityCompensate(true); // Turning on Gravity Compenstation
// Make sure all tangs are released correctly
for (size_t m = 0; m < args.size(); m++) {
if (args[m] == 6) // Motor 6 tensions using the single tang for 5 & 6
puck[4]->setProperty(Puck::TENSION, false);
else
puck[args[m] - 1]->setProperty(Puck::TENSION, false);
}
wam.moveHome(); // Move the WAM to home position
// Tell our EM to start managing
pm.getExecutionManager()->startManaging(motorRamp); //starting ramp manager
pm.getExecutionManager()->startManaging(watchdog);
pm.getExecutionManager()->startManaging(exposedGravity);
// Some DOF dependent items
jpInitial.resize(DOF);
jpStart.resize(DOF);
jpSlack1.resize(DOF);
jpSlack2.resize(DOF);
// Initialize hand if present
if (pm.foundHand()) {
jp_type handBufJT = wam.getJointPositions();
if (DOF == 4)
handBufJT[3] -= 0.35; // Lift the elbow to give room for BHand HI
else
handBufJT[5] = jpStopLow[5]; // Set J6 on its positive stop
wam.moveTo(handBufJT);
hand = pm.getHand();
hand->initialize();
hand->close(Hand::GRASP);
}
// Joint 1 Tensioning will be added in and run simultaneously if present.
jpInitial[0] = wam.getJointPositions();
// Joint 2
jpInitial[1] = wam.getJointPositions();
jpStart[1] = jpInitial[1];
jpStart[1][1] = jpStopHigh[1] - tangBuffer[1];
jpStart[1][2] = jpStopLow[2] + tangBuffer[2];
jpStart[1][3] = jpStopHigh[3] - stopBuffer[3];
jpSlack1[1] = jpStart[1];
jpSlack1[1][1] = jpStopHigh[1] - stopBuffer[1]; // This is so we dont drive into and wear the joint stops
jpSlack1[1][2] = jpStopLow[2] + stopBuffer[2];
jpSlack2[1] = jpSlack1[1];
jpSlack2[1][1] = jpStopLow[1] + stopBuffer[1];
jpSlack2[1][2] = jpStopHigh[2] - stopBuffer[2];
jpSlack2[1][3] = jpStopLow[3] + stopBuffer[3];
// Joint 3
jpInitial[2] = wam.getJointPositions();
jpInitial[2][1] = 0.0;
jpStart[2] = jpInitial[2];
jpStart[2][1] = jpStopLow[1] + tangBuffer[1];
jpStart[2][2] = jpStopLow[2] + tangBuffer[2];
jpStart[2][3] = jpStopLow[3] + stopBuffer[3];
jpSlack1[2] = jpStart[2];
jpSlack1[2][1] = jpStopLow[1] + stopBuffer[1];
jpSlack1[2][2] = jpStopLow[2] + stopBuffer[1];
jpSlack2[2] = jpSlack1[2];
jpSlack2[2][1] = jpStopHigh[1] - stopBuffer[1];
jpSlack2[2][2] = jpStopHigh[2] - stopBuffer[2];
jpSlack2[2][3] = jpStopHigh[3] - stopBuffer[3];
// Joint 4
jpInitial[3] = wam.getJointPositions();
jpStart[3] = jpInitial[3];
jpStart[3][1] = 0.0;
jpStart[3][3] = jpStopLow[3] + tangBuffer[3];
jpSlack1[3] = jpStart[3];
jpSlack1[3][3] = jpStopLow[3] + stopBuffer[3];
jpSlack2[3] = jpSlack1[3];
jpSlack2[3][3] = jpStopHigh[3] - stopBuffer[3];
if (DOF == 7) {
// Joint 5
jpInitial[4] = wam.getJointPositions();
jpStart[4] = jpInitial[4];
jpStart[4][3] = M_PI / 2.0;
jpStart[4][4] = jpStopHigh[4] - tangBuffer[4];
jpStart[4][5] = jpStopLow[5] + tangBuffer[5];
jpSlack1[4] = jpStart[4];
jpSlack1[4][4] = jpStopHigh[4] - stopBuffer[4];
jpSlack1[4][5] = jpStopLow[5] + stopBuffer[5];
jpSlack2[4] = jpSlack1[4];
jpSlack2[4][4] = jpStopLow[4] + stopBuffer[4];
jpSlack2[4][5] = jpStopHigh[5] - stopBuffer[5];
// Joint 6
jpInitial[5] = wam.getJointPositions();
jpStart[5] = jpInitial[5];
jpStart[5][3] = M_PI / 2.0;
jpStart[5][4] = jpStopHigh[4] - tangBuffer[4];
jpStart[5][5] = jpStopHigh[5] - tangBuffer[5];
jpSlack1[5] = jpStart[5];
jpSlack1[5][4] = jpStopHigh[4] - stopBuffer[4];
jpSlack1[5][5] = jpStopHigh[5] - stopBuffer[5];
jpSlack2[5] = jpSlack1[5];
jpSlack2[5][4] = jpStopLow[4] + stopBuffer[4];
jpSlack2[5][5] = jpStopLow[5] + stopBuffer[5];
}
// Connect our systems and tell the supervisory control to control jpController in motor space.
connectSystems();
}
int wam_main(int argc, char** argv, ProductManager& pm,
systems::Wam<DOF>& wam) {
BARRETT_UNITS_TEMPLATE_TYPEDEFS(DOF);
//
typedef boost::tuple<double, jp_type, jv_type, ja_type, jt_type, jt_type> tuple_type;
typedef systems::TupleGrouper<double, jp_type, jv_type, ja_type, jt_type,
jt_type> tg_type;
tg_type tg;
char tmpFile[] = "btXXXXXX";
if (mkstemp(tmpFile) == -1) {
printf("ERROR: Couldn't create temporary file!\n");
return 1;
}
double Frequency;
double Amplitude;
double omega1;
double offset;
if (argc == 2) {
Frequency = 0.1;
Amplitude = 0.525;
offset = 0;
}
if (argc == 3) {
Amplitude = 0.525;
offset = 0;
}
if (argc == 4) {
offset = 0;
}
else {
Frequency = boost::lexical_cast<double>(argv[2]);
Amplitude = boost::lexical_cast<double>(argv[3]);
offset = boost::lexical_cast<double>(argv[4]);
}
const double JT_AMPLITUDE = Amplitude;
const double FREQUENCY = Frequency;
const double OFFSET = offset;
omega1 = 120;
wam.gravityCompensate();
const double TRANSITION_DURATION = 0.5; // seconds
jp_type startpos(0.0);
startpos[3] = +3.14;
startpos[1] = offset;
systems::Ramp time(pm.getExecutionManager(), 1.0);
printf("Press [Enter] to turn on torque control to go to zero position");
wam.moveTo(startpos);
printf("Press [Enter] to turn on torque control to joint 2.");
waitForEnter();
// Joint acc calculator
systems::FirstOrderFilter<jv_type> filter;
wam.jvFilter.setLowPass(jv_type(omega1));
filter.setHighPass(jp_type(omega1), jp_type(omega1));
pm.getExecutionManager()->startManaging(filter);
systems::Gain<jv_type, double, ja_type> changeUnits1(1.0);
systems::connect(wam.jvOutput, filter.input);
systems::connect(filter.output, changeUnits1.input);
//
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);
//
J2control<DOF> jtc(startpos, JT_AMPLITUDE, FREQUENCY, OFFSET);
systems::connect(tg.output, logger.input);
systems::connect(time.output, jtc.input);
systems::connect(time.output, tg.template getInput<0>());
systems::connect(wam.jpOutput, tg.template getInput<1>());
systems::connect(wam.jvOutput, tg.template getInput<2>());
systems::connect(changeUnits1.output, tg.template getInput<3>());
systems::connect(wam.supervisoryController.output,
tg.template getInput<4>());
systems::connect(wam.gravity.output, tg.template getInput<5>());
wam.trackReferenceSignal(jtc.output);
time.smoothStart(TRANSITION_DURATION);
printf("Press [Enter] to stop.");
waitForEnter();
logger.closeLog();
time.smoothStop(TRANSITION_DURATION);
wam.idle();
// Wait for the user to press Shift-idle
pm.getSafetyModule()->waitForMode(SafetyModule::IDLE);
log::Reader<boost::tuple<tuple_type> > lr(tmpFile);
lr.exportCSV(argv[1]);
printf("Output written to %s.\n", argv[1]);
std::remove(tmpFile);
return 0;
}
int wam_main(int argc, char** argv, ProductManager& pm, systems::Wam<DOF>& wam) {
BARRETT_UNITS_TEMPLATE_TYPEDEFS(DOF);
typedef Hand::jp_type hjp_t;
const char* path = pm.getWamDefaultConfigPath();
std::cout<<"got configuration "<<std::endl;
size_t i=0;
while(path[i]!='\0'){
std::cout<<path[i];
i++;
}
jp_type zero(0.0);
double O = 0.0;
double C = 2.4;
double SC = M_PI;
hjp_t open(O);
hjp_t closed(C);
closed[3] = SC;
jp_type PickPos, placePos;
PickPos<<-0.002, 1.200, 0.002, 1.536, -0.054, -1.262, -1.517;
placePos<<0.545, 1.199, 0.532, 1.481, -0.478, -0.917, -1.879;
double OR = -0.75;
double CR = 0.75;
Hand::jv_type opening(OR);
Hand::jv_type closing(CR);
wam.gravityCompensate();
wam.moveTo(zero);
if ( !pm.foundHand() ) {
printf("ERROR: No Hand found on bus!\n");
return 1;
}
Hand& hand = *pm.getHand();
std::cout<<"[Enter] to initialize Hand"<<std::endl;
detail::waitForEnter();
hand.initialize();
std::cout<<"operate on Hand"<<std::endl;
detail::waitForEnter();
{
assertPosition(hand, open);
std::cout<<"[Enter] to close Hand"<<std::endl;
detail::waitForEnter();
hand.close();
assertPosition(hand, closed);
std::cout<<"[Enter] to open Hand"<<std::endl;
detail::waitForEnter();
hand.open();
assertPosition(hand, open);
std::cout<<"[Enter] to close spread Hand"<<std::endl;
detail::waitForEnter();
hand.close(Hand::SPREAD);
assertPosition(hand, hjp_t(O,O,O,SC));
std::cout<<"[Enter] to close grasp Hand"<<std::endl;
detail::waitForEnter();
hand.close(Hand::GRASP);
assertPosition(hand, closed);
std::cout<<"[Enter] to open spread Hand"<<std::endl;
detail::waitForEnter();
hand.open(Hand::GRASP, false);
btsleep(0.5);
assertPosition(hand, hjp_t(1.6,1.6,1.6,SC));
std::cout<<"[Enter] to open Hand"<<std::endl;
detail::waitForEnter();
hand.open();
assertPosition(hand, open);
wam.moveTo(PickPos );
btsleep(0.5);
assertPosition(hand, open);
std::cout<<"[Enter] to close Hand"<<std::endl;
detail::waitForEnter();
hand.close();
}
std::cout<<"[Enter] to move home"<<std::endl;
detail::waitForEnter();
wam.moveHome();
std::cout<<"configuration should be printed"<<std::endl;
pm.getSafetyModule()->waitForMode(SafetyModule::IDLE);
return 0;
}
int wam_main(int argc, char** argv, ProductManager& pm, systems::Wam<DOF>& wam) {
BARRETT_UNITS_TEMPLATE_TYPEDEFS(DOF);
wam.gravityCompensate();
bool gComp = true;
pm.getSafetyModule()->setVelocityLimit(0.3);
jp_type jp;
size_t joint = 0;
systems::ExposedOutput<jp_type> setPoint;
std::string line;
bool going = true;
while (going) {
printf(">>> ");
std::getline(std::cin, line);
if (line.size() == 0) {
wam.moveTo(jp);
libconfig::Config config;
config.readFile("tuning.conf");
systems::PIDController<jp_type, jt_type> jpc(config.lookup(pm.getWamDefaultConfigPath())["joint_position_control"]);
systems::connect(setPoint.output, jpc.referenceInput);
systems::connect(wam.jpOutput, jpc.feedbackInput);
wam.idle();
usleep(100000);
systems::connect(jpc.controlOutput, wam.input);
printf("Controller updated.\n");
printf("Press enter for step.\n");
std::getline(std::cin, line);
jp[joint] += 0.02;
setPoint.setValue(jp);
std::getline(std::cin, line);
systems::disconnect(wam.input);
wam.moveTo(jp);
printf("Original controller now in use.\n");
continue;
}
switch (line[0]) {
case 'h':
printf("Set point updated.\n");
jp = wam.getJointPositions();
wam.moveTo(jp);
setPoint.setValue(jp);
break;
case 'i':
printf("WAM idled.\n");
wam.idle();
break;
case 'j':
joint = (joint + 1) % DOF;
printf("Step joint %zu.\n", joint+1);
break;
case 'g':
gComp = ! gComp;
wam.gravityCompensate(gComp);
break;
case 'q':
case 'x':
printf("Quitting.\n");
going = false;
break;
default:
printf("Unrecognized option.\n");
break;
}
}
wam.idle();
pm.getSafetyModule()->waitForMode(SafetyModule::IDLE);
return 0;
}
int wam_main(int argc, char** argv, ProductManager& pm, systems::Wam<DOF>& wam){
BARRETT_UNITS_TEMPLATE_TYPEDEFS(DOF);
double dW,dL,bF;
if(argc == 3){
dL = atof(argv[2]);
dL = dL*0.0254;
dW = atof(argv[1]);
}
else if(argc == 2){
dW = atof(argv[1]);
dL = 0.762;
}else{
dW = 30.0;
dL = 0.762;
}
bF = (dW / dL)*1.35581795;
// Lets prevent the torque fault by limiting acceleration?
//pm.safetyModule->setVelocityLimit(2.0);
// Lets instantiate the system calls
systems::ExposedOutput<cp_type> homePos;
systems::Summer<cp_type> posErr("+-");
systems::Gain<cp_type, double, cf_type> gain(bF);
systems::ToolForceToJointTorques<DOF> tf2jt;
connect(homePos.output, posErr.getInput(0));
connect(wam.toolPosition.output, posErr.getInput(1));
connect(posErr.output, gain.input);
connect(wam.kinematicsBase.kinOutput, tf2jt.kinInput);
connect(gain.output, tf2jt.input);
homePos.setValueUndefined();
connect(tf2jt.output, wam.input);
wam.gravityCompensate();
/*
std::string line;
bool active = true,set=false;
while(active){
std::getline(std::cin, line);
switch (line[0]) {
case 'x':
case 'q':
active = false;
break;
default:
if (line.size() != 0) {
if(set){
homePos.setValue(wam.getToolPosition());
}else{
homePos.setValueUndefined();
}
set = !set;
}
break;
}
}*/
while(pm.getSafetyModule()->getMode() == SafetyModule::ACTIVE){
waitForEnter();
homePos.setValue(wam.getToolPosition());
printf(">>> Bow Position Locked.");
waitForEnter();
homePos.setValueUndefined();
printf(">>> Bow Position Unlocked.");
}
wam.idle();
// wam.moveHome();
pm.getSafetyModule()->waitForMode(SafetyModule::IDLE);
return 0;
}
int wam_main(int argc, char** argv, ProductManager& pm, systems::Wam<DOF>& wam) {
wam.gravityCompensate();
printMenu();
std::string line;
bool going = true;
while (going) {
printf(">>> ");
std::getline(std::cin, line);
switch (line[0]) {
case 'j':
printf("Holding joint positions.\n");
wam.moveTo(wam.getJointPositions());
break;
case 'p':
printf("Holding tool position.\n");
wam.moveTo(wam.getToolPosition());
break;
case 'o':
printf("Holding tool orientation.\n");
wam.moveTo(wam.getToolOrientation());
break;
case 'b':
printf("Holding both tool position and orientation.\n");
wam.moveTo(wam.getToolPose());
break;
case 'i':
printf("WAM idled.\n");
// Note that this use of the word "idle" does not mean "Shift-idle".
// Calling Wam::idle() will disable any of the controllers that may
// be connected (joint position, tool position, tool orientation,
// etc.) leaving only gravity compensation. (More specifically,
// Wam::idle() disconnects any inputs that were connected using
// Wam::trackReferenceSignal().)
wam.idle();
break;
case 'q':
case 'x':
printf("Quitting.\n");
wam.moveHome();
going = false;
break;
default:
if (line.size() != 0) {
printf("Unrecognized option.\n");
printMenu();
}
break;
}
}
// Release the WAM if we're holding. This is convenient because it allows
// users to move the WAM back to some collapsed position before exiting, if
// they want.
wam.idle();
// Wait for the user to press Shift-idle
pm.getSafetyModule()->waitForMode(SafetyModule::IDLE);
return 0;
}