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 main(int argc, const char* argv[])
{
std::string opt_ip = "131.254.10.126";
bool opt_language_english = true;
bool opt_debug = false;
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]) == "--fr")
opt_language_english = false;
else if (std::string(argv[i]) == "--debug")
opt_debug = true;
else if (std::string(argv[i]) == "--help") {
std::cout << "Usage: " << argv[0] << "[--ip <robot address>] [--fr]" << std::endl;
return 0;
}
}
std::string camera_name = "CameraTopPepper";
// Open the grabber for the acquisition of the images from the robot
vpNaoqiGrabber g;
if (! opt_ip.empty())
g.setRobotIp(opt_ip);
g.setFramerate(30);
g.setCamera(0);
g.open();
vpCameraParameters cam = vpNaoqiGrabber::getIntrinsicCameraParameters(AL::kQVGA,camera_name, vpCameraParameters::perspectiveProjWithDistortion);
vpHomogeneousMatrix eMc = vpNaoqiGrabber::getExtrinsicCameraParameters(camera_name,vpCameraParameters::perspectiveProjWithDistortion);
std::cout << "eMc:" << std::endl << eMc << std::endl;
std::cout << "cam:" << std::endl << cam << std::endl;
vpNaoqiRobot robot;
// Connect to the robot
if (! opt_ip.empty())
robot.setRobotIp(opt_ip);
robot.open();
if (robot.getRobotType() != vpNaoqiRobot::Pepper)
{
std::cout << "ERROR: You are not connected to Pepper, but to a different Robot. Check the IP. " << std::endl;
return 0;
}
std::vector<std::string> jointNames_head = robot.getBodyNames("Head");
// Open Proxy for the speech
AL::ALTextToSpeechProxy tts(opt_ip, 9559);
std::string phraseToSay;
if (opt_language_english)
{
tts.setLanguage("English");
phraseToSay = " \\emph=2\\ Hi,\\pau=200\\ How are you ?";
}
else
{
tts.setLanguage("French");
phraseToSay = " \\emph=2\\ Bonjour,\\pau=200\\ comment vas tu ?";
}
// Inizialize PeoplePerception
AL::ALPeoplePerceptionProxy people_proxy(opt_ip, 9559);
AL::ALMemoryProxy m_memProxy(opt_ip, 9559);
people_proxy.subscribe("People", 30, 0.0);
std::cout << "period: " << people_proxy.getCurrentPeriod() << std::endl;
// Open Proxy for the recognition speech
AL::ALSpeechRecognitionProxy asr(opt_ip, 9559);
// asr.unsubscribe("Test_ASR");
// return 0 ;
asr.setVisualExpression(false);
asr.setLanguage("English");
std::vector<std::string> vocabulary;
vocabulary.push_back("follow me");
vocabulary.push_back("stop");
// Set the vocabulary
asr.setVocabulary(vocabulary,false);
// Start the speech recognition engine with user Test_ASR
asr.subscribe("Test_ASR");
std::cout << "Speech recognition engine started" << std::endl;
// Proxy to control the leds
AL::ALLedsProxy led_proxy(opt_ip, 9559);
//Declare plots
vpPlot * plotter_diff_vel; vpPlot * plotter_vel;
vpPlot * plotter_error; vpPlot * plotter_distance;
if (opt_debug)
{
// Plotting
plotter_diff_vel = new vpPlot (2);
plotter_diff_vel->initGraph(0, 2);
plotter_diff_vel->initGraph(1, 2);
plotter_diff_vel->setTitle(0, "HeadYaw");
plotter_diff_vel->setTitle(1, "HeadPitch");
plotter_vel= new vpPlot (1);
plotter_vel->initGraph(0, 5);
plotter_vel->setLegend(0, 0, "vx");
plotter_vel->setLegend(0, 1, "vy");
plotter_vel->setLegend(0, 2, "wz");
plotter_vel->setLegend(0, 3, "q_yaw");
plotter_vel->setLegend(0, 4, "q_pitch");
plotter_error = new vpPlot(1);
plotter_error->initGraph(0, 3);
plotter_error->setLegend(0, 0, "x");
plotter_error->setLegend(0, 1, "y");
plotter_error->setLegend(0, 2, "Z");
plotter_distance = new vpPlot (1);
plotter_distance->initGraph(0, 1);
plotter_distance->setLegend(0, 0, "dist");
}
try {
vpImage<unsigned char> I(g.getHeight(), g.getWidth());
vpDisplayX d(I);
vpDisplay::setTitle(I, "ViSP viewer");
vpFaceTrackerOkao face_tracker(opt_ip,9559);
double dist = 0.0; // Distance between person detected from peoplePerception and faceDetection
// Set Visual Servoing:
vpServo task;
task.setServo(vpServo::EYEINHAND_L_cVe_eJe) ;
task.setInteractionMatrixType(vpServo::CURRENT, vpServo::PSEUDO_INVERSE);
// vpAdaptiveGain lambda_adapt;
// lambda_adapt.initStandard(1.6, 1.8, 15);
vpAdaptiveGain lambda_base(1.2, 1.0, 10); // 2.3, 0.7, 15
vpAdaptiveGain lambda_nobase(5, 2.9, 15); // 4, 0.5, 15
task.setLambda(lambda_base) ;
double Z = 0.9;
double Zd = 0.9;
bool stop_vxy = false;
bool move_base = true;
bool move_base_prev = true;
// Create the desired visual feature
vpFeaturePoint s;
vpFeaturePoint sd;
vpImagePoint ip(I.getHeight()/2, I.getWidth()/2);
// Create the current x visual feature
vpFeatureBuilder::create(s, cam, ip);
vpFeatureBuilder::create(sd, cam, ip);
// sd.buildFrom( I.getWidth()/2, I.getHeight()/2, Zd);
AL::ALValue limit_yaw = robot.getProxy()->getLimits("HeadYaw");
std::cout << limit_yaw[0][0] << " " << limit_yaw[0][1] << std::endl;
// Add the feature
task.addFeature(s, sd) ;
vpFeatureDepth s_Z, s_Zd;
s_Z.buildFrom(s.get_x(), s.get_y(), Z , 0); // log(Z/Z*) = 0 that's why the last parameter is 0
s_Zd.buildFrom(sd.get_x(), sd.get_y(), Zd , 0); // log(Z/Z*) = 0 that's why the last parameter is 0
// Add the feature
task.addFeature(s_Z, s_Zd);
// Jacobian 6x5 (vx,vy,wz,q_yaq,q_pitch)
vpMatrix tJe(6,5);
tJe[0][0]= 1;
tJe[1][1]= 1;
tJe[5][2]= 1;
vpMatrix eJe(6,5);
double servo_time_init = 0;
vpImagePoint head_cog_cur;
vpImagePoint head_cog_des(I.getHeight()/2, I.getWidth()/2);
vpColVector q_dot;
bool reinit_servo = true;
unsigned long loop_iter = 0;
std::vector<std::string> recognized_names;
std::map<std::string,unsigned int> detected_face_map;
bool detection_phase = true;
unsigned int f_count = 0;
// AL::ALValue leg_names = AL::ALValue::array("HipRoll","HipPitch", "KneePitch" );
// AL::ALValue values = AL::ALValue::array(0.0, 0.0, 0.0 );
// robot.getProxy()->setMoveArmsEnabled(false,false);
std::vector<std::string> arm_names = robot.getBodyNames("LArm");
std::vector<std::string> rarm_names = robot.getBodyNames("RArm");
std::vector<std::string> leg_names(3);
leg_names[0]= "HipRoll";
leg_names[1]= "HipPitch";
leg_names[2]= "KneePitch";
arm_names.insert( arm_names.end(), rarm_names.begin(), rarm_names.end() );
arm_names.insert( arm_names.end(), leg_names.begin(), leg_names.end() );
std::vector<float> arm_values(arm_names.size(),0.0);
for (unsigned int i = 0; i < arm_names.size(); i++ )
std::cout << arm_names[i]<< std::endl;
robot.getPosition(arm_names, arm_values,false);
vpImagePoint best_cog_face_peoplep;
bool person_found = false;
double t_prev = vpTime::measureTimeSecond();
while(1) {
if (reinit_servo) {
servo_time_init = vpTime::measureTimeSecond();
t_prev = vpTime::measureTimeSecond();
reinit_servo = false;
led_proxy.fadeRGB("FaceLeds","white",0.1);
}
double t = vpTime::measureTimeMs();
if (0) // (opt_debug)
{
g.acquire(I);
}
vpDisplay::display(I);
// Detect face
bool face_found = face_tracker.detect();
stop_vxy = false;
//std::cout << "Loop time face_tracker: " << vpTime::measureTimeMs() - t << " ms" << std::endl;
// Check speech recognition result
AL::ALValue result_speech = m_memProxy.getData("WordRecognized");
if ( ((result_speech[0]) == vocabulary[0]) && (double (result_speech[1]) > 0.4 )) //move
{
//std::cout << "Recognized: " << result_speech[0] << "with confidence of " << result_speech[1] << std::endl;
task.setLambda(lambda_base) ;
move_base = true;
}
else if ( (result_speech[0] == vocabulary[1]) && (double(result_speech[1]) > 0.4 )) //stop
{
//std::cout << "Recognized: " << result_speech[0] << "with confidence of " << result_speech[1] << std::endl;
task.setLambda(lambda_nobase) ;
move_base = false;
}
if (move_base != move_base_prev)
{
if (move_base)
{
phraseToSay = "Ok, I will follow you.";
tts.post.say(phraseToSay);
}
else
{
phraseToSay = "Ok, I will stop.";
tts.post.say(phraseToSay);
}
}
//robot.setStiffness(arm_names,0.0);
//robot.getProxy()->setAngles(arm_names,arm_values,1.0);
//robot.getProxy()->setAngles("HipRoll",0.0,1.0);
//std::cout << "Loop time check_speech: " << vpTime::measureTimeMs() - t << " ms" << std::endl;
move_base_prev = move_base;
if (face_found) {
std::ostringstream text;
text << "Found " << face_tracker.getNbObjects() << " face(s)";
vpDisplay::displayText(I, 10, 10, text.str(), vpColor::red);
for(size_t i=0; i < face_tracker.getNbObjects(); i++) {
vpRect bbox = face_tracker.getBBox(i);
if (i == 0)
vpDisplay::displayRectangle(I, bbox, vpColor::red, false, 2);
else
vpDisplay::displayRectangle(I, bbox, vpColor::green, false, 1);
vpDisplay::displayText(I, (int)bbox.getTop()-10, (int)bbox.getLeft(), face_tracker.getMessage(i) , vpColor::red);
}
led_proxy.post.fadeRGB("FaceLeds","blue",0.1);
double u = face_tracker.getCog(0).get_u();
double v = face_tracker.getCog(0).get_v();
if (u<= g.getWidth() && v <= g.getHeight())
head_cog_cur.set_uv(u,v);
vpRect bbox = face_tracker.getBBox(0);
std::string name = face_tracker.getMessage(0);
//std::cout << "Loop time face print " << vpTime::measureTimeMs() - t << " ms" << std::endl;
}
AL::ALValue result = m_memProxy.getData("PeoplePerception/VisiblePeopleList");
//std::cout << "Loop time get Data PeoplePerception " << vpTime::measureTimeMs() - t << " ms" << std::endl;
person_found = false;
if (result.getSize() > 0)
{
AL::ALValue info = m_memProxy.getData("PeoplePerception/PeopleDetected");
int num_people = info[1].getSize();
std::ostringstream text;
text << "Found " << num_people << " person(s)";
vpDisplay::displayText(I, 10, 10, text.str(), vpColor::red);
person_found = true;
if (face_found) // Try to find the match between two detection
{
vpImagePoint cog_face;
double dist_min = 1000;
unsigned int index_person = 0;
for (unsigned int i = 0; i < num_people; i++)
{
float alpha = info[1][i][2];
float beta = info[1][i][3];
//Z = Zd;
// Centre of face into the image
float x = g.getWidth()/2 - g.getWidth() * beta;
float y = g.getHeight()/2 + g.getHeight() * alpha;
cog_face.set_uv(x,y);
dist = vpImagePoint::distance(cog_face, head_cog_cur);
if (dist < dist_min)
{
dist_min = dist;
best_cog_face_peoplep = cog_face;
index_person = i;
}
}
vpDisplay::displayCross(I, best_cog_face_peoplep, 10, vpColor::white);
if (dist_min < 55.)
{
Z = info[1][index_person][1]; // Current distance
}
}
else // Take the first one on the list
{
float alpha = info[1][0][2];
float beta = info[1][0][3];
//Z = Zd;
// Centre of face into the image
float x = g.getWidth()/2 - g.getWidth() * beta;
float y = g.getHeight()/2 + g.getHeight() * alpha;
head_cog_cur.set_uv(x,y);
Z = info[1][0][1]; // Current distance
}
}
else
{
std::cout << "No distance computed " << std::endl;
stop_vxy = true;
robot.getProxy()->setAngles("HipRoll",0.0,1.0);
//Z = Zd;
}
// float alpha = info[1][0][2];
// float beta = info[1][0][3];
// //Z = Zd;
// // Centre of face into the image
// float x = g.getWidth()/2 - g.getWidth() * beta;
// float y = g.getHeight()/2 + g.getHeight() * alpha;
// vpImagePoint cog_face(y,x);
// dist = vpImagePoint::distance(cog_face,head_cog_cur);
// if (dist < 55.)
// Z = info[1][0][1]; // Current distance
// else
// stop_vxy = true;
// }
// else
// {
// std::cout << "No distance computed " << std::endl;
// stop_vxy = true;
// //Z = Zd;
// }
//std::cout << "Loop time before VS: " << vpTime::measureTimeMs() - t << " ms" << std::endl;
if (face_found || person_found )
{
// Get Head Jacobian (6x2)
vpMatrix torso_eJe_head;
robot.get_eJe("Head",torso_eJe_head);
// Add column relative to the base rotation (Wz)
// vpColVector col_wz(6);
// col_wz[5] = 1;
for (unsigned int i = 0; i < 6; i++)
for (unsigned int j = 0; j < torso_eJe_head.getCols(); j++)
tJe[i][j+3] = torso_eJe_head[i][j];
// std::cout << "tJe" << std::endl << tJe << std::endl;
// vpHomogeneousMatrix torsoMHeadPith( robot.getProxy()->getTransform(jointNames_head[jointNames_head.size()-1], 0, true));// get transformation matrix between torso and HeadRoll
vpHomogeneousMatrix torsoMHeadPith( robot.getProxy()->getTransform("HeadPitch", 0, true));// get transformation matrix between torso and HeadRoll
vpVelocityTwistMatrix HeadPitchVLtorso(torsoMHeadPith.inverse());
for(unsigned int i=0; i< 3; i++)
for(unsigned int j=0; j< 3; j++)
HeadPitchVLtorso[i][j+3] = 0;
//std::cout << "HeadPitchVLtorso: " << std::endl << HeadPitchVLtorso << std::endl;
// Transform the matrix
eJe = HeadPitchVLtorso *tJe;
// std::cout << "eJe" << std::endl << eJe << std::endl;
task.set_eJe( eJe );
task.set_cVe( vpVelocityTwistMatrix(eMc.inverse()) );
vpDisplay::displayCross(I, head_cog_des, 10, vpColor::blue);
vpDisplay::displayCross(I, head_cog_cur, 10, vpColor::green);
// std::cout << "head_cog_des:" << std::endl << head_cog_des << std::endl;
// std::cout << "head_cog_cur:" << std::endl << head_cog_cur << std::endl;
// Update the current x feature
double x,y;
vpPixelMeterConversion::convertPoint(cam, head_cog_cur, x, y);
s.buildFrom(x, y, Z);
//s.set_xyZ(head_cog_cur.get_u(), head_cog_cur.get_v(), Z);
// Update log(Z/Z*) feature. Since the depth Z change, we need to update the intection matrix
s_Z.buildFrom(s.get_x(), s.get_y(), Z, log(Z/Zd)) ;
q_dot = task.computeControlLaw(vpTime::measureTimeSecond() - servo_time_init);
//std::cout << "Loop time compute VS: " << vpTime::measureTimeMs() - t << " ms" << std::endl;
vpMatrix P = task.getI_WpW();
double alpha = -3.3;
double min = limit_yaw[0][0];
double max = limit_yaw[0][1];
vpColVector z_q2 (q_dot.size());
vpColVector q_yaw = robot.getPosition(jointNames_head[0]);
z_q2[3] = 2 * alpha * q_yaw[0]/ pow((max - min),2);
vpColVector q3 = P * z_q2;
//if (q3.euclideanNorm()<10.0)
q_dot = q_dot + q3;
std::vector<float> vel(jointNames_head.size());
vel[0] = q_dot[3];
vel[1] = q_dot[4];
// Compute the distance in pixel between the target and the center of the image
double distance = vpImagePoint::distance(head_cog_cur, head_cog_des);
//if (distance > 0.03*I.getWidth())
std::cout << "q:" << std::endl << q_dot << std::endl;
// std::cout << "vel" << std::endl << q_dot << std::endl;
//std::cout << "distance" << std::endl << distance <<" -- " << 0.03*I.getWidth() << " ++ " << I.getWidth() << std::endl;
// if (distance > 0.1*I.getWidth())
robot.setVelocity(jointNames_head,vel);
// else
// {
// std::cout << "Setting hipRoll to zero" << std::endl;
// robot.getProxy()->setAngles("HipRoll",0.0,1.0);
// }
// std::cout << "errorZ: " << task.getError()[2] << std::endl;
// std::cout << "stop_vxy: " << stop_vxy << std::endl;
if (std::fabs(Z -Zd) < 0.05 || stop_vxy || !move_base)
robot.setBaseVelocity(0.0, 0.0, q_dot[2]);
else
robot.setBaseVelocity(q_dot[0], q_dot[1], q_dot[2]);
if (opt_debug)
{
vpColVector vel_head = robot.getJointVelocity(jointNames_head);
for (unsigned int i=0 ; i < jointNames_head.size() ; i++) {
plotter_diff_vel->plot(i, 1, loop_iter, q_dot[i+3]);
plotter_diff_vel->plot(i, 0, loop_iter, vel_head[i]);
}
plotter_error->plot(0,loop_iter,task.getError());
plotter_vel->plot(0,loop_iter, q3);
plotter_distance->plot(0,0,loop_iter,Z);
}
// if (detection_phase)
// {
// //if (score >= 0.4 && distance < 0.06*I.getWidth() && bbox.getSize() > 3000)
// if (distance < 0.06*I.getWidth() && bbox.getSize() > 3000)
// {
// if (opt_debug)
// {
// vpDisplay::displayRectangle(I, bbox, vpColor::red, false, 1);
// vpDisplay::displayText(I, (int)bbox.getTop()-10, (int)bbox.getLeft(), name, vpColor::red);
// }
// detected_face_map[name]++;
// f_count++;
// }
// else
// {
// if (opt_debug)
// {
// vpDisplay::displayRectangle(I, bbox, vpColor::green, false, 1);
// vpDisplay::displayText(I, (int)bbox.getTop()-10, (int)bbox.getLeft(), name, vpColor::green);
// }
// }
// if (f_count>10)
// {
// detection_phase = false;
// f_count = 0;
// }
// }
// else
// {
// std::string recognized_person_name = std::max_element(detected_face_map.begin(), detected_face_map.end(), pred)->first;
// unsigned int times = std::max_element(detected_face_map.begin(), detected_face_map.end(), pred)->second;
// if (!in_array(recognized_person_name, recognized_names) && recognized_person_name != "Unknown") {
// if (opt_language_english)
// {
// phraseToSay = "\\emph=2\\ Hi \\wait=200\\ \\emph=2\\" + recognized_person_name + "\\pau=200\\ How are you ?";
// }
// else
// {
// phraseToSay = "\\emph=2\\ Salut \\wait=200\\ \\emph=2\\" + recognized_person_name + "\\pau=200\\ comment vas tu ?";;
// }
// std::cout << phraseToSay << std::endl;
// tts.post.say(phraseToSay);
// recognized_names.push_back(recognized_person_name);
// }
// if (!in_array(recognized_person_name, recognized_names) && recognized_person_name == "Unknown"
// && times > 15)
// {
// if (opt_language_english)
// {
// phraseToSay = "\\emph=2\\ Hi \\wait=200\\ \\emph=2\\. I don't know you! \\emph=2\\ What's your name?";
// }
// else
// {
// phraseToSay = " \\emph=2\\ Salut \\wait=200\\ \\emph=2\\. Je ne te connais pas! \\emph=2\\ Comment t'appelles-tu ?";
// }
// std::cout << phraseToSay << std::endl;
// tts.post.say(phraseToSay);
// recognized_names.push_back(recognized_person_name);
// }
// detection_phase = true;
// detected_face_map.clear();
// }
}
else {
robot.stop(jointNames_head);
robot.stopBase();
std::cout << "Stop!" << std::endl;
reinit_servo = true;
}
//if (opt_debug)
vpDisplay::flush(I);
if (vpDisplay::getClick(I, false))
break;
loop_iter ++;
std::cout << "Loop time: " << vpTime::measureTimeMs() - t << " ms" << std::endl;
}
robot.stop(jointNames_head);
robot.stopBase();
tts.setLanguage("French");
// tts.exit();
people_proxy.unsubscribe("People");
// people_proxy.exit();
asr.unsubscribe("Test_ASR");
asr.setVisualExpression(true);
// asr.exit();
tts.setLanguage("French");
// tts.exit();
led_proxy.fadeRGB("FaceLeds","white",0.1);
// led_proxy.exit();
vpDisplay::getClick(I, true);
}
catch(vpException &e) {
std::cout << e.getMessage() << std::endl;
}
catch (const AL::ALError& e)
{
std::cerr << "Caught exception " << e.what() << std::endl;
}
std::cout << "The end: stop the robot..." << std::endl;
//tts.setLanguage("French");
// tts.exit();
robot.stop(jointNames_head);
robot.stopBase();
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);
}
