bool ApcCortex::testingGrasping()
{
geometry_msgs::PoseStamped pose;
ROS_INFO("Moving arms to home position");
//Set arm to home positions
arms->loadCartPose(ArmsCartesian::LEFT, ArmsCartesian::homeLeft,pose);
arms->loadCartPose(ArmsCartesian::RIGHT, ArmsCartesian::homeRight,pose);
arms->waitForMotion(5.0);
ApcCortex::GraspStrategy currentGraspingStrategy = FRONT;
ApcCortex::WhichArm graspingArm = RIGHT;
apc_msgs::GetBinInfo currentBinInfo;
geometry_msgs::PoseStamped currentGripperPose;
geometry_msgs::Pose currentObjPose;
int bin = 5;
double x = 0.30;
for(int i=4;i<13;i++)
{
graspingArm = determineArmFromBin(i);
moveToBinPlane(i, graspingArm, x);
// Create a fake location for testing
// Get bin info
getBinInfo(i, currentBinInfo);
currentBinInfo.response.center.x = x+0.05; // fake bin location
currentGripperPose.pose.position.x = currentBinInfo.response.center.x;
currentGripperPose.pose.position.y = currentBinInfo.response.center.y;
currentGripperPose.pose.position.z = currentBinInfo.response.center.z;
currentGripperPose.pose.orientation.w = 1;
currentGripperPose.header.frame_id="base_link";
currentObjPose.position.x = currentBinInfo.response.center.x +0.35;
currentObjPose.position.y = currentBinInfo.response.center.y;
currentObjPose.position.z = currentBinInfo.response.center.z;
currentObjPose.orientation.w = 1;
std::cout<<currentObjPose;
grasp(currentGraspingStrategy, graspingArm, currentGripperPose, currentObjPose, currentBinInfo);
//Set arm to home positions
if(graspingArm==LEFT)
arms->loadCartPose(ArmsCartesian::LEFT, ArmsCartesian::homeLeft,pose);
else
arms->loadCartPose(ArmsCartesian::RIGHT, ArmsCartesian::homeRight,pose);
while( !arms->motionComplete() )
{
std::cout<<"Waiting for arm to move home...";
ros::Duration(1.0).sleep();
}
}
return true;
}
bool graspSrv(wsg_50_common::Move::Request &req, wsg_50_common::Move::Response &res)
{
if ( (req.width >= 0.0 && req.width <= 110.0) && (req.speed > 0.0 && req.speed <= 420.0) ){
ROS_INFO("Grasping object at %f with %f mm/s.", req.width, req.speed);
res.error = grasp(req.width, req.speed);
}else if (req.width < 0.0 || req.width > 110.0){
ROS_ERROR("Imposible to move to this position. (Width values: [0.0 - 110.0] ");
res.error = 255;
return false;
}else{
ROS_WARN("Speed or position values are outside the gripper's physical limits (Position: [0.0 - 110.0] / Speed: [0.1 - 420.0]) Using clamped values.");
res.error = grasp(req.width, req.speed);
}
ROS_INFO("Object grasped correctly.");
objectGraspped=true;
return true;
}
GraspPtr ObjectIO::processGrasp(rapidxml::xml_node<char>* graspXMLNode, const std::string& robotType, const std::string& eef, const std::string& objName)
{
THROW_VR_EXCEPTION_IF(!graspXMLNode, "No <Grasp> tag ?!");
// get name
std::string name = processNameAttribute(graspXMLNode, true);
std::string method = processStringAttribute("creation", graspXMLNode, true);
float quality = processFloatAttribute(std::string("quality"), graspXMLNode, true);
std::string preshapeName = processStringAttribute("preshape", graspXMLNode, true);
Eigen::Matrix4f pose;
pose.setIdentity();
std::vector< RobotConfig::Configuration > configDefinitions;
std::string configName;
rapidxml::xml_node<>* node = graspXMLNode->first_node();
while (node)
{
std::string nodeName = getLowerCase(node->name());
if (nodeName == "transform")
{
processTransformNode(graspXMLNode, name, pose);
}
else if (nodeName == "configuration")
{
THROW_VR_EXCEPTION_IF(configDefinitions.size() > 0, "Only one configuration per grasp allowed");
bool c*K = processConfigurationNode(node, configDefinitions, configName);
THROW_VR_EXCEPTION_IF(!c*K, "Invalid configuration defined in grasp tag '" << name << "'." << endl);
}
else
{
THROW_VR_EXCEPTION("XML definition <" << nodeName << "> not supported in Grasp <" << name << ">." << endl);
}
node = node->next_sibling();
}
GraspPtr grasp(new Grasp(name, robotType, eef, pose, method, quality, preshapeName));
if (configDefinitions.size() > 0)
{
// create & register configs
std::map< std::string, float > rc;
for (size_t i = 0; i < configDefinitions.size(); i++)
{
rc[ configDefinitions[i].name ] = configDefinitions[i].value;
}
grasp->setConfiguration(rc);
}
return grasp;
}
bool ApcCortex::testingGripperTilt()
{
/**
* Need:
* 1. Strategy (DONE)
* 2. Arm (DONE)
* 3. GripperPose
* 4. Object Pose
* 5. BinInfo (DONE)
*/
int bin = 9;
double x = 0.30;
ApcCortex::GraspStrategy currentGraspingStrategy = ANGLEDLIFT;
ApcCortex::WhichArm graspingArm = RIGHT;
apc_msgs::GetBinInfo currentBinInfo;
geometry_msgs::PoseStamped currentGripperPose;
geometry_msgs::Pose currentObjPose;
graspingArm = determineArmFromBin(bin);
getBinInfo(bin, currentBinInfo);
currentGripperPose.pose.position.x = currentBinInfo.response.center.x;
currentGripperPose.pose.position.y = currentBinInfo.response.center.y;
currentGripperPose.pose.position.z = currentBinInfo.response.center.z;
currentGripperPose.pose.orientation.w = 1;
currentGripperPose.header.frame_id="base_link";
currentObjPose.position.x = currentBinInfo.response.center.x +0.35;
currentObjPose.position.y = currentBinInfo.response.center.y;
currentObjPose.position.z = currentBinInfo.response.center.z;
currentObjPose.orientation.w = 1;
base->move("center");
while(!base->motionComplete())
{
}
base->move("col2");
base->move("center");
while(!base->motionComplete())
{
}
grasp( currentGraspingStrategy,
graspingArm,
currentGripperPose,
currentObjPose,
currentBinInfo);
}
bool ApcCortex::runSM(ApcCortex::RunState start, ApcCortex::RunState stop)
{
executingRun=true;
currentRunState = start;
while(executingRun)
{
switch(currentRunState)
{
//-------------------------------------------------------------------------------//
case START:
ROS_INFO("*SM* -> START");
receivedJSON = false;
torso->sendGoal(0.295);
grippers->close(Gripper::BOTH);
lookAtBin(5,ApcCortex::CENTER);
stateVariables.currentToolRight = ELECTRIC;
stateVariables.currentToolLeft = ELECTRIC;
stateVariables.currentItemNumber = 0;
//Load Gains
arms->setGains("initial",ArmsCartesian::LEFT);
arms->setGains("initial",ArmsCartesian::RIGHT);
// Load Posture
arms->loadPosture(ArmsCartesian::RIGHT,ArmsCartesian::elbowupr);
arms->loadPosture(ArmsCartesian::LEFT,ArmsCartesian::elbowupl);
//Set arm to home positions
arms->loadCartPose(ArmsCartesian::LEFT, ArmsCartesian::homeLeft, stateVariables.currentGripperPoseLeft);
arms->loadCartPose(ArmsCartesian::RIGHT, ArmsCartesian::homeRight, stateVariables.currentGripperPoseRight);
nextRunState = WAIT_FOR_JSON;
currentRunState = WAIT_FOR_MOTION;
break;
//-------------------------------------------------------------------------------//
case WAIT_FOR_MOTION: // FIXME? this is a blocking state
ROS_INFO("*SM* -> WAIT_FOR_MOTION");
motionsComplete = false;
time_now = ros::Time::now();
time_stop = ros::Time::now()+ros::Duration(5.0);
while (time_now < time_stop && !motionsComplete)
{
if(grippers->motionComplete()){
//ROS_INFO_STREAM("Grippers->motionComplete(): True");
}else{
ROS_INFO_STREAM("Grippers->motionComplete(): False");
}
if(torso->motionComplete()){
//ROS_INFO_STREAM("torso->motionComplete(): True");
}else{
ROS_INFO_STREAM("torso->motionComplete(): False");
}
if(head->motionComplete()){
//ROS_INFO_STREAM("head->motionComplete(): True");
}else{
ROS_INFO_STREAM("head->motionComplete(): False");
}
if(arms->motionComplete()){
//ROS_INFO_STREAM("arms->motionComplete(): True");
}else{
ROS_INFO_STREAM("arms->motionComplete(): False");
}
motionsComplete = grippers->motionComplete()
&& torso->motionComplete()
&& head->motionComplete()
&& arms->motionComplete();
ros::Duration(0.2).sleep();
time_now = ros::Time::now();
}
if(!motionsComplete)
{
ROS_INFO("Time out - did not complete motions!"); // TODO add troubleshooting
grippers->cancelMotion();
torso->cancelMotion();
head->cancelMotion();
}
else
{
ROS_INFO("runSM: Motions completed!");
}
currentRunState = nextRunState;
break;
//-------------------------------------------------------------------------------//
case WAIT_FOR_JSON:
ROS_INFO("*SM* -> WAIT_FOR_JSON");
srv_msg_GetBool.response.variable = false;
while (!srv_msg_GetBool.response.variable)
{
srvClient_get_status.call(srv_msg_GetBool);
ros::Duration(0.2).sleep();
}
currentRunState = SORT_WORK_ORDER;
break;
//-------------------------------------------------------------------------------//
case SORT_WORK_ORDER:
ROS_INFO("*SM* -> SORT_WORK_ORDER");
if( !srvClient_get_work_order.call(srv_msg_GetWorkOrder) )
{
ROS_ERROR("Failed to get workOrder");
currentRunState=WAIT_FOR_JSON;
}
else
{
receivedJSON = true;
}
// Electric
// a) col 1,2 -> left
// b) col 3 -> right
// Vacuum
// a) both arms
// b) single arms
if(! sortWorkOrder(srv_msg_GetWorkOrder.response.work_order ) )
ROS_ERROR("Could not sort work order!");
orderIter = work_order.vacuumLift.begin();
currentRunState = START_PICKING;
break;
//-------------------------------------------------------------------------------//
//-------------------------------------------------------------------------------//
case START_PICKING:
ROS_INFO("*SM* -> START_PICKING");
// Decide what object to pick
if(items_left.vacuumLift>0)
{
orderIter++;
items_left.vacuumLift--;
}
else if(items_left.electricFront>0)
{
orderIter++;
items_left.electricFront--;
}
else if(items_left.vacuumFront>0)
{
orderIter++;
items_left.vacuumFront--;
}
else if(items_left.electricAngledLift>0)
{
orderIter++;
items_left.electricAngledLift--;
}
stateVariables.itemsLeftToPick = items_left.vacuumLift;
stateVariables.currentObject = (*orderIter).name;
stateVariables.currentBin = (*orderIter).bin;
stateVariables.requiredArm = (*orderIter).requiredArm;
stateVariables.requiredArmCartesian = (*orderIter).requiredArmCartesian;
stateVariables.requiredArmGripper = (*orderIter).requiredArmGripper;
stateVariables.requiredStrategy = (*orderIter).requiredStrategy;
stateVariables.requiredTool = (*orderIter).requiredTool;
// srv_msg_GetItemFromWorkOrder.request.item_num = stateVariables.currentItemNumber;
// if(!srvClient_work_order_request.call(srv_msg_GetItemFromWorkOrder) )
// ROS_ERROR("Failed to get item from work order %d", stateVariables.currentItemNumber);
//
// stateVariables.itemsLeftToPick = srv_msg_GetItemFromWorkOrder.response.items_left;
// stateVariables.currentObject = srv_msg_GetItemFromWorkOrder.response.item;
// stateVariables.currentBin = srv_msg_GetItemFromWorkOrder.response.bin;
// stateVariables.totalNumberOfItems = srv_msg_GetItemFromWorkOrder.response.item_number;
//
// stateVariables.currentToolRight = ELECTRIC;
std::cout<<"Items left to pick: "<<stateVariables.itemsLeftToPick<<"/"<<stateVariables.totalNumberOfItems<<"\n";
std::cout<<"Object to pick: "<<stateVariables.currentObject<<"\n";
std::cout<<"Bin number: "<<stateVariables.currentBin <<"\n";
// Get bin info
getBinInfo(stateVariables.currentBin, stateVariables.currentBinInfo);
// Point Head
head->sendGoalCart( robot_frame,
stateVariables.currentBinInfo.response.center.x,
stateVariables.currentBinInfo.response.center.y,
stateVariables.currentBinInfo.response.center.z,
1.0);
//nextRunState = GET_OBJECT_LOCATION; FIXME since we don't care about orienation, we don't have to detect the item right now
//currentRunState = WAIT_FOR_MOTION;
currentRunState = DETERMINE_STRATEGY;
break;
//-------------------------------------------------------------------------------//
case GET_OBJECT_LOCATION:
ROS_INFO("*SM* -> GET_OBJECT_LOCATION");
// Start ORK
srv_msg_SetBin.request.number = stateVariables.currentBin;
if(! srvClient_startORK.call(srv_msg_SetBin) )
ROS_ERROR("Could not start ORK");
// Get Object pose
srv_msg_GetObjectPose.request.obj_name = stateVariables.currentObject;
srv_msg_GetObjectPose.response.found_obj = false;
while(!srv_msg_GetObjectPose.response.found_obj)
{
if(! srvClient_get_object_pose.call(srv_msg_GetObjectPose) )
ROS_ERROR("Could not stop ORK");
ros::Duration(0.5).sleep();
}
// Stop ORK
if(! srvClient_stopORK.call(srv_msg_Empty) )
ROS_ERROR("Could not stop ORK");
// TODO: convert pose to string with orientation name
currentRunState = DETERMINE_STRATEGY;
break;
//-------------------------------------------------------------------------------//
case DETERMINE_STRATEGY:
ROS_INFO("*SM* -> DETERMINE_STRATEGY");
// Set arm, strategy, and tool
updateGraspingStrategy( stateVariables.currentObject,
stateVariables.currentBin,
"all"); // TODO update
// Check if we need to switch
switchLeftTool = false;
switchRightTool = false;
if(stateVariables.requiredArm == LEFT)
{
stateVariables.requiredToolLeft = stateVariables.requiredTool;
std::cout<<"Required tool for left: "<<stateVariables.requiredToolLeft<<"\n";
std::cout<<"Current tool for left: "<<stateVariables.currentToolLeft<<"\n";
if(stateVariables.requiredToolLeft != stateVariables.currentToolLeft)
{
switchLeftTool = true;
currentRunState = MOVE_TO_TOOL;
}
}
if(stateVariables.requiredArm == RIGHT)
{
stateVariables.requiredToolRight = stateVariables.requiredTool;
std::cout<<"Required tool for left: "<<stateVariables.requiredToolRight<<"\n";
std::cout<<"Current tool for left: "<<stateVariables.currentToolRight<<"\n";
if(stateVariables.requiredToolRight != stateVariables.currentToolRight)
{
switchRightTool = true;
currentRunState = MOVE_TO_TOOL;
}
}
if(!switchLeftTool && !switchRightTool)
currentRunState = MOVE_TO_CENTER;
break;
//-------------------------------------------------------------------------------//
case MOVE_TO_TOOL:
ROS_INFO("*SM* -> MOVE_TO_TOOL");
base->move("tool");
//base->motionComplete(); TODO implement, for now just wait
ros::Duration(3.0).sleep();
currentRunState = PICK_TOOL;
break;
//-------------------------------------------------------------------------------//
case PICK_TOOL:
ROS_INFO("*SM* -> PICK_TOOL");
// Do arm movements
if(switchLeftTool)
{
//Put down current tool
//Pick up next tool
}
if(switchRightTool)
{
//Put down current tool
//Pick up next tool
}
currentRunState = MOVE_TO_CENTER;
break;
//-------------------------------------------------------------------------------//
case MOVE_TO_CENTER:
ROS_INFO("*SM* -> MOVE_TO_CENTER");
// Do arm movements
if(switchLeftTool)
{
// Move arms to home position
}
if(switchRightTool)
{
// Move arms to home position
}
base->move("center");
//base->motionComplete(); TODO implement, for now just wait
ros::Duration(3.0).sleep();
currentRunState = MOVE_ARM;
break;
//-------------------------------------------------------------------------------//
case MOVE_ARM:
ROS_INFO("*SM* -> MOVE_ARM");
arms->getCurrentPose( stateVariables.requiredArmCartesian,
stateVariables.currentGripperPose);
// Prepare arms for picking
moveToBinPlane(stateVariables.currentBin, stateVariables.requiredArm, 0.4);
//arms->waitForMotion(5.0);
currentRunState = WAIT_FOR_MOTION;
nextRunState = MOVE_TOWARDS_BIN;
break;
//-------------------------------------------------------------------------------//
case MOVE_TOWARDS_BIN:
ROS_INFO("*SM* -> MOVE_TOWARDS_BIN");
base->move("col2");
//base->motionComplete(); TODO implement, for now just wait
ros::Duration(3.0).sleep();
currentRunState = GET_OBJECT_LOCATION_2;
break;
//-------------------------------------------------------------------------------//
case GET_OBJECT_LOCATION_2:
ROS_INFO("*SM* -> GET_OBJECT_LOCATION");
// Get bin info
getBinInfo(stateVariables.currentBin, stateVariables.currentBinInfo);
// Point Head
head->sendGoalCart( robot_frame,
stateVariables.currentBinInfo.response.center.x,
stateVariables.currentBinInfo.response.center.y,
stateVariables.currentBinInfo.response.center.z,
1.0);
/*
// Start ORK
srv_msg_SetBin.request.number = stateVariables.currentBin;
if(! srvClient_startORK.call(srv_msg_SetBin) )
ROS_ERROR("Could not start ORK");
// Get Object pose
srv_msg_GetObjectPose.request.obj_name = stateVariables.currentObject;
srv_msg_GetObjectPose.response.found_obj = false;
while(!srv_msg_GetObjectPose.response.found_obj)
{
if(! srvClient_get_object_pose.call(srv_msg_GetObjectPose) )
ROS_ERROR("Could not stop ORK");
ros::Duration(0.5).sleep();
}
// Stop ORK
if(! srvClient_stopORK.call(srv_msg_Empty) )
ROS_ERROR("Could not stop ORK");
*/
// TODO: convert pose to string with orientation name
///stateVariables.currentObjPose = srv_msg_GetObjectPose.response.pose; // TODO
// Create a fake location for testing
stateVariables.currentGripperPose.pose.position.x = stateVariables.currentBinInfo.response.center.x;
stateVariables.currentGripperPose.pose.position.y = stateVariables.currentBinInfo.response.center.y;
stateVariables.currentGripperPose.pose.position.z = stateVariables.currentBinInfo.response.center.z;
stateVariables.currentGripperPose.pose.orientation.w = 1;
std::cout << stateVariables.currentGripperPose;
stateVariables.currentObjPose.position.x = stateVariables.currentBinInfo.response.center.x + 0.15;
stateVariables.currentObjPose.position.y = stateVariables.currentBinInfo.response.center.y;
stateVariables.currentObjPose.position.z = stateVariables.currentBinInfo.response.center.z;
stateVariables.currentObjPose.orientation.w = 1;
std::cout << stateVariables.currentObjPose;
currentRunState = PICK_ITEM;
break;
//-------------------------------------------------------------------------------//
case PICK_ITEM:
ROS_INFO("*SM* -> PICK_ITEM");
// Set gripper pose
arms->getCurrentPose( stateVariables.requiredArmCartesian,
stateVariables.currentGripperPose);
// Perform grasp
grasp( stateVariables.requiredStrategy,
stateVariables.requiredArm,
stateVariables.currentGripperPose,
stateVariables.currentObjPose,
stateVariables.currentBinInfo);
currentRunState = MOVE_TO_CENTER_2;
break;
//-------------------------------------------------------------------------------//
case MOVE_TO_CENTER_2:
ROS_INFO("*SM* -> MOVE_TO_CENTER_2");
base->move("center");
//base->motionComplete(); TODO implement, for now just wait
ros::Duration(3.0).sleep();
currentRunState = MOVE_TO_ORDER_BIN;
break;
//-------------------------------------------------------------------------------//
case MOVE_TO_ORDER_BIN:
ROS_INFO("*SM* -> MOVE_TO_ORDER_BIN");
base->move("bin");
//base->motionComplete(); TODO implement, for now just wait
ros::Duration(3.0).sleep();
currentRunState = PLACE_ITEM;
break;
//-------------------------------------------------------------------------------//
case PLACE_ITEM:
ROS_INFO("*SM* -> PLACE_ITEM");
arms->loadCartPose(stateVariables.requiredArmCartesian,ArmsCartesian::binHigh,stateVariables.currentGripperPose);
arms->waitForMotion(max_arm_execution_time);
arms->loadCartPose(stateVariables.requiredArmCartesian,ArmsCartesian::binMid,stateVariables.currentGripperPose);
arms->waitForMotion(max_arm_execution_time);
arms->loadCartPose(stateVariables.requiredArmCartesian,ArmsCartesian::binLow,stateVariables.currentGripperPose);
arms->waitForMotion(max_arm_execution_time);
if(stateVariables.currentToolLeft == ELECTRIC)
grippers->position(0,-1,stateVariables.requiredArmGripper);
if(stateVariables.currentToolRight == ELECTRIC)
grippers->position(0,-1,stateVariables.requiredArmGripper);
arms->loadCartPose(stateVariables.requiredArmCartesian,ArmsCartesian::binHigh,stateVariables.currentGripperPose);
arms->waitForMotion(max_arm_execution_time);
// Move arm to order bin
currentRunState = MOVE_TO_HOME;
break;
//-------------------------------------------------------------------------------//
case MOVE_TO_HOME:
ROS_INFO("*SM* -> MOVE_TO_HOME");
arms->loadCartPose(ArmsCartesian::LEFT,ArmsCartesian::homeLeft,stateVariables.currentGripperPoseLeft);
arms->loadCartPose(ArmsCartesian::RIGHT,ArmsCartesian::homeRight,stateVariables.currentGripperPoseRight);
arms->waitForMotion(max_arm_execution_time);
base->move("center");
//base->motionComplete(); TODO implement, for now just wait
ros::Duration(3.0).sleep();
currentRunState = DONE_PICKING;
break;
//-------------------------------------------------------------------------------//
case DONE_PICKING:
ROS_INFO("*SM* -> DONE_PICKING");
// some error handling
if(items_left.vacuumLift == 0)
{
orderIter = work_order.electricFront.begin();
ROS_INFO(" ## Done with vacuumLift!");
}
if(items_left.electricFront == 0)
{
orderIter = work_order.vacuumFront.begin();
ROS_INFO(" ## Done with electricFront!");
}
if(items_left.vacuumFront == 0)
{
orderIter = work_order.vacuumFront.begin();
ROS_INFO(" ## Done with electricFront!");
}
if(items_left.electricAngledLift == 0)
{
ROS_INFO(" ## Done with electricAngledLift!");
executingRun = false;
}
if(!executingRun)
{
ROS_INFO("### DONE PICKING ###!");
currentRunState = DONE;
}
else
{
currentRunState = SELECT_NEXT_ITEM;
}
break;
//-------------------------------------------------------------------------------//
//-------------------------------------------------------------------------------//
case SELECT_NEXT_ITEM:
ROS_INFO("*SM* -> SELECT_NEXT_ITEM");
stateVariables.currentItemNumber++;
currentRunState = START_PICKING;
break;
//-------------------------------------------------------------------------------//
case DONE:
ROS_INFO("runSM: DONE");
return true;
break;
//-------------------------------------------------------------------------------//
default:
ROS_WARN("runSM: invalid state");
break;
}
if(currentRunState == stop)
{
currentRunState = DONE;
}
ros::Duration(loopRateRunSM).sleep();
}
return true;
}
int main(int argc, char *argv[])
{
if (argc <= 1)
{
std::cout << "Error: Please inform options and filename." << std::endl;
printHelp(argv[0]);
return 1;
}
std::string option = argv[1];
if (option == "--test")
{
testRun();
return 0;
}
if (option == "--help")
{
printHelp(argv[0]);
return 0;
}
tsp::TSPLibData data;
std::ifstream file(argv[2]);
if (!file.is_open())
{
std::cout << "Error: cannot open file." << std::endl;
printHelp(argv[0]);
return 0;
}
data.load(file);
if (option == "--only-constructive")
{
tsp::TSPNearestNeighborConstruct nn(data);
tsp::TSPTour tour = nn.run();
if (tour.isValid())
{
tour.print(std::cout);
}
return 0;
}
if (option == "--localsearch")
{
tsp::TSPNearestNeighborConstruct nn(data);
tsp::TSPTour tour = nn.run();
tsp::TSP2opt local(data, tour);
tour = local.run();
if (tour.isValid())
{
tour.print(std::cout);
}
return 0;
}
if (option == "--grasp")
{
tsp::TSPGrasp grasp(data, 100);
tsp::TSPTour tourGrasp = grasp.run();
if ( tourGrasp.isValid() )
{
tourGrasp.print(std::cout);
}
return 0;
}
if (option == "--tabu")
{
tsp::TSPTabu tabu(data, 100, 10, 10);
tsp::TSPTour tourTabu = tabu.run();
if ( tourTabu.isValid() )
{
tourTabu.print(std::cout);
}
return 0;
}
return 0;
}
bool tactileGrasp_IDLServer::read(yarp::os::ConnectionReader& connection) {
yarp::os::idl::WireReader reader(connection);
reader.expectAccept();
if (!reader.readListHeader()) { reader.fail(); return false; }
yarp::os::ConstString tag = reader.readTag();
while (!reader.isError()) {
// TODO: use quick lookup, this is just a test
if (tag == "open") {
bool _return;
_return = open();
yarp::os::idl::WireWriter writer(reader);
if (!writer.isNull()) {
if (!writer.writeListHeader(1)) return false;
if (!writer.writeBool(_return)) return false;
}
reader.accept();
return true;
}
if (tag == "grasp") {
bool _return;
_return = grasp();
yarp::os::idl::WireWriter writer(reader);
if (!writer.isNull()) {
if (!writer.writeListHeader(1)) return false;
if (!writer.writeBool(_return)) return false;
}
reader.accept();
return true;
}
if (tag == "crush") {
bool _return;
_return = crush();
yarp::os::idl::WireWriter writer(reader);
if (!writer.isNull()) {
if (!writer.writeListHeader(1)) return false;
if (!writer.writeBool(_return)) return false;
}
reader.accept();
return true;
}
if (tag == "quit") {
bool _return;
_return = quit();
yarp::os::idl::WireWriter writer(reader);
if (!writer.isNull()) {
if (!writer.writeListHeader(1)) return false;
if (!writer.writeBool(_return)) return false;
}
reader.accept();
return true;
}
if (tag == "setThreshold") {
int32_t aFinger;
double aThreshold;
if (!reader.readI32(aFinger)) {
reader.fail();
return false;
}
if (!reader.readDouble(aThreshold)) {
reader.fail();
return false;
}
bool _return;
_return = setThreshold(aFinger,aThreshold);
yarp::os::idl::WireWriter writer(reader);
if (!writer.isNull()) {
if (!writer.writeListHeader(1)) return false;
if (!writer.writeBool(_return)) return false;
}
reader.accept();
return true;
}
if (tag == "help") {
std::string functionName;
if (!reader.readString(functionName)) {
functionName = "--all";
}
std::vector<std::string> _return=help(functionName);
yarp::os::idl::WireWriter writer(reader);
if (!writer.isNull()) {
if (!writer.writeListHeader(2)) return false;
if (!writer.writeTag("many",1, 0)) return false;
if (!writer.writeListBegin(BOTTLE_TAG_INT, static_cast<uint32_t>(_return.size()))) return false;
std::vector<std::string> ::iterator _iterHelp;
for (_iterHelp = _return.begin(); _iterHelp != _return.end(); ++_iterHelp)
{
if (!writer.writeString(*_iterHelp)) return false;
}
if (!writer.writeListEnd()) return false;
}
reader.accept();
return true;
}
if (reader.noMore()) { reader.fail(); return false; }
yarp::os::ConstString next_tag = reader.readTag();
if (next_tag=="") break;
tag = tag + "_" + next_tag;
}
return false;
}
/*
* Parametros:
* argv[1]: arquivo de instância
* maxItex: maximo de iteracoes
* alfa: valor de threshold da LRC
* bl: busca local: hc,sa
* n: numero de iteracoes sem melhora
* ti: temperatura inicial
* tf: temperatura final
* beta: taxa de resfriamento
* seed: seed de numero aleatorio
* time: tempo limite
*/
int main(int argc, char** argv) {
int i, j;
clock_t t1, t2;
Individuo *ind;
char arquivoResposta[30];
FILE *fp;
//srand(time(0));
t1 = clock();
Problema *p = lerInstancia(argv[1]);
configuraParametros(p, argc, argv);
srand(p->seed);
//p->maxIterGrasp = atoi(argv[2]);
//p->buscaLocalGrasp = atoi(argv[3]);
//p->txSwap = atof(argv[4]);
//p->nIterSemMelhoras = atoi(argv[5]);
//p->threshold = atof(argv[6]);
//p->pAproveitamento = 0.95;//atof(argv[7]);
p->pesoHard = 10000;
/*for (i = 0; i < p->nCurriculos; i++) {
printf("Curr: %s (%d) pos=%d\n", (p->curriculos + i)->nomeCurriculo,
(p->curriculos + i)->nDisciplinas, (p->curriculos + i)->pVetor);
}
for (j = 0; j < p->nDisciplinas; j++) {
printf("Disc: %s (%d): \n", (p->disciplinas + j)->nomeDisciplina,
(p->disciplinas + j)->nCurriculos);
for (i = 0; i < (p->disciplinas + j)->nCurriculos; i++) {
Curriculo *c = (p->disciplinas + j)->curriculos[i];
printf("\tCur: %s (%d) \n", c->nomeCurriculo, c->pVetor);
}
}*/
//ind = geraIndividuoAleatorio(p, 100);
//imprimeIndividuo3(p,ind);
//inicializaMatrizesAuxiliares(p, ind);
//exit(0);
for (i = 0; i < 0; i++) {
somaViolacoesSoft2(p, ind,0);
Neighbour *mov;
if (1) {
mov = geraIsolatedLectureMove(p, ind);
} else {
mov = geraMove(p, ind);
}
imprimeIndividuo3(p, ind);
imprimeMatCurrDiasPeriodo(p, ind);
float deltaF = mov->deltaHard + mov->deltaSoft;
printf("DeltaF=%f (%.1f + %.1f)\n", deltaF, mov->deltaHard, mov->deltaSoft);
float f1 = funcaoObjetivo2(p, ind, 1);
float f1Soft = somaViolacoesSoft2(p, ind,0);
//printf("1) H=%f, S=%f: %d,%d,%d,%d\n", somaViolacoesHard(p, ind), somaViolacoesSoft(p, ind),
//ind->soft1, ind->soft2, ind->soft3, ind->soft4);
printf("1) H=%f, S=%f: %d,%d,%d,%d\n", somaViolacoesHard(p, ind), somaViolacoesSoft2(p, ind,0),
ind->soft1, ind->soft2, ind->soft3, ind->soft4);
troca_par_completo(p, ind, mov->p1, mov->p2);
printf("POS: (%d, %d)\n", mov->p1, mov->p2);
float f2 = funcaoObjetivo2(p, ind, 1);
float f2Soft = somaViolacoesSoft2(p, ind,0);
//printf("2) H=%f, S=%f: %d,%d,%d,%d\n", somaViolacoesHard(p, ind), somaViolacoesSoft(p, ind),
//ind->soft1, ind->soft2, ind->soft3, ind->soft4);
printf("2) H=%f, S=%f: %d,%d,%d,%d\n", somaViolacoesHard(p, ind), somaViolacoesSoft2(p, ind,0),
ind->soft1, ind->soft2, ind->soft3, ind->soft4);
imprimeIndividuo3(p, ind);
imprimeMatCurrDiasPeriodo(p, ind);
printf("\n\n%d___________________________________________________________________________________\n\n", i);
if (f2Soft - f1Soft != mov->deltaSoft) {
printf("[%d] F1/2: %.1f/%.1f D=%.1f\n", i, f1Soft, f2Soft, mov->deltaSoft);
scanf("%d", &j);
} else {
continue;
}
}
//exit(0);
/*Gerador *gerador = getGeradorInicial(p->dimensao);
while (1) {
incrementaPosGerador(gerador);
}*/
/*for(i=0;i<p->nDisciplinas;i++){
printf("%s\t%d\n",p->disciplinas[i].nomeDisciplina,p->disciplinas[i].nIndisponibilidades);
}*/
p->inicio = clock();
ind = grasp(p);
p->fim = clock();
/*p->t0 = atof(argv[2]);
p->rho = atof(argv[3]);
p->beta = atof(argv[4]);
p->aceitaPioraSA = atoi(argv[5]);*/
//ind = buscaLocalGraspProfundidade(p, ind);
zeraMatCurrDiasPeriodos(p, ind);
inicializaMatrizesAuxiliares(p, ind);
printf("FO: %f\n", funcaoObjetivo2(p, ind, 10000));
printf("HARD: %f\n", somaViolacoesHard(p, ind));
printf("SOFT: %f\n", somaViolacoesSoft2(p, ind,0));
printf("MEDIA: %f\n", p->mediaSolucoes);
printf("RoomCapacity: %f\n", p->soft1);
printf("MinWorkDays: %f\n", p->soft2);
printf("CurrCompactness: %f\n", p->soft3);
printf("RoomStability: %f\n", p->soft4);
printf("%.2f <-> %.2f <-> %.2f\n", p->f1, p->f2, p->f3);
printf("F1 -> F2: %f\n", p->f2 - p->f1);
printf("F2 -> F3: %f\n", p->f3 - p->f2);
printf("TEMPO: %f\n", ((double) (p->fim - p->inicio)) / CLOCKS_PER_SEC);
printf("%d, %d, %d, %d\n", p->vHard, p->semEfeito, p->comPiora, p->comMelhora);
//imprimeResposta(p,ind, stdout);
//printf("\nWARNING: ESCOLHA DO P2 NO MOVE COMPACT\n");
t2 = clock();
/*for(i=0;i<disc->nSlotsDisponiveis;i++){
printf("%d ", disc->slotsDisponiveis[i]);
}
printf("\n");*/
//printf("T: %f\n", (float) (t2 - t1) / CLOCKS_PER_SEC);
/* resposta em arquivo
*
strcpy(arquivoResposta,"sol_");
fp = fopen(strcat(arquivoResposta,getNomeInstancia(argv[1])), "w");
imprimeResposta(p, ind, fp);
fclose(fp);*/
//ind = geraIndividuoAleatorio(p, 100);
//printf("%f\n", funcaoObjetivo(p, ind));
/*Movimento movimentos[7] = {MOVE, SWAP, LECTURE_MOVE, TIME_MOVE, ROOM_MOVE,
ROOMS, COMPACT};
int iteracoesMax[7] = {0, 0, 0, 0, 0, 0, 0};
ind->soft1 = 10;
ind->soft2 = 0;
ind->soft3 = 5;
ind->soft4 = 0;
configuraQtMovsVNS(p, ind, movimentos, iteracoesMax, 7);
printf("[");
for (i = 0; i < 7; i++) {
printf("%d ", iteracoesMax[i]);
}
printf("]\n");*/
liberaIndividuo(ind);
desalocaProblema(p);
return (EXIT_SUCCESS);
}
