/**
* Parses a read in fastq format.
* Returns FASTQ_END at end of file, FASTQ_OK on success, FASTQ_ERR on problem
*/
static int parse_fastq_read(ReadSeq *read, gzFile f) {
size_t qual_len;
read->status = FASTQ_OK;
read_fastq_lines(read, f);
if(read->status != FASTQ_OK) {
return read->status;
}
/* check_header(read); */
/* if(read->status != FASTQ_OK) { */
/* return read->status; */
/* } */
/* third line should start with '+' separator */
if(read->line3[0] != '+') {
my_warn("%s:%d: third line does not start with '+'",
__FILE__, __LINE__);
read->status = FASTQ_ERR;
return read->status;
}
/* check length of read and quality */
read->read_len = strlen(read->line2);
qual_len = strlen(read->line4);
if(read->read_len < 1) {
my_warn("%s:%d: read has no bases\n", __FILE__, __LINE__);
return read->status;
}
/* next line should be quality scores */
if(read->read_len != qual_len) {
my_warn("%s:%d: read len (%ld) does not match quality score len (%ld)",
__FILE__, __LINE__, read->read_len, qual_len);
read->status = FASTQ_ERR;
return read->status;
}
check_seq(read);
check_qual(read);
return read->status;
}
int main(int argc, char** argv)
{
ros::init(argc, argv, "test_learning");
ros::NodeHandle node;
ros::Rate r(0.5);
// setup all clients and publisher
ros::ServiceClient detect_client = node.serviceClient<table_object::detect_touch>("detect_touch");
table_object::detect_touch detect_srv;
detect_srv.request.detect = true;
ros::ServiceClient move_client = node.serviceClient<baxter_moveit::move_to_target_pose>("move_to_target_pose");
baxter_moveit::move_to_target_pose move_srv;
ros::Publisher gripper_command_publisher = node.advertise<baxter_core_msgs::EndEffectorCommand>("/robot/end_effector/left_gripper/command", 1);
baxter_core_msgs::EndEffectorCommand gripper_command;
ros::ServiceClient feature_client = node.serviceClient<table_object::record_feature>("record_feature");
table_object::record_feature feature_srv;
feature_srv.request.record_name = "test";
/***************************************
* qualitative representation from human_demo
* - (bottle,hand), (bottle,tabletop), (hand,tabletop)
***************************************/
std::vector<int> q0; q0.push_back(0); q0.push_back(1); q0.push_back(0);
std::vector<int> q1; q1.push_back(1); q1.push_back(1); q1.push_back(0);
std::vector<int> q2; q2.push_back(1); q2.push_back(0); q2.push_back(0);
human_demo.push_back(q0);
human_demo.push_back(q1);
human_demo.push_back(q2);
/***************************************
* construct explorer
***************************************/
TableObjectAction::Explorer explorer(human_demo);
int qual_index = 0;
/***************************************
* apply learned policy
***************************************/
int count=0;
int success_count=0;
int exp_num=10;
bool move_srv_success = false;
while(count < exp_num)
{
bool imitation_success[2];
imitation_success[0] = false;
imitation_success[1] = false;
for(qual_index=0; qual_index<human_demo.size(); qual_index++)
{
// random choose one
srand ( time(NULL) ); //initialize the random seed
int RandIndex = rand() % good_poses_index[qual_index].size(); //generates a random number between 0 and godd_poses[qual_index].size()
if(qual_index==0)
{
// get to home pose first
move_srv_success = false;
move_srv.request.target_pose=explorer.getHomePose();
std::cout << "send target pose to server: go to home pose " << std::endl;
while(!move_srv_success)
{
if (move_client.call(move_srv))
{
ROS_INFO("plan and execution succeed: %d", move_srv.response.succeed);
move_srv_success = move_srv.response.succeed;
}
else
{
ROS_ERROR("Failed to call service move_to_target_pose...Retrying...");
}
}
if(good_poses_gripper_open[qual_index][RandIndex])
{
gripper_command.id = 65664;
gripper_command.command = baxter_core_msgs::EndEffectorCommand::CMD_GO;
char args_buffer[50];
std::string position_str("position");
sprintf(args_buffer, "{\"%s\": 100.0}", position_str.c_str());
gripper_command.args = std::string(args_buffer);
gripper_command_publisher.publish(gripper_command);
}else{
gripper_command.id = 65664;
gripper_command.command = baxter_core_msgs::EndEffectorCommand::CMD_GO;
char args_buffer[50];
std::string position_str("position");
sprintf(args_buffer, "{\"%s\": 0.0}", position_str.c_str());
gripper_command.args = std::string(args_buffer);
gripper_command_publisher.publish(gripper_command);
}
}
move_srv_success = false;
//move_srv.request.target_pose=good_poses[qual_index][RandIndex];
move_srv.request.target_pose=explorer.getAvailablePose(good_poses_index[qual_index][RandIndex]);
std::cout << "send target pose to server: go to good_poses[" << qual_index << "][" << RandIndex << "]" << std::endl;
while(!move_srv_success)
{
if (move_client.call(move_srv))
{
ROS_INFO("plan and execution succeed: %d", move_srv.response.succeed);
move_srv_success = move_srv.response.succeed;
}
else
{
ROS_ERROR("Failed to call service move_to_target_pose...Retrying...");
}
}
// verify that the scene and robot have translated into the correct qual state
std::cout << "pause to wait for tf and msgs to be buffered ..." << std::endl;
sleep(10.0);
bool detect_service_succeed = false;
while(!detect_service_succeed)
{
if (detect_client.call(detect_srv))
{
ROS_INFO("detection results: %d, %d, %d", detect_srv.response.bottle_hand, detect_srv.response.bottle_tabletop, detect_srv.response.hand_tabletop);
detect_service_succeed = detect_srv.response.succeed;
}
else
{
ROS_ERROR("Failed to call service detect_touch...Retrying...");
return 1;
}
r.sleep();
}
if(check_qual(detect_srv.response, qual_index+1))
{
printf(" transition q[%d]->q[%d]: complete", qual_index, qual_index+1);
imitation_success[qual_index] = true;
// record feature
bool service_succeed = false;
while(!service_succeed)
{
if (feature_client.call(feature_srv))
{
ROS_INFO("succeed: %d", feature_srv.response.succeed);
service_succeed = feature_srv.response.succeed;
}
else
{
ROS_ERROR("Failed to call service record_feature");
return 1;
}
r.sleep();
}
}else {
printf(" transition q[%d]->q[%d]: incomplete", qual_index, qual_index+1);
break;
}
}
if(imitation_success[0] & imitation_success[1]) success_count++;
count ++;
}
printf("imitation success rate: %f\n", (float)success_count/exp_num);
}
int main(int argc, char** argv)
{
ros::init(argc, argv, "action_learning");
ros::NodeHandle node;
ros::Rate r(0.5);
// setup all clients and publisher
ros::ServiceClient detect_client = node.serviceClient<table_object::detect_touch>("detect_touch");
table_object::detect_touch detect_srv;
detect_srv.request.detect = true;
ros::ServiceClient move_client = node.serviceClient<baxter_moveit::move_to_target_pose>("move_to_target_pose");
baxter_moveit::move_to_target_pose move_srv;
ros::Publisher gripper_command_publisher = node.advertise<baxter_core_msgs::EndEffectorCommand>("/robot/end_effector/left_gripper/command", 1);
baxter_core_msgs::EndEffectorCommand gripper_command;
ros::Publisher trigger_publisher = node.advertise<table_object::palm_reflex_triggered>("palm_reflex_triggered", 1);
// ros::Publisher collision_object_publisher = node_handle.advertise<moveit_msgs::CollisionObject>("collision_object", 1);
// ros::Publisher attached_object_publisher = node_handle.advertise<moveit_msgs::AttachedCollisionObject>("attached_collision_object", 1);
/***************************************
* qualitative representation from human_demo
* - (bottle,hand), (bottle,tabletop), (hand,tabletop)
***************************************/
std::vector<int> q0; q0.push_back(0); q0.push_back(1); q0.push_back(0);
std::vector<int> q1; q1.push_back(1); q1.push_back(1); q1.push_back(0);
std::vector<int> q2; q2.push_back(1); q2.push_back(0); q2.push_back(0);
human_demo.push_back(q0);
human_demo.push_back(q1);
human_demo.push_back(q2);
/***************************************
* construct explorer
***************************************/
TableObjectAction::Explorer explorer(human_demo);
std::vector<std::vector<geometry_msgs::Pose> > good_poses;
std::vector<std::vector<int> > good_poses_gripper_open; //open: 1; close: 0
std::vector<std::vector<int> > good_poses_index;
int qual_index;
/***************************************
* search all available actions (home pose -> 12 different target poses)
***************************************/
//int RandIndex = rand() % good_poses[qual_index-1].size(); //generates a random number between 0 and 3
//move_srv.request.target_pose=good_poses[qual_index-1][RandIndex];
for(qual_index=0; qual_index<human_demo.size(); qual_index++)
{
ROS_INFO("learning action for qual state transition: %d -> %d", qual_index, qual_index+1);
std::vector<geometry_msgs::Pose> cur_transition_good_poses;
std::vector<int> cur_transition_good_poses_gripper_open;
std::vector<int> cur_good_poses_index;
for(int gripper_open=0; gripper_open<=1; gripper_open++)
{
for(int available_pose_index=0; available_pose_index < 12; available_pose_index++)
{
// close or open gripper
if(gripper_open)
{
gripper_command.id = 65664;
gripper_command.command = baxter_core_msgs::EndEffectorCommand::CMD_GO;
char args_buffer[50];
std::string position_str("position");
sprintf(args_buffer, "{\"%s\": 100.0}", position_str.c_str());
gripper_command.args = std::string(args_buffer);
gripper_command_publisher.publish(gripper_command);
}else{
gripper_command.id = 65664;
gripper_command.command = baxter_core_msgs::EndEffectorCommand::CMD_GO;
char args_buffer[50];
std::string position_str("position");
sprintf(args_buffer, "{\"%s\": 0.0}", position_str.c_str());
gripper_command.args = std::string(args_buffer);
gripper_command_publisher.publish(gripper_command);
}
// get to home pose first
bool move_srv_success = false;
move_srv.request.target_pose=explorer.getHomePose();
std::cout << "send target pose to server: go to home pose " << std::endl;
while(!move_srv_success)
{
if (move_client.call(move_srv))
{
ROS_INFO("plan and execution succeed: %d", move_srv.response.succeed);
move_srv_success = move_srv.response.succeed;
}
else
{
ROS_ERROR("Failed to call service move_to_target_pose...Retrying...");
}
}
sleep(2.0);
if(qual_index == 1) // hand and bottle should be touching
{
move_srv_success = false;
// random choose one
srand ( time(NULL) ); //initialize the random seed
int RandIndex = rand() % good_poses[qual_index-1].size(); //generates a random number between 0 and 3
if(good_poses_gripper_open[qual_index][RandIndex])
{
gripper_command.id = 65664;
gripper_command.command = baxter_core_msgs::EndEffectorCommand::CMD_GO;
char args_buffer[50];
std::string position_str("position");
sprintf(args_buffer, "{\"%s\": 100.0}", position_str.c_str());
gripper_command.args = std::string(args_buffer);
gripper_command_publisher.publish(gripper_command);
}else{
gripper_command.id = 65664;
gripper_command.command = baxter_core_msgs::EndEffectorCommand::CMD_GO;
char args_buffer[50];
std::string position_str("position");
sprintf(args_buffer, "{\"%s\": 0.0}", position_str.c_str());
gripper_command.args = std::string(args_buffer);
gripper_command_publisher.publish(gripper_command);
}
move_srv.request.target_pose=good_poses[qual_index-1][RandIndex];
std::cout << "send target pose to server: go to good_poses[" << qual_index-1 << "][" << RandIndex << "]" << std::endl;
while(!move_srv_success)
{
if (move_client.call(move_srv))
{
ROS_INFO("plan and execution succeed: %d", move_srv.response.succeed);
move_srv_success = move_srv.response.succeed;
}
else
{
ROS_ERROR("Failed to call service move_to_target_pose...Retrying...");
}
}
}
// verify that the scene and robot are in the correct qual state
std::cout << "pause to wait for tf and msgs to be buffered ..." << std::endl;
sleep(10.0);
std::cout << "verify correct current qual state: send request to detect service" << std::endl;
bool detect_service_succeed = false;
while(!detect_service_succeed)
{
if (detect_client.call(detect_srv))
{
ROS_INFO("detection results: %d, %d, %d", detect_srv.response.bottle_hand, detect_srv.response.bottle_tabletop, detect_srv.response.hand_tabletop);
detect_service_succeed = detect_srv.response.succeed;
}
else
{
ROS_ERROR("Failed to call service detect_touch...Retrying...");
return 1;
}
r.sleep();
}
if(check_qual(detect_srv.response, qual_index))
{
std::cout << "current qual state is satisfied, start learning..." << std::endl;
// attach bottle to robot gripper if needed
if(qual_index==1 & available_pose_index==1)
{
// moved to palm_reflex_server.cpp
}
}else {
std::cout << "current qual state is not satisfied..." << std::endl;
exit(1);
}
// close or open gripper: again here since palm reflex might be triggered previously
if(gripper_open)
{
gripper_command.id = 65664;
gripper_command.command = baxter_core_msgs::EndEffectorCommand::CMD_GO;
char args_buffer[50];
std::string position_str("position");
sprintf(args_buffer, "{\"%s\": 100.0}", position_str.c_str());
gripper_command.args = std::string(args_buffer);
gripper_command_publisher.publish(gripper_command);
table_object::palm_reflex_triggered signal;
signal.palm_reflex_triggered = false;
trigger_publisher.publish(signal);
}else{
gripper_command.id = 65664;
gripper_command.command = baxter_core_msgs::EndEffectorCommand::CMD_GO;
char args_buffer[50];
std::string position_str("position");
sprintf(args_buffer, "{\"%s\": 0.0}", position_str.c_str());
gripper_command.args = std::string(args_buffer);
gripper_command_publisher.publish(gripper_command);
}
sleep(2.0);
// try out available_pose_index pose
move_srv_success = false;
move_srv.request.target_pose=explorer.getAvailablePose(available_pose_index);
std::cout << "send target pose to server: go to " << available_pose_index << "th avaialbe pose" << std::endl;
//while(!move_srv_success)
//{
if (move_client.call(move_srv))
{
ROS_INFO("plan and execution succeed: %d", move_srv.response.succeed);
move_srv_success = move_srv.response.succeed;
}
else
{
ROS_ERROR("Failed to call service move_to_target_pose...Retrying...");
}
//}
if(move_srv_success)
{
// pause and apply event detectors
std::cout << "pause to wait for tf and msgs to be buffered ..." << std::endl;
sleep(10.0);
std::cout << "send request to detect service" << std::endl;
detect_service_succeed = false;
while(!detect_service_succeed)
{
if (detect_client.call(detect_srv))
{
ROS_INFO("detection results: %d, %d, %d", detect_srv.response.bottle_hand, detect_srv.response.bottle_tabletop, detect_srv.response.hand_tabletop);
detect_service_succeed = detect_srv.response.succeed;
}
else
{
ROS_ERROR("Failed to call service detect_touch...Retrying...");
return 1;
}
r.sleep();
}
if(check_qual(detect_srv.response, qual_index+1))
{
std::cout << "good pose! recording..." << std::endl;
cur_transition_good_poses.push_back(explorer.getAvailablePose(available_pose_index));
cur_transition_good_poses_gripper_open.push_back(gripper_open);
cur_good_poses_index.push_back(available_pose_index);
}else{
std::cout << "state transition is not completed" << std::endl;
}
}
// open gripper
gripper_command.id = 65664;
gripper_command.command = baxter_core_msgs::EndEffectorCommand::CMD_GO;
char args_buffer[50];
std::string position_str("position");
sprintf(args_buffer, "{\"%s\": 100.0}", position_str.c_str());
gripper_command.args = std::string(args_buffer);
gripper_command_publisher.publish(gripper_command);
}
}
good_poses.push_back(cur_transition_good_poses);
good_poses_gripper_open.push_back(cur_transition_good_poses_gripper_open);
good_poses_index.push_back(cur_good_poses_index);
}
/***************************************
* printout learning results
***************************************/
std::cout << "learning results: " << std::endl;
for(int i=0; i<good_poses.size(); i++)
{
printf(" transition q[%d]->q[%d]: good poses are", i, i+1);
for(int j=0; j<good_poses[i].size(); j++)
{
printf(" available_pose[%d]: %f, %f, %f, gripper open %d\n", good_poses_index[i][j], good_poses[i][j].position.x, good_poses[i][j].position.y,
good_poses[i][j].position.z, good_poses_gripper_open[i][j]);
}
}
}
