int
main()
{
// Log file creation in /tmp/$USERNAME/log.dat
// This file contains by line:
// - the 6 computed joint velocities (m/s, rad/s) to achieve the task
// - the 6 mesured joint velocities (m/s, rad/s)
// - the 6 mesured joint positions (m, rad)
// - the 8 values of s - s*
std::string username;
// Get the user login name
vpIoTools::getUserName(username);
// Create a log filename to save velocities...
std::string logdirname;
logdirname ="/tmp/" + username;
// Test if the output path exist. If no try to create it
if (vpIoTools::checkDirectory(logdirname) == false) {
try {
// Create the dirname
vpIoTools::makeDirectory(logdirname);
}
catch (...) {
std::cerr << std::endl
<< "ERROR:" << std::endl;
std::cerr << " Cannot create " << logdirname << std::endl;
return(-1);
}
}
std::string logfilename;
logfilename = logdirname + "/log.dat";
// Open the log file name
std::ofstream flog(logfilename.c_str());
try {
vpRobotViper650 robot ;
// Load the end-effector to camera frame transformation obtained
// using a camera intrinsic model with distortion
vpCameraParameters::vpCameraParametersProjType projModel =
vpCameraParameters::perspectiveProjWithDistortion;
robot.init(vpRobotViper650::TOOL_PTGREY_FLEA2_CAMERA, projModel);
vpServo task ;
vpImage<unsigned char> I ;
int i ;
bool reset = false;
vp1394TwoGrabber g(reset);
g.setVideoMode(vp1394TwoGrabber::vpVIDEO_MODE_640x480_MONO8);
g.setFramerate(vp1394TwoGrabber::vpFRAMERATE_60);
g.open(I) ;
g.acquire(I) ;
#ifdef VISP_HAVE_X11
vpDisplayX display(I,100,100,"Current image") ;
#elif defined(VISP_HAVE_OPENCV)
vpDisplayOpenCV display(I,100,100,"Current image") ;
#elif defined(VISP_HAVE_GTK)
vpDisplayGTK display(I,100,100,"Current image") ;
#endif
vpDisplay::display(I) ;
vpDisplay::flush(I) ;
std::cout << std::endl ;
std::cout << "-------------------------------------------------------" << std::endl ;
std::cout << " Test program for vpServo " <<std::endl ;
std::cout << " Eye-in-hand task control, velocity computed in the joint space" << std::endl ;
std::cout << " Use of the Afma6 robot " << std::endl ;
std::cout << " task : servo 4 points on a square with dimention " << L << " meters" << std::endl ;
std::cout << "-------------------------------------------------------" << std::endl ;
std::cout << std::endl ;
vpDot2 dot[4] ;
vpImagePoint cog;
std::cout << "Click on the 4 dots clockwise starting from upper/left dot..."
<< std::endl;
for (i=0 ; i < 4 ; i++) {
dot[i].setGraphics(true) ;
dot[i].initTracking(I) ;
cog = dot[i].getCog();
vpDisplay::displayCross(I, cog, 10, vpColor::blue) ;
vpDisplay::flush(I);
}
vpCameraParameters cam ;
// Update camera parameters
robot.getCameraParameters (cam, I);
std::cout << "Camera parameters: \n" << cam << std::endl;
// Sets the current position of the visual feature
vpFeaturePoint p[4] ;
for (i=0 ; i < 4 ; i++)
vpFeatureBuilder::create(p[i], cam, dot[i]); //retrieve x,y of the vpFeaturePoint structure
// Set the position of the square target in a frame which origin is
// centered in the middle of the square
vpPoint point[4] ;
point[0].setWorldCoordinates(-L, -L, 0) ;
point[1].setWorldCoordinates( L, -L, 0) ;
point[2].setWorldCoordinates( L, L, 0) ;
point[3].setWorldCoordinates(-L, L, 0) ;
// Initialise a desired pose to compute s*, the desired 2D point features
vpHomogeneousMatrix cMo;
vpTranslationVector cto(0, 0, 0.5); // tz = 0.5 meter
vpRxyzVector cro(vpMath::rad(0), vpMath::rad(10), vpMath::rad(20));
vpRotationMatrix cRo(cro); // Build the rotation matrix
cMo.buildFrom(cto, cRo); // Build the homogeneous matrix
// Sets the desired position of the 2D visual feature
vpFeaturePoint pd[4] ;
// Compute the desired position of the features from the desired pose
for (int i=0; i < 4; i ++) {
vpColVector cP, p ;
point[i].changeFrame(cMo, cP) ;
point[i].projection(cP, p) ;
pd[i].set_x(p[0]) ;
pd[i].set_y(p[1]) ;
pd[i].set_Z(cP[2]);
}
// We want to see a point on a point
for (i=0 ; i < 4 ; i++)
task.addFeature(p[i],pd[i]) ;
// Set the proportional gain
task.setLambda(0.3) ;
// Display task information
task.print() ;
// Define the task
// - we want an eye-in-hand control law
// - articular velocity are computed
task.setServo(vpServo::EYEINHAND_L_cVe_eJe) ;
task.setInteractionMatrixType(vpServo::CURRENT, vpServo::PSEUDO_INVERSE) ;
task.print() ;
vpVelocityTwistMatrix cVe ;
robot.get_cVe(cVe) ;
task.set_cVe(cVe) ;
task.print() ;
// Set the Jacobian (expressed in the end-effector frame)
vpMatrix eJe ;
robot.get_eJe(eJe) ;
task.set_eJe(eJe) ;
task.print() ;
// Initialise the velocity control of the robot
robot.setRobotState(vpRobot::STATE_VELOCITY_CONTROL) ;
std::cout << "\nHit CTRL-C to stop the loop...\n" << std::flush;
for ( ; ; ) {
// Acquire a new image from the camera
g.acquire(I) ;
// Display this image
vpDisplay::display(I) ;
try {
// For each point...
for (i=0 ; i < 4 ; i++) {
// Achieve the tracking of the dot in the image
dot[i].track(I) ;
// Display a green cross at the center of gravity position in the
// image
cog = dot[i].getCog();
vpDisplay::displayCross(I, cog, 10, vpColor::green) ;
}
}
catch(...) {
flog.close() ; // Close the log file
vpTRACE("Error detected while tracking visual features") ;
robot.stopMotion() ;
return(1) ;
}
// During the servo, we compute the pose using LOWE method. For the
// initial pose used in the non linear minimisation we use the pose
// computed at the previous iteration.
compute_pose(point, dot, 4, cam, cMo, cto, cro, false);
for (i=0 ; i < 4 ; i++) {
// Update the point feature from the dot location
vpFeatureBuilder::create(p[i], cam, dot[i]);
// Set the feature Z coordinate from the pose
vpColVector cP;
point[i].changeFrame(cMo, cP) ;
p[i].set_Z(cP[2]);
}
// Get the jacobian of the robot
robot.get_eJe(eJe) ;
// Update this jacobian in the task structure. It will be used to compute
// the velocity skew (as an articular velocity)
// qdot = -lambda * L^+ * cVe * eJe * (s-s*)
task.set_eJe(eJe) ;
vpColVector v ;
// Compute the visual servoing skew vector
v = task.computeControlLaw() ;
// Display the current and desired feature points in the image display
vpServoDisplay::display(task,cam,I) ;
// Apply the computed joint velocities to the robot
robot.setVelocity(vpRobot::ARTICULAR_FRAME, v) ;
// Save velocities applied to the robot in the log file
// v[0], v[1], v[2] correspond to joint translation velocities in m/s
// v[3], v[4], v[5] correspond to joint rotation velocities in rad/s
flog << v[0] << " " << v[1] << " " << v[2] << " "
<< v[3] << " " << v[4] << " " << v[5] << " ";
// Get the measured joint velocities of the robot
vpColVector qvel;
robot.getVelocity(vpRobot::ARTICULAR_FRAME, qvel);
// Save measured joint velocities of the robot in the log file:
// - qvel[0], qvel[1], qvel[2] correspond to measured joint translation
// velocities in m/s
// - qvel[3], qvel[4], qvel[5] correspond to measured joint rotation
// velocities in rad/s
flog << qvel[0] << " " << qvel[1] << " " << qvel[2] << " "
<< qvel[3] << " " << qvel[4] << " " << qvel[5] << " ";
// Get the measured joint positions of the robot
vpColVector q;
robot.getPosition(vpRobot::ARTICULAR_FRAME, q);
// Save measured joint positions of the robot in the log file
// - q[0], q[1], q[2] correspond to measured joint translation
// positions in m
// - q[3], q[4], q[5] correspond to measured joint rotation
// positions in rad
flog << q[0] << " " << q[1] << " " << q[2] << " "
<< q[3] << " " << q[4] << " " << q[5] << " ";
// Save feature error (s-s*) for the 4 feature points. For each feature
// point, we have 2 errors (along x and y axis). This error is expressed
// in meters in the camera frame
flog << ( task.getError() ).t() << std::endl;
// Flush the display
vpDisplay::flush(I) ;
// vpTRACE("\t\t || s - s* || = %f ", ( task.getError() ).sumSquare()) ;
}
vpTRACE("Display task information " ) ;
task.print() ;
task.kill();
flog.close() ; // Close the log file
return 0;
}
catch (...)
{
flog.close() ; // Close the log file
vpERROR_TRACE(" Test failed") ;
return 0;
}
}
static inline void forth_vm_execute_instruction(forth_context_type *fc, char cmd)
{
// printf("%c\n",cmd);
// getchar();
switch(cmd)
{
case '0': push(fc,0); break;
case '1': push(fc,1); break;
case '2': push(fc,2); break;
case '3': push(fc,3); break;
case '4': push(fc,4); break;
case '5': push(fc,5); break;
case '6': push(fc,6); break;
case '7': push(fc,7); break;
case '8': push(fc,8); break;
case '9': push(fc,9); break;
case '@': at(fc); break; //@
case '!': to(fc); break; //!
case 'd': fc->SP+=fc->cell; break; //drop
case 'D': dup(fc); break; //dup
case 's': swap_(fc); break; //swap
case 'l': push(fc,next_cell(fc)); break; //lit
case '+': add(fc); break; //+
case '-': sub(fc); break; //-
case '*': mul(fc); break; //*
case '/': div_(fc); break; // /
case '%': mod(fc); break; //mod
case '&': and(fc); break; // and
case '|': or(fc); break; // or
case '^': xor(fc); break; // xor
case '>': more(fc); break; // >
case '<': less(fc); break; // <
case '=': eq(fc); break; // =
case 'b': branch(fc); break; // branch
case '?': cbranch(fc); break; // ?branch
case 'c': call(fc); break; // call
case 'r': ret(fc); break; // ret
case 't': to_r(fc); break; // >R
case 'f': from_r(fc); break; // R>
case 'i': in(fc); break; // in
case 'o': out(fc); break; // out
case '_': fc->stop=1; break; // stop
case 'A': adr0(fc); break; // @0
case 1: push(fc,fc->SP); break; // SP@
case 2: fc->SP=pop(fc); break; // SP!
case 3: push(fc,fc->RP); break; // RP@
case 4: fc->RP=pop(fc); break; // RP!
case 5: shl(fc); break; // <<
case 6: shr(fc); break; // >>
case 7: push(fc,*(size_t *)(fc->mem+fc->RP)); break; // i
case 8: cat(fc); break; // c@
case 9: cto(fc); break; // c!
case 10: set_current_state(TASK_INTERRUPTIBLE); schedule_timeout(1); break; // nop
case 11: in_ready(fc); break; // ?in
case 12: out_ready(fc); break; // ?out
case 16: umod(fc); break; // umod
case 17: udiv(fc); break; // u/
// kernel
case 'K': kalsym_lookup(fc); break; // lookup kallsym address
case 18: kcall(fc); break; // kcall
}
}
void TriconnectedShellingOrder::doCall(
const Graph& G,
adjEntry adj,
List<ShellingOrderSet>& partition)
{
// prefer nodes to faces?
bool preferNodes = false;
#ifdef OUTPUT_TSO
cout << "Graph G is planar == " << isPlanar(G) << endl;
cout << "Graph G has no self loops == " << isLoopFree(G) << endl;
cout << "Graph G is connected == " << isConnected(G) << endl;
cout << "Graph G is triconnected == " << isTriconnected(G) << endl;
#endif
OGDF_ASSERT(isPlanar(G) == true);
OGDF_ASSERT(isLoopFree(G) == true);
OGDF_ASSERT(isTriconnected(G) == true);
// crate an embedding for G
ConstCombinatorialEmbedding E(G);
// set outerFace so adj is on it or to face with maximal size
face outerFace = (adj != nullptr) ? E.rightFace(adj) : E.maximalFace();
#ifdef OUTPUT_TSO
cout << "faces:" << endl;
for(face fh : E.faces) {
if (fh == outerFace)
cout << " face *" << fh->index() << ":";
else
cout << " face " << fh->index() << ":";
for(adjEntry adj : fh->entries)
cout << " " << adj;
cout << endl;
}
cout << "adjacency lists:" << endl;
for(node vh : G.nodes) {
cout << " node " << vh << ":";
for(adjEntry adj : vh->adjEntries)
cout << " " << adj;
cout << endl;
}
#endif
adjEntry firstAdj = outerFace->firstAdj();
// set firstAdj that the outer face is on the left of firstAdj
if (E.rightFace(firstAdj) == outerFace)
firstAdj = firstAdj->cyclicSucc();
// set "base" nodes v1, v2 on outer face with edge [v1,v2]
node v1 = firstAdj->theNode();
node v2 = firstAdj->cyclicPred()->twinNode();
ComputeTricOrder cto(G, E, outerFace, m_baseRatio, preferNodes);
// if outerFace == {v_1,...,v_q}
// adjPred(v_i) == v_i -> v_{i-1}
// adjSucc(v_i) == v_1 -> v_{i+1}
// these arrays will be updated during the algo so they define the outer face
NodeArray<adjEntry> adjPred(G),
adjSucc(G);
// init adjPred and adjSucc for the nodes of the outer face
adjSucc[v1] = firstAdj;
adjEntry adjRun = firstAdj->twin()->cyclicSucc();
do {
adjPred[adjRun->theNode()] = adjRun->cyclicPred();
adjSucc[adjRun->theNode()] = adjRun;
adjRun = adjRun->twin()->cyclicSucc();
} while (adjRun != firstAdj);
adjPred[v1] = adjSucc[v2] = nullptr;
// init outer nodes and outer edges
cto.initOuterNodes(v1, v2);
cto.initOuterEdges();
// init the first possible node as the node in the middle of v_1
// and v_2 on the outer face
int l = (outerFace->size() -2)/2;
if (l == 0)
l = 1;
adjRun = firstAdj;
for (int i=1; i <= l; i++)
adjRun = adjRun->twin()->cyclicSucc();
cto.initPossible(adjRun->theNode());
// node and face that are selected during the algorithm
node vk;
face Fk;
// left and right node of current nodeset
node cl, cr;
// the actual nodeset V in the shelling order
ShellingOrderSet V;
// further auxiliary variables
#ifdef OUTPUT_TSO
cout << "finished initialization of cto, adjSucc, adjPred." << endl << flush;
cout << "v1 = " << v1 << ", v2 = " << v2 << ", first possible node = " << adjRun->theNode() << endl;
for(adjEntry adj1 : outerFace->entries) {
cout << " node " << adj1->theNode() << ": adjPred=(" << adjPred[adj1->theNode()]
<< "), adjSucc=" << adjSucc[adj1->theNode()] << endl;
}
cto.output();
cout << "starting main loop" << endl;
#endif
// main loop
while (cto.isPossible()){
// get the next possible nodeset for the order
cto.getNextPossible(vk, Fk);
// check if the current selection is a node or a face
if (cto.isNode()){
#ifdef OUTPUT_TSO
cout << " nextPossible is node " << vk << endl << flush;
#endif
// current item is a node
V = ShellingOrderSet(1, adjPred[vk], adjSucc[vk]);
V[1] = vk;
cl = (adjPred[vk])->twinNode();
cr = (adjSucc[vk])->twinNode();
// insert actual nodeset to the front of the shelling order
partition.pushFront(V);
}
else{
#ifdef OUTPUT_TSO
cout << " nextPossible is face " << Fk->index() << endl << flush;
#endif
// current item is a face
// create set with chain {z_1,...,z_l}
V = ShellingOrderSet(cto.getOutv(Fk)-2);
// now find node v on Fk with degree 2
cl = cto.getOuterNodeDeg2(Fk, adjPred, adjSucc);
// find end of chain cl and cr
// traverse to left while degree == 2
while ((cl != v1) && (adjPred[cl] == adjSucc[cl]->cyclicSucc()))
cl = (adjPred[cl])->twinNode();
// traverse to the right while degree == 2
// and insert nodes into the ShellingOrderSet
cr = adjSucc[cl]->twinNode();
int i = 1;
while ((cr != v2) && (adjPred[cr] == adjSucc[cr]->cyclicSucc())){
V[i] = cr;
cr = (adjSucc[cr])->twinNode();
i++;
}
cto.decSepf(cl);
cto.decSepf(cr);
// set left and right node in the shelling order set
V.left(cl);
V.right(cr);
// set left and right adjacency entry
V.leftAdj((adjPred[cr])->twin());
V.rightAdj((adjSucc[cl])->twin());
// insert actual nodeset to the front of the shelling order
partition.pushFront(V);
}// current item is a face
#ifdef OUTPUT_TSO
cout << " set cl = " << cl << endl;
cout << " set cr = " << cr << endl;
#endif
// update adjSucc[cl] and adjPred[cr]
adjSucc[cl] = adjSucc[cl]->cyclicSucc();
adjPred[cr] = adjPred[cr]->cyclicPred();
// increase number of outer edges of face left of adjPred[cr]
cto.incOute(E.leftFace(adjPred[cr]));
cto.incVisited(cl);
cto.incVisited(cr);
// traverse from cl to cr on the new outer face
// and update adjSucc[] and adjPred[]
adjEntry adj1 = adjSucc[cl]->twin();
for (node u = adj1->theNode(); u != cr; u = adj1->theNode()){
// increase oute for the right face of adj1
cto.incOute(E.leftFace(adj1));
// set new predecessor
adjPred[u] = adj1;
// go to next adj-entry
adj1 = adj1->cyclicSucc();
// if the actual node has an edge to the deleted node
// increase the visited value for the actual node...
if (adj1->twinNode() == vk){
cto.incVisited(u);
// ... and skip the actual adjEntry
adj1 = adj1->cyclicSucc();
}
adjSucc[u] = adj1;
// add actual node to outerNodes[f]
for (adjEntry adj2 = adjPred[u]; adj2 != adjSucc[u]; adj2 = adj2->cyclicPred()){
cto.addOuterNode(u, E.leftFace(adj2));
}
adj1 = adj1->twin();
}
if (!cto.isNode()){
if ( ((adjSucc[cl])->twinNode() == cr)
&& ( cto.isOnlyEdge(E.rightFace(adjSucc[cl])) ) ){
cto.decSepf(cl);
cto.decSepf(cr);
}
}
// update cto
cto.doUpdate();
#ifdef OUTPUT_TSO
cto.output();
#endif
}// while (cto.isPossible())
// finally push the base (v1,v2) to the order
V = ShellingOrderSet(2);
V[1] = v1;
V[2] = v2;
partition.pushFront(V);
#ifdef OUTPUT_TSO
cout << "output of the computed partition:" << endl;
int k = 1;
for (const ShellingOrderSet &S : partition) {
int size = S.len();
cout << "nodeset with nr " << k << ":" << endl;
for (int j=1; j<=size; j++)
cout << " node " << S[j] <<", ";
cout << "." << endl;
}
#endif
}// void TriconnectedShellingOrder::doCall
