void Teach<DOF>::createSpline() {
saveName = "recorded/" + saveName;
if (recordType == 0) {
jpLogger->closeLog();
disconnect(jpLogger->input);
// Build spline between recorded points
log::Reader<jp_sample_type> lr(tmpFile);
lr.exportCSV(saveName.c_str());
}
else{
poseLogger->closeLog();
disconnect(poseLogger->input);
log::Reader<pose_sample_type> pr(tmpFile);
pr.exportCSV(saveName.c_str());
}
// Adding our datatype as the first line of the recorded trajectory
fileOut = saveName + ".csv";
std::ifstream in(saveName.c_str());
std::ofstream out(fileOut.c_str());
if (recordType == 0)
out << "jp_type\n";
else
out << "pose_type\n";
out << in.rdbuf();
out.close();
in.close();
remove(saveName.c_str());
printf("Trajectory saved to the location: %s \n\n ", fileOut.c_str());
}
void Teach<DOF>::record() {
BARRETT_SCOPED_LOCK(pm.getExecutionManager()->getMutex());
if (recordType == 0) {
connect(time.output, jpLogTg.template getInput<0>());
// connect(wam.jpOutput, jpLogTg.template getInput<1>());
connect(jpLogTg.output, jpLogger->input);
} else {
// connect(time.output, poseLogTg.template getInput<0>());
// connect(wam.toolPose.output, poseLogTg.template getInput<1>());
// connect(poseLogTg.output, poseLogger->input);
}
time.start();
}
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) {
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 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;
}