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