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;
  }
}
Beispiel #2
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