int main(int argc, const char* argv[])
{
std::string opt_ip = "198.18.0.1";;
std::string opt_data_folder = std::string(ROMEOTK_DATA_FOLDER);
bool opt_Reye = false;
bool opt_calib = true;
// Learning folder in /tmp/$USERNAME
std::string username;
// Get the user login name
vpIoTools::getUserName(username);
// Create a log filename to save new files...
std::string learning_folder;
#if defined(_WIN32)
learning_folder ="C:/temp/" + username;
#else
learning_folder ="/tmp/" + username;
#endif
// Test if the output path exist. If no try to create it
if (vpIoTools::checkDirectory(learning_folder) == false) {
try {
// Create the dirname
vpIoTools::makeDirectory(learning_folder);
}
catch (vpException &e) {
std::cout << "Cannot create " << learning_folder << std::endl;
std::cout << "Error: " << e.getMessage() <<std::endl;
return 0;
}
}
for (unsigned int i=0; i<argc; i++) {
if (std::string(argv[i]) == "--ip")
opt_ip = argv[i+1];
else if ( std::string(argv[i]) == "--reye")
opt_Reye = true;
else if ( std::string(argv[i]) == "--calib")
opt_calib = true;
else if (std::string(argv[i]) == "--help") {
std::cout << "Usage: " << argv[0] << "[--ip <robot address>]" << std::endl;
return 0;
}
}
std::vector<std::string> chain_name(2); // LArm or RArm
chain_name[0] = "LArm";
chain_name[1] = "RArm";
/************************************************************************************************/
/** Open the grabber for the acquisition of the images from the robot*/
vpNaoqiGrabber g;
g.setFramerate(15);
// Initialize constant transformations
vpHomogeneousMatrix eMc;
if (opt_Reye)
{
std::cout << "Using camera Eye Right" << std::endl;
g.setCamera(3); // CameraRightEye
eMc = g.get_eMc(vpCameraParameters::perspectiveProjWithDistortion,"CameraRightEye");
}
else
{
std::cout << "Using camera Eye Right" << std::endl;
g.setCamera(2); // CameraLeftEye
eMc = g.get_eMc(vpCameraParameters::perspectiveProjWithDistortion,"CameraLeftEye");
}
std::cout << "eMc: " << eMc << std::endl;
if (! opt_ip.empty())
g.setRobotIp(opt_ip);
g.open();
std::string camera_name = g.getCameraName();
std::cout << "Camera name: " << camera_name << std::endl;
vpCameraParameters cam = g.getCameraParameters(vpCameraParameters::perspectiveProjWithDistortion);
std::cout << "Camera parameters: " << cam << std::endl;
/** Initialization Visp Image, display and camera paramenters*/
vpImage<unsigned char> I(g.getHeight(), g.getWidth());
vpDisplayX d(I);
vpDisplay::setTitle(I, "Camera view");
//Initialize opencv color image
cv::Mat cvI = cv::Mat(cv::Size(g.getWidth(), g.getHeight()), CV_8UC3);
/************************************************************************************************/
/** Initialization target pen*/
std::string opt_name_file_color_pen_target = opt_data_folder + "/" +"target/box_blob/color.txt";
vpBlobsTargetTracker pen_tracker;
bool status_pen_tracker;
vpHomogeneousMatrix cMpen;
const double L = 0.025/2;
std::vector <vpPoint> points(4);
points[2].setWorldCoordinates(-L,-L, 0) ;
points[1].setWorldCoordinates(-L,L, 0) ;
points[0].setWorldCoordinates(L,L, 0) ;
points[3].setWorldCoordinates(L,-L,0) ;
const double L1 = 0.021/2;
std::vector <vpPoint> points_pen(4);
points_pen[2].setWorldCoordinates(-L1,-L1, 0) ;
points_pen[1].setWorldCoordinates(-L1,L1, 0) ;
points_pen[0].setWorldCoordinates(L1,L1, 0) ;
points_pen[3].setWorldCoordinates(L1,-L1,0) ;
pen_tracker.setName("pen");
pen_tracker.setCameraParameters(cam);
pen_tracker.setPoints(points_pen);
//pen_tracker.setManualBlobInit(true);
pen_tracker.setFullManual(true);
pen_tracker.setLeftHandTarget(true);
if(!pen_tracker.loadHSV(opt_name_file_color_pen_target))
{
std::cout << "Error opening the file "<< opt_name_file_color_pen_target << std::endl;
}
vpHomogeneousMatrix pen_M_point(0.0, 0.0, 0.163, 0.0, 0.0, 0.0);
/************************************************************************************************/
/** Initialize the qrcode tracker*/
bool status_qrcode_tracker;
vpHomogeneousMatrix cM_tab;
vpQRCodeTracker qrcode_tracker;
qrcode_tracker.setCameraParameters(cam);
qrcode_tracker.setQRCodeSize(0.045);
qrcode_tracker.setMessage("romeo_left_arm");
/************************************************************************************************/
/** Initialization target hands*/
std::string opt_name_file_color_target_path = opt_data_folder + "/" +"target/";
std::string opt_name_file_color_target_l = opt_name_file_color_target_path + chain_name[0] +"/color.txt";
std::string opt_name_file_color_target_r = opt_name_file_color_target_path + chain_name[1] +"/color.txt";
std::string opt_name_file_color_target1_l = opt_name_file_color_target_path + chain_name[0] +"/color1.txt";
std::string opt_name_file_color_target1_r = opt_name_file_color_target_path + chain_name[1] +"/color1.txt";
std::vector<vpBlobsTargetTracker*> hand_tracker;
std::vector<bool> status_hand_tracker(2);
std::vector<vpHomogeneousMatrix> cMo_hand(2);
vpBlobsTargetTracker hand_tracker_l;
hand_tracker_l.setName(chain_name[0]);
hand_tracker_l.setCameraParameters(cam);
hand_tracker_l.setPoints(points);
hand_tracker_l.setLeftHandTarget(true);
if(!hand_tracker_l.loadHSV(opt_name_file_color_target_l))
std::cout << "Error opening the file "<< opt_name_file_color_target_l << std::endl;
vpBlobsTargetTracker hand_tracker_r;
hand_tracker_r.setName(chain_name[1]);
hand_tracker_r.setCameraParameters(cam);
hand_tracker_r.setPoints(points);
hand_tracker_r.setLeftHandTarget(false);
if(!hand_tracker_r.loadHSV(opt_name_file_color_target1_r))
std::cout << "Error opening the file "<< opt_name_file_color_target_r << std::endl;
hand_tracker.push_back(&hand_tracker_l);
hand_tracker.push_back(&hand_tracker_r);
/************************************************************************************************/
// Constant transformation Target Frame to Arm end-effector (WristPitch)
std::vector<vpHomogeneousMatrix> hand_Me_Arm(2);
std::string filename_transform = std::string(ROMEOTK_DATA_FOLDER) + "/transformation.xml";
// Create twist matrix from pen to Arm end-effector (WristPitch)
std::vector <vpVelocityTwistMatrix> pen_Ve_Arm(2);
// Create twist matrix from target Frame to Arm end-effector (WristPitch)
std::vector <vpVelocityTwistMatrix> hand_Ve_Arm(2);
// Constant transformation Target Frame to pen to target Hand
std::vector<vpHomogeneousMatrix> pen_Mhand(2);
for (unsigned int i = 0; i < 2; i++)
{
std::string name_transform = "qrcode_M_e_" + chain_name[i];
vpXmlParserHomogeneousMatrix pm; // Create a XML parser
if (pm.parse(hand_Me_Arm[i], filename_transform, name_transform) != vpXmlParserHomogeneousMatrix::SEQUENCE_OK) {
std::cout << "Cannot found the homogeneous matrix named " << name_transform << "." << std::endl;
return 0;
}
else
std::cout << "Homogeneous matrix " << name_transform <<": " << std::endl << hand_Me_Arm[i] << std::endl;
hand_Ve_Arm[i].buildFrom(hand_Me_Arm[i]);
}
/************************************************************************************************/
/** Create a new istance NaoqiRobot*/
vpNaoqiRobot robot;
if (! opt_ip.empty())
robot.setRobotIp(opt_ip);
robot.open();
std::vector<std::string> jointNames = robot.getBodyNames("Head");
//jointNames.pop_back(); // We don't consider the last joint of the head = HeadRoll
std::vector<std::string> jointNamesLEye = robot.getBodyNames("LEye");
std::vector<std::string> jointNamesREye = robot.getBodyNames("REye");
jointNames.insert(jointNames.end(), jointNamesLEye.begin(), jointNamesLEye.end());
std::vector<std::string> jointHeadNames_tot = jointNames;
jointHeadNames_tot.push_back(jointNamesREye.at(0));
jointHeadNames_tot.push_back(jointNamesREye.at(1));
/************************************************************************************************/
// Initialize state
State_t state;
state = CalibratePen;
/************************************************************************************************/
std::vector<bool> grasp_servo_converged(2);
grasp_servo_converged[0]= false;
grasp_servo_converged[1]= false;
std::vector<vpMatrix> eJe(2);
// Initialize arms servoing
vpServoArm servo_larm_l;
vpServoArm servo_larm_r;
std::vector<vpServoArm*> servo_larm;
servo_larm.push_back(&servo_larm_l);
servo_larm.push_back(&servo_larm_r);
std::vector < std::vector<std::string > > jointNames_arm(2);
jointNames_arm[0] = robot.getBodyNames(chain_name[0]);
jointNames_arm[1] = robot.getBodyNames(chain_name[1]);
// Delete last joint Hand, that we don't consider in the servo
jointNames_arm[0].pop_back();
jointNames_arm[1].pop_back();
std::vector<std::string> jointArmsNames_tot = jointNames_arm[0];
//jointArmsNames_tot.insert(jointArmsNames_tot.end(), jointNames_arm[1].begin(), jointNames_arm[1].end());
const unsigned int numArmJoints = jointNames_arm[0].size();
std::vector<vpHomogeneousMatrix> pen_dMpen(2);
// Vector containing the joint velocity vectors of the arms
std::vector<vpColVector> q_dot_arm;
// Vector containing the joint velocity vectors of the arms for the secondary task
std::vector<vpColVector> q_dot_arm_sec;
// Vector joint position of the arms
std::vector<vpColVector> q;
// Vector joint real velocities of the arms
std::vector<vpColVector> q_dot_real;
vpColVector q_temp(numArmJoints);
q_dot_arm.push_back(q_temp);
q_dot_arm.push_back(q_temp);
q_dot_arm_sec.push_back(q_temp);
q_dot_arm_sec.push_back(q_temp);
q.push_back(q_temp);
q.push_back(q_temp);
q_dot_real.push_back(q_temp);
q_dot_real.push_back(q_temp);
// Initialize the joint avoidance scheme from the joint limits
std::vector<vpColVector> jointMin;
std::vector<vpColVector> jointMax;
jointMin.push_back(q_temp);
jointMin.push_back(q_temp);
jointMax.push_back(q_temp);
jointMax.push_back(q_temp);
for (unsigned int i = 0; i< 2; i++)
{
jointMin[i] = robot.getJointMin(chain_name[i]);
jointMax[i] = robot.getJointMax(chain_name[i]);
jointMin[i].resize(numArmJoints,false);
jointMax[i].resize(numArmJoints,false);
// std::cout << jointMin[i].size() << std::endl;
// std::cout << jointMax[i].size() << std::endl;
// std::cout << "max " << jointMax[i] << std::endl;
}
std::vector<bool> first_time_arm_servo(2);
first_time_arm_servo[0] = true;
first_time_arm_servo[1] = true;
std::vector< double> servo_arm_time_init(2);
servo_arm_time_init[0] = 0.0;
servo_arm_time_init[1] = 0.0;
std::vector<int> cpt_iter_servo_grasp(2);
cpt_iter_servo_grasp[0] = 0;
cpt_iter_servo_grasp[1] = 0;
unsigned int index_hand = 0; //Left Arm
// std::string handMpen_name = "handMpen_" + chain_name[index_hand];
// std::string filename_transform_pen = std::string(ROMEOTK_DATA_FOLDER) +"/pen/transformation_pen.xml";
// vpHomogeneousMatrix hand_M_pen ;
// if (state != CalibratePen)
// {
// vpXmlParserHomogeneousMatrix pm; // Create a XML parser
// if (pm.parse(hand_M_pen, filename_transform_pen, handMpen_name) != vpXmlParserHomogeneousMatrix::SEQUENCE_OK) {
// std::cout << "Cannot found the homogeneous matrix named " << handMpen_name << "." << std::endl;
// return 0;
// }
// else
// std::cout << "Homogeneous matrix " << handMpen_name <<": " << std::endl << hand_M_pen << std::endl;
// }
// vpHomogeneousMatrix M_offset;
// M_offset[1][3] = 0.00;
// M_offset[0][3] = 0.00;
// M_offset[2][3] = 0.00;
vpHomogeneousMatrix M_offset;
M_offset.buildFrom(0.0, 0.0, 0.0, 0.0, 0.0, 0.0) ;
double d_t = 0.01;
double d_r = 0.0;
bool first_time_pen_pose = true;
/************************************************************************************************/
/** Initialization drawing*/
// vpHomogeneousMatrix p1(0.03, 0.0, 0.0, 0.0, 0.0, 0.0);
// vpHomogeneousMatrix p2(0.03, 0.03, 0.0, 0.0, 0.0, 0.0);
// vpHomogeneousMatrix p3(0.0, 0.03, 0.0, 0.0, 0.0, 0.0);
// vpHomogeneousMatrix p4(0.0, 0.0, 0.0, 0.0, 0.0, 0.0);
std::vector<vpHomogeneousMatrix> P;
// P.push_back(p1);
// P.push_back(p2);
// P.push_back(p3);
// P.push_back(p4);
for (unsigned int i = 0;i<50;i++ )
{
vpHomogeneousMatrix p1(0.002*i, 0.0, 0.0, 0.0, 0.0, 0.0);
P.push_back(p1);
}
/************************************************************************************************/
vpMouseButton::vpMouseButtonType button;
std::vector<vpHomogeneousMatrix> cMpen_d;
unsigned int index_p = 0;
//AL::ALValue names_head = AL::ALValue::array("NeckYaw","NeckPitch","HeadPitch","HeadRoll","LEyeYaw", "LEyePitch","LEyeYaw", "LEyePitch" );
// AL::ALValue angles_head = AL::ALValue::array(vpMath::rad(1.6), vpMath::rad(28.6), vpMath::rad(6.6), vpMath::rad(4.8), vpMath::rad(0.6) , vpMath::rad(13.6), 0.0, 0.0 );
// float fractionMaxSpeed = 0.1f;
// robot.getProxy()->setAngles(jointHeadNames_tot, angles_head, fractionMaxSpeed);
while(1) {
double loop_time_start = vpTime::measureTimeMs();
g.acquire(cvI);
vpImageConvert::convert(cvI, I);
vpDisplay::display(I);
bool click_done = vpDisplay::getClick(I, button, false);
// if (state < VSpen)
// {
char key[10];
bool ret = vpDisplay::getKeyboardEvent(I, key, false);
std::string s = key;
if (ret)
{
if (s == "r")
{
M_offset.buildFrom(0.0, 0.0, 0.0, 0.0, 0.0, 0.0) ;
d_t = 0.0;
d_r = 0.2;
std::cout << "Rotation mode. " << std::endl;
}
if (s == "t")
{
M_offset.buildFrom(0.0, 0.0, 0.0, 0.0, 0.0, 0.0) ;
d_t = 0.01;
d_r = 0.0;
std::cout << "Translation mode. " << std::endl;
}
if (s == "h")
{
hand_tracker[index_hand]->setManualBlobInit(true);
hand_tracker[index_hand]->setForceDetection(true);
}
if (s == "q")
{
pen_tracker.setFullManual(true);
pen_tracker.setForceDetection(true);
}
else if ( s == "+")
{
unsigned int value = hand_tracker[index_hand]->getGrayLevelMaxBlob() +10;
hand_tracker[index_hand]->setGrayLevelMaxBlob(value);
std::cout << "Set to "<< value << "the value of " << std::endl;
}
else if (s == "-")
{
unsigned int value = hand_tracker[1]->getGrayLevelMaxBlob()-10;
hand_tracker[index_hand]->setGrayLevelMaxBlob(value-10);
std::cout << "Set to "<< value << " GrayLevelMaxBlob. " << std::endl;
}
// |x
// z\ |
// \ |
// \|_____ y
//
// else if (s == "4") //-y
// {
// M_offset.buildFrom(0.0, -d_t, 0.0, 0.0, -d_r, 0.0) ;
// }
// else if (s == "6") //+y
// {
// M_offset.buildFrom(0.0, d_t, 0.0, 0.0, d_r, 0.0) ;
// }
// else if (s == "8") //+x
// {
// M_offset.buildFrom(d_t, 0.0, 0.0, d_r, 0.0, 0.0) ;
// }
// else if (s == "2") //-x
// {
// M_offset.buildFrom(-d_t, 0.0, 0.0, -d_r, 0.0, 0.0) ;
// }
// // else if (s == "7")//-z
// // {
// // M_offset.buildFrom(0.0, 0.0, -d_t, 0.0, 0.0, -d_r) ;
// // }
// // else if (s == "9") //+z
// // {
// // M_offset.buildFrom(0.0, 0.0, d_t, 0.0, 0.0, d_r) ;
// // }
// cMpen_d = cMpen_d[index_p] * M_offset;
}
if (state < WaitPreDraw)
{
status_hand_tracker[index_hand] = hand_tracker[index_hand]->track(cvI,I);
if (status_hand_tracker[index_hand] ) { // display the tracking results
cMo_hand[index_hand] = hand_tracker[index_hand]->get_cMo();
//printPose("cMo qrcode: ", cMo_hand);
// The qrcode frame is only displayed when PBVS is active or learning
vpDisplay::displayFrame(I, cMo_hand[index_hand], cam, 0.04, vpColor::none, 3);
}
}
status_pen_tracker = pen_tracker.track(cvI,I);
if (status_pen_tracker ) { // display the tracking results
cMpen = pen_tracker.get_cMo();
//printPose("cMo qrcode: ", cMo_hand);
// The qrcode frame is only displayed when PBVS is active or learning
vpDisplay::displayFrame(I, cMpen, cam, 0.04, vpColor::none, 3);
}
if (state == CalibratePen && status_hand_tracker[index_hand] )
{
if (status_pen_tracker)
{
vpDisplay::displayText(I, vpImagePoint(I.getHeight() - 10, 10), "Left click to calibrate pen", vpColor::red);
cMpen = pen_tracker.get_cMo();
//printPose("cMo qrcode: ", cMo_hand);
vpDisplay::displayFrame(I, cMpen, cam, 0.04, vpColor::none, 1);
vpHomogeneousMatrix pen_Me_Arm; // Homogeneous matrix from the pen to the left wrist
pen_Mhand[index_hand] = cMpen.inverse() * cMo_hand[index_hand];
pen_Me_Arm = pen_Mhand[index_hand] * hand_Me_Arm[index_hand] ; // from pen to WristPitch
if (click_done && button == vpMouseButton::button1) {
pen_Ve_Arm[index_hand].buildFrom(pen_Me_Arm);
// vpXmlParserHomogeneousMatrix p_; // Create a XML parser
// if (p_.save(hand_M_pen, "transformation_pen.xml", handMpen_name) != vpXmlParserHomogeneousMatrix::SEQUENCE_OK)
// {
// std::cout << "Cannot save the Homogeneous matrix" << std::endl;
// return 0;
// }
// std::cout << "Homogeneous matrix from the hand to the pen saved " << std::endl;
// hand_M_pen.print();
state = WaitPreDraw;
click_done = false;
}
}
}
if (state == WaitPreDraw )
{
vpDisplay::displayText(I, vpImagePoint(I.getHeight() - 10, 10), "Left click to continue", vpColor::red);
status_qrcode_tracker = qrcode_tracker.track(I);
if (status_qrcode_tracker) { // display the tracking results
cM_tab = qrcode_tracker.get_cMo();
//printPose("cMo qrcode: ", cMo_qrcode);
// The qrcode frame is only displayed when PBVS is active or learning
vpDisplay::displayFrame(I, cM_tab, cam, 0.07, vpColor::none, 2);
}
// Point of the pen wrt camera with same orientation of pen target
vpHomogeneousMatrix cM_point = cMpen * pen_M_point;
// Point of the pen wrt camera with same orientation of the table
vpHomogeneousMatrix cMpen_t = cM_tab;
cMpen_t[0][3] = cM_point[0][3];
cMpen_t[1][3] = cM_point[1][3];
cMpen_t[2][3] = cM_point[2][3];
for (unsigned int i = 0; i < P.size(); i++)
{
// Point of the pen wrt camera with table R at P[i]
vpHomogeneousMatrix cMpen_p1 = cMpen_t * P[i];
vpDisplay::displayFrame(I, cMpen_t, cam, 0.03, vpColor::none, 2);
//vpDisplay::displayFrame(I, cMpen_p1, cam, 0.03, vpColor::none, 2);
// Point of the pen wrt camera with pen target R at P[i]
vpHomogeneousMatrix cMpen_p1_rot_hand = cMpen;
cMpen_p1_rot_hand[0][3] = cMpen_p1[0][3];
cMpen_p1_rot_hand[1][3] = cMpen_p1[1][3];
cMpen_p1_rot_hand[2][3] = cMpen_p1[2][3];
//vpDisplay::displayFrame(I, cMpen_p1_rot_hand, cam, 0.03, vpColor::none, 2);
// Desired pose Pen target at P[i]
cMpen_d.push_back( cMpen_p1_rot_hand * pen_M_point.inverse());
vpDisplay::displayFrame(I, cMpen_d[i], cam, 0.01, vpColor::none, 1);
}
if (click_done && button == vpMouseButton::button1 ) {
state = PreDraw;
click_done = false;
}
}
if (state == PreDraw && status_pen_tracker )
{
vpDisplay::displayText(I, vpImagePoint(I.getHeight() - 10, 10), "Left click to start the servo", vpColor::red);
// if (first_time_pen_pose)
// {
// // Compute desired pen position cMpen_d
// cMpen_d = cMpen * M_offset;
// first_time_pen_pose = false;
// }
for (unsigned int i = 0; i < P.size(); i++)
vpDisplay::displayFrame(I, cMpen_d[i], cam, 0.01, vpColor::none, 1);
// vpDisplay::displayFrame(I, cMpen *pen_Mhand[1] , cam, 0.05, vpColor::none, 3);
// vpDisplay::displayFrame(I, cMpen *pen_Mhand[0] , cam, 0.05, vpColor::none, 3);
if (click_done && button == vpMouseButton::button1 ) {
state = VSpen;
click_done = false;
}
}
if (state == VSpen)
{
//Get Actual position of the arm joints
q[0] = robot.getPosition(jointNames_arm[0]);
q[1] = robot.getPosition(jointNames_arm[1]);
// if(status_pen_tracker && status_hand_tracker[index_hand] )
if(status_pen_tracker )
{
if (! grasp_servo_converged[0]) {
// for (unsigned int i = 0; i<2; i++)
// {
unsigned int i = 0;
//vpAdaptiveGain lambda(0.8, 0.05, 8);
// vpAdaptiveGain lambda(0.2, 0.01, 2);
// servo_larm[i]->setLambda(lambda);
servo_larm[i]->setLambda(0.2);
eJe[i] = robot.get_eJe(chain_name[i]);
servo_larm[i]->set_eJe(eJe[i]);
servo_larm[i]->m_task.set_cVe(pen_Ve_Arm[i]);
pen_dMpen[i] = cMpen_d[index_p].inverse() * cMpen;
// printPose("pen_dMpen: ", pen_dMpen[i]);
servo_larm[i]->setCurrentFeature(pen_dMpen[i]) ;
vpDisplay::displayFrame(I, cMpen_d[index_p] , cam, 0.05, vpColor::none, 3);
// vpDisplay::displayFrame(I, cMpen *pen_Mhand[1] , cam, 0.05, vpColor::none, 3);
// vpDisplay::displayFrame(I, cMpen *pen_Mhand[0] , cam, 0.05, vpColor::none, 3);
if (first_time_arm_servo[i]) {
std::cout << "-- Start visual servoing of the arm" << chain_name[i] << "." << std::endl;
servo_arm_time_init[i] = vpTime::measureTimeSecond();
first_time_arm_servo[i] = false;
}
q_dot_arm[i] = - servo_larm[i]->computeControlLaw(servo_arm_time_init[i]);
// Compute joint limit avoidance
q_dot_arm_sec[0] = servo_larm[0]->m_task.secondaryTaskJointLimitAvoidance(q[0], q_dot_real[0], jointMin[0], jointMax[0]);
cpt_iter_servo_grasp[i] ++;
vpColVector q_dot_tot = q_dot_arm[0] + q_dot_arm_sec[0];
robot.setVelocity(jointArmsNames_tot, q_dot_tot);
vpTranslationVector t_error_grasp = pen_dMpen[0].getTranslationVector();
vpRotationMatrix R_error_grasp;
pen_dMpen[0].extract(R_error_grasp);
vpThetaUVector tu_error_grasp;
tu_error_grasp.buildFrom(R_error_grasp);
double theta_error_grasp;
vpColVector u_error_grasp;
tu_error_grasp.extract(theta_error_grasp, u_error_grasp);
std::cout << "error: " << sqrt(t_error_grasp.sumSquare()) << " " << vpMath::deg(theta_error_grasp) << std::endl;
// if (cpt_iter_servo_grasp[0] > 100) {
// vpDisplay::displayText(I, vpImagePoint(10,10), "Cannot converge. Click to continue", vpColor::red);
// }
// double error_t_treshold = 0.007;
// if ( (sqrt(t_error_grasp.sumSquare()) < error_t_treshold) && (theta_error_grasp < vpMath::rad(3)) || (click_done && button == vpMouseButton::button1 /*&& cpt_iter_servo_grasp > 150*/) )
// {
// robot.stop(jointArmsNames_tot);
// state = End;
// grasp_servo_converged[0] = true;
// if (click_done && button == vpMouseButton::button1)
// click_done = false;
// }
}
}
else {
// state grasping but one of the tracker fails
robot.stop(jointArmsNames_tot);
}
}
if (state == End)
{
std::cout<< "End" << std::endl;
}
vpDisplay::flush(I) ;
if (click_done && button == vpMouseButton::button3) { // Quit the loop
break;
}
else if (click_done && button == vpMouseButton::button1) { // Quit the loop
if (index_p < P.size())
index_p ++;
}
//std::cout << "Loop time: " << vpTime::measureTimeMs() - loop_time_start << std::endl;
}
robot.stop(jointArmsNames_tot);
}
int ip_fw_chk(struct iphdr *ip, struct net_device *rif, __u16 *redirport,
struct ip_fw *chain, int policy, int mode)
{
struct ip_fw *f;
struct tcphdr *tcp=(struct tcphdr *)((__u32 *)ip+ip->ihl);
struct udphdr *udp=(struct udphdr *)((__u32 *)ip+ip->ihl);
struct icmphdr *icmp=(struct icmphdr *)((__u32 *)ip+ip->ihl);
__u32 src, dst;
__u16 src_port=0xFFFF, dst_port=0xFFFF, icmp_type=0xFF;
unsigned short f_prt=0, prt;
char notcpsyn=0, notcpack=0, match;
unsigned short offset;
int answer;
unsigned char tosand, tosxor;
/*
* If the chain is empty follow policy. The BSD one
* accepts anything giving you a time window while
* flushing and rebuilding the tables.
*/
src = ip->saddr;
dst = ip->daddr;
/*
* This way we handle fragmented packets.
* we ignore all fragments but the first one
* so the whole packet can't be reassembled.
* This way we relay on the full info which
* stored only in first packet.
*
* Note that this theoretically allows partial packet
* spoofing. Not very dangerous but paranoid people may
* wish to play with this. It also allows the so called
* "fragment bomb" denial of service attack on some types
* of system.
*/
offset = ntohs(ip->frag_off) & IP_OFFSET;
/*
* Don't allow a fragment of TCP 8 bytes in. Nobody
* normal causes this. Its a cracker trying to break
* in by doing a flag overwrite to pass the direction
* checks.
*/
if (offset == 1 && ip->protocol == IPPROTO_TCP)
return FW_BLOCK;
if (offset!=0 && !(mode & (IP_FW_MODE_ACCT_IN|IP_FW_MODE_ACCT_OUT)) &&
(ip->protocol == IPPROTO_TCP || ip->protocol == IPPROTO_UDP ||
ip->protocol == IPPROTO_ICMP))
return FW_ACCEPT;
/*
* Header fragment for TCP is too small to check the bits.
*/
if(ip->protocol==IPPROTO_TCP && (ip->ihl<<2)+16 > ntohs(ip->tot_len))
return FW_BLOCK;
/*
* Too short.
*
* But only too short for a packet with ports...
*/
else if((ntohs(ip->tot_len)<8+(ip->ihl<<2))&&(ip->protocol==IPPROTO_TCP || ip->protocol==IPPROTO_UDP))
return FW_BLOCK;
src = ip->saddr;
dst = ip->daddr;
/*
* If we got interface from which packet came
* we can use the address directly. This is unlike
* 4.4BSD derived systems that have an address chain
* per device. We have a device per address with dummy
* devices instead.
*/
dprintf1("Packet ");
switch(ip->protocol)
{
case IPPROTO_TCP:
dprintf1("TCP ");
/* ports stay 0xFFFF if it is not the first fragment */
if (!offset) {
src_port=ntohs(tcp->source);
dst_port=ntohs(tcp->dest);
if(!tcp->ack && !tcp->rst)
/* We do NOT have ACK, value TRUE */
notcpack=1;
if(!tcp->syn || !notcpack)
/* We do NOT have SYN, value TRUE */
notcpsyn=1;
}
prt=IP_FW_F_TCP;
break;
case IPPROTO_UDP:
dprintf1("UDP ");
/* ports stay 0xFFFF if it is not the first fragment */
if (!offset) {
src_port=ntohs(udp->source);
dst_port=ntohs(udp->dest);
}
prt=IP_FW_F_UDP;
break;
case IPPROTO_ICMP:
/* icmp_type stays 255 if it is not the first fragment */
if (!offset)
icmp_type=(__u16)(icmp->type);
dprintf2("ICMP:%d ",icmp_type);
prt=IP_FW_F_ICMP;
break;
default:
dprintf2("p=%d ",ip->protocol);
prt=IP_FW_F_ALL;
break;
}
#ifdef DEBUG_IP_FIREWALL
dprint_ip(ip->saddr);
if (ip->protocol==IPPROTO_TCP || ip->protocol==IPPROTO_UDP)
/* This will print 65535 when it is not the first fragment! */
dprintf2(":%d ", src_port);
dprint_ip(ip->daddr);
if (ip->protocol==IPPROTO_TCP || ip->protocol==IPPROTO_UDP)
/* This will print 65535 when it is not the first fragment! */
dprintf2(":%d ",dst_port);
dprintf1("\n");
#endif
for (f=chain;f;f=f->fw_next)
{
/*
* This is a bit simpler as we don't have to walk
* an interface chain as you do in BSD - same logic
* however.
*/
/*
* Match can become 0x01 (a "normal" match was found),
* 0x02 (a reverse match was found), and 0x03 (the
* IP addresses match in both directions).
* Now we know in which direction(s) we should look
* for a match for the TCP/UDP ports. Both directions
* might match (e.g., when both addresses are on the
* same network for which an address/mask is given), but
* the ports might only match in one direction.
* This was obviously wrong in the original BSD code.
*/
match = 0x00;
if ((src&f->fw_smsk.s_addr)==f->fw_src.s_addr
&& (dst&f->fw_dmsk.s_addr)==f->fw_dst.s_addr)
/* normal direction */
match |= 0x01;
if ((f->fw_flg & IP_FW_F_BIDIR) &&
(dst&f->fw_smsk.s_addr)==f->fw_src.s_addr
&& (src&f->fw_dmsk.s_addr)==f->fw_dst.s_addr)
/* reverse direction */
match |= 0x02;
if (!match)
continue;
/*
* Look for a VIA device match
*/
if(f->fw_viadev)
{
if(rif!=f->fw_viadev)
continue; /* Mismatch */
}
/* This looks stupid, because we scan almost static
list, searching for static key. However, this way seems
to be only reasonable way of handling fw_via rules
(btw bsd makes the same thing).
It will not affect performance if you will follow
the following simple rules:
- if inteface is aliased, ALWAYS specify fw_viadev,
so that previous check will guarantee, that we will
not waste time when packet arrive on another interface.
- avoid using fw_via.s_addr if fw_via.s_addr is owned
by an aliased interface.
--ANK
*/
if (f->fw_via.s_addr && rif) {
struct in_ifaddr *ifa;
if (rif->ip_ptr == NULL)
continue; /* Mismatch */
for (ifa = ((struct in_device*)(rif->ip_ptr))->ifa_list;
ifa; ifa = ifa->ifa_next) {
if (ifa->ifa_local == f->fw_via.s_addr)
goto ifa_ok;
}
continue; /* Mismatch */
ifa_ok:;
}
/*
* Ok the chain addresses match.
*/
#ifdef CONFIG_IP_ACCT
/*
* See if we're in accounting mode and only want to
* count incoming or outgoing packets.
*/
if (mode & (IP_FW_MODE_ACCT_IN|IP_FW_MODE_ACCT_OUT) &&
((mode == IP_FW_MODE_ACCT_IN && f->fw_flg&IP_FW_F_ACCTOUT) ||
(mode == IP_FW_MODE_ACCT_OUT && f->fw_flg&IP_FW_F_ACCTIN)))
continue;
#endif
/*
* For all non-TCP packets and/or non-first fragments,
* notcpsyn and notcpack will always be FALSE,
* so the IP_FW_F_TCPSYN and IP_FW_F_TCPACK flags
* are actually ignored for these packets.
*/
if((f->fw_flg&IP_FW_F_TCPSYN) && notcpsyn)
continue;
if((f->fw_flg&IP_FW_F_TCPACK) && notcpack)
continue;
f_prt=f->fw_flg&IP_FW_F_KIND;
if (f_prt!=IP_FW_F_ALL)
{
/*
* Specific firewall - packet's protocol
* must match firewall's.
*/
if(prt!=f_prt)
continue;
if((prt==IP_FW_F_ICMP &&
! port_match(&f->fw_pts[0], f->fw_nsp,
icmp_type,f->fw_flg&IP_FW_F_SRNG)) ||
!(prt==IP_FW_F_ICMP || ((match & 0x01) &&
port_match(&f->fw_pts[0], f->fw_nsp, src_port,
f->fw_flg&IP_FW_F_SRNG) &&
port_match(&f->fw_pts[f->fw_nsp], f->fw_ndp, dst_port,
f->fw_flg&IP_FW_F_DRNG)) || ((match & 0x02) &&
port_match(&f->fw_pts[0], f->fw_nsp, dst_port,
f->fw_flg&IP_FW_F_SRNG) &&
port_match(&f->fw_pts[f->fw_nsp], f->fw_ndp, src_port,
f->fw_flg&IP_FW_F_DRNG))))
{
continue;
}
}
#ifdef CONFIG_IP_FIREWALL_VERBOSE
if (f->fw_flg & IP_FW_F_PRN)
{
char buf[16];
print_packet(ip, src_port, dst_port, icmp_type,
chain_name(chain, mode),
rule_name(f, mode, buf),
rif ? rif->name : "-");
}
#endif
if (mode != IP_FW_MODE_CHK) {
f->fw_bcnt+=ntohs(ip->tot_len);
f->fw_pcnt++;
}
if (!(mode & (IP_FW_MODE_ACCT_IN|IP_FW_MODE_ACCT_OUT)))
break;
} /* Loop */
if (!(mode & (IP_FW_MODE_ACCT_IN|IP_FW_MODE_ACCT_OUT))) {
/*
* We rely on policy defined in the rejecting entry or, if no match
* was found, we rely on the general policy variable for this type
* of firewall.
*/
if (f!=NULL) {
policy=f->fw_flg;
tosand=f->fw_tosand;
tosxor=f->fw_tosxor;
} else {
tosand=0xFF;
tosxor=0x00;
}
if (policy&IP_FW_F_ACCEPT) {
/* Adjust priority and recompute checksum */
__u8 old_tos = ip->tos;
ip->tos = (old_tos & tosand) ^ tosxor;
if (ip->tos != old_tos)
ip_send_check(ip);
#ifdef CONFIG_IP_TRANSPARENT_PROXY
if (policy&IP_FW_F_REDIR) {
if (redirport)
if ((*redirport = htons(f->fw_pts[f->fw_nsp+f->fw_ndp])) == 0) {
/* Wildcard redirection.
* Note that redirport will become
* 0xFFFF for non-TCP/UDP packets.
*/
*redirport = htons(dst_port);
}
answer = FW_REDIRECT;
} else
#endif
#ifdef CONFIG_IP_MASQUERADE
if (policy&IP_FW_F_MASQ)
answer = FW_MASQUERADE;
else
#endif
answer = FW_ACCEPT;
} else if(policy&IP_FW_F_ICMPRPL)
answer = FW_REJECT;
else
answer = FW_BLOCK;
#ifdef CONFIG_IP_FIREWALL_NETLINK
if((policy&IP_FW_F_PRN) && (answer == FW_REJECT || answer == FW_BLOCK))
{
struct sk_buff *skb=alloc_skb(128, GFP_ATOMIC);
if(skb)
{
int len = min_t(unsigned int,
128, ntohs(ip->tot_len));
skb_put(skb,len);
memcpy(skb->data,ip,len);
if(netlink_post(NETLINK_FIREWALL, skb))
kfree_skb(skb);
}
}
#endif
return answer;
} else
/* we're doing accounting, always ok */
return 0;
int main(int argc, const char* argv[])
{
std::string opt_ip = "198.18.0.1";;
std::string opt_data_folder = std::string(ROMEOTK_DATA_FOLDER);
bool opt_Reye = false;
// Learning folder in /tmp/$USERNAME
std::string username;
// Get the user login name
vpIoTools::getUserName(username);
// Create a log filename to save new files...
std::string learning_folder;
#if defined(_WIN32)
learning_folder ="C:/temp/" + username;
#else
learning_folder ="/tmp/" + username;
#endif
// Test if the output path exist. If no try to create it
if (vpIoTools::checkDirectory(learning_folder) == false) {
try {
// Create the dirname
vpIoTools::makeDirectory(learning_folder);
}
catch (vpException &e) {
std::cout << "Cannot create " << learning_folder << std::endl;
std::cout << "Error: " << e.getMessage() <<std::endl;
return 0;
}
}
for (unsigned int i=0; i<argc; i++) {
if (std::string(argv[i]) == "--ip")
opt_ip = argv[i+1];
else if ( std::string(argv[i]) == "--reye")
opt_Reye = true;
else if (std::string(argv[i]) == "--help") {
std::cout << "Usage: " << argv[0] << "[--ip <robot address>]" << std::endl;
return 0;
}
}
std::vector<std::string> chain_name(2); // LArm or RArm
chain_name[0] = "LArm";
chain_name[1] = "RArm";
/************************************************************************************************/
/** Open the grabber for the acquisition of the images from the robot*/
vpNaoqiGrabber g;
g.setFramerate(15);
// Initialize constant transformations
vpHomogeneousMatrix eMc;
if (opt_Reye)
{
std::cout << "Using camera Eye Right" << std::endl;
g.setCamera(3); // CameraRightEye
eMc = g.get_eMc(vpCameraParameters::perspectiveProjWithDistortion,"CameraRightEye");
}
else
{
std::cout << "Using camera Eye Right" << std::endl;
g.setCamera(2); // CameraLeftEye
eMc = g.get_eMc(vpCameraParameters::perspectiveProjWithDistortion,"CameraLeftEye");
}
std::cout << "eMc: " << eMc << std::endl;
if (! opt_ip.empty())
g.setRobotIp(opt_ip);
g.open();
std::string camera_name = g.getCameraName();
std::cout << "Camera name: " << camera_name << std::endl;
vpCameraParameters cam = g.getCameraParameters(vpCameraParameters::perspectiveProjWithDistortion);
std::cout << "Camera parameters: " << cam << std::endl;
/** Initialization Visp Image, display and camera paramenters*/
vpImage<unsigned char> I(g.getHeight(), g.getWidth());
vpDisplayX d(I);
vpDisplay::setTitle(I, "Camera view");
//Initialize opencv color image
cv::Mat cvI = cv::Mat(cv::Size(g.getWidth(), g.getHeight()), CV_8UC3);
/************************************************************************************************/
/** Initialization target box*/
std::string opt_name_file_color_box_target = opt_data_folder + "/" +"target/plate_blob/color.txt";
vpBlobsTargetTracker box_tracker;
bool status_box_tracker;
vpHomogeneousMatrix cMbox;
const double L = 0.04/2;
std::vector <vpPoint> points(4);
points[2].setWorldCoordinates(-L,-L, 0) ;
points[1].setWorldCoordinates(-L,L, 0) ;
points[0].setWorldCoordinates(L,L, 0) ;
points[3].setWorldCoordinates(L,-L,0) ;
box_tracker.setName("plate");
box_tracker.setCameraParameters(cam);
box_tracker.setPoints(points);
box_tracker.setGrayLevelMaxBlob(60);
box_tracker.setLeftHandTarget(false);
if(!box_tracker.loadHSV(opt_name_file_color_box_target))
{
std::cout << "Error opening the file "<< opt_name_file_color_box_target << std::endl;
}
/************************************************************************************************/
/** Initialization target hands*/
std::string opt_name_file_color_target_path = opt_data_folder + "/" +"target/";
std::string opt_name_file_color_target_l = opt_name_file_color_target_path + chain_name[0] +"/color.txt";
std::string opt_name_file_color_target_r = opt_name_file_color_target_path + chain_name[1] +"/color.txt";
std::string opt_name_file_color_target1_l = opt_name_file_color_target_path + chain_name[0] +"/color1.txt";
std::string opt_name_file_color_target1_r = opt_name_file_color_target_path + chain_name[1] +"/color1.txt";
std::vector<vpBlobsTargetTracker*> hand_tracker;
std::vector<bool> status_hand_tracker(2);
std::vector<vpHomogeneousMatrix> cMo_hand(2);
const double L1 = 0.025/2;
std::vector <vpPoint> points1(4);
points1[2].setWorldCoordinates(-L1,-L1, 0) ;
points1[1].setWorldCoordinates(-L1,L1, 0) ;
points1[0].setWorldCoordinates(L1,L1, 0) ;
points1[3].setWorldCoordinates(L1,-L1,0) ;
vpBlobsTargetTracker hand_tracker_l;
hand_tracker_l.setName(chain_name[0]);
hand_tracker_l.setCameraParameters(cam);
hand_tracker_l.setPoints(points1);
hand_tracker_l.setLeftHandTarget(true);
if(!hand_tracker_l.loadHSV(opt_name_file_color_target_l))
std::cout << "Error opening the file "<< opt_name_file_color_target_l << std::endl;
vpBlobsTargetTracker hand_tracker_r;
hand_tracker_r.setName(chain_name[1]);
hand_tracker_r.setCameraParameters(cam);
hand_tracker_r.setPoints(points);
hand_tracker_r.setLeftHandTarget(false);
if(!hand_tracker_r.loadHSV(opt_name_file_color_target1_r))
std::cout << "Error opening the file "<< opt_name_file_color_target_r << std::endl;
hand_tracker.push_back(&hand_tracker_l);
hand_tracker.push_back(&hand_tracker_r);
/************************************************************************************************/
// Constant transformation Target Frame to Arm end-effector (WristPitch)
std::vector<vpHomogeneousMatrix> hand_Me_Arm(2);
std::string filename_transform = std::string(ROMEOTK_DATA_FOLDER) + "/transformation.xml";
// Create twist matrix from box to Arm end-effector (WristPitch)
std::vector <vpVelocityTwistMatrix> box_Ve_Arm(2);
// Create twist matrix from target Frame to Arm end-effector (WristPitch)
std::vector <vpVelocityTwistMatrix> hand_Ve_Arm(2);
// Constant transformation Target Frame to box to target Hand
std::vector<vpHomogeneousMatrix> box_Mhand(2);
for (unsigned int i = 0; i < 2; i++)
{
std::string name_transform = "qrcode_M_e_" + chain_name[i];
vpXmlParserHomogeneousMatrix pm; // Create a XML parser
if (pm.parse(hand_Me_Arm[i], filename_transform, name_transform) != vpXmlParserHomogeneousMatrix::SEQUENCE_OK) {
std::cout << "Cannot found the homogeneous matrix named " << name_transform << "." << std::endl;
return 0;
}
else
std::cout << "Homogeneous matrix " << name_transform <<": " << std::endl << hand_Me_Arm[i] << std::endl;
hand_Ve_Arm[i].buildFrom(hand_Me_Arm[i]);
}
/************************************************************************************************/
/** Create a new istance NaoqiRobot*/
vpNaoqiRobot robot;
if (! opt_ip.empty())
robot.setRobotIp(opt_ip);
robot.open();
std::vector<std::string> jointNames = robot.getBodyNames("Head");
//jointNames.pop_back(); // We don't consider the last joint of the head = HeadRoll
std::vector<std::string> jointNamesLEye = robot.getBodyNames("LEye");
std::vector<std::string> jointNamesREye = robot.getBodyNames("REye");
jointNames.insert(jointNames.end(), jointNamesLEye.begin(), jointNamesLEye.end());
std::vector<std::string> jointHeadNames_tot = jointNames;
jointHeadNames_tot.push_back(jointNamesREye.at(0));
jointHeadNames_tot.push_back(jointNamesREye.at(1));
/************************************************************************************************/
std::vector<bool> grasp_servo_converged(2);
grasp_servo_converged[0]= false;
grasp_servo_converged[1]= false;
std::vector<vpMatrix> eJe(2);
// Initialize arms servoing
vpServoArm servo_larm_l;
vpServoArm servo_larm_r;
std::vector<vpServoArm*> servo_larm;
servo_larm.push_back(&servo_larm_l);
servo_larm.push_back(&servo_larm_r);
std::vector < std::vector<std::string > > jointNames_arm(2);
jointNames_arm[0] = robot.getBodyNames(chain_name[0]);
jointNames_arm[1] = robot.getBodyNames(chain_name[1]);
// Delete last joint Hand, that we don't consider in the servo
jointNames_arm[0].pop_back();
jointNames_arm[1].pop_back();
std::vector<std::string> jointArmsNames_tot = jointNames_arm[0];
jointArmsNames_tot.insert(jointArmsNames_tot.end(), jointNames_arm[1].begin(), jointNames_arm[1].end());
const unsigned int numArmJoints = jointNames_arm[0].size();
std::vector<vpHomogeneousMatrix> box_dMbox(2);
// Vector containing the joint velocity vectors of the arms
std::vector<vpColVector> q_dot_arm;
// Vector containing the joint velocity vectors of the arms for the secondary task
std::vector<vpColVector> q_dot_arm_sec;
// Vector joint position of the arms
std::vector<vpColVector> q;
// Vector joint real velocities of the arms
std::vector<vpColVector> q_dot_real;
vpColVector q_temp(numArmJoints);
q_dot_arm.push_back(q_temp);
q_dot_arm.push_back(q_temp);
q_dot_arm_sec.push_back(q_temp);
q_dot_arm_sec.push_back(q_temp);
q.push_back(q_temp);
q.push_back(q_temp);
q_dot_real.push_back(q_temp);
q_dot_real.push_back(q_temp);
// Initialize the joint avoidance scheme from the joint limits
std::vector<vpColVector> jointMin;
std::vector<vpColVector> jointMax;
jointMin.push_back(q_temp);
jointMin.push_back(q_temp);
jointMax.push_back(q_temp);
jointMax.push_back(q_temp);
for (unsigned int i = 0; i< 2; i++)
{
jointMin[i] = robot.getJointMin(chain_name[i]);
jointMax[i] = robot.getJointMax(chain_name[i]);
jointMin[i].resize(numArmJoints,false);
jointMax[i].resize(numArmJoints,false);
// std::cout << jointMin[i].size() << std::endl;
// std::cout << jointMax[i].size() << std::endl;
// std::cout << "max " << jointMax[i] << std::endl;
}
std::vector<bool> first_time_arm_servo(2);
first_time_arm_servo[0] = true;
first_time_arm_servo[1] = true;
std::vector< double> servo_arm_time_init(2);
servo_arm_time_init[0] = 0.0;
servo_arm_time_init[1] = 0.0;
std::vector<int> cpt_iter_servo_grasp(2);
cpt_iter_servo_grasp[0] = 0;
cpt_iter_servo_grasp[1] = 0;
unsigned int index_hand = 1;
// vpHomogeneousMatrix M_offset;
// M_offset[1][3] = 0.00;
// M_offset[0][3] = 0.00;
// M_offset[2][3] = 0.00;
vpHomogeneousMatrix M_offset;
M_offset.buildFrom(0.0, 0.0, 0.0, 0.0, 0.0, 0.0) ;
double d_t = 0.01;
double d_r = 0.0;
bool first_time_box_pose = true;
/************************************************************************************************/
vpMouseButton::vpMouseButtonType button;
vpHomogeneousMatrix elMb; // Homogeneous matrix from right wrist roll to box
vpHomogeneousMatrix cMbox_d; // Desired box final pose
State_t state;
state = CalibrateRigthArm;
//AL::ALValue names_head = AL::ALValue::array("NeckYaw","NeckPitch","HeadPitch","HeadRoll","LEyeYaw", "LEyePitch","REyeYaw", "REyePitch" );
// Big Plate
// AL::ALValue angles_head = AL::ALValue::array(vpMath::rad(-23.2), vpMath::rad(16.6), vpMath::rad(10.3), vpMath::rad(0.0), 0.0 , 0.0, 0.0, 0.0 );
// small plate
AL::ALValue angles_head = AL::ALValue::array(vpMath::rad(-15.2), vpMath::rad(17.6), vpMath::rad(10.3), vpMath::rad(0.0), 0.0 , vpMath::rad(9.8), 0.0, 0.0 );
float fractionMaxSpeed = 0.1f;
robot.getProxy()->setAngles(jointHeadNames_tot, angles_head, fractionMaxSpeed);
while(1) {
double loop_time_start = vpTime::measureTimeMs();
g.acquire(cvI);
vpImageConvert::convert(cvI, I);
vpDisplay::display(I);
bool click_done = vpDisplay::getClick(I, button, false);
// if (state < VSBox)
// {
char key[10];
bool ret = vpDisplay::getKeyboardEvent(I, key, false);
std::string s = key;
if (ret)
{
if (s == "r")
{
M_offset.buildFrom(0.0, 0.0, 0.0, 0.0, 0.0, 0.0) ;
d_t = 0.0;
d_r = 0.2;
std::cout << "Rotation mode. " << std::endl;
}
if (s == "t")
{
M_offset.buildFrom(0.0, 0.0, 0.0, 0.0, 0.0, 0.0) ;
d_t = 0.01;
d_r = 0.0;
std::cout << "Translation mode. " << std::endl;
}
if (s == "h")
{
hand_tracker[index_hand]->setManualBlobInit(true);
hand_tracker[index_hand]->setForceDetection(true);
}
else if ( s == "+")
{
unsigned int value = hand_tracker[index_hand]->getGrayLevelMaxBlob() +10;
hand_tracker[index_hand]->setGrayLevelMaxBlob(value);
std::cout << "Set to "<< value << "the value of " << std::endl;
}
else if (s == "-")
{
unsigned int value = hand_tracker[1]->getGrayLevelMaxBlob()-10;
hand_tracker[index_hand]->setGrayLevelMaxBlob(value-10);
std::cout << "Set to "<< value << " GrayLevelMaxBlob. " << std::endl;
}
// |x
// z\ |
// \ |
// \|_____ y
//
else if (s == "4") //-y
{
M_offset.buildFrom(0.0, -d_t, 0.0, 0.0, -d_r, 0.0) ;
}
else if (s == "6") //+y
{
M_offset.buildFrom(0.0, d_t, 0.0, 0.0, d_r, 0.0) ;
}
else if (s == "8") //+x
{
M_offset.buildFrom(d_t, 0.0, 0.0, d_r, 0.0, 0.0) ;
}
else if (s == "2") //-x
{
M_offset.buildFrom(-d_t, 0.0, 0.0, -d_r, 0.0, 0.0) ;
}
else if (s == "7")//-z
{
M_offset.buildFrom(0.0, 0.0, -d_t, 0.0, 0.0, -d_r) ;
}
else if (s == "9") //+z
{
M_offset.buildFrom(0.0, 0.0, d_t, 0.0, 0.0, d_r) ;
}
cMbox_d = cMbox_d * M_offset;
}
if (state < WaitPreGrasp)
{
status_hand_tracker[index_hand] = hand_tracker[index_hand]->track(cvI,I);
if (status_hand_tracker[index_hand] ) { // display the tracking results
cMo_hand[index_hand] = hand_tracker[index_hand]->get_cMo();
printPose("cMo right arm: ", cMo_hand[index_hand]);
// The qrcode frame is only displayed when PBVS is active or learning
vpDisplay::displayFrame(I, cMo_hand[index_hand], cam, 0.04, vpColor::none, 3);
}
}
// }
status_box_tracker = box_tracker.track(cvI,I);
if (status_box_tracker ) { // display the tracking results
cMbox = box_tracker.get_cMo();
printPose("cMo box: ", cMbox);
// The qrcode frame is only displayed when PBVS is active or learning
vpDisplay::displayFrame(I, cMbox, cam, 0.04, vpColor::none, 1);
// if (first_time_box_pose)
// {
// // Compute desired box position cMbox_d
// cMbox_d = cMbox * M_offset;
// first_time_box_pose = false;
// }
// vpDisplay::displayFrame(I, cMbox_d, cam, 0.04, vpColor::red, 1);
}
if (state == CalibrateRigthArm && status_box_tracker && status_hand_tracker[index_hand] )
{
vpDisplay::displayText(I, vpImagePoint(I.getHeight() - 10, 10), "Left click to calibrate right hand", vpColor::red);
vpHomogeneousMatrix box_Me_Arm; // Homogeneous matrix from the box to the left wrist
box_Mhand[index_hand] = cMbox.inverse() * cMo_hand[index_hand];
box_Me_Arm = box_Mhand[index_hand] * hand_Me_Arm[index_hand] ; // from box to WristPitch
// vpDisplay::displayFrame(I, cMo_hand[index_hand] * (cMbox.inverse() * cMo_hand[index_hand]).inverse() , cam, 0.04, vpColor::green, 1);
if (click_done && button == vpMouseButton::button1 ) {
box_Ve_Arm[index_hand].buildFrom(box_Me_Arm);
index_hand = 0;
state = CalibrateLeftArm;
// BIG plate
//AL::ALValue names_head = AL::ALValue::array("NeckYaw","NeckPitch","HeadPitch","HeadRoll","LEyeYaw", "LEyePitch","LEyeYaw", "LEyePitch" );
// AL::ALValue angles_head = AL::ALValue::array(vpMath::rad(8.7), vpMath::rad(16.6), vpMath::rad(10.3), vpMath::rad(0.0), 0.0 , 0.0, 0.0, 0.0 );
// Small Plate
AL::ALValue angles_head = AL::ALValue::array(vpMath::rad(2.4), vpMath::rad(17.6), vpMath::rad(10.3), vpMath::rad(0.0), 0.0 , vpMath::rad(9.8), 0.0, 0.0 );
float fractionMaxSpeed = 0.1f;
robot.getProxy()->setAngles(jointHeadNames_tot, angles_head, fractionMaxSpeed);
click_done = false;
}
}
if (state == CalibrateLeftArm && status_box_tracker && status_hand_tracker[index_hand] )
{
vpDisplay::displayText(I, vpImagePoint(I.getHeight() - 10, 10), "Left click to calibrate left hand", vpColor::red);
vpHomogeneousMatrix box_Me_Arm; // Homogeneous matrix from the box to the left wrist
box_Mhand[index_hand] = cMbox.inverse() * cMo_hand[index_hand];
box_Me_Arm = box_Mhand[index_hand] * hand_Me_Arm[index_hand] ; // from box to WristPitch
// if (first_time_box_pose)
// {
// // Compute desired box position cMbox_d
// cMbox_d = cMbox * M_offset;
// first_time_box_pose = false;
// }
// vpDisplay::displayFrame(I, cMbox_d, cam, 0.04, vpColor::red, 1);
// vpDisplay::displayFrame(I, cMo_hand[index_hand] * (cMbox.inverse() * cMo_hand[index_hand]).inverse() , cam, 0.04, vpColor::green, 1);
if (click_done && button == vpMouseButton::button1 ) {
box_Ve_Arm[index_hand].buildFrom(box_Me_Arm);
state = WaitPreGrasp;
click_done = false;
//AL::ALValue names_head = AL::ALValue::array("NeckYaw","NeckPitch","HeadPitch","HeadRoll","LEyeYaw", "LEyePitch","LEyeYaw", "LEyePitch" );
AL::ALValue angles_head = AL::ALValue::array(vpMath::rad(-6.8), vpMath::rad(16.6), vpMath::rad(10.3), vpMath::rad(0.0), 0.0 , 0.0, 0.0, 0.0 );
float fractionMaxSpeed = 0.05f;
robot.getProxy()->setAngles(jointHeadNames_tot, angles_head, fractionMaxSpeed);
}
}
if (state == WaitPreGrasp )
{
index_hand = 1;
if (click_done && button == vpMouseButton::button1 ) {
state = PreGraps;
click_done = false;
}
}
if (state == PreGraps && status_box_tracker )
{
vpDisplay::displayText(I, vpImagePoint(I.getHeight() - 10, 10), "Left click to start the servo", vpColor::red);
if (first_time_box_pose)
{
// Compute desired box position cMbox_d
cMbox_d = cMbox * M_offset;
first_time_box_pose = false;
}
vpDisplay::displayFrame(I, cMbox_d , cam, 0.05, vpColor::none, 3);
vpDisplay::displayFrame(I, cMbox *box_Mhand[1] , cam, 0.05, vpColor::none, 3);
vpDisplay::displayFrame(I, cMbox *box_Mhand[0] , cam, 0.05, vpColor::none, 3);
if (click_done && button == vpMouseButton::button1 ) {
state = VSBox;
click_done = false;
hand_tracker[index_hand] = &box_tracker; // Trick to use keys
}
}
if (state == VSBox)
{
//Get Actual position of the arm joints
q[0] = robot.getPosition(jointNames_arm[0]);
q[1] = robot.getPosition(jointNames_arm[1]);
// if(status_box_tracker && status_hand_tracker[index_hand] )
if(status_box_tracker )
{
if (! grasp_servo_converged[0]) {
// for (unsigned int i = 0; i<2; i++)
// {
unsigned int i = 0;
//vpAdaptiveGain lambda(0.8, 0.05, 8);
vpAdaptiveGain lambda(0.4, 0.02, 4);
//servo_larm[i]->setLambda(lambda);
servo_larm[i]->setLambda(0.2);
eJe[i] = robot.get_eJe(chain_name[i]);
servo_larm[i]->set_eJe(eJe[i]);
servo_larm[i]->m_task.set_cVe(box_Ve_Arm[i]);
box_dMbox[i] = cMbox_d.inverse() * cMbox;
printPose("box_dMbox: ", box_dMbox[i]);
servo_larm[i]->setCurrentFeature(box_dMbox[i]) ;
vpDisplay::displayFrame(I, cMbox_d , cam, 0.05, vpColor::none, 3);
// vpDisplay::displayFrame(I, cMbox *box_Mhand[1] , cam, 0.05, vpColor::none, 3);
// vpDisplay::displayFrame(I, cMbox *box_Mhand[0] , cam, 0.05, vpColor::none, 3);
if (first_time_arm_servo[i]) {
std::cout << "-- Start visual servoing of the arm" << chain_name[i] << "." << std::endl;
servo_arm_time_init[i] = vpTime::measureTimeSecond();
first_time_arm_servo[i] = false;
}
q_dot_arm[i] = - servo_larm[i]->computeControlLaw(servo_arm_time_init[i]);
q_dot_real[0] = robot.getJointVelocity(jointNames_arm[0]);
q_dot_real[1] = robot.getJointVelocity(jointNames_arm[1]);
// std::cout << "real_q: " << std::endl << real_q << std::endl;
// std::cout << " box_Ve_Arm[i]: " << std::endl << box_Ve_Arm[i] << std::endl;
// std::cout << " eJe[i][i]: " << std::endl << eJe[i] << std::endl;
//vpColVector real_v = (box_Ve_Arm[i] * eJe[i]) * q_dot_real[0];
vpColVector real_v = (box_Ve_Arm[i] * eJe[i]) * q_dot_arm[0];
// std::cout << "real_v: " << std::endl <<real_v << std::endl;
// vpVelocityTwistMatrix cVo(cMo_hand);
// vpMatrix cJe = cVo * oJo;
// Compute the feed-forward terms
// vpColVector sec_ter = 0.5 * ((servo_head.m_task_head.getTaskJacobianPseudoInverse() * (servo_head.m_task_head.getInteractionMatrix() * cJe)) * q_dot_larm);
// std::cout <<"Second Term:" <<sec_ter << std::endl;
//q_dot_head = q_dot_head + sec_ter;
// Compute joint limit avoidance
q_dot_arm_sec[0] = servo_larm[0]->m_task.secondaryTaskJointLimitAvoidance(q[0], q_dot_real[0], jointMin[0], jointMax[0]);
//q_dot_arm_sec[1] = servo_larm[1]->m_task.secondaryTaskJointLimitAvoidance(q[1], q_dot_real[1], jointMin[1], jointMax[1]);
// vpColVector q_dot_arm_head = q_dot_larm + q2_dot;
//q_dot_arm_head.stack(q_dot_tot);
// robot.setVelocity(joint_names_arm_head,q_dot_arm_head);
//robot.setVelocity(jointNames_arm[i], q_dot_larm);
// if (opt_plotter_arm) {
// plotter_arm->plot(0, cpt_iter_servo_grasp, servo_larm.m_task.getError());
// plotter_arm->plot(1, cpt_iter_servo_grasp, q_dot_larm);
// }
// if (opt_plotter_q_sec_arm)
// {
// plotter_q_sec_arm->plot(0,loop_iter,q2_dot);
// plotter_q_sec_arm->plot(1,loop_iter,q_dot_larm + q2_dot);
// }
cpt_iter_servo_grasp[i] ++;
// }
// // Visual servoing slave
// i = 1;
// //vpAdaptiveGain lambda(0.4, 0.02, 4);
// servo_larm[i]->setLambda(0.07);
// servo_larm[i]->set_eJe(robot.get_eJe(chain_name[i]));
// servo_larm[i]->m_task.set_cVe(hand_Ve_Arm[i]);
// box_dMbox[i] = (cMbox *box_Mhand[1]).inverse() * cMo_hand[1] ;
// printPose("box_dMbox: ", box_dMbox[i]);
// servo_larm[i]->setCurrentFeature(box_dMbox[i]) ;
// vpDisplay::displayFrame(I, cMbox_d , cam, 0.025, vpColor::red, 2);
// if (first_time_arm_servo[i]) {
// std::cout << "-- Start visual servoing of the arm" << chain_name[i] << "." << std::endl;
// servo_arm_time_init[i] = vpTime::measureTimeSecond();
// first_time_arm_servo[i] = false;
// }
// q_dot_arm[i] = - servo_larm[i]->computeControlLaw(servo_arm_time_init[i]);
eJe[1] = robot.get_eJe(chain_name[1]);
// q_dot_arm[1] += (box_Ve_Arm[1] * eJe[1]).pseudoInverse() * real_v;
q_dot_arm[1] = (box_Ve_Arm[1] * eJe[1]).pseudoInverse() * real_v;
vpColVector q_dot_tot = q_dot_arm[0] + q_dot_arm_sec[0];
q_dot_tot.stack( q_dot_arm[1] + q_dot_arm_sec[1]);
robot.setVelocity(jointArmsNames_tot, q_dot_tot);
vpTranslationVector t_error_grasp = box_dMbox[0].getTranslationVector();
vpRotationMatrix R_error_grasp;
box_dMbox[0].extract(R_error_grasp);
vpThetaUVector tu_error_grasp;
tu_error_grasp.buildFrom(R_error_grasp);
double theta_error_grasp;
vpColVector u_error_grasp;
tu_error_grasp.extract(theta_error_grasp, u_error_grasp);
std::cout << "error: " << t_error_grasp << " " << vpMath::deg(theta_error_grasp) << std::endl;
// if (cpt_iter_servo_grasp[0] > 100) {
// vpDisplay::displayText(I, vpImagePoint(10,10), "Cannot converge. Click to continue", vpColor::red);
// }
// double error_t_treshold = 0.007;
// if ( (sqrt(t_error_grasp.sumSquare()) < error_t_treshold) && (theta_error_grasp < vpMath::rad(3)) || (click_done && button == vpMouseButton::button1 /*&& cpt_iter_servo_grasp > 150*/) )
// {
// robot.stop(jointArmsNames_tot);
// state = End;
// grasp_servo_converged[0] = true;
// if (click_done && button == vpMouseButton::button1)
// click_done = false;
// }
}
}
else {
// state grasping but one of the tracker fails
robot.stop(jointArmsNames_tot);
}
}
if (state == End)
{
std::cout<< "End" << std::endl;
}
vpDisplay::flush(I) ;
if (click_done && button == vpMouseButton::button3) { // Quit the loop
break;
}
//std::cout << "Loop time: " << vpTime::measureTimeMs() - loop_time_start << std::endl;
}
robot.stop(jointArmsNames_tot);
}
