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