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);
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);
//
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);
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();
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) {
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;
}
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);
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);
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);
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);
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;
}
int wam_main(int argc, char** argv, ProductManager& pm, systems::Wam<DOF>& wam) {
BARRETT_UNITS_TEMPLATE_TYPEDEFS(DOF);
jp_type jp;
cp_type cp;
ControlModeSwitcher<DOF> cms(pm, wam, 1.17, 0.82);
wam.gravityCompensate();
printMenu();
std::string line;
bool going = true;
while (going) {
printf(">>> ");
std::getline(std::cin, line);
switch (line[0]) {
case 'j':
moveToStr(wam, &jp, "joint positions", line.substr(1));
break;
case 'p':
moveToStr(wam, &cp, "tool position", 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 'v':
printf("Switching to voltage control!\n");
cms.voltageControl();
break;
case 'c':
printf("Switching to current control!\n");
cms.currentControl();
break;
case 'g':
printf("Calculating torque gain...\n");
cms.calculateTorqueGain();
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;
}