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