コード例 #1
0
ファイル: utils.cpp プロジェクト: asimay/vizzy
Matrix alignJointsBounds(iKinChain *chain, PolyDriver *drvTorso, PolyDriver *drvHead,
                         const double eyeTiltMin, const double eyeTiltMax)
{
    IEncoders      *encs;
    IControlLimits *lims;

    double min, max;
    int nJointsTorso=1;

    if (drvTorso!=NULL)
    {
        drvTorso->view(encs);
        drvTorso->view(lims);        
        encs->getAxes(&nJointsTorso);

        for (int i=0; i<nJointsTorso; i++)
        {   
            lims->getLimits(i,&min,&max);
        
            (*chain)[nJointsTorso-1-i].setMin(CTRL_DEG2RAD*min); // reversed order
            (*chain)[nJointsTorso-1-i].setMax(CTRL_DEG2RAD*max);
        }
    }

    drvHead->view(encs);
    drvHead->view(lims);
    int nJointsHead;
    encs->getAxes(&nJointsHead);
    Matrix lim(nJointsHead,2);

    for (int i=0; i<nJointsHead; i++)
    {   
        lims->getLimits(i,&min,&max);

        // limit eye's tilt due to eyelids
        if (i==2)
        {
            min=std::max(min,eyeTiltMin);
            max=std::min(max,eyeTiltMax);
        }

        lim(i,0)=CTRL_DEG2RAD*min;
        lim(i,1)=CTRL_DEG2RAD*max;

        // just one eye's got only 5 dofs
        if (i<nJointsHead-1)
        {
            (*chain)[nJointsTorso+i].setMin(lim(i,0));
            (*chain)[nJointsTorso+i].setMax(lim(i,1));
        }
    }

    return lim;
}
コード例 #2
0
ファイル: main.cpp プロジェクト: lorejam/icub-main
int main(int argc, char *argv[]) 
{
    // just list the devices if no argument given
    if (argc <= 2) {
        printf("You can call %s like this:\n", argv[0]);
        printf("   %s --robot ROBOTNAME --OPTION VALUE ...\n", argv[0]);
        printf("For example:\n");
        printf("   %s --robot icub --local /talkto/james --remote /controlboard/rpc\n", argv[0]);
        printf("Here are devices listed for your system:\n");
        printf("%s", Drivers::factory().toString().c_str());
        return 0;
    }

    // get command line options
    Property options;
    options.fromCommand(argc, argv);
    if (!options.check("robot") || !options.check("part")) {
        printf("Missing either --robot or --part options\n");
        return 0;
    }

    Network::init();
	Time::turboBoost();
    
    std::string name;
    Value& v = options.find("robot");
    Value& part = options.find("part");

    Value *val;
    if (!options.check("device", val)) {
        options.put("device", "remote_controlboard");
    }
    if (!options.check("local", val)) {
		name="/"+std::string(v.asString().c_str())+"/"+std::string(part.asString().c_str())+"/simpleclient";
        //sprintf(&name[0], "/%s/%s/client", v.asString().c_str(), part.asString().c_str());
        options.put("local", name.c_str());
    }
    if (!options.check("remote", val)) {
        name="/"+std::string(v.asString().c_str())+"/"+std::string(part.asString().c_str());    
		//sprintf(&name[0], "/%s/%s", v.asString().c_str(), part.asString().c_str());
        options.put("remote", name.c_str());
    }

	fprintf(stderr, "%s", options.toString().c_str());

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

    IPositionControl *pos;
    IPositionDirect  *posDir;
    IVelocityControl *vel;
    IEncoders *enc;
    IPidControl *pid;
    IAmplifierControl *amp;
    IControlLimits *lim;
//    IControlMode *icm;
    IControlMode2 *iMode2;
    ITorqueControl *itorque;
    IOpenLoopControl *iopenloop;
	IImpedanceControl *iimp;
    IInteractionMode *iInteract;

    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);
//    ok &= dd.view(icm);
    ok &= dd.view(itorque);
    ok &= dd.view(iopenloop);
	ok &= dd.view(iimp);
    ok &= dd.view(posDir);
    ok &= dd.view(iMode2);
    ok &= dd.view(iInteract);

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

    pos->getAxes(&jnts);
    printf("Working with %d axes\n", jnts);
    double *tmp = new double[jnts];

    printf("Device active...\n");
    while (dd.isValid()) {
        std::string s;
        s.resize(1024);
        
        printf("-> ");
        char c = 0;
        int i = 0;
        while (c != '\n') {
            c = (char)fgetc(stdin);
            s[i++] = c;
        }
        s[i-1] = s[i] = 0;

        Bottle p;
        Bottle response;
        bool ok=false;
        bool rec=false;
        p.fromString(s.c_str());
        printf("Bottle: %s\n", p.toString().c_str());

        switch(p.get(0).asVocab()) {      
        case VOCAB_HELP:
            printf("\n\n");
            printf("Available commands:\n");
			printf("-------------------\n\n");

            printf("IOpenLoop:\ntype [%s] and one of the following:\n", Vocab::decode(VOCAB_IOPENLOOP).c_str());
            printf("	[set] [%s] <int> <float>\n",
                    Vocab::decode(VOCAB_OUTPUT).c_str());
            printf("	[get] [%s] <int>\n",
                    Vocab::decode(VOCAB_OUTPUT).c_str());
            printf("	[get] [%s]\n\n",
                    Vocab::decode(VOCAB_OUTPUTS).c_str());

            printf("IControlMode:\ntype [%s] and one of the following:\n", Vocab::decode(VOCAB_ICONTROLMODE).c_str());
            printf("	[set] [%s]|[%s]|[%s]|[%s]|[%s]|[%s]|[%s]|[%s][%s]|[%s]\n",
                    Vocab::decode(VOCAB_CM_POSITION).c_str(),
                    Vocab::decode(VOCAB_CM_POSITION_DIRECT).c_str(),
                    Vocab::decode(VOCAB_CM_VELOCITY).c_str(),
                    Vocab::decode(VOCAB_CM_MIXED).c_str(),
                    Vocab::decode(VOCAB_CM_TORQUE).c_str(),
                    Vocab::decode(VOCAB_CM_OPENLOOP).c_str(),
                    Vocab::decode(VOCAB_CM_IDLE).c_str(),
                    Vocab::decode(VOCAB_CM_FORCE_IDLE).c_str(),
                    Vocab::decode(VOCAB_CM_IMPEDANCE_POS).c_str(),
                    Vocab::decode(VOCAB_CM_IMPEDANCE_VEL).c_str());
            
            printf("	[get] [%s] <int>\n\n",
                Vocab::decode(VOCAB_CM_CONTROL_MODE).c_str());

            printf("ITorqueControl:\ntype [%s] and one of the following:\n", Vocab::decode(VOCAB_TORQUE).c_str());
            printf("	[get] [%s] <int> to read the measured torque for a single axis\n",                  Vocab::decode(VOCAB_TRQ).c_str());
            printf("	[get] [%s]  to read the measured torque for all axes\n",                      Vocab::decode(VOCAB_TRQS).c_str());
            printf("	[set] [%s] <int> <float> to set the reference torque for a single axis\n",          Vocab::decode(VOCAB_REF).c_str());
            printf("	[set] [%s] <float list> to set the reference torque for all axes\n",        Vocab::decode(VOCAB_REFS).c_str());
            printf("	[get] [%s] <int> to read the reference torque for a single axis\n",                  Vocab::decode(VOCAB_REF).c_str());
            printf("	[get] [%s] to read the reference torque for all axes\n\n",                      Vocab::decode(VOCAB_REFS).c_str());

			printf("IImpedanceControl:\ntype [%s] and one of the following:\n", Vocab::decode(VOCAB_IMPEDANCE).c_str());
            printf("	[set] [%s] <int> <float> <float> \n", 
                Vocab::decode(VOCAB_IMP_PARAM).c_str());
            printf("	[set] [%s] <int> <float>\n\n", 
                Vocab::decode(VOCAB_IMP_OFFSET).c_str());

            printf("	[get] [%s] <int>\n", 
                Vocab::decode(VOCAB_IMP_PARAM).c_str());
            printf("	[get] [%s] <int>\n\n", 
                Vocab::decode(VOCAB_IMP_OFFSET).c_str());

            printf("IInteractionMode:\ntype [%s] and one of the following:\n", Vocab::decode(VOCAB_INTERFACE_INTERACTION_MODE).c_str());
            printf("	[set] [%s]|[%s] <int>\n",
                    Vocab::decode(VOCAB_IM_STIFF).c_str(),
                    Vocab::decode(VOCAB_IM_COMPLIANT).c_str());

            printf("	[get] [%s] <int>\n",
                Vocab::decode(VOCAB_INTERACTION_MODE).c_str());
            printf("	[get] [%s] \n\n",
                Vocab::decode(VOCAB_INTERACTION_MODES).c_str());

			printf("Standard Interfaces:\n");
            printf("type [get] and one of the following:\n");
            printf("	[%s] to read the number of controlled axes\n", Vocab::decode(VOCAB_AXES).c_str());
            printf("	[%s] to read the encoder value for all axes\n", Vocab::decode(VOCAB_ENCODERS).c_str());
            printf("	[%s] to read the PID values for all axes\n", Vocab::decode(VOCAB_PIDS).c_str());
            printf("	[%s] <int> to read the PID values for a single axis\n", Vocab::decode(VOCAB_PID).c_str());
            printf("	[%s] <int> to read the limit values for a single axis\n", Vocab::decode(VOCAB_LIMITS).c_str());
            printf("	[%s] to read the PID error for all axes\n", Vocab::decode(VOCAB_ERRS).c_str());
            printf("	[%s] to read the PID output for all axes\n", Vocab::decode(VOCAB_OUTPUTS).c_str());
            printf("	[%s] to read the reference position for all axes\n", Vocab::decode(VOCAB_REFERENCES).c_str());
			printf("	[%s] <int> to read the reference position for a single axis\n", Vocab::decode(VOCAB_REFERENCE).c_str());
            printf("	[%s] to read the reference speed for all axes\n", Vocab::decode(VOCAB_REF_SPEEDS).c_str());
			printf("	[%s] <int> to read the reference speed for a single axis\n", Vocab::decode(VOCAB_REF_SPEED).c_str());
            printf("	[%s] to read the reference acceleration for all axes\n", Vocab::decode(VOCAB_REF_ACCELERATIONS).c_str());
			printf("	[%s] <int> to read the reference acceleration for a single axis\n", Vocab::decode(VOCAB_REF_ACCELERATION).c_str());
            printf("	[%s] to read the current consumption for all axes\n", Vocab::decode(VOCAB_AMP_CURRENTS).c_str());

            printf("\n");

            printf("type [set] and one of the following:\n");
            printf("	[%s] <int> <double> to move a single axis\n", Vocab::decode(VOCAB_POSITION_MOVE).c_str());
            printf("	[%s] <int> <double> to accelerate a single axis to a given speed\n", Vocab::decode(VOCAB_VELOCITY_MOVE).c_str());            
            printf("	[%s] <int> <double> to set the reference speed for a single axis\n", Vocab::decode(VOCAB_REF_SPEED).c_str());
            printf("	[%s] <int> <double> to set the reference acceleration for a single axis\n", Vocab::decode(VOCAB_REF_ACCELERATION).c_str());
            printf("	[%s] <list> to move multiple axes\n", Vocab::decode(VOCAB_POSITION_MOVES).c_str());
            printf("	[%s] <list> to accelerate multiple axes to a given speed\n", Vocab::decode(VOCAB_VELOCITY_MOVES).c_str());
            printf("	[%s] <list> to set the reference speed for all axes\n", Vocab::decode(VOCAB_REF_SPEEDS).c_str());
            printf("	[%s] <list> to set the reference acceleration for all axes\n", Vocab::decode(VOCAB_REF_ACCELERATIONS).c_str());          
            printf("	[%s] <int> to stop a single axis\n", Vocab::decode(VOCAB_STOP).c_str());
            printf("	[%s] <int> to stop all axes\n", Vocab::decode(VOCAB_STOPS).c_str());
            printf("	[%s] <int> <list> to set the PID values for a single axis\n", Vocab::decode(VOCAB_PID).c_str());
            printf("	[%s] <int> <list> to set the limits for a single axis\n", Vocab::decode(VOCAB_LIMITS).c_str());
            printf("	[%s] <int> to disable the PID control for a single axis\n", Vocab::decode(VOCAB_DISABLE).c_str());
            printf("	[%s] <int> to enable the PID control for a single axis\n", Vocab::decode(VOCAB_ENABLE).c_str());
            printf("	[%s] <int> <double> to set the encoder value for a single axis\n", Vocab::decode(VOCAB_ENCODER).c_str());
            printf("	[%s] <list> to set the encoder value for all axes\n", Vocab::decode(VOCAB_ENCODERS).c_str());
			printf("\n");
			printf("NOTES: - A list is a sequence of numbers in parenthesis, e.g. (10 2 1 10)\n");
			printf("       - Pids are expressed as a list of 7 numbers, type get pid <int> to see an example\n");
            printf("\n");
            break;

        case VOCAB_QUIT:
            goto ApplicationCleanQuit;
            break;

        case VOCAB_ICONTROLMODE:
            {
                handleControlModeMsg(iMode2, p, response, &rec, &ok);
                printf("%s\n", response.toString().c_str());
                break;
            }

        case VOCAB_IMPEDANCE:
            {
                handleImpedanceMsg(iimp, p, response, &rec, &ok);
                printf("%s\n", response.toString().c_str());
                break;
            }

		case VOCAB_TORQUE:
			{
				handleTorqueMsg(itorque, p, response, &rec, &ok);
				printf("%s\n", response.toString().c_str());
				break;
			}

        case VOCAB_INTERFACE_INTERACTION_MODE:
            {
                handleInteractionModeMsg(iInteract, p, response, &rec, &ok);
                printf("%s\n", response.toString().c_str());
                break;
            }

        case VOCAB_GET:
            switch(p.get(1).asVocab()) {
                case VOCAB_AXES: {
                    int nj = 0;
                    enc->getAxes(&nj);
                    printf ("%s: %d\n", Vocab::decode(VOCAB_AXES).c_str(), nj);
                }
                break;

                case VOCAB_ENCODERS: {
                    enc->getEncoders(tmp);
                    printf ("%s: (", Vocab::decode(VOCAB_ENCODERS).c_str());
                    for(i = 0; i < jnts; i++)
                        printf ("%.2f ", tmp[i]);
                    printf (")\n");
                }
                break;

                case VOCAB_PID: {
                    Pid pd;
                    int j = p.get(2).asInt();
                    pid->getPid(j, &pd);
                    printf("%s: ", Vocab::decode(VOCAB_PID).c_str());
                    printf("kp %.2f ", pd.kp);
                    printf("kd %.2f ", pd.kd);
                    printf("ki %.2f ", pd.ki);
                    printf("maxi %.2f ", pd.max_int);
                    printf("maxo %.2f ", pd.max_output);
                    printf("off %.2f ", pd.offset);
                    printf("scale %.2f ", pd.scale);
                    printf("\n");
                }
                break;

               case VOCAB_PIDS: {
                    Pid *p = new Pid[jnts];
                    ok = pid->getPids(p);
                    Bottle& b = response.addList();
                    int i;
                    for (i = 0; i < jnts; i++)
                        {
                          Bottle& c = b.addList();
                          c.addDouble(p[i].kp);
                          c.addDouble(p[i].kd);
                          c.addDouble(p[i].ki);
                          c.addDouble(p[i].max_int);
                          c.addDouble(p[i].max_output);
                          c.addDouble(p[i].offset);
                          c.addDouble(p[i].scale);
                        }
                    printf("%s\n", b.toString().c_str());
                    delete[] p;
                }
                break;

                case VOCAB_LIMITS: {
                    double min, max;
                    int j = p.get(2).asInt();
                    lim->getLimits(j, &min, &max);
                    printf("%s: ", Vocab::decode(VOCAB_LIMITS).c_str());
                    printf("limits: (%.2f %.2f)\n", min, max);
                }
                break;

                case VOCAB_ERRS: {
					pid->getErrors(tmp);
                    printf ("%s: (", Vocab::decode(VOCAB_ERRS).c_str());
                    for(i = 0; i < jnts; i++)
                        printf ("%.2f ", tmp[i]);
                    printf (")\n");
                }
                break;

                case VOCAB_OUTPUTS: {
                    iopenloop->getOutputs(tmp);
                    printf ("%s: (", Vocab::decode(VOCAB_OUTPUTS).c_str());
                    for(i = 0; i < jnts; i++)
                        printf ("%.2f ", tmp[i]);
                    printf (")\n");
                }
                break;

                case VOCAB_OUTPUT: {
                    int j = p.get(2).asInt();
                    double v;
                    iopenloop->getOutput(j, &v);
                    printf("%s: ", Vocab::decode(VOCAB_OUTPUT).c_str());
                    printf("%.2f ", v);
                    printf("\n");
                }
                break;

				case VOCAB_REFERENCE: {
					double ref_pos;
					int j = p.get(2).asInt();
                    pid->getReference(j,&ref_pos);
                    printf ("%s: (", Vocab::decode(VOCAB_REFERENCE).c_str());
                    printf ("%.2f ", ref_pos);
                    printf (")\n");                    
                }
                break;

                case VOCAB_REFERENCES: {
                    pid->getReferences(tmp);
                    printf ("%s: (", Vocab::decode(VOCAB_REFERENCES).c_str());
                    for(i = 0; i < jnts; i++)
                        printf ("%.2f ", tmp[i]);
                    printf (")\n");                    
                }
                break;

                case VOCAB_REF_SPEEDS: {
                    pos->getRefSpeeds(tmp);
                    printf ("%s: (", Vocab::decode(VOCAB_REF_SPEEDS).c_str());
                    for(i = 0; i < jnts; i++)
                        printf ("%.2f ", tmp[i]);
                    printf (")\n");                    
                }
                break;

				case VOCAB_REF_SPEED: {
					double ref_speed;
					int j = p.get(2).asInt();
                    pos->getRefSpeed(j,&ref_speed);
                    printf ("%s: (", Vocab::decode(VOCAB_REF_SPEED).c_str());
                    printf ("%.2f ", ref_speed);
                    printf (")\n");                    
                }
                break;

				case VOCAB_REF_ACCELERATION: {
					double ref_acc;
					int j = p.get(2).asInt();
                    pos->getRefAcceleration(j,&ref_acc);
                    printf ("%s: (", Vocab::decode(VOCAB_REF_ACCELERATION).c_str());
                    printf ("%.2f ", ref_acc);
                    printf (")\n");                    
                }
                break;

                case VOCAB_REF_ACCELERATIONS: {
                    pos->getRefAccelerations(tmp);
                    printf ("%s: (", Vocab::decode(VOCAB_REF_ACCELERATIONS).c_str());
                    for(i = 0; i < jnts; i++)
                        printf ("%.2f ", tmp[i]);
                    printf (")\n");                    
                }
                break;

                case VOCAB_AMP_CURRENTS: {
                    amp->getCurrents(tmp);
                    printf ("%s: (", Vocab::decode(VOCAB_AMP_CURRENTS).c_str());
                    for(i = 0; i < jnts; i++)
                        printf ("%.2f ", tmp[i]);
                    printf (")\n");
                }
                break;
            }
            break;

        case VOCAB_SET:
            switch(p.get(1).asVocab()) {
                case VOCAB_POSITION_MOVE: {
                    int j = p.get(2).asInt();
                    double ref = p.get(3).asDouble();
                    printf("%s: moving %d to %.2f\n", Vocab::decode(VOCAB_POSITION_MOVE).c_str(), j, ref);
                    pos->positionMove(j, ref);
                }
                break;

                case VOCAB_VELOCITY_MOVE: {
                    int j = p.get(2).asInt();
                    double ref = p.get(3).asDouble();
                    printf("%s: accelerating %d to %.2f\n", Vocab::decode(VOCAB_VELOCITY_MOVE).c_str(), j, ref);
                    vel->velocityMove(j, ref);
                }
                break;

                case VOCAB_REF_SPEED: {
                    int j = p.get(2).asInt();
                    double ref = p.get(3).asDouble();
                    printf("%s: setting speed for %d to %.2f\n", Vocab::decode(VOCAB_REF_SPEED).c_str(), j, ref);
                    pos->setRefSpeed(j, ref);
                }
                break;

                case VOCAB_REF_ACCELERATION: {
                    int j = p.get(2).asInt();
                    double ref = p.get(3).asDouble();
                    printf("%s: setting acceleration for %d to %.2f\n", Vocab::decode(VOCAB_REF_ACCELERATION).c_str(), j, ref);
                    pos->setRefAcceleration(j, ref);
                }
                break;

                case VOCAB_POSITION_MOVES: {
                    Bottle *l = p.get(2).asList();
                    for (i = 0; i < jnts; i++) {
                        tmp[i] = l->get(i).asDouble();
                    }
                    printf("%s: moving all joints\n", Vocab::decode(VOCAB_POSITION_MOVES).c_str());
                    pos->positionMove(tmp);
                }
                break;

                case VOCAB_VELOCITY_MOVES: {
                    Bottle *l = p.get(2).asList();
                    for (i = 0; i < jnts; i++) {
                        tmp[i] = l->get(i).asDouble();
                    }
                    printf("%s: moving all joints\n", Vocab::decode(VOCAB_VELOCITY_MOVES).c_str());
                    vel->velocityMove(tmp);
                }
                break;

                case VOCAB_REF_SPEEDS: {
                    Bottle *l = p.get(2).asList();
                    for (i = 0; i < jnts; i++) {
                        tmp[i] = l->get(i).asDouble();
                    }
                    printf("%s: setting speed for all joints\n", Vocab::decode(VOCAB_REF_SPEEDS).c_str());
                    pos->setRefSpeeds(tmp);
                }
                break;

                case VOCAB_REF_ACCELERATIONS: {
                    Bottle *l = p.get(2).asList();
                    for (i = 0; i < jnts; i++) {
                        tmp[i] = l->get(i).asDouble();
                    }
                    printf("%s: setting acceleration for all joints\n", Vocab::decode(VOCAB_REF_ACCELERATIONS).c_str());
                    pos->setRefAccelerations(tmp);
                }
                break;

                case VOCAB_STOP: {
                    int j = p.get(2).asInt();
                    printf("%s: stopping axis %d\n", Vocab::decode(VOCAB_STOP).c_str(), j);
                    pos->stop(j);
                }
                break;

                case VOCAB_STOPS: {
                    printf("%s: stopping all axes\n", Vocab::decode(VOCAB_STOPS).c_str());
                    pos->stop();
                }
                break;

                case VOCAB_ENCODER: {
                    int j = p.get(2).asInt();
                    double ref = p.get(3).asDouble();
                    printf("%s: setting the encoder value for %d to %.2f\n", Vocab::decode(VOCAB_ENCODER).c_str(), j, ref);
                    enc->setEncoder(j, ref);                    
                }
                break; 

                case VOCAB_ENCODERS: {
                    Bottle *l = p.get(2).asList();
                    for (i = 0; i < jnts; i++) {
                        tmp[i] = l->get(i).asDouble();
                    }
                    printf("%s: setting the encoder value for all joints\n", Vocab::decode(VOCAB_ENCODERS).c_str());
                    enc->setEncoders(tmp);
                }
                break;

                case VOCAB_PID: {
                    Pid pd;
                    int j = p.get(2).asInt();
                    Bottle *l = p.get(3).asList();
                    if (l==0)
                        {
                            printf("Check you specify a 7 elements list, e.g. set pid 0 (2000 20 1 300 300 0 0)\n");
                        }
                    else
                        {
                            int elems=l->size();
                            if (elems>=3)
                                {
                                    pd.kp = l->get(0).asDouble();
                                    pd.kd = l->get(1).asDouble();
                                    pd.ki = l->get(2).asDouble();
                                    if (elems>=7)
                                        {
                                            pd.max_int = l->get(3).asDouble();
                                            pd.max_output = l->get(4).asDouble();
                                            pd.offset = l->get(5).asDouble();
                                            pd.scale = l->get(6).asDouble();
                                        }
                                    printf("%s: setting PID values for axis %d\n", Vocab::decode(VOCAB_PID).c_str(), j);
                                    pid->setPid(j, pd);
                                }
                            else
                                {
                                    printf("Error, check you specify at least 7 elements, e.g. set pid 0 (2000 20 1 300 300 0 0)\n");
                                }
                        }
                }
                break;

                case VOCAB_DISABLE: {
                    int j = p.get(2).asInt();
                    printf("%s: disabling control for axis %d\n", Vocab::decode(VOCAB_DISABLE).c_str(), j);
                    pid->disablePid(j);
                    amp->disableAmp(j);
                }
                break;

                case VOCAB_ENABLE: {
                    int j = p.get(2).asInt();
                    printf("%s: enabling control for axis %d\n", Vocab::decode(VOCAB_ENABLE).c_str(), j);
                    amp->enableAmp(j);
                    pid->enablePid(j);
                }
                break;

                case VOCAB_LIMITS: {
                    int j = p.get(2).asInt();
                    printf("%s: setting limits for axis %d\n", Vocab::decode(VOCAB_LIMITS).c_str(), j);
                    Bottle *l = p.get(3).asList();
                    lim->setLimits(j, l->get(0).asDouble(), l->get(1).asDouble());
                }
                break;

                case VOCAB_OUTPUT: {
                    int j=p.get(2).asInt();
                    double v=p.get(3).asDouble();
                    iopenloop->setRefOutput(j,v);
                    printf("%s: setting output for axis %d to %f\n", Vocab::decode(VOCAB_OUTPUT).c_str(), j, v);            
                }
                break;
            }
            break;
        } /* switch get(0) */

    } /* while () */

ApplicationCleanQuit:
    dd.close();
    delete[] tmp;

    Network::fini();
    return 0;
}
コード例 #3
0
ファイル: main.cpp プロジェクト: MagnusJohnsson/wysiwyd
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;
}
コード例 #4
0
bool SpringyFingersModel::calibrate(const Property &options)
{
    if (configured)
    {
        IControlMode2    *imod; driver.view(imod);
        IControlLimits   *ilim; driver.view(ilim);
        IEncoders        *ienc; driver.view(ienc);
        IPositionControl *ipos; driver.view(ipos);

        int nAxes; ienc->getAxes(&nAxes);
        Vector qmin(nAxes),qmax(nAxes),vel(nAxes),acc(nAxes);

        printMessage(1,"steering the hand to a suitable starting configuration\n");
        for (int j=7; j<nAxes; j++)
        {
            imod->setControlMode(j,VOCAB_CM_POSITION);
            ilim->getLimits(j,&qmin[j],&qmax[j]);            

            ipos->getRefAcceleration(j,&acc[j]);
            ipos->getRefSpeed(j,&vel[j]);
            
            ipos->setRefAcceleration(j,1e9);
            ipos->setRefSpeed(j,60.0);
            ipos->positionMove(j,(j==8)?qmax[j]:qmin[j]);   // thumb in opposition
        }

        printMessage(1,"proceeding with the calibration\n");
        Property &opt=const_cast<Property&>(options);
        string tag=opt.check("finger",Value("all")).asString().c_str();
        if (tag=="thumb")
        {
            calibrateFinger(fingers[0],10,qmin[10],qmax[10]);
        }
        else if (tag=="index")
        {
            calibrateFinger(fingers[1],12,qmin[12],qmax[12]);
        }
        else if (tag=="middle")
        {
            calibrateFinger(fingers[2],14,qmin[14],qmax[14]);
        }
        else if (tag=="ring")
        {
            calibrateFinger(fingers[3],15,qmin[15],qmax[15]);
        }
        else if (tag=="little")
        {
            calibrateFinger(fingers[4],15,qmin[15],qmax[15]);
        }
        else if ((tag=="all") || (tag=="all_serial"))
        {
            calibrateFinger(fingers[0],10,qmin[10],qmax[10]);
            calibrateFinger(fingers[1],12,qmin[12],qmax[12]);
            calibrateFinger(fingers[2],14,qmin[14],qmax[14]);
            calibrateFinger(fingers[3],15,qmin[15],qmax[15]);
            calibrateFinger(fingers[4],15,qmin[15],qmax[15]);
        }
        else if (tag=="all_parallel")
        {
            CalibThread thr[5];
            thr[0].setInfo(this,fingers[0],10,qmin[10],qmax[10]);
            thr[1].setInfo(this,fingers[1],12,qmin[12],qmax[12]);
            thr[2].setInfo(this,fingers[2],14,qmin[14],qmax[14]);
            thr[3].setInfo(this,fingers[3],15,qmin[15],qmax[15]);
            thr[4].setInfo(this,fingers[4],15,qmin[15],qmax[15]);

            thr[0].start(); thr[1].start(); thr[2].start();
            thr[3].start(); thr[4].start();

            bool done=false;
            while (!done)
            {
                done=true;
                for (int i=0; i<5; i++)
                {
                    done&=thr[i].isDone();
                    if (thr[i].isDone() && thr[i].isRunning())
                        thr[i].stop();
                }

                Time::delay(0.1);
            }
        }
        else
        {
            printMessage(1,"unknown finger request %s\n",tag.c_str());
            return false;
        }

        for (int j=7; j<nAxes; j++)
        {
            ipos->setRefAcceleration(j,acc[j]);
            ipos->setRefSpeed(j,vel[j]);
        }

        return true;
    }
    else
        return false;
}
コード例 #5
0
int main(int argc, char *argv[]) 
{
    // just list the devices if no argument given
    if (argc <= 2) {
        printf("You can call %s like this:\n", argv[0]);
        printf("   %s --robot ROBOTNAME --OPTION VALUE ...\n", argv[0]);
        printf("For example:\n");
        printf("   %s --robot icub --part any --remote /controlboard\n", argv[0]);
        printf("Here are devices listed for your system:\n");
        printf("%s", Drivers::factory().toString().c_str());
        return 0;
    }

    // get command line options
    Property options;
    options.fromCommand(argc, argv);
    if (!options.check("robot") || !options.check("part")) {
        printf("Missing either --robot or --part options\n");
        return 0;
    }

    Network yarp;
	Time::turboBoost();
    
    char name[1024];
    Value& v = options.find("robot");
    Value& part = options.find("part");

    Value *val;
    if (!options.check("device", val)) {
        options.put("device", "remote_controlboard");
    }
    if (!options.check("local", val)) {
        sprintf(name, "/%s/%s/client", v.asString().c_str(), part.asString().c_str());
        options.put("local", name);
    }
    if (!options.check("remote", val)) {
        sprintf(name, "/%s/%s", v.asString().c_str(), part.asString().c_str());
        options.put("remote", name);
    }

	fprintf(stderr, "%s", options.toString().c_str());

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

    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) {
        printf("Problems acquiring interfaces\n");
        return 1;
    }

    int jnts = 0;
    pos->getAxes(&jnts);
    printf("Working with %d axes\n", jnts);
    double *tmp = new double[jnts];
    assert (tmp != NULL);

    printf("Device active...\n");
    while (dd.isValid()) {
        char s[1024];
        
        printf("-> ");
        char c = 0;
        int i = 0;
        while (c != '\n') {
            c = (char)fgetc(stdin);
            s[i++] = c;
        }
        s[i-1] = s[i] = 0;

        Bottle p;
        p.fromString(s);
        printf("Bottle: %s\n", p.toString().c_str());

        switch(p.get(0).asVocab()) {        
        case VOCAB_HELP:
            printf("\n\n");
            printf("Available commands:\n\n");

            printf("type [get] and one of the following:\n");
            printf("[%s] to read the number of controlled axes\n", Vocab::decode(VOCAB_AXES).c_str());
            printf("[%s] to read the encoder value for all axes\n", Vocab::decode(VOCAB_ENCODERS).c_str());
            printf("[%s] <int> to read the PID values for a single axis\n", Vocab::decode(VOCAB_PID).c_str());
            printf("[%s] <int> to read the limit values for a single axis\n", Vocab::decode(VOCAB_LIMITS).c_str());
            printf("[%s] to read the PID error for all axes\n", Vocab::decode(VOCAB_ERRS).c_str());
            printf("[%s] to read the PID output for all axes\n", Vocab::decode(VOCAB_OUTPUTS).c_str());
            printf("[%s] to read the reference position for all axes\n", Vocab::decode(VOCAB_REFERENCES).c_str());
            printf("[%s] to read the reference speed for all axes\n", Vocab::decode(VOCAB_REF_SPEEDS).c_str());
            printf("[%s] to read the reference acceleration for all axes\n", Vocab::decode(VOCAB_REF_ACCELERATIONS).c_str());
            printf("[%s] to read the current consumption for all axes\n", Vocab::decode(VOCAB_AMP_CURRENTS).c_str());

            printf("\n");

            printf("type [set] and one of the following:\n");
            printf("[%s] <int> <double> to move a single axis\n", Vocab::decode(VOCAB_POSITION_MOVE).c_str());
            printf("[%s] <int> <double> to accelerate a single axis to a given speed\n", Vocab::decode(VOCAB_VELOCITY_MOVE).c_str());            
            printf("[%s] <int> <double> to set the reference speed for a single axis\n", Vocab::decode(VOCAB_REF_SPEED).c_str());
            printf("[%s] <int> <double> to set the reference acceleration for a single axis\n", Vocab::decode(VOCAB_REF_ACCELERATION).c_str());
            printf("[%s] <list> to move multiple axes\n", Vocab::decode(VOCAB_POSITION_MOVES).c_str());
            printf("[%s] <list> to accelerate multiple axes to a given speed\n", Vocab::decode(VOCAB_VELOCITY_MOVES).c_str());
            printf("[%s] <list> to set the reference speed for all axes\n", Vocab::decode(VOCAB_REF_SPEEDS).c_str());
            printf("[%s] <list> to set the reference acceleration for all axes\n", Vocab::decode(VOCAB_REF_ACCELERATIONS).c_str());          
            printf("[%s] <int> to stop a single axis\n", Vocab::decode(VOCAB_STOP).c_str());
            printf("[%s] <int> to stop all axes\n", Vocab::decode(VOCAB_STOPS).c_str());
            printf("[%s] <int> <list> to set the PID values for a single axis\n", Vocab::decode(VOCAB_PID).c_str());
            printf("[%s] <int> <list> to set the limits for a single axis\n", Vocab::decode(VOCAB_LIMITS).c_str());
            printf("[%s] <int> to disable the PID control for a single axis\n", Vocab::decode(VOCAB_DISABLE).c_str());
            printf("[%s] <int> to enable the PID control for a single axis\n", Vocab::decode(VOCAB_ENABLE).c_str());
            printf("[%s] <int> <double> to set the encoder value for a single axis\n", Vocab::decode(VOCAB_ENCODER).c_str());
            printf("[%s] <list> to set the encoder value for all axes\n", Vocab::decode(VOCAB_ENCODERS).c_str());
            printf("\n");
            break;

        case VOCAB_QUIT:
            goto ApplicationCleanQuit;
            break;

        case VOCAB_GET:
            switch(p.get(1).asVocab()) {
                case VOCAB_AXES: {
                    int nj = 0;
                    enc->getAxes(&nj);
                    printf ("%s: %d\n", Vocab::decode(VOCAB_AXES).c_str(), nj);
                }
                break;

                case VOCAB_ENCODERS: {
                    enc->getEncoders(tmp);
                    printf ("%s: (", Vocab::decode(VOCAB_ENCODERS).c_str());
                    for(i = 0; i < jnts; i++)
                        printf ("%.2f ", tmp[i]);
                    printf (")\n");
                }
                break;

                case VOCAB_PID: {
                    Pid pd;
                    int j = p.get(2).asInt();
                    pid->getPid(j, &pd);
                    printf("%s: ", Vocab::decode(VOCAB_PID).c_str());
                    printf("kp %.2f ", pd.kp);
                    printf("kd %.2f ", pd.kd);
                    printf("ki %.2f ", pd.ki);
                    printf("maxi %.2f ", pd.max_int);
                    printf("maxo %.2f ", pd.max_output);
                    printf("off %.2f ", pd.offset);
                    printf("scale %.2f ", pd.scale);
                    printf("\n");
                }
                break;

                case VOCAB_LIMITS: {
                    double min, max;
                    int j = p.get(2).asInt();
                    lim->getLimits(j, &min, &max);
                    printf("%s: ", Vocab::decode(VOCAB_LIMITS).c_str());
                    printf("limits: (%.2f %.2f)\n", min, max);
                }
                break;

                case VOCAB_ERRS: {
                    pid->getErrorLimits(tmp);
                    printf ("%s: (", Vocab::decode(VOCAB_ERRS).c_str());
                    for(i = 0; i < jnts; i++)
                        printf ("%.2f ", tmp[i]);
                    printf (")\n");
                }
                break;

                case VOCAB_OUTPUTS: {
                    pid->getErrors(tmp);
                    printf ("%s: (", Vocab::decode(VOCAB_OUTPUTS).c_str());
                    for(i = 0; i < jnts; i++)
                        printf ("%.2f ", tmp[i]);
                    printf (")\n");
                }
                break;

                case VOCAB_REFERENCES: {
                    pid->getReferences(tmp);
                    printf ("%s: (", Vocab::decode(VOCAB_REFERENCES).c_str());
                    for(i = 0; i < jnts; i++)
                        printf ("%.2f ", tmp[i]);
                    printf (")\n");                    
                }
                break;

                case VOCAB_REF_SPEEDS: {
                    pos->getRefSpeeds(tmp);
                    printf ("%s: (", Vocab::decode(VOCAB_REF_SPEEDS).c_str());
                    for(i = 0; i < jnts; i++)
                        printf ("%.2f ", tmp[i]);
                    printf (")\n");                    
                }
                break;

                case VOCAB_REF_ACCELERATIONS: {
                    pos->getRefAccelerations(tmp);
                    printf ("%s: (", Vocab::decode(VOCAB_REF_ACCELERATIONS).c_str());
                    for(i = 0; i < jnts; i++)
                        printf ("%.2f ", tmp[i]);
                    printf (")\n");                    
                }
                break;

                case VOCAB_AMP_CURRENTS: {
                    amp->getCurrents(tmp);
                    printf ("%s: (", Vocab::decode(VOCAB_AMP_CURRENTS).c_str());
                    for(i = 0; i < jnts; i++)
                        printf ("%.2f ", tmp[i]);
                    printf (")\n");
                }
                break;
            }
            break;

        case VOCAB_SET:
            switch(p.get(1).asVocab()) {
                case VOCAB_POSITION_MOVE: {
                    int j = p.get(2).asInt();
                    double ref = p.get(3).asDouble();
                    printf("%s: moving %d to %.2f\n", Vocab::decode(VOCAB_POSITION_MOVE).c_str(), j, ref);
                    pos->positionMove(j, ref);
                }
                break;

                case VOCAB_VELOCITY_MOVE: {
                    int j = p.get(2).asInt();
                    double ref = p.get(3).asDouble();
                    printf("%s: accelerating %d to %.2f\n", Vocab::decode(VOCAB_VELOCITY_MOVE).c_str(), j, ref);
                    vel->velocityMove(j, ref);
                }
                break;

                case VOCAB_REF_SPEED: {
                    int j = p.get(2).asInt();
                    double ref = p.get(3).asDouble();
                    printf("%s: setting speed for %d to %.2f\n", Vocab::decode(VOCAB_REF_SPEED).c_str(), j, ref);
                    pos->setRefSpeed(j, ref);
                }
                break;

                case VOCAB_REF_ACCELERATION: {
                    int j = p.get(2).asInt();
                    double ref = p.get(3).asDouble();
                    printf("%s: setting acceleration for %d to %.2f\n", Vocab::decode(VOCAB_REF_ACCELERATION).c_str(), j, ref);
                    pos->setRefAcceleration(j, ref);
                }
                break;

                case VOCAB_POSITION_MOVES: {
                    Bottle *l = p.get(2).asList();
                    for (i = 0; i < jnts; i++) {
                        tmp[i] = l->get(i).asDouble();
                    }
                    printf("%s: moving all joints\n", Vocab::decode(VOCAB_POSITION_MOVES).c_str());
                    pos->positionMove(tmp);
                }
                break;

                case VOCAB_VELOCITY_MOVES: {
                    Bottle *l = p.get(2).asList();
                    for (i = 0; i < jnts; i++) {
                        tmp[i] = l->get(i).asDouble();
                    }
                    printf("%s: moving all joints\n", Vocab::decode(VOCAB_VELOCITY_MOVES).c_str());
                    vel->velocityMove(tmp);
                }
                break;

                case VOCAB_REF_SPEEDS: {
                    Bottle *l = p.get(2).asList();
                    for (i = 0; i < jnts; i++) {
                        tmp[i] = l->get(i).asDouble();
                    }
                    printf("%s: setting speed for all joints\n", Vocab::decode(VOCAB_REF_SPEEDS).c_str());
                    pos->setRefSpeeds(tmp);
                }
                break;

                case VOCAB_REF_ACCELERATIONS: {
                    Bottle *l = p.get(2).asList();
                    for (i = 0; i < jnts; i++) {
                        tmp[i] = l->get(i).asDouble();
                    }
                    printf("%s: setting acceleration for all joints\n", Vocab::decode(VOCAB_REF_ACCELERATIONS).c_str());
                    pos->setRefAccelerations(tmp);
                }
                break;

                case VOCAB_STOP: {
                    int j = p.get(2).asInt();
                    printf("%s: stopping axis %d\n", Vocab::decode(VOCAB_STOP).c_str());
                    pos->stop(j);
                }
                break;

                case VOCAB_STOPS: {
                    printf("%s: stopping all axes %d\n", Vocab::decode(VOCAB_STOPS).c_str());
                    pos->stop();
                }
                break;

                case VOCAB_ENCODER: {
                    int j = p.get(2).asInt();
                    double ref = p.get(3).asDouble();
                    printf("%s: setting the encoder value for %d to %.2f\n", Vocab::decode(VOCAB_ENCODER).c_str(), j, ref);
                    enc->setEncoder(j, ref);                    
                }
                break; 

                case VOCAB_ENCODERS: {
                    Bottle *l = p.get(2).asList();
                    for (i = 0; i < jnts; i++) {
                        tmp[i] = l->get(i).asDouble();
                    }
                    printf("%s: setting the encoder value for all joints\n", Vocab::decode(VOCAB_ENCODERS).c_str());
                    enc->setEncoders(tmp);
                }
                break;

                case VOCAB_PID: {
                    Pid pd;
                    int j = p.get(2).asInt();
                    Bottle *l = p.get(3).asList();
                    pd.kp = l->get(0).asDouble();
                    pd.kd = l->get(1).asDouble();
                    pd.ki = l->get(2).asDouble();
                    pd.max_int = l->get(3).asDouble();
                    pd.max_output = l->get(4).asDouble();
                    pd.offset = l->get(5).asDouble();
                    pd.scale = l->get(6).asDouble();
                    printf("%s: setting PID values for axis %d\n", Vocab::decode(VOCAB_PID).c_str(), j);
                    pid->setPid(j, pd);
                }
                break;

                case VOCAB_DISABLE: {
                    int j = p.get(2).asInt();
                    printf("%s: disabling control for axis %d\n", Vocab::decode(VOCAB_DISABLE).c_str(), j);
                    pid->disablePid(j);
                    amp->disableAmp(j);
                }
                break;

                case VOCAB_ENABLE: {
                    int j = p.get(2).asInt();
                    printf("%s: enabling control for axis %d\n", Vocab::decode(VOCAB_ENABLE).c_str(), j);
                    amp->enableAmp(j);
                    pid->enablePid(j);
                }
                break;

                case VOCAB_LIMITS: {
                    int j = p.get(2).asInt();
                    printf("%s: setting limits for axis %d\n", Vocab::decode(VOCAB_LIMITS).c_str(), j);
                    Bottle *l = p.get(3).asList();
                    lim->setLimits(j, l->get(0).asDouble(), l->get(1).asDouble());
                }
                break;
            }
            break;
        } /* switch get(0) */

    } /* while () */

ApplicationCleanQuit:
    dd.close();
    delete[] tmp;

    return 0;
}
コード例 #6
0
ファイル: main.cpp プロジェクト: misaki43/codyco-modules
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");
        fprintf(stderr, "--part name (right_arm or left_arm)\n");
        fprintf(stderr, "--durationInS duration (in seconds)\n");
        fprintf(stderr, "--sampleInMS sample time (in milliseconds)\n");
        fprintf(stderr, "--refTimeInMS Reference time of minimum jerk trajectory generator (in milliseconds)\n");
        return 1;
    }

    if( !params.check("sampleInMS") ||
        !params.check("durationInS") ||
        !params.check("refTimeInMS") )
    {
        fprintf(stderr, "Necessary params not passed\n");
        return EXIT_FAILURE;
    }

    std::string robotName=params.find("robot").asString().c_str();
    std::string partName=params.find("part").asString().c_str();
    double durationInS = params.find("durationInS").asDouble();
    double samplesInMS = params.find("sampleInMS").asDouble();
    double refTimeInMS = params.find("refTimeInMS").asDouble();

    std::cout << "robotName : " << robotName << std::endl;
    std::cout << "partName : " << partName << std::endl;
    std::cout << "durationInS : " << durationInS << std::endl;
    std::cout << "samplesInMS : " << samplesInMS << std::endl;
    std::cout << "refTimeInMS : " << refTimeInMS << std::endl;

    std::string remotePorts="/";
    remotePorts+=robotName;
    remotePorts+="/"+partName;

    std::string localPorts="/randomShoulderMovements/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;
    }

    IPositionDirect *pos;
    IEncoders *encs;
    IControlLimits * lims;
    IControlMode2 * ictrl;


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

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

    int nj=0;
    pos->getAxes(&nj);
    Vector encodersInDeg(nj);
    Vector desPosInDeg(3);
    Vector commandInDeg(3);
    Vector minShoulderInDeg(3);
    Vector maxShoulderInDeg(3);
    Vector lowerBoundConstraint;
    Vector upperBoundConstraint;
    Matrix constraintMatrix;

    for(int i = 0; i < 3; i++ )
    {
        lims->getLimits(i,&(minShoulderInDeg[i]),&(maxShoulderInDeg[i]));
    }

    bool encodersRead = encs->getEncoders(encodersInDeg.data());

    while( !encodersRead )
    {
        encodersRead = encs->getEncoders(encodersInDeg.data());
    }


    getShoulderConstraint(lowerBoundConstraint,upperBoundConstraint,constraintMatrix);

    // Create the minimum jerk filter
    iCub::ctrl::minJerkTrajGen filter(3,samplesInMS,refTimeInMS);

    filter.init(encodersInDeg.subVector(0,2));

    std::cout << "Filter initial value " << encodersInDeg.subVector(0,2).toString() << std::endl;

    std::vector<int> indexToControl;
    indexToControl.push_back(0);
    indexToControl.push_back(1);
    indexToControl.push_back(2);

    // Set control modes
    ictrl->setControlMode(0,VOCAB_CM_POSITION_DIRECT);
    ictrl->setControlMode(1,VOCAB_CM_POSITION_DIRECT);
    ictrl->setControlMode(2,VOCAB_CM_POSITION_DIRECT);


    double samplesInS = samplesInMS/1000.0;
    int samples = (int)(durationInS/samplesInS);
    int samplesForNewPosition = (int)(refTimeInMS/samplesInMS);
    for(int i=0; i < samples; i++ )
    {
        if( i % samplesForNewPosition == 0 )
        {
            // Generate a new desired shoulder position
            generateRandomShoulderPosition(desPosInDeg,minShoulderInDeg,maxShoulderInDeg,
                                           lowerBoundConstraint,upperBoundConstraint,constraintMatrix);

            std::cout << "New position generated" << std::endl;
            std::cout << "Position generated " << desPosInDeg.toString() << std::endl;
            std::cout << "Remaining time " << (samples-i)*(samplesInS) << std::endl;
        }

        filter.computeNextValues(desPosInDeg);

        commandInDeg = filter.getPos();

        std::cout << "Position set " << commandInDeg.toString() << std::endl;


        pos->setPositions(3,indexToControl.data(),commandInDeg.data());

        yarp::os::Time::delay(samplesInS);
    }

    robotDevice.close();

    return 0;
}
コード例 #7
0
ファイル: main.cpp プロジェクト: pattacini/minJerk
    bool configure(ResourceFinder &rf)
    {
        Time::turboBoost();
        string name=rf.check("name",Value("yarpMinJerk")).asString().c_str();
        string robot=rf.check("robot",Value("icubSim")).asString().c_str();
        string part=rf.check("part",Value("left_arm")).asString().c_str();
        joint=rf.check("joint",Value(3)).asInt();
        compliant=rf.check("compliant");

        Property option;
        option.put("device","remote_controlboard");
        option.put("remote",("/"+robot+"/"+part).c_str());
        option.put("local",("/"+name+"/"+part).c_str());
        if (!drv.open(option))
            return false;

        drv.view(imod);
        drv.view(iint);
        drv.view(ienc);
        drv.view(ivel);

        imod->setControlMode(joint,VOCAB_CM_VELOCITY);
        if (compliant)
            iint->setInteractionMode(joint,VOCAB_IM_COMPLIANT);

        IControlLimits *ilim; drv.view(ilim);
        double min_joint,max_joint;
        ilim->getLimits(joint,&min_joint,&max_joint);

        double enc;
        while (!ienc->getEncoder(joint,&enc))
            Time::delay(0.1);
        
        Controller_P.Plant_IC=enc;
        Controller_P.Plant_Max=max_joint;
        Controller_P.Plant_Min=min_joint;
        Controller_P.AutoCompensator_ThresHystMax=0.5;
        Controller_P.AutoCompensator_ThresHystMin=0.1;

        yInfo("enc=%g in [%g, %g] deg",enc,min_joint,max_joint);

        // Pack model data into RTM
        Controller_M->ModelData.defaultParam = &Controller_P;
        Controller_M->ModelData.blockIO = &Controller_B;
        Controller_M->ModelData.dwork = &Controller_DW;

        // Initialize model
        Controller_initialize(Controller_M, &Controller_U_reference,
                            &Controller_U_compensator_state,
                            &Controller_U_plant_output,
                            &Controller_Y_controller_output,
                            &Controller_Y_controller_reference,
                            &Controller_Y_plant_reference,
                            &Controller_Y_error_statistics,
                            &Controller_Y_enable_compensation);

        Controller_U_reference=enc;
        Controller_U_compensator_state=CompensatorState::Off;
        Controller_U_plant_output=enc;        
        
        dataIn.open(("/"+name+"/data:i").c_str());
        dataOut.open(("/"+name+"/data:o").c_str());
        rpc.open(("/"+name+"/rpc").c_str());
        attach(rpc);
        
        return true;
    }
コード例 #8
0
ファイル: main.cpp プロジェクト: apaikan/icub-main
    bool configure(ResourceFinder &rf)
    {
        string name=rf.find("name").asString().c_str();
        string robot=rf.find("robot").asString().c_str();
        string hand=rf.find("hand").asString().c_str();
        string modelType=rf.find("modelType").asString().c_str();
        fingerName=rf.find("finger").asString().c_str();

        if (fingerName=="thumb")
            joint=10;
        else if (fingerName=="index")
            joint=12;
        else if (fingerName=="middle")
            joint=14;
        else if (fingerName=="ring")
            joint=15;
        else if (fingerName=="little")
            joint=15;
        else
        {
            fprintf(stdout,"unknown finger!\n");
            return false;
        }

        Property driverOpt("(device remote_controlboard)");
        driverOpt.put("remote",("/"+robot+"/"+hand+"_arm").c_str());
        driverOpt.put("local",("/"+name).c_str());
        if (!driver.open(driverOpt))
            return false;

        driver.view(ipos);
        driver.view(ienc);

        IControlLimits *ilim;
        driver.view(ilim);

        ilim->getLimits(joint,&min,&max);
        double margin=0.1*(max-min);
        min=min+margin;
        max=max-margin;
        val=&min;

        Property genOpt;
        genOpt.put("name",(name+"/"+modelType).c_str());
        genOpt.put("robot",robot.c_str());
        genOpt.put("type",hand.c_str());
        genOpt.put("verbose",1);
        string general(genOpt.toString().c_str());
        string thumb( "(thumb  (name thumb))");
        string index( "(index  (name index))");
        string middle("(middle (name middle))");
        string ring(  "(ring   (name ring))");
        string little("(little (name little))");

        Property options((general+" "+thumb+" "+index+" "+middle+" "+ring+" "+little).c_str());
        fprintf(stdout,"configuring options: %s\n",options.toString().c_str());

        if (modelType=="springy")
            model=new SpringyFingersModel;
        else if (modelType=="tactile")
            model=new TactileFingersModel;
        else
        {
            fprintf(stdout,"unknown model type!\n");
            return false;
        }

        if (model->fromProperty(options))
            return true;
        else
        {
            delete model;
            return false;
        }
    }