Beispiel #1
0
int main(int argc, char *argv[]){

	Network yarp;
	Port armPlan;
	Port armPred;
	BufferedPort<Vector> objAngles;
	BufferedPort<Vector> armOut;
	armPlan.open("/learnedReach/plan");
	armPred.open("/learnedReach/pred");
	objAngles.open("/learnedReach/loc:i");
	armOut.open("/learnedReach/arm:o");
	bool fwCvOn = 0;
	fwCvOn = Network::connect("/learnedReach/plan","/fwdConv:i");
	fwCvOn *= Network::connect("/fwdConv:o","/learnedReach/pred");
	if (!fwCvOn){
		printf("Please run command:\n ./fwdConv --input /fwdConv:i --output /fwdConv:o\n");
		return -1;
	}

	Property params;
	params.fromCommand(argc,argv);

	if (!params.check("robot")){
		fprintf(stderr, "Please specify robot name\n");
		fprintf(stderr, "e.g. --robot icub\n");
		return -1;
	}
	std::string robotName = params.find("robot").asString().c_str();
	std::string remotePorts = "/";
	remotePorts += robotName;
	remotePorts += "/";
	if (params.check("arm")){
		remotePorts += params.find("arm").asString().c_str();
	}
	else{
		remotePorts += "left";
	}
	remotePorts += "_arm";
	std::string localPorts = "/learnedReach/cmd";
	if(!params.check("map")){
		fprintf(stderr, "Please specify learned visuomotor map file\n");
		fprintf(stderr, "e.g. --map map.dat\n");
		return -1;
	}
	string fName = params.find("map").asString().c_str();

	Property options;
	options.put("device", "remote_controlboard");
	options.put("local", localPorts.c_str());
	options.put("remote", remotePorts.c_str());

	PolyDriver robotDevice(options);
	if (!robotDevice.isValid()){
		printf("Device not available. Here are known devices: \n");
		printf("%s", Drivers::factory().toString().c_str());
		Network::fini();
		return 1;
	}

	IPositionControl *pos;
	IEncoders *enc;

	bool ok;
	ok = robotDevice.view(pos);
	ok = ok && robotDevice.view(enc);

	if (!ok){
		printf("Problems acquiring interfaces\n");
		return 0;
	}

	int nj = 0;
	pos->getAxes(&nj);
	Vector encoders;
	Vector command;
	Vector commandCart;
	Vector tmp;
	encoders.resize(nj);
	tmp.resize(nj);
	command.resize(nj);
	commandCart.resize(3);

	for (int i = 0; i < nj; i++) {
		tmp[i] = 25.0;
	}
	pos->setRefAccelerations(tmp.data());

	for (int i = 0; i < nj; i++) {
		tmp[i] = 20.0;
	    pos->setRefSpeed(i, tmp[i]);
	}

	command = 0;

	//set the arm joints to "middle" values
	command[0] = -45;
	command[1] = 45;
	command[2] = 0;
	command[3] = 45;
	pos->positionMove(command.data());

	bool done = false;
	while (!done){
		pos->checkMotionDone(&done);
		Time::delay(0.1);
	}

	//not really yaw and pitch
	int azMin = -80; int azMax = 0;
	int elMin = -60; int elMax = 0;
	int verMin = 0; int verMax = 20;

	int Y; int P; int V;
	int mmapSize; int usedJoints;

	//read in first lines to get map dimensions
	string line;
	string buf;
	ifstream mapFile(fName.c_str());
	if(mapFile.is_open()){
		getline(mapFile,line);
		stringstream ss(line);
		ss >> buf;
		Y = atoi(buf.c_str());
		ss >> buf;
		P = atoi(buf.c_str());
		ss >> buf;
		V = atoi(buf.c_str());
		ss.clear();
		getline(mapFile,line);
		ss.str(line);
		ss >> buf;
		mmapSize = atoi(buf.c_str());
		ss >> buf;
		usedJoints = atoi(buf.c_str());
	}
Beispiel #2
0
    bool configure(ResourceFinder &rf)
    {
        std::string robotname = rf.check("robot",yarp::os::Value("icubSim")).asString();
        Property options;
        options.put("robot", robotname); // typically from the command line.
        options.put("device", "remote_controlboard");

        string s("/");
        s += robotname;
        s += "/right_arm/keyBabbling";
        options.put("local", s.c_str());
        s.clear();
        s += "/";
        s += robotname;
        s += "/right_arm";
        options.put("remote", s.c_str());

        if (!dd.open(options)) {
            cout << "Device not available.  Here are the known devices:\n"<< endl;
            cout << Drivers::factory().toString().c_str() << endl;;
            return false;
        }
        
        portStreamer.open("/keyboardBabbling/stream:o");
        portFromCart.open("/keyboardBabbling/stream:i");
        portRpc.open("/keyboarBabbling/rpc");
        attach(portRpc);

        Property optionCart("(device cartesiancontrollerclient)");
        optionCart.put("remote", "/" + robotname +"/cartesianController/right_arm");
        optionCart.put("local","/keyboardBabbling/cartesian_client/right_arm");
        if (!driverCart.open(optionCart))
            return false;

        bool ok;
        ok = dd.view(pos);
        ok &= dd.view(vel);
        ok &= dd.view(pid);
        ok &= dd.view(amp);
        ok &= dd.view(armCtrlModeUsed);
        ok &= dd.view(armImpUsed);
        ok &= driverCart.view(armCart);
        ok &= dd.view(ilimRight);

        iCubFinger* fingerRight = new iCubFinger("right_index");
        deque<IControlLimits*> limRight;
        limRight.push_back(ilimRight);
        fingerRight->alignJointsBounds(limRight);

        if (!ok) {
            cout << "Device not able to acquire views" << endl;
            dd.close();
            return false;
        }

        int jnts = 0;
        pos->getAxes(&jnts);
        printf("Working with %d axes\n", jnts);

        vector<bool> mask;
        mask.resize(jnts);
        mask[0] = false;
        mask[1] = false;
        mask[2] = false;
        mask[3] = false;
        mask[4] = true;
        mask[5] = false;
        mask[6] = true;
        mask[7] = false;
        mask[8] = true;
        mask[9] = true;
        mask[10] = true;
        mask[11] = true;
        mask[12] = true;
        mask[13] = true;
        mask[14] = true;
        mask[15] = true;

        cout << "Closing the hand" << endl;
        // closing the hand:
        pos->positionMove(4, 30.0);
        pos->positionMove(6, 10.0);
        pos->positionMove(8, 10.0);
        pos->positionMove(9, 17.0);
        pos->positionMove(10, 170.0);
        pos->positionMove(11, 0.0);
        pos->positionMove(12, 0.0);
        pos->positionMove(13, 90.0);
        pos->positionMove(14, 180.0);
        pos->positionMove(15, 180.0);

        cout << "Waiting to be in initial position...";
        bool motionDone=false;
        while (!motionDone)
        {
            motionDone=true;
            for (int i=0; i<jnts; i++)
            {
                if (mask[i])
                {
                    bool jntMotionDone = false;
                    pos->checkMotionDone(i, &jntMotionDone);
                    motionDone &= jntMotionDone;
                }
            }

            Time::yield();  // to avoid killing cpu
        }
        cout << "ok" << endl;
        // wait for the hand to be in initial position

        Matrix R=zeros(3,3);
        R(0,0)=-1.0; R(1,1)=1; R(2,2)=-1.0;
        orientation=dcm2axis(R);
        
        // enable torso movements as well
        // in order to enlarge the workspace
        Vector dof;
        armCart->getDOF(dof);
        dof=1.0; dof[1]=0.0;    // every dof but the torso roll
        armCart->setDOF(dof,dof);
        armCart->setTrajTime(1.0);

        Vector initPos(3,0.0);
        if (rf.check("rightHandInitial"))
        {
            Bottle *botPos = rf.find("rightHandInitial").asList();
            initPos[0] = botPos->get(0).asDouble();
            initPos[1] = botPos->get(1).asDouble();
            initPos[2] = botPos->get(2).asDouble();
            cout<<"Reaching initial position with right hand"<<initPos.toString(3,3).c_str()<<endl;            
            armCart->goToPoseSync(initPos,orientation);
            armCart->waitMotionDone(0.1,3.0);
        }
        else
        {
            cout << "Cannot find the inital position" << endl << "closing module" << endl;
            return false;
        }        

        minY    = rf.find("min").asDouble();
        maxY    = rf.find("max").asDouble();
        gap     = rf.find("gap").asDouble();
        delay   = rf.find("delay").asDouble();
        thrMove = rf.find("threshold_move").asDouble();
        yDivisions = rf.check("yDivisions",Value(10)).asInt();
        Bottle* bsequenceToPlay = rf.find("sequence").asList();
        if (bsequenceToPlay)
        {
            cout << "Using sequence, not random." << endl;
            for (int i = 0; i < bsequenceToPlay->size(); i++)
                sequenceToPlay.push_back(bsequenceToPlay->get(i).asInt());
            indexInSequence = 0;
        }

        tempPos=initPos;
        state=up;
        forward = false;

        timeBeginIdle = Time::now();

        Rand::init();
        return true;
    }
	virtual void run(){
		tmp = command;
		(*command)[0] = -60*(gsl_rng_uniform(r));
		(*command)[1] = 100*(gsl_rng_uniform(r));
		(*command)[2] = -35 + 95*(gsl_rng_uniform(r));
		(*command)[3] = 10 + 90*(gsl_rng_uniform(r));
		printf("%.1lf %.1lf %.1lf %.1lf\n", (*command)[0], (*command)[1], (*command)[2], (*command)[3]);
		//above 0 doesn't seem to be safe for joint 0
		if ((*command)[0] > 0 || (*command)[0] < -60){
			(*command)[0] = (*tmp)[0];
		}
		if ((*command)[1] > 100 || (*command)[1] < -0){
			(*command)[1] = (*tmp)[1];
		}
		if ((*command)[2] > 60 || (*command)[2] < -35){
			(*command)[2] = (*tmp)[2];
		}
		if ((*command)[3] > 100 || (*command)[3] < 10){
			(*command)[3] = (*tmp)[3];
		}
		//use fwd kin to find end effector position
		Bottle plan, pred;
		for (int i = 0; i < nj; i++){
			plan.add((*command)[i]);
		}
		armPlan->write(plan);
		armPred->read(pred);
		Vector commandCart(3);
		for (int i = 0; i < 3; i++){
			commandCart[i] = pred.get(i).asDouble();
		}
		printf("Cartesian safety check\n");
		double rad = sqrt(commandCart[0]*commandCart[0]+commandCart[1]*commandCart[1]);
		// safety radius back to 30 cm
		if (rad > 0.3){
			pos->positionMove(command->data());
			bool done = false;
			while(!done){
				pos->checkMotionDone(&done);
				Time::delay(0.1);
			}
			printf("Moved arm to new location\n");
			Vector &armJ = armLocJ->prepare();
			Vector encoders(nj);
			enc->getEncoders(encoders.data());
			armJ = encoders;
			Vector noisyArm(3);
			for(int i = 0; i < 3; i++){
				//noisyArm[i] = commandCart[i] + 0.01*(2*gsl_rng_uniform(r)-1);
				//sanity check
				noisyArm[i] = commandCart[i] + 0.005*(2*gsl_rng_uniform(r)-1);
			}
			//insert here:
			//read off peak saliences
			//fixate there
			//calculate cartesian value, compare to real cart. value of arm


			printf("Looking at arm\n");
			igaze->lookAtFixationPoint(noisyArm);
			done = false;
			while(!done){
				igaze->checkMotionDone(&done);
				Time::delay(0.5);
			}
			//igaze->waitMotionDone(0.1,30);

			printf("Saw arm\n");

			Vector &headAng = headLoc->prepare();
			igaze->getAngles(headAng);
			Bottle tStamp;
			tStamp.clear();
			tStamp.add(Time::now());
			headLoc->setEnvelope(tStamp);


			headLoc->write();
			armLocJ->write();
			headLoc->unprepare();
			armLocJ->unprepare();

		}
		else{
			printf("Self collision detected!\n");
		}
	}
	virtual bool threadInit(){
		if(!handleParams()){
			return false;
		}

		armPlan = new Port;
		armPred = new Port;
		armLocJ = new BufferedPort<Vector>;
		headLoc = new BufferedPort<Vector>;

		armPlan->open("/babbleTrack/plan:o");
		armPred->open("/babbleTrack/pred:i");
		armLocJ->open("/babbleTrack/arm:o");
		headLoc->open("/babbleTrack/head:o");

		gsl_rng_env_setup();
		T = gsl_rng_default;
		r = gsl_rng_alloc(T);

		igaze = NULL;

		Property options;
		options.put("device","gazecontrollerclient");
		options.put("remote","/iKinGazeCtrl");
		options.put("local","/client/gaze");

		clientGazeCtrl = new PolyDriver;
		clientGazeCtrl->open(options);

		options.clear();
		string localPorts = "/babbleTrack/cmd";
		string remotePorts = "/" + robotName + "/" + arm + "_arm";

		options.put("device", "remote_controlboard");
		options.put("local", localPorts.c_str());
		options.put("remote", remotePorts.c_str());

		robotDevice = new PolyDriver;
		robotDevice->open(options);

		if(clientGazeCtrl->isValid()){
			clientGazeCtrl->view(igaze);
		}
		else{
			return false;
		}


		if (!robotDevice->isValid()){
			printf("Device not available. Here are known devices: \n");
			printf("%s", Drivers::factory().toString().c_str());
			Network::fini();
			return false;
		}

		bool ok;
		ok = robotDevice->view(pos);
		ok = ok && robotDevice->view(enc);

		if (!ok){
			printf("Problems acquiring interfaces\n");
			return false;
		}

		pos->getAxes(&nj);

		command = new Vector;
		tmp = new Vector;
		command->resize(nj);
		tmp->resize(nj);

		for (int i = 0; i < nj; i++) {
		         (*tmp)[i] = 25.0;
		}
		pos->setRefAccelerations(tmp->data());

		for (int i = 0; i < nj; i++) {
			(*tmp)[i] = 20.0;
			pos->setRefSpeed(i, (*tmp)[i]);
		}

		*command = 0;

		//set the arm joints to "middle" values
		(*command)[0] = -45;
		(*command)[1] = 45;
		(*command)[2] = 0;
		(*command)[3] = 45;

		//flex hand
		(*command)[4] = 60;
		(*command)[7] = 20;
		(*command)[10] = 15;
		(*command)[11] = 15;
		(*command)[12] = 15;
		(*command)[13] = 15;
		(*command)[14] = 15;
		(*command)[15] = 15;
		pos->positionMove(command->data());

		bool done = false;
		while (!done){
			pos->checkMotionDone(&done);
			Time::delay(0.1);
		}

		bool fwCvOn = 0;
		fwCvOn = Network::connect("/babbleTrack/plan:o","/fwdConv:i");
		fwCvOn *= Network::connect("/fwdConv:o","/babbleTrack/pred:i");
		if (!fwCvOn){
			printf("Please run command:\n ./fwdConv --input /fwdConv:i --output /fwdConv:o\n");
			return false;
		}

		return true;
	}
Beispiel #5
0
int main(int argc, char **argv)
{
    Network yarp;

    printf("Going to stress rpc connections to the robot\n");
    printf("Run as --id unique-id\n");
    printf("Optionally:\n");
    printf("--part robot-part\n");
    printf("--prot protocol\n");
    printf("--time dt (seconds)\n");
    printf("--robot name \n");

    Property parameters;
    parameters.fromCommand(argc, argv);

    ConstString part=parameters.find("part").asString();
    int id=parameters.find("id").asInt();
    double time=0;
    if (parameters.check("time"))
        {
            time=parameters.find("time").asDouble();
        }
    else
        time=-1;

    ConstString protocol;
    if (parameters.check("prot"))
    {
        protocol=parameters.find("prot").asString();
    }
    else
        protocol="tcp";

    ConstString rname;
    if (parameters.check("robot"))
    {
        rname=parameters.find("robot").asString();
    }
    else
        rname="controlboard";

    PolyDriver dd;
    Property p;

    Random::seed((unsigned int)Time::now());
     
    string remote=string("/")+rname.c_str();
    if (part!="")
        {
            remote+=string("/");
            remote+=part;
        }
    string local=string("/")+string(rname.c_str());
    if (part!="")
        {
            local+=string("/");
            local+=part;
        }
    local+=string("/stress");

    stringstream lStream; 
    lStream << id;
    local += lStream.str();

    p.put("device", "remote_controlboard");
    p.put("local", local.c_str());
    p.put("remote", remote.c_str());
    p.put("carrier", protocol.c_str());
    dd.open(p);
    
    if (!dd.isValid())
    {
        fprintf(stderr, "Error, could not open controlboard\n");
        return -1;
    }

    IEncoders *ienc;
    IPositionControl *ipos;
    IAmplifierControl *iamp;
    IPidControl *ipid;
    IControlLimits *ilim;
    IControlCalibration2 *ical;

    dd.view(ienc);
    dd.view(ipos);
    dd.view(iamp);
    dd.view(ipid);
    dd.view(ilim);
    dd.view(ical);
    
    int c=100;
    int nj;
    Vector encoders;
    ienc->getAxes(&nj);
    encoders.resize(nj);

    int count=0;
    bool done=false;
    double startT=Time::now();
    double now=0;
    while((!done) || (time==-1))
        {
            count++;
            double v;
            int jj=0;
            
            for(jj=0; jj<nj; jj++)
                {
                    //    ienc->getEncoder(jj, encoders.data()+jj);
                    //    iamp->enableAmp(jj);
                    //    ilim->setLimits(jj, 0, 0);
                    //    double max;
                    //    double min;
                    //    ilim->getLimits(jj, &min, &max);
                    fprintf(stderr, ".");
                    // Pid pid;
                    // ipid->getPid(jj, &pid);
                    bool done;
                    ipos->checkMotionDone(jj, &done);
                    fprintf(stderr, "#\n");
                }

            fprintf(stderr, "%u\n", count);

            Time::delay(0.1);

            now=Time::now();
            if ((now-startT)>time)
                done=true;
        }

    printf("bye bye from %d\n", id);
    
    return 0;
}
bool partMover::entry_update(partMover *currentPart)
{
  GdkColor color_black;
  GdkColor color_grey;
  GdkColor color_yellow;
  GdkColor color_green;
  GdkColor color_green_blue;
  GdkColor color_dark_green;
  GdkColor color_red;
  GdkColor color_fault_red;
  GdkColor color_pink;
  GdkColor color_indaco;
  GdkColor color_white;
  GdkColor color_blue;
  
  color_pink.red=219*255;
  color_pink.green=166*255;
  color_pink.blue=171*255;

  color_fault_red.red=255*255;
  color_fault_red.green=10*255;
  color_fault_red.blue=10*255;

  color_black.red=10*255;
  color_black.green=10*255;
  color_black.blue=10*255;

  color_red.red=255*255;
  color_red.green=100*255;
  color_red.blue=100*255;

  color_grey.red=220*255;
  color_grey.green=220*255;
  color_grey.blue=220*255;

  color_white.red=250*255;
  color_white.green=250*255;
  color_white.blue=250*255;

  color_green.red=149*255;
  color_green.green=221*255;
  color_green.blue=186*255;

  color_dark_green.red=(149-30)*255;
  color_dark_green.green=(221-30)*255;
  color_dark_green.blue=(186-30)*255;

  color_blue.red=150*255;
  color_blue.green=190*255;
  color_blue.blue=255*255;

  color_green_blue.red=(149+150)/2*255;
  color_green_blue.green=(221+190)/2*255;
  color_green_blue.blue=(186+255)/2*255;

  color_indaco.red=220*255;
  color_indaco.green=190*255;
  color_indaco.blue=220*255;

  color_yellow.red=249*255;
  color_yellow.green=236*255;
  color_yellow.blue=141*255;
  
  GdkColor* pColor= &color_grey;

  static int slowSwitcher = 0;

  IControlMode     *ictrl = currentPart->ctrlmode2;
  IInteractionMode *iint  = currentPart->iinteract;
  IPositionControl  *ipos = currentPart->pos;
  IVelocityControl  *ivel = currentPart->iVel;
  IPositionDirect   *iDir = currentPart->iDir;
  IEncoders       *iiencs = currentPart->iencs;
  ITorqueControl    *itrq = currentPart->trq;
  IAmplifierControl *iamp = currentPart->amp;

  GtkEntry * *pos_entry   = (GtkEntry **)  currentPart->currPosArray;
  GtkEntry  **trq_entry   = (GtkEntry **)  currentPart->currTrqArray;
  GtkEntry  **speed_entry = (GtkEntry **)  currentPart->currSpeedArray;
  GtkEntry    **inEntry   = (GtkEntry **)  currentPart->inPosArray;
  GtkWidget **colorback   = (GtkWidget **) currentPart->frameColorBack;

  GtkWidget **sliderAry = currentPart->sliderArray;
  bool *POS_UPDATE = currentPart->CURRENT_POS_UPDATE;

  char buffer[40] = {'i', 'n', 'i', 't'};
  char frame_title [255];

  double positions[MAX_NUMBER_OF_JOINTS];
  double torques[MAX_NUMBER_OF_JOINTS];
  double speeds[MAX_NUMBER_OF_JOINTS];
  double max_torques[MAX_NUMBER_OF_JOINTS];
  double min_torques[MAX_NUMBER_OF_JOINTS];
  static int controlModes[MAX_NUMBER_OF_JOINTS];
  static int controlModesOld[MAX_NUMBER_OF_JOINTS];
  static yarp::dev::InteractionModeEnum interactionModes[MAX_NUMBER_OF_JOINTS];
  static yarp::dev::InteractionModeEnum interactionModesOld[MAX_NUMBER_OF_JOINTS];

  int k;
  int NUMBER_OF_JOINTS=0;
  bool done = false;
  bool ret = false;
  ipos->getAxes(&NUMBER_OF_JOINTS);

  if (NUMBER_OF_JOINTS == 0)
  {
      fprintf(stderr,"Lost connection with iCubInterface. You should save and restart.\n" );
      Time::delay(0.1);
      pColor=&color_grey;
      strcpy(frame_title,"DISCONNECTED");
      for (k = 0; k < MAX_NUMBER_OF_JOINTS; k++)
      {   
          if (currentPart->framesArray[k]!=0)
          {
              gtk_frame_set_label   (GTK_FRAME(currentPart->framesArray[k]),frame_title);
              gtk_widget_modify_bg (colorback[k], GTK_STATE_NORMAL, pColor);
          }
      }
      return true;
  }

  for (k = 0; k < NUMBER_OF_JOINTS; k++) 
  {
      max_torques[k]=0;
      min_torques[k]=0;
      torques[k]=0;
  }

  if (!iiencs->getEncoders(positions)) 
      return true;
  itrq->getTorques(torques);
  iiencs->getEncoderSpeeds(speeds);
  
  //update all joints positions
  for (k = 0; k < NUMBER_OF_JOINTS; k++)
    {
      sprintf(buffer, "%.1f", positions[k]);  
      gtk_entry_set_text((GtkEntry*) pos_entry[k],  buffer);
      sprintf(buffer, "%.3f", torques[k]);  
      gtk_entry_set_text((GtkEntry*) trq_entry[k],  buffer);
      sprintf(buffer, "%.1f", speeds[k]);  
      gtk_entry_set_text((GtkEntry*) speed_entry[k],  buffer);
    }
  //update all joint sliders
  for (k = 0; k < NUMBER_OF_JOINTS; k++) 
    if(POS_UPDATE[k])
      gtk_range_set_value((GtkRange*)sliderAry[k],  positions[k]);

  // *** update the checkMotionDone box section ***
  // (only one at a time in order to save badwidth)
  k = slowSwitcher%NUMBER_OF_JOINTS;
  slowSwitcher++;
#if DEBUG_GUI
  gtk_entry_set_text((GtkEntry*) inEntry[k],  "off");
#else
  ipos->checkMotionDone(k, &done);
  if (!done)
      gtk_entry_set_text((GtkEntry*) inEntry[k],  " "); 
  else
      gtk_entry_set_text((GtkEntry*) inEntry[k],  "@");
#endif

  // *** update the controlMode section ***
  // the new icubinterface does not increase the bandwidth consumption
  // ret = true; useless guys!
  ret=ictrl->getControlModes(controlModes);
  if (ret==false) fprintf(stderr,"ictrl->getControlMode failed\n" );
  ret=iint->getInteractionModes(interactionModes);
  if (ret==false) fprintf(stderr,"iint->getInteractionlMode failed\n" );

  for (k = 0; k < NUMBER_OF_JOINTS; k++)
  {
      if (currentPart->first_time==false && controlModes[k] == controlModesOld[k]) continue;
      controlModesOld[k]=controlModes[k];
      sprintf(frame_title,"Joint %d ",k );

      switch (controlModes[k])
      {
          case VOCAB_CM_IDLE:
              pColor=&color_yellow;
                strcat(frame_title," (IDLE)");
                gtk_frame_set_label   (GTK_FRAME(currentPart->framesArray[k]),frame_title);
              gtk_widget_modify_bg (colorback[k], GTK_STATE_NORMAL, pColor);
          break;
          case VOCAB_CM_POSITION:
              pColor=&color_green;
              strcat(frame_title," (POSITION)");
              gtk_frame_set_label   (GTK_FRAME(currentPart->framesArray[k]),frame_title);
              gtk_frame_set_label   (GTK_FRAME(currentPart->frame_slider1[k]),"Position:");
              gtk_frame_set_label   (GTK_FRAME(currentPart->frame_slider2[k]),"Velocity:");
              gtk_widget_modify_bg (colorback[k], GTK_STATE_NORMAL, pColor);
          break;
          case VOCAB_CM_POSITION_DIRECT:
              pColor=&color_dark_green;
              strcat(frame_title," (POSITION_DIRECT)");
              gtk_frame_set_label   (GTK_FRAME(currentPart->framesArray[k]),frame_title);
              gtk_frame_set_label   (GTK_FRAME(currentPart->frame_slider1[k]),"Position:");
              gtk_frame_set_label   (GTK_FRAME(currentPart->frame_slider2[k]),"---");
              gtk_widget_modify_bg (colorback[k], GTK_STATE_NORMAL, pColor);
          break;
          case VOCAB_CM_MIXED:
              pColor=&color_green_blue;
              strcat(frame_title," (MIXED_MODE)");
              gtk_frame_set_label   (GTK_FRAME(currentPart->framesArray[k]),frame_title);
              gtk_frame_set_label   (GTK_FRAME(currentPart->frame_slider1[k]),"Position:");
              gtk_frame_set_label   (GTK_FRAME(currentPart->frame_slider2[k]),"Velocity");
              gtk_widget_modify_bg (colorback[k], GTK_STATE_NORMAL, pColor);
          break;
          case VOCAB_CM_VELOCITY:
              pColor=&color_blue;
              strcat(frame_title," (VELOCITY)");
              gtk_frame_set_label   (GTK_FRAME(currentPart->framesArray[k]),frame_title);
              gtk_widget_modify_bg (colorback[k], GTK_STATE_NORMAL, pColor);
          break;
          case VOCAB_CM_TORQUE:
              pColor=&color_pink;
              strcat(frame_title," (TORQUE)");
                gtk_frame_set_label   (GTK_FRAME(currentPart->framesArray[k]),frame_title);
              gtk_frame_set_label   (GTK_FRAME(currentPart->frame_slider1[k]),"Torque:");
              gtk_frame_set_label   (GTK_FRAME(currentPart->frame_slider2[k]),"Torque2:");
              gtk_widget_modify_bg (colorback[k], GTK_STATE_NORMAL, pColor);
              break;
          case VOCAB_CM_IMPEDANCE_POS:
              pColor=&color_indaco;
              strcat(frame_title," (IMPEDANCE POS)");
                gtk_frame_set_label   (GTK_FRAME(currentPart->framesArray[k]),frame_title);
              gtk_widget_modify_bg (colorback[k], GTK_STATE_NORMAL, pColor);
          break;
            case VOCAB_CM_IMPEDANCE_VEL:
              pColor=&color_indaco;
              strcat(frame_title," (IMPEDANCE VEL)");
                gtk_frame_set_label   (GTK_FRAME(currentPart->framesArray[k]),frame_title);
              gtk_widget_modify_bg (colorback[k], GTK_STATE_NORMAL, pColor);
          break;
          case VOCAB_CM_OPENLOOP:
              pColor=&color_white;
              strcat(frame_title," (OPENLOOP)");
                gtk_frame_set_label   (GTK_FRAME(currentPart->framesArray[k]),frame_title);
              gtk_widget_modify_bg (colorback[k], GTK_STATE_NORMAL, pColor);
          break;
          case VOCAB_CM_HW_FAULT:
              pColor=&color_fault_red;
              strcat(frame_title," (HARDWARE_FAULT)");
              gtk_frame_set_label   (GTK_FRAME(currentPart->framesArray[k]),frame_title);
              gtk_frame_set_label   (GTK_FRAME(currentPart->frame_slider1[k]),"---");
              gtk_frame_set_label   (GTK_FRAME(currentPart->frame_slider2[k]),"---");
              gtk_widget_modify_bg (colorback[k], GTK_STATE_NORMAL, pColor);
              break;
         case VOCAB_CM_CALIBRATING:
              pColor=&color_grey;
              strcat(frame_title," (CALIBRATING)");
              gtk_frame_set_label   (GTK_FRAME(currentPart->framesArray[k]),frame_title);
              gtk_widget_modify_bg (colorback[k], GTK_STATE_NORMAL, pColor);
          break;
         case VOCAB_CM_CALIB_DONE:
              pColor=&color_grey;
              strcat(frame_title," (CALIB DONE)");
              gtk_frame_set_label   (GTK_FRAME(currentPart->framesArray[k]),frame_title);
              gtk_widget_modify_bg (colorback[k], GTK_STATE_NORMAL, pColor);
          break;
          case VOCAB_CM_NOT_CONFIGURED:
              pColor=&color_grey;
              strcat(frame_title," (NOT CONFIGURED)");
              gtk_frame_set_label   (GTK_FRAME(currentPart->framesArray[k]),frame_title);
              gtk_widget_modify_bg (colorback[k], GTK_STATE_NORMAL, pColor);
          break;
          case VOCAB_CM_CONFIGURED:
              pColor=&color_grey;
              strcat(frame_title," (CONFIGURED)");
              gtk_frame_set_label   (GTK_FRAME(currentPart->framesArray[k]),frame_title);
              gtk_widget_modify_bg (colorback[k], GTK_STATE_NORMAL, pColor);
          break;
          default:
          case VOCAB_CM_UNKNOWN:
              pColor=&color_grey;
              strcat(frame_title," (UNKNOWN)");
              gtk_frame_set_label   (GTK_FRAME(currentPart->framesArray[k]),frame_title);
              gtk_widget_modify_bg (colorback[k], GTK_STATE_NORMAL, pColor);
          break;
      }
  }
  for (k = 0; k < NUMBER_OF_JOINTS; k++)
  {
      if (currentPart->first_time==false && interactionModes[k] == interactionModesOld[k]) continue;
      interactionModesOld[k]=interactionModes[k];
      switch (interactionModes[k])
      {
           case VOCAB_IM_STIFF:
               gtk_widget_modify_base ((GtkWidget*)inEntry[k], GTK_STATE_NORMAL, &color_green);
           break;
           case VOCAB_IM_COMPLIANT:
               gtk_widget_modify_base ((GtkWidget*)inEntry[k], GTK_STATE_NORMAL, &color_fault_red);
           break;
           default:
           case VOCAB_CM_UNKNOWN:
               gtk_widget_modify_base ((GtkWidget*)inEntry[k], GTK_STATE_NORMAL, &color_white);
           break;
      }
  }

  currentPart->first_time =false;
  return true;
    
}
Beispiel #7
0
int main()
{
	Network::init();
    srand(time(NULL));
    
    Property options;
    options.put("robot", "icub"); // typically from the command line.
    options.put("device", "remote_controlboard");

    Value& robotname = options.find("robot");
    string s("/");
    s += robotname.asString();
    s += "/right_arm/babbling";
    options.put("local", s.c_str());
    s.clear();
    s += "/";
    s += robotname.asString();
    s += "/right_arm";
    options.put("remote", s.c_str());
    

    PolyDriver dd(options);
    if (!dd.isValid()) {
        cout << "Device not available.  Here are the known devices:\n"<< endl;
        cout << Drivers::factory().toString().c_str() << endl;;
        Network::fini();
        return 0;
    }

    IPositionControl *pos;
    IVelocityControl *vel;
    IEncoders *enc;
    IPidControl *pid;
    IAmplifierControl *amp;
    IControlLimits *lim;

    bool ok;
    ok = dd.view(pos);
    ok &= dd.view(vel);
    ok &= dd.view(enc);
    ok &= dd.view(pid);
    ok &= dd.view(amp);
    ok &= dd.view(lim);

	if (!ok) {
		cout << "Device not able to acquire views" << endl;
		Network::fini();
		dd.close();
		return 0;
	}


     int jnts = 0;
     pos->getAxes(&jnts);
     printf("Working with %d axes\n", jnts);

     
     // limits

     vector<pair<int, int> > vLimitJoints;
     for (int i = 0; i < jnts; i++) {
         double min, max;
         lim->getLimits(i, &min, &max);
         vLimitJoints.push_back(pair<int, int> ((int)min, (int)max));
     }

     // get value of the arm
     // we move : 4 5 6 7 8 9 10 11 12 13 14 15 
     // we don't move: 0 1 2 3
    
	 vector<bool> mask;
	 mask.resize(jnts);
	 mask[0]	= false;
	 mask[1]	= false;
	 mask[2]	= false;
	 mask[3]	= false;
	 mask[4] = false;
	 mask[5] = false;
	 mask[6] = false;
	 mask[7] = false;
	 mask[8] = false;
	 mask[9] = false;
	 mask[10] = false;
	 mask[11] = true;
	 mask[12] = true;
	 mask[13] = true;
	 mask[14] = true;
	 mask[15] = true;
     
	 Vector tmp;
	 tmp.resize(jnts,0.0);

	 for (int i = 4; i < jnts; i++)
	 {
		pos->positionMove(i, 0.0);
	 }

	 bool initDone = false;
	 while (!initDone)
	 {
		 initDone = true;
		 for (int i = 4; i < jnts; i++)
		 {
				 bool jntMotionDone = false;
				 pos->checkMotionDone(i, &jntMotionDone);
				 initDone &= jntMotionDone;
			 }
	 }



     while(true)
     {
		 cout << "Moving to new posture..." << endl;

         for (int i = 4; i < jnts; i++) 
		 {
			 if (mask[i])
			 {
				 double newValue = yarp::os::Random::uniform(vLimitJoints[i].first, vLimitJoints[i].second);
				 tmp[i] = newValue;
				 pos->positionMove(i, tmp[i]);
			 }
		 }

		 cout << "Waiting for posture to be reached... ("<<tmp.toString(3,3)<<" ) ..." ;

		 bool motionDone = false;
		 while (!motionDone)
		 {
			 motionDone = true;
			 for (int i = 4; i < jnts; i++)
			 {
				 if (mask[i])
				 {
					 bool jntMotionDone = false;
					 pos->checkMotionDone(i, &jntMotionDone);
					 motionDone &= jntMotionDone;
				 }
			 }
		 }
		 Time::delay(15.0);
		 cout << "ok" << endl;
     }

	 dd.close();

    return 0;
}
Beispiel #8
0
int main(int argc, char *argv[]) 
{
    Network yarp;

    Property params;
    params.fromCommand(argc, argv);

    if (!params.check("robot"))
    {
        fprintf(stderr, "Please specify the name of the robot\n");
        fprintf(stderr, "--robot name (e.g. icub)\n");
        return -1;
    }
    std::string robotName=params.find("robot").asString().c_str();
    std::string remotePorts="/";
    remotePorts+=robotName;
    remotePorts+="/left_arm";

    std::string localPorts="/test/client";

    Property options;
    options.put("device", "remote_controlboard");
    options.put("local", localPorts.c_str());   //local port names
    options.put("remote", remotePorts.c_str());         //where we connect to

    // create a device
    PolyDriver robotDevice(options);
    if (!robotDevice.isValid()) {
        printf("Device not available.  Here are the known devices:\n");
        printf("%s", Drivers::factory().toString().c_str());
        return 0;
    }

    IPositionControl *pos;
    IEncoders *encs;

    bool ok;
    ok = robotDevice.view(pos);
    ok = ok && robotDevice.view(encs);

    if (!ok) {
        printf("Problems acquiring interfaces\n");
        return 0;
    }

    int nj=0;
    pos->getAxes(&nj);
    Vector encoders;
    Vector command;
    Vector tmp;
    encoders.resize(nj);
    tmp.resize(nj);
    command.resize(nj);
    
    int i;
    for (i = 0; i < nj; i++) {
         tmp[i] = 50.0;
    }
    pos->setRefAccelerations(tmp.data());

    for (i = 0; i < nj; i++) {
        tmp[i] = 4.0;
        pos->setRefSpeed(i, tmp[i]);
    }

    //pos->setRefSpeeds(tmp.data()))
    
    //fisrst zero all joints
    //
    command=0;
    //now set the shoulder to some value
    command[0]=-26.0;
    command[1]=20.0;
    command[2]=0.0;
    command[3]=49.0;
    command[4]=0.0;

    pos->positionMove(command.data());
    
    bool done=false;
    while(!done) {
        Time::delay(0.5);
        pos->checkMotionDone(&done);
    }

    printf("\niCub @ HOME. Press any key...");
    mygetch();

    int times=0;
    while(true)
    {
        times++;
        if (times%2)
        {
             printf("\n\nSet pos1: ");
             //command[0]=-50;
             command[1]=64.0;
             //command[2]=-10;
             //command[3]=50;
             //command[4]=0;
        }
        else
        {
             printf("\n\nSet pos2: ");
             //command[0]=-20;
             command[1]=20.0;
             //command[2]=-10;
             //command[3]=30;
             //command[4]=0;
        }

        pos->positionMove(command.data());

        printf("waiting");
        bool done3=false;
        while(!done3) {
            Time::delay(1.0);
            pos->checkMotionDone(&done3);
            printf(".");
        }
            printf("ok!\n");
    mygetch();

    }


    robotDevice.close();
    
    return 0;
}
Beispiel #9
0
int main(int argc, char *argv[])
{
    Network yarp;

    int maxSpeed;
    Property params;
    params.fromCommand(argc, argv);

    if (!params.check("robot"))
    {
        fprintf(stderr, "Please specify the name of the robot\n");
        fprintf(stderr, "--robot name (e.g. icub)\n");
        return -1;
    }

    if (!params.check("repetitions"))
    {
        fprintf(stderr, "Please specify number of repetitions\n");
        fprintf(stderr, "--repetitions num (e.g. 10)\n");
        return -1;
    }

    if (!params.check("speed"))
    {
        fprintf(stderr, "Speed not specified using default\n");
        fprintf(stderr, "--speed num (e.g. 2)\n");
        maxSpeed = 10.0;
    }
    else
    {
        maxSpeed = params.find("speed").asInt();
    }

    // sanity check on argument value
    if(maxSpeed <0 || maxSpeed>50)
    {
        maxSpeed = 10;
    }

    std::string robotName=params.find("robot").asString().c_str();
    std::string remotePorts="/";
    remotePorts+=robotName;
    remotePorts+="/head";

    int numTimes = params.find("repetitions").asInt();
    std::string localPorts="/headMovement_koroibot/client";

    Property options;
    options.put("device", "remote_controlboard");
    options.put("local", localPorts.c_str());   //local port names
    options.put("remote", remotePorts.c_str());         //where we connect to

    // create a device
    PolyDriver robotDevice(options);
    if (!robotDevice.isValid()) {
        printf("Device not available.  Here are the known devices:\n");
        printf("%s", Drivers::factory().toString().c_str());
        return 0;
    }

    IPositionControl *pos;
    IEncoders *encs;

    bool ok;
    ok = robotDevice.view(pos);
    ok = ok && robotDevice.view(encs);

    if (!ok) {
        printf("Problems acquiring interfaces\n");
        return 0;
    }

    int nj=0;
    pos->getAxes(&nj);
    Vector encoders;
    Vector command;
    Vector tmp;
    encoders.resize(nj);
    tmp.resize(nj);
    command.resize(nj);

    int i;
    for (i = 0; i < nj; i++) {
        tmp[i] = 50.0;
    }
    pos->setRefAccelerations(tmp.data());

    for (i = 0; i < nj; i++) {
        tmp[i] = 10.0;
        pos->setRefSpeed(i, tmp[i]);
    }

    //first read all encoders
    printf("waiting for encoders");
    while(!encs->getEncoders(encoders.data()))
    {
        Time::delay(0.1);
        printf(".");
    }
    printf("\n;");

    int ctr =0;
    bool done = false;
    while(ctr<numTimes)
    {


        printf("Starting headMovement\n");
        command=encoders;
        if(ctr%2 == 0)
            command[2]=HEAD_YAW_MAX;
        else
            command[2]=HEAD_YAW_MIN;
        done = false;
        pos->positionMove(command.data());
        while(!done)
        {
            pos->checkMotionDone(&done);
            Time::delay(0.1);
        }
        ctr++;
    }

    command[2] = 0;
    pos->positionMove(command.data());
    while(!done)
    {

        pos->checkMotionDone(&done);
        Time::delay(0.1);
    }

    robotDevice.close();

    return 0;
}
Beispiel #10
0
bool partMover::entry_update(partMover *currentPart)
{
  GdkColor color_grey;
  GdkColor color_yellow;
  GdkColor color_green;
  GdkColor color_red;
  GdkColor color_pink;
  GdkColor color_indaco;
  GdkColor color_white;
  GdkColor color_blue;
  
  color_pink.red=219*255;
  color_pink.green=166*255;
  color_pink.blue=171*255;

  color_red.red=255*255;
  color_red.green=100*255;
  color_red.blue=100*255;

  color_grey.red=220*255;
  color_grey.green=220*255;
  color_grey.blue=220*255;

  color_white.red=250*255;
  color_white.green=250*255;
  color_white.blue=250*255;

  color_green.red=149*255;
  color_green.green=221*255;
  color_green.blue=186*255;

  color_blue.red=150*255;
  color_blue.green=190*255;
  color_blue.blue=255*255;

  color_indaco.red=220*255;
  color_indaco.green=190*255;
  color_indaco.blue=220*255;

  color_yellow.red=249*255;
  color_yellow.green=236*255;
  color_yellow.blue=141*255;
  
  GdkColor* pColor= &color_grey;

  static int slowSwitcher = 0;

  IControlMode     *ictrl = currentPart->ctrlmode;
  IPositionControl  *ipos = currentPart->pos;
  IVelocityControl  *ivel = currentPart->iVel;
  IEncoders       *iiencs = currentPart->iencs;
  ITorqueControl    *itrq = currentPart->trq;
  IAmplifierControl *iamp = currentPart->amp;

  GtkEntry * *pos_entry   = (GtkEntry **)  currentPart->currPosArray;
  GtkEntry  **trq_entry   = (GtkEntry **)  currentPart->currTrqArray;
  GtkEntry  **speed_entry = (GtkEntry **)  currentPart->currSpeedArray;
  GtkEntry    **inEntry   = (GtkEntry **)  currentPart->inPosArray;
  GtkWidget **colorback   = (GtkWidget **) currentPart->frameColorBack;

  GtkWidget **sliderAry = currentPart->sliderArray;
  bool *POS_UPDATE = currentPart->CURRENT_POS_UPDATE;

  char buffer[40] = {'i', 'n', 'i', 't'};
  char frame_title [255];

  double positions[MAX_NUMBER_OF_JOINTS];
  double torques[MAX_NUMBER_OF_JOINTS];
  double speeds[MAX_NUMBER_OF_JOINTS];
  double max_torques[MAX_NUMBER_OF_JOINTS];
  double min_torques[MAX_NUMBER_OF_JOINTS];
  static int controlModes[MAX_NUMBER_OF_JOINTS];
  static int controlModesOld[MAX_NUMBER_OF_JOINTS];

  int k;
  int NUMBER_OF_JOINTS=0;
  bool done = false;
  bool ret = false;
  ipos->getAxes(&NUMBER_OF_JOINTS);

  if (NUMBER_OF_JOINTS == 0)
  {
      fprintf(stderr,"Lost connection with iCubInterface. You should save and restart.\n" );
      Time::delay(0.1);
      pColor=&color_grey;
      strcpy(frame_title,"DISCONNECTED");
      for (k = 0; k < MAX_NUMBER_OF_JOINTS; k++)
      {   
          if (currentPart->framesArray[k]!=0)
          {
              gtk_frame_set_label   (GTK_FRAME(currentPart->framesArray[k]),frame_title);
              gtk_widget_modify_bg (colorback[k], GTK_STATE_NORMAL, pColor);
          }
      }
      return true;
  }

  for (k = 0; k < NUMBER_OF_JOINTS; k++) 
  {
	  max_torques[k]=0;
	  min_torques[k]=0;
	  torques[k]=0;
  }

  if (!iiencs->getEncoders(positions)) 
	  return true;
  itrq->getTorques(torques);
  iiencs->getEncoderSpeeds(speeds);
  
  //update all joints positions
  for (k = 0; k < NUMBER_OF_JOINTS; k++)
    {
      sprintf(buffer, "%.1f", positions[k]);  
      gtk_entry_set_text((GtkEntry*) pos_entry[k],  buffer);
      sprintf(buffer, "%.3f", torques[k]);  
	  gtk_entry_set_text((GtkEntry*) trq_entry[k],  buffer);
      sprintf(buffer, "%.1f", speeds[k]);  
	  gtk_entry_set_text((GtkEntry*) speed_entry[k],  buffer);
    }
  //update all joint sliders
  for (k = 0; k < NUMBER_OF_JOINTS; k++) 
    if(POS_UPDATE[k])
      gtk_range_set_value((GtkRange*)sliderAry[k],  positions[k]);

  // *** update the checkMotionDone box section ***
  // (only one at a time in order to save badwidth)
  k = slowSwitcher%NUMBER_OF_JOINTS;
  slowSwitcher++;
#if DEBUG_GUI
  gtk_entry_set_text((GtkEntry*) inEntry[k],  "off");
#else
  ipos->checkMotionDone(k, &done);
  if (!done)
      gtk_entry_set_text((GtkEntry*) inEntry[k],  " "); 
  else
      gtk_entry_set_text((GtkEntry*) inEntry[k],  "@");
#endif

  // *** update the controlMode section ***
  // the new icubinterface does not increase the bandwidth consumption
  // ret = true; useless guys!
  ret=ictrl->getControlModes(controlModes);

  if (ret==false) fprintf(stderr,"ictrl->getControlMode failed\n" );
  for (k = 0; k < NUMBER_OF_JOINTS; k++)
  {
	  if (currentPart->first_time==false && controlModes[k] == controlModesOld[k]) continue;
	  controlModesOld[k]=controlModes[k];
	  sprintf(frame_title,"Joint %d ",k );

	  switch (controlModes[k])
	  {
		  case VOCAB_CM_IDLE:
			  pColor=&color_yellow;
  			  strcat(frame_title," (IDLE)");
  			  gtk_frame_set_label   (GTK_FRAME(currentPart->framesArray[k]),frame_title);
			  gtk_widget_modify_bg (colorback[k], GTK_STATE_NORMAL, pColor);
		  break;
		  case VOCAB_CM_POSITION:
			  pColor=&color_green;
			  strcat(frame_title," (POSITION)");
			  gtk_frame_set_label   (GTK_FRAME(currentPart->framesArray[k]),frame_title);
			  gtk_frame_set_label   (GTK_FRAME(currentPart->frame_slider1[k]),"Position:");
			  gtk_frame_set_label   (GTK_FRAME(currentPart->frame_slider2[k]),"Velocity:");
			  gtk_widget_modify_bg (colorback[k], GTK_STATE_NORMAL, pColor);
		  break;
		  case VOCAB_CM_VELOCITY:
			  pColor=&color_blue;
			  strcat(frame_title," (VELOCITY)");
			  gtk_frame_set_label   (GTK_FRAME(currentPart->framesArray[k]),frame_title);
			  gtk_widget_modify_bg (colorback[k], GTK_STATE_NORMAL, pColor);
		  break;
		  case VOCAB_CM_TORQUE:
			  pColor=&color_pink;
			  strcat(frame_title," (TORQUE)");
  			  gtk_frame_set_label   (GTK_FRAME(currentPart->framesArray[k]),frame_title);
			  gtk_frame_set_label   (GTK_FRAME(currentPart->frame_slider1[k]),"Torque:");
			  gtk_frame_set_label   (GTK_FRAME(currentPart->frame_slider2[k]),"Torque2:");
			  gtk_widget_modify_bg (colorback[k], GTK_STATE_NORMAL, pColor);
			  break;
		  case VOCAB_CM_IMPEDANCE_POS:
			  pColor=&color_indaco;
			  strcat(frame_title," (IMPEDANCE POS)");
  			  gtk_frame_set_label   (GTK_FRAME(currentPart->framesArray[k]),frame_title);
			  gtk_widget_modify_bg (colorback[k], GTK_STATE_NORMAL, pColor);
		  break;
  		  case VOCAB_CM_IMPEDANCE_VEL:
			  pColor=&color_indaco;
			  strcat(frame_title," (IMPEDANCE VEL)");
  			  gtk_frame_set_label   (GTK_FRAME(currentPart->framesArray[k]),frame_title);
			  gtk_widget_modify_bg (colorback[k], GTK_STATE_NORMAL, pColor);
		  break;
		  case VOCAB_CM_OPENLOOP:
			  pColor=&color_white;
			  strcat(frame_title," (OPENLOOP)");
  			  gtk_frame_set_label   (GTK_FRAME(currentPart->framesArray[k]),frame_title);
			  gtk_widget_modify_bg (colorback[k], GTK_STATE_NORMAL, pColor);
		  break;
		  default:
		  case VOCAB_CM_UNKNOWN:
			  pColor=&color_grey;
			  //strcat(frame_title," (UNKNOWN)");
			  gtk_frame_set_label   (GTK_FRAME(currentPart->framesArray[k]),frame_title);
			  gtk_widget_modify_bg (colorback[k], GTK_STATE_NORMAL, pColor);
		  break;
	  }

	  //  pColor=&color_blue;
	  //  gtk_widget_modify_bg (colorback[k], GTK_STATE_NORMAL, pColor);
	  
	  int curr_amp_status=0;
	  int amp_status[60];                             //fix this!!!
	  for (int i=0; i<60; i++) amp_status[i]=0;       //fix this!!!
	  iamp->getAmpStatus(amp_status);                 //fix this!!!
	  curr_amp_status=amp_status[k];                  //fix this!!!

#if 0
	  if ((amp_status[k] & 0xFF)!=0)
	  {
	     //fprintf(stderr, "FAULT DETECTED: %x\n", curr_amp_status);
		 //pColor=&color_red;
		 //strcat(frame_title," (FAULT)");
		 //gtk_frame_set_label   (GTK_FRAME(currentPart->framesArray[k]),frame_title);
		 //gtk_widget_modify_bg (colorback[k], GTK_STATE_NORMAL, pColor);
	  }
#endif
  }

  currentPart->first_time =false;
  return true;
	
}
Beispiel #11
0
int main(int argc, char *argv[]){
	Network yarp;
	//Port<Bottle> armPlan;
	//Port<Bottle> armPred;
	Port armPlan;
	Port armPred;
	armPlan.open("/randArm/plan");
	armPred.open("/randArm/pred");
	bool fwCvOn = 0;
	fwCvOn = Network::connect("/randArm/plan","/fwdConv:i");
	fwCvOn *= Network::connect("/fwdConv:o","/randArm/pred");
	if (!fwCvOn){
		printf("Please run command:\n ./fwdConv --input /fwdConv:i --output /fwdConv:o");
		return 1;
	}

	const gsl_rng_type *T;
	gsl_rng *r;
	gsl_rng_env_setup();
	T = gsl_rng_default;
	r = gsl_rng_alloc(T);

	Property params;
	params.fromCommand(argc,argv);

	if (!params.check("robot")){
		fprintf(stderr, "Please specify robot name");
		fprintf(stderr, "e.g. --robot icub");
		return -1;
	}
	std::string robotName = params.find("robot").asString().c_str();
	std::string remotePorts = "/";
	remotePorts += robotName;
	remotePorts += "/";
	if (params.check("side")){
		remotePorts += params.find("side").asString().c_str();
	}
	else{
		remotePorts += "left";
	}
	remotePorts += "_arm";
	std::string localPorts = "/randArm/cmd";

	Property options;
	options.put("device", "remote_controlboard");
	options.put("local", localPorts.c_str());
	options.put("remote", remotePorts.c_str());

	PolyDriver robotDevice(options);
	if (!robotDevice.isValid()){
		printf("Device not available. Here are known devices: \n");
		printf("%s", Drivers::factory().toString().c_str());
		Network::fini();
		return 1;
	}

	IPositionControl *pos;
	IEncoders *enc;

	bool ok;
	ok = robotDevice.view(pos);
	ok = ok && robotDevice.view(enc);

	if (!ok){
		printf("Problems acquiring interfaces\n");
		return 0;
	}

	int nj = 0;
	pos->getAxes(&nj);
	Vector encoders;
	Vector command;
	Vector commandCart;
	Vector tmp;
	encoders.resize(nj);
	tmp.resize(nj);
	command.resize(nj);
	commandCart.resize(nj);

    for (int i = 0; i < nj; i++) {
         tmp[i] = 25.0;
    }
    pos->setRefAccelerations(tmp.data());

    for (int i = 0; i < nj; i++) {
        tmp[i] = 5.0;
        pos->setRefSpeed(i, tmp[i]);
    }

    command = 0;

    //set the arm joints to "middle" values
    command[0] = -45;
    command[1] = 45;
    command[2] = 0;
    command[3] = 45;
    pos->positionMove(command.data());

    bool done = false;
    while (!done){
    	pos->checkMotionDone(&done);
    	Time::delay(0.1);
    }

    while (true){
    	tmp = command;
    	command[0] += 15*(2*gsl_rng_uniform(r)-1);
    	command[1] += 15*(2*gsl_rng_uniform(r)-1);
    	command[2] += 15*(2*gsl_rng_uniform(r)-1);
    	command[3] += 15*(2*gsl_rng_uniform(r)-1);
    	printf("%.1lf %.1lf %.1lf %.1lf\n", command[0], command[1], command[2], command[3]);
    	//above 0 doesn't seem to be safe for joint 0
    	if (command[0] > 0 || command[0] < -90){
    		command[0] = tmp[0];
    	}
    	if (command[1] > 160 || command[1] < -0){
    		command[1] = tmp[1];
    	}
    	if (command[2] > 100 || command[2] < -35){
    		command[2] = tmp[2];
    	}
    	if (command[3] > 100 || command[3] < 10){
    		command[3] = tmp[3];
    	}
    	//use fwd kin to find end effector position
    	Bottle plan, pred;
    	for (int i = 0; i < nj; i++){
    		plan.add(command[i]);
    	}
    	armPlan.write(plan);
    	armPred.read(pred);
    	for (int i = 0; i < 3; i++){
    		commandCart[i] = pred.get(i).asDouble();
    	}
    	double rad = sqrt(commandCart[0]*commandCart[0]+commandCart[1]*commandCart[1]);
    	// safety radius back to 30 cm
    	if (rad > 0.3){
    		pos->positionMove(command.data());
    		done = false;
    		while(!done){
    			pos->checkMotionDone(&done);
    			Time::delay(0.1);
    		}
    	}
    	else{
    		printf("Self collision detected!\n");
    	}
    }

    robotDevice.close();
    gsl_rng_free(r);

    return 0;
}