示例#1
0
文件: helpers.cpp 项目: caomw/wysiwyd
void proactiveTagging::configureOPC(yarp::os::ResourceFinder &rf)
{
    //Populate the OPC if required
    yDebug() << "Populating OPC...";

    //1. Populate AddOrRetrieve part
    Bottle grpOPC_AOR = rf.findGroup("OPC_AddOrRetrieve");
    bool shouldPopulate_AOR = grpOPC_AOR.find("populateOPC").asInt() == 1;
    if (shouldPopulate_AOR)
    {
        Bottle *objectList = grpOPC_AOR.find("objectName").asList();
        subPopulateObjects(objectList, true);

        Bottle *bodyPartList = grpOPC_AOR.find("bodypartName").asList();
        Bottle *bodyPartJointList = grpOPC_AOR.find("bodypartJoint").asList();
        subPopulateBodyparts(bodyPartList, bodyPartJointList, true);
    }

    //2. Populate Add part (allows several object with same base name, e.g. object, object_1, object_2, ..., object_n)
    Bottle grpOPC_Add = rf.findGroup("OPC_Add");
    bool shouldPopulate_Add = grpOPC_Add.find("populateOPC").asInt() == 1;
    if (shouldPopulate_Add)
    {
        Bottle *objectList = grpOPC_Add.find("objectName").asList();
        subPopulateObjects(objectList, false);

        Bottle *bodyPartList = grpOPC_Add.find("bodypartName").asList();
        Bottle *bodyPartJointList = grpOPC_Add.find("bodypartJoint").asList();
        subPopulateBodyparts(bodyPartList, bodyPartJointList, false);
    }

    yDebug() << "configureOPC done";
}
示例#2
0
bool ModelOTL::configure(yarp::os::ResourceFinder &rf) {
    bool bEveryThingisGood = true;

    moduleName = rf.check("name",Value("modelOTL"),"module name (string)").asString();

    part = rf.check("part",Value("left_arm")).asString();

    Bottle &babbl_par = rf.findGroup("babbling_param");
    train_duration = babbl_par.check("train_duration", Value(20.0)).asDouble();
    test_duration = babbl_par.check("test_duration", Value(10.0)).asDouble();

    Bottle &oesgp_par = rf.findGroup("oesgp_learner");
    p_input_dim = oesgp_par.check("input_dim", Value(2)).asInt();
    p_output_dim = oesgp_par.check("output_dim", Value(1)).asInt();
    p_reservoir_size = oesgp_par.check("reservoir_size", Value(100)).asDouble();
    p_input_weight = oesgp_par.check("input_weight", Value(1.0)).asDouble();
    p_output_feedback_weight = oesgp_par.check("output_feedback_weight", Value(0.0)).asDouble();
    p_leak_rate = oesgp_par.check("leak_rate", Value(0.9)).asDouble();
    p_connectivity = oesgp_par.check("connectivity", Value(0.1)).asDouble();
    p_spectral_radius = oesgp_par.check("spectral_radius", Value(0.90)).asDouble();
    p_use_inputs_in_state = oesgp_par.check("use_inputs_in_state", Value(1)).asInt();
    p_noise = oesgp_par.check("noise", Value(0.000001)).asDouble();
    p_epsilon = oesgp_par.check("epsilon", Value(1e-3)).asDouble();
    p_capacity = oesgp_par.check("capacity", Value(10)).asInt();
    p_random_seed = oesgp_par.check("random_seed", Value(0)).asInt();

    setName(moduleName.c_str());

    // Open handler port
    if (!handlerPort.open("/" + getName() + "/rpc")) {
        cout << getName() << ": Unable to open port " << "/" << getName() << "/rpc" << endl;
        bEveryThingisGood = false;
    }

    if (!portVelocityOut.open("/" + getName() + "/" + part + "/velocity:o")) {
        cout << getName() << ": Unable to open port " << "/" << getName() << "/" << part << "/velocity:o" << endl;
        bEveryThingisGood = false;
    }

    if (!portPredictionErrors.open("/" + getName() + "/portPredictionErrors")) {
        cout << getName() << ": Unable to open port " << "/" + getName() + "/portPredictionErrors" << endl;
        bEveryThingisGood = false;
    }

    if (!portInputsfromBabbling.open("/" + getName() + "/portInputs")) {
        cout << getName() << ": Unable to open port " << "/" + getName() + "/portInputs" << endl;
        bEveryThingisGood = false;
    }

    if (!portOutputsfromSensoryProc.open("/" + getName() + "/portOutputsfromSensoryProc")) {
        cout << getName() << ": Unable to open port " << "/" + getName() + "/portOutputsfromSensoryProc" << endl;
        bEveryThingisGood = false;
    }


    attach(handlerPort);

    return bEveryThingisGood;
}
bool homeostaticModule::configure(yarp::os::ResourceFinder &rf)
{
    manager = homeostasisManager();
    
    moduleName = rf.check("name",Value("homeostasis")).asString().c_str();
    setName(moduleName.c_str());

    cout<<moduleName<<": finding configuration files..."<<endl;
    period = rf.check("period",Value(0.1)).asDouble();

    cout << "Initializing drives";
    Bottle grpHomeostatic = rf.findGroup("HOMEOSTATIC");
    Bottle *drivesList = grpHomeostatic.find("drives").asList();
    cout << "Initializing Drives... " << endl;
    if (drivesList)
    {
        cout << "Configuration: Found " << drivesList->size() << " drives. " << endl;
        for (int d = 0; d<drivesList->size(); d++)
        {
            cout << d << endl;
            //Read Drive Configuration
            string driveName = drivesList->get(d).asString().c_str();
            addNewDrive(driveName, grpHomeostatic);
            
        }
    }
    cout << "Opening RPC..."<< endl;
     
    rpc.open ( ("/"+moduleName+"/rpc").c_str());
    attach(rpc);

	
	cout<<"Configuration done."<<endl;
    return true;
}
示例#4
0
void ReactiveLayer::configureTactile(yarp::os::ResourceFinder &rf)
{
    //Initialise the tactile response
    Bottle grpTactile = rf.findGroup("TACTILE");
    Bottle *tactileStimulus = grpTactile.find("stimuli").asList();

    if (tactileStimulus)
    {
        for (int d = 0; d<tactileStimulus->size(); d++)
        {
            string tactileStimulusName = tactileStimulus->get(d).asString().c_str();
            StimulusEmotionalResponse response;
            Bottle * bSentences = grpTactile.find((tactileStimulusName + "-sentence").c_str()).asList();
            for (int s = 0; s<bSentences->size(); s++)
            {
                response.m_sentences.push_back(bSentences->get(s).asString().c_str());
            }
            //choregraphies
            Bottle *bChore = grpTactile.find((tactileStimulusName + "-chore").c_str()).asList();
            for (int sC = 0; bChore && sC<bChore->size(); sC++)
            {
                response.m_choregraphies.push_back(bChore->get(sC).asString().c_str());
            }
            std::string sGroupTemp = tactileStimulusName;
            sGroupTemp += "-effect";
            Bottle *bEffects = grpTactile.find(sGroupTemp.c_str()).asList();
            for (int i = 0; bEffects && i<bEffects->size(); i += 2)
            {
                response.m_emotionalEffect[bEffects->get(i).asString().c_str()] = bEffects->get(i + 1).asDouble();
            }
            tactileEffects[tactileStimulusName] = response;
        }
    }
}
示例#5
0
void ICubClient::LoadPostures(yarp::os::ResourceFinder &rf)
{
    posturesKnown.clear();

    int posCount = rf.check("posturesCount", yarp::os::Value(0)).asInt();
    //cout<<"Loading posture: "<<endl;
    for (int i = 0; i < posCount; i++)
    {
        std::stringstream ss;
        ss<<"posture_" << i;
        Bottle postureGroup = rf.findGroup(ss.str().c_str());
        BodyPosture p;
        std::string name = postureGroup.find("name").asString().c_str();
        //std::cout<<"\t"<<name<<std::endl;
        Bottle* bHead = postureGroup.find("head").asList();
        Bottle* bLArm = postureGroup.find("left_arm").asList();
        Bottle* bRArm = postureGroup.find("right_arm").asList();
        Bottle* bTorso = postureGroup.find("torso").asList();

        p.head.resize(6);
        for(int i=0;i<6;i++)
            p.head[i] = bHead->get(i).asDouble();
        p.left_arm.resize(16);
        for(int i=0;i<16;i++)
            p.left_arm[i] = bLArm->get(i).asDouble();        
        p.right_arm.resize(16);
        for(int i=0;i<16;i++)
            p.right_arm[i] = bRArm->get(i).asDouble();
        p.torso.resize(3);
        for(int i=0;i<3;i++)
            p.torso[i] = bTorso->get(i).asDouble();

        posturesKnown[name] = p;
    }
}
示例#6
0
void ICubClient::LoadChoregraphies(yarp::os::ResourceFinder &rf)
{
    choregraphiesKnown.clear();

    int posCount = rf.check("choregraphiesCount", yarp::os::Value(0)).asInt();
    cout<<"Loading Choregraphies: "<<endl;
    for (int i = 0; i < posCount; i++)
    {
        std::stringstream ss;
        ss<<"chore_" << i;
        Bottle postureGroup = rf.findGroup(ss.str().c_str());

        std::string name = postureGroup.find("name").asString().c_str();
        std::cout<<"\t"<<name<<std::endl;
        Bottle* sequence = postureGroup.find("sequence").asList();

        std::list< std::pair<std::string, double> > seq;
        for(int s=0; s<sequence->size(); s++)
        {
            Bottle* element = sequence->get(s).asList();
            std::string elementName = element->get(0).asString().c_str();
            double elementTime = element->get(1).asDouble();
            seq.push_back(std::pair<std::string,double>(elementName,elementTime));
            //std::cout<<"\t \t"<<elementName<< "\t" << elementTime << std::endl;
        }
        choregraphiesKnown[name] = seq;
    }
}
示例#7
0
void ReactiveLayer::configureSalutation(yarp::os::ResourceFinder &rf)
{
    ;
    //Initialise the gestures response
    Bottle grpSocial = rf.findGroup("SOCIAL");
    salutationLifetime = grpSocial.check("salutationLifetime", Value(15.0)).asDouble();
    Bottle *socialStimulus = grpSocial.find("stimuli").asList();

    if (socialStimulus)
    {
        for (int d = 0; d<socialStimulus->size(); d++)
        {
            string socialStimulusName = socialStimulus->get(d).asString().c_str();
            StimulusEmotionalResponse response;
            Bottle * bSentences = grpSocial.find((socialStimulusName + "-sentence").c_str()).asList();
            for (int s = 0; s<bSentences->size(); s++)
            {
                response.m_sentences.push_back(bSentences->get(s).asString().c_str());
            }
            std::string sGroupTemp = socialStimulusName;
            sGroupTemp += "-effect";
            Bottle *bEffects = grpSocial.find(sGroupTemp.c_str()).asList();
            for (int i = 0; bEffects && i<bEffects->size(); i += 2)
            {
                response.m_emotionalEffect[bEffects->get(i).asString().c_str()] = bEffects->get(i + 1).asDouble();
            }
            salutationEffects[socialStimulusName] = response;
        }
    }

    //Add the relevant Entities for handling salutation
    iCub->opc->addOrRetrieveAction("is");
    iCub->opc->addOrRetrieveAdjective("saluted");
}
示例#8
0
void AdaptiveLayer::configureGestures(yarp::os::ResourceFinder &rf)
{
    //Initialise the gestures response
    Bottle grpGesture = rf.findGroup("GESTURES");
    Bottle *gestureStimulus = grpGesture.find("stimuli").asList();

    if (gestureStimulus)
    {
        for(int d=0; d<gestureStimulus->size(); d++)
        {
            string gestureStimulusName = gestureStimulus->get(d).asString().c_str();
            StimulusEmotionalResponse response;
            Bottle * bSentences = grpGesture.find((gestureStimulusName + "-sentence").c_str()).asList();
            for(int s=0;s<bSentences->size();s++)
            {
                response.m_sentences.push_back(bSentences->get(s).asString().c_str());
            }
            std::string sGroupTemp = gestureStimulusName;
            sGroupTemp += "-effect";
            Bottle *bEffects = grpGesture.find( sGroupTemp.c_str()).asList();
            for(int i=0; bEffects && i<bEffects->size(); i += 2)
            {
                response.m_emotionalEffect[bEffects->get(i).asString().c_str()] = bEffects->get(i+1).asDouble();
            }
            gestureEffects[gestureStimulusName] = response;
        }
    }
}
示例#9
0
bool QuaternionEKF::readEstimatorParams(yarp::os::ResourceFinder &rf, quaternionEKFParams &estimatorParams)
{
    yarp::os::Bottle botParams;
    botParams = rf.findGroup("QuaternionEKF");
    yInfo("QuaternionEKF params are: %s ", botParams.toString().c_str());
    if ( botParams.isNull() )
    {
        yError("[QuaternionEKF::readEstimatorParams] No parameters were read from QuaternionEKF group");
        return false;
    } else {
        estimatorParams.stateSize = botParams.find("state_size").asInt();
        estimatorParams.inputSize = botParams.find("input_size").asInt();
        estimatorParams.measurementSize = botParams.find("measurement_size").asInt();
        estimatorParams.muSystemNoise = botParams.find("mu_system_noise").asDouble();
        estimatorParams.sigmaSystemNoise = botParams.find("sigma_system_noise").asDouble();
        estimatorParams.sigmaMeasurementNoise = botParams.find("sigma_measurement_noise").asDouble();
        estimatorParams.sigmaGyro = botParams.find("sigma_gyro_noise").asDouble();
        estimatorParams.piorMu = botParams.find("prior_mu_state").asDouble();
        estimatorParams.priorCovariance = botParams.find("prior_cov_state").asDouble();
        estimatorParams.muGyroNoise = botParams.find("mu_gyro_noise").asDouble();
        estimatorParams.floatingBaseAttitude = botParams.find("floating_base_attitude").asBool();
        estimatorParams.rot_from_ft_to_acc_bottle = new yarp::os::Bottle(*botParams.find("rot_from_ft_to_acc").asList());
    }

    botParams.clear();
    botParams = rf.findGroup("module_parameters");
    if ( botParams.isNull() )
    {
        yError("[QuaternionEKF::readEstimatorParams] No parameters were read from 'module_params' group. period is needed!");
        return false;
    } else {
        estimatorParams.period = botParams.find("period").asInt();
        estimatorParams.robotPrefix = botParams.find("robot").asString();
        estimatorParams.streamMeasurements = botParams.find("stream_measurements").asBool();
    }
    return true;
}
示例#10
0
    /**
    * Performs module configuration, parsing user options stored in the resource finder.
    * Available options are described in main module documentation.
    * @return true if the module was successfully configured and opened, false otherwise.
    */
    virtual bool configure(yarp::os::ResourceFinder &rf)
    {
        yarp::os::Time::turboBoost();

        m_rpcPort.open("/"+m_module_name+"/rpc");
        attach(m_rpcPort);
        //attachTerminal();
        m_rf = rf;
        
        //configuration file checking
        Bottle general_group = m_rf.findGroup("GENERAL");
        if (general_group.isNull())
        {
            yError() << "Missing GENERAL group!";
            return false;
        }

        Bottle initial_group = m_rf.findGroup("INITIAL_POS");
        if (initial_group.isNull())
        {
            yError() << "Missing INITIAL_POS group!";
            return false;
        }

        Bottle localization_group = m_rf.findGroup("LOCALIZATION");
        if (localization_group.isNull())
        {
            yError() << "Missing LOCALIZATION group!";
            return false;
        }

        Bottle ros_group = m_rf.findGroup("ROS");

        Bottle tf_group = m_rf.findGroup("TF");
        if (tf_group.isNull())
        {
            yError() << "Missing TF group!";
            return false;
        }

        Bottle odometry_group = m_rf.findGroup("ODOMETRY");
        if (odometry_group.isNull())
        {
            yError() << "Missing ODOMETRY group!";
            return false;
        }

        Bottle map_group = m_rf.findGroup("MAP");
        if (map_group.isNull())
        {
            yError() << "Missing MAP group!";
            return false;
        }
        yDebug() << map_group.toString();

        //general group
        if (general_group.check("module_name") == false)
        {
            yError() << "Missing `module_name` in [GENERAL] group";
            return false;
        }
        m_module_name = general_group.find("module_name").asString();

        if (general_group.check("enable_ros") == false)
        {
            yError() << "Missing `ros_enable` in [GENERAL] group";
            return false;
        }
        m_ros_enabled = (general_group.find("enable_ros").asInt()==1);

        //ROS group
        if (m_ros_enabled)
        {
            m_rosNode = new yarp::os::Node("/"+m_module_name);
            
            //initialize an occupancy grid publisher (every time the localization is re-initialized, the map is published too)
            if (ros_group.check ("occupancygrid_topic"))
            {
                m_topic_occupancyGrid = ros_group.find ("occupancygrid_topic").asString();
                if (!m_rosPublisher_occupancyGrid.topic(m_topic_occupancyGrid))
                {
                     if (m_rosNode)
                     {
                         delete m_rosNode;
                         m_rosNode=0;
                     }
                     yError() << "localizationModule: unable to publish data on " << m_topic_occupancyGrid << " topic, check your yarp-ROS network configuration";
                     return false;
                }
            }

             //initialize an initial pose publisher
            if (ros_group.check ("initialpose_topic")) 
            {
                m_topic_initial_pose = ros_group.find ("initialpose_topic").asString();
                {
                    if (!m_rosPublisher_initial_pose.topic(m_topic_initial_pose))
                    {
                        if (m_rosNode)
                        {
                            delete m_rosNode;
                            m_rosNode=0;
                        }
                        yError() << "localizationModule: unable to publish data on " << m_topic_initial_pose << " topic, check your yarp-ROS network configuration";
                        return false;
                    }
                }
            }
        }

        //localization group
        if (localization_group.check("use_localization_from_odometry_port")) { m_use_localization_from_odometry_port = (localization_group.find("use_localization_from_odometry_port").asInt() == 1); }
        if (localization_group.check("use_localization_from_tf"))   { m_use_localization_from_tf = (localization_group.find("use_localization_from_tf").asInt() == 1); }

        if (m_use_localization_from_odometry_port == true && m_use_localization_from_tf == true)
        {
            yError() << "`use_localization_from_tf` and `use_localization_from_odometry_port` cannot be true simultaneously!";
            return false;
        }

        //map server group
        yDebug() << map_group.toString();

        if (map_group.check("connect_to_yarp_mapserver") == false)
        {
            yError() << "Missing `connect_to_yarp_mapserver` in [MAP] group";
            return false;
        }
        m_use_map_server= (map_group.find("connect_to_yarp_mapserver").asInt()==1);

        //tf group
        if (tf_group.check("map_frame_id") == false)
        {
            yError() << "Missing `map_frame_id` in [TF] group";
            return false;
        }
        if (tf_group.check("robot_frame_id") == false)
        {
            yError() << "Missing `robot_frame_id` in [TF] group";
            return false;
        }
        m_frame_map_id = tf_group.find("map_frame_id").asString();
        m_frame_robot_id = tf_group.find("robot_frame_id").asString();

        //odometry group
        if (odometry_group.check("odometry_broadcast_port") == false)
        {
            yError() << "Missing `odometry_port` in [ODOMETRY] group";
            return false;
        }
        m_port_broadcast_odometry_name = odometry_group.find("odometry_broadcast_port").asString();

        //device driver opening and/or connections
        if (m_use_localization_from_odometry_port)
        {
            //opens a YARP port to receive odometry data
            std::string odom_portname = "/" + m_module_name + "/odometry:i";
            bool b1 = m_port_odometry_input.open(odom_portname.c_str());
            bool b2 = yarp::os::Network::sync(odom_portname.c_str(),false);
            bool b3 = yarp::os::Network::connect(m_port_broadcast_odometry_name.c_str(), odom_portname.c_str());
            if (b1 == false || b2 ==false || b3==false)
            {
                yError() << "Unable to initialize odometry port connection from " << m_port_broadcast_odometry_name.c_str()<< "to:" << odom_portname.c_str();
                return false;
            }
        }

        if (m_use_localization_from_tf)
        {
            //opens a client to receive localization data from transformServer
            Property options;
            options.put("device", "transformClient");
            options.put("local", "/"+m_module_name + "/TfClient");
            options.put("remote", "/transformServer");
            if (m_ptf.open(options) == false)
            {
                yError() << "Unable to open transform client";
                return false;
            }
            m_ptf.view(m_iTf);
            if (m_ptf.isValid() == false || m_iTf == 0)
            {
                yError() << "Unable to view iTransform interface";
                return false;
            }
        }

        if (m_use_map_server)
        {
            //opens a client to send/received data from mapServer
            Property map_options;
            map_options.put("device", "map2DClient");
            map_options.put("local", "/" +m_module_name); //This is just a prefix. map2DClient will complete the port name.
            map_options.put("remote", "/mapServer");
            if (m_pmap.open(map_options) == false)
            {
                yWarning() << "Unable to open mapClient";
            }
            else
            {
                yInfo() << "Opened mapClient";
                m_pmap.view(m_iMap);
                if (m_pmap.isValid() == false || m_iMap == 0)
                {
                    yError() << "Unable to view map interface";
                    return false;
                }
            }
        }

        //initial location initialization
        if (initial_group.check("initial_x"))     { m_initial_loc.x = initial_group.find("initial_x").asDouble(); }
        else { yError() << "missing initial_x param"; return false; }
        if (initial_group.check("initial_y"))     { m_initial_loc.y = initial_group.find("initial_y").asDouble(); }
        else { yError() << "missing initial_y param"; return false; }
        if (initial_group.check("initial_theta")) { m_initial_loc.theta = initial_group.find("initial_theta").asDouble(); }
        else { yError() << "missing initial_theta param"; return false; }
        if (initial_group.check("initial_map"))   { m_initial_loc.map_id = initial_group.find("initial_map").asString(); }
        else { yError() << "missing initial_map param"; return false; }
        this->initializeLocalization(m_initial_loc);

        return true;
    }
示例#11
0
bool BehaviorManager::configure(yarp::os::ResourceFinder &rf)
{
    moduleName = rf.check("name",Value("BehaviorManager")).asString();
    setName(moduleName.c_str());
    yInfo()<<moduleName<<": finding configuration files...";//<<endl;
    period = rf.check("period",Value(1.0)).asDouble();

    Bottle grp = rf.findGroup("BEHAVIORS");
    Bottle behaviorList = *grp.find("behaviors").asList();

    rpc_in_port.open("/" + moduleName + "/trigger:i");
    yInfo() << "RPC_IN : " << rpc_in_port.getName();

    for (int i = 0; i<behaviorList.size(); i++)
    {
        behavior_name = behaviorList.get(i).asString();
        if (behavior_name == "tagging") {
            behaviors.push_back(new Tagging(&mut, rf, "tagging"));
        } else if (behavior_name == "pointing") {
            behaviors.push_back(new Pointing(&mut, rf, "pointing"));
        } else if (behavior_name == "dummy") {
            behaviors.push_back(new Dummy(&mut, rf, "dummy"));
        } else if (behavior_name == "dummy2") {
            behaviors.push_back(new Dummy(&mut, rf, "dummy2"));
        }  else if (behavior_name == "reactions") {
            behaviors.push_back(new Reactions(&mut, rf, "reactions"));
        }  else if (behavior_name == "followingOrder") {
            behaviors.push_back(new FollowingOrder(&mut, rf, "followingOrder"));
        }  else if (behavior_name == "narrate") {
            behaviors.push_back(new Narrate(&mut, rf, "narrate"));
        }  else if (behavior_name == "recognitionOrder") {
            behaviors.push_back(new RecognitionOrder(&mut, rf, "recognitionOrder"));
        }  else if (behavior_name == "greeting") {
            behaviors.push_back(new Greeting(&mut, rf, "greeting"));
        }  else if (behavior_name == "ask") {
            behaviors.push_back(new Ask(&mut, rf, "ask"));
        }  else if (behavior_name == "speech") {
            behaviors.push_back(new Speech(&mut, rf, "speech"));
        } else if (behavior_name == "moveObject") {
            behaviors.push_back(new MoveObject(&mut, rf, "moveObject"));
            // other behaviors here
        }  else {
            yDebug() << "Behavior " + behavior_name + " not implemented";
            return false;
        }
    }

    //Create an iCub Client and check that all dependencies are here before starting
    bool isRFVerbose = false;
    iCub = new wysiwyd::wrdac::ICubClient(moduleName, "behaviorManager","client.ini",isRFVerbose);

    if (!iCub->connect())
    {
        yInfo() << "iCubClient : Some dependencies are not running...";
        Time::delay(1.0);
    }
    if (rf.check("use_ears",Value("false")).asBool())
    {
        yDebug()<<"using ears";
        while (!Network::connect("/ears/behavior:o", rpc_in_port.getName())) {
            yWarning() << "Ears is not reachable";
            yarp::os::Time::delay(0.5);
        }
    }else{
        yDebug()<<"not using ears";
    }

    // id = 0;
    for(auto& beh : behaviors) {
        beh->configure();
        beh->openPorts(moduleName);
        beh->iCub = iCub;

        if (beh->from_sensation_port_name != "None") {
            while (!Network::connect(beh->from_sensation_port_name, beh->sensation_port_in.getName())) {
                yInfo()<<"Connecting "<< beh->from_sensation_port_name << " to " <<  beh->sensation_port_in.getName();// <<endl;
                yarp::os::Time::delay(0.5);
            }
        }
        if (beh->external_port_name != "None") {
            while (!Network::connect(beh->rpc_out_port.getName(), beh->external_port_name)) {
                yInfo()<<"Connecting "<< beh->rpc_out_port.getName() << " to " <<  beh->external_port_name;// <<endl;
                yarp::os::Time::delay(0.5);
            }   
        }

    }

    attach(rpc_in_port);
    yInfo("Init done");

    return true;
}
示例#12
0
bool   SimToolLoaderModule::configure(yarp::os::ResourceFinder &rf) {

    Bottle table;
    Bottle temp;
    string objectName = "obj";

    /* module name */
    moduleName = rf.check("name", Value("simtoolloader"),
                          "Module name (string)").asString();

    setName(moduleName.c_str());

    /* Hand used */
    hand=rf.find("hand").asString().c_str();
    if ((hand!="left") && (hand!="right"))
        hand="right";

    /* port names */
    simToolLoaderSimOutputPortName  = "/";
    simToolLoaderSimOutputPortName += getName( rf.check("simToolLoaderSimOutputPort",
                                     Value("/sim:rpc"),
                                     "Loader output port(string)")
                                     .asString() );

    simToolLoaderCmdInputPortName  = "/";
    simToolLoaderCmdInputPortName += getName( rf.check("simToolLoaderCmdInputPort",
                                     Value("/cmd:i"),
                                     "Loader input port(string)")
                                     .asString() );

    /* open ports */
    if (!simToolLoaderSimOutputPort.open(
            simToolLoaderSimOutputPortName.c_str())) {

        cout << getName() << ": unable to open port"
        << simToolLoaderSimOutputPortName << endl;
        return false;
    }

    if (!simToolLoaderCmdInputPort.open(
            simToolLoaderCmdInputPortName.c_str())) {

        cout << getName() << ": unable to open port"
        << simToolLoaderCmdInputPortName << endl;
        return false;
    }

    /* Rate thread period */
    threadPeriod = rf.check("threadPeriod", Value(0.5),
        "Control rate thread period key value(double) in seconds ").asDouble();

    /* Read Table Configuration */
    table = rf.findGroup("table");

    /* Read the Objects configurations */
    vector<Bottle> object;

    cout << "Loading objects to buffer" << endl;
    bool noMoreModels = false;
    int n =0;
    while (!noMoreModels){      // read until there are no more objects.
        stringstream s;
        s.str("");
        s << objectName << n;
        string objNameNum = s.str();
        temp = rf.findGroup("objects").findGroup(objNameNum);

        if(!temp.isNull()) {
            cout << "object" << n << ", id: " << objNameNum;
            cout << ", model: " << temp.get(2).asString() << endl;
            object.push_back(temp);
            temp.clear();
            n++;
        }else {
            noMoreModels = true;
        }
    }
    numberObjs = n;

    cout << "Loaded " << object.size() << " objects " << endl;

    /* Create the control rate thread */
    ctrlThread = new CtrlThread(&simToolLoaderSimOutputPort,
                                &simToolLoaderCmdInputPort,
                                threadPeriod,
                                table, object,hand);
    /* Starts the thread */
    if (!ctrlThread->start()) {
        delete ctrlThread;
        return false;
    }

    return true;
}
示例#13
0
void AllostaticController::configureAllostatic(yarp::os::ResourceFinder &rf)
{
    //The homeostatic module should be running in parallel, independent from this, so the objective of
    //this config would be to have a proper list  and connect to each port

    homeo_name = "homeostasis";
    string homeo_rpc_name = "/" + homeo_name + "/rpc";
    string to_h_rpc_name="/"+moduleName+"/toHomeoRPC:o";
    to_homeo_rpc.open(to_h_rpc_name);

    while(!Network::connect(to_h_rpc_name,homeo_rpc_name))
    {
        yDebug()<<"Trying to connect to homeostasis...";
        yDebug() << "from " << to_h_rpc_name << " to " << homeo_rpc_name;
        yarp::os::Time::delay(0.2);
    }

    yInfo() << "Initializing drives...";//<<endl;
    Bottle grpAllostatic = rf.findGroup("ALLOSTATIC");
    drivesList = *grpAllostatic.find("drives").asList();
    
    Bottle cmd;

    priority_sum = 0.;
    double priority;
    for (int d = 0; d<drivesList.size(); d++)
    {
        cmd.clear();
        string driveName = drivesList.get(d).asString();
        yInfo() << ("Initializing drive " + driveName);

        cmd.addString("add");
        cmd.addString("conf");
        Bottle drv;
        drv.clear();
        Bottle aux;
        aux.clear();
        aux.addString("name");
        aux.addString(driveName);
        drv.addList()=aux;
        aux.clear();
        drv.addList()=grpAllostatic;
        cmd.addList()=drv;
        Bottle rply;
        rply.clear();
        rply.get(0).asString();
        to_homeo_rpc.write(cmd,rply);

        AllostaticDrive alloDrive;
        alloDrive.name = driveName;

        Value cmds = grpAllostatic.check((driveName + "-sensation-on"), Value("None"));
        alloDrive.sensationOnCmd = *cmds.asList();
        cmds = grpAllostatic.check((driveName + "-sensation-off"), Value("None"));
        alloDrive.sensationOffCmd = *cmds.asList();

        cmds = grpAllostatic.check((driveName + "-before-trigger"), Value("None"));
        if (!(cmds.isString() && cmds.asString() == "None")) {
            alloDrive.beforeTriggerCmd = *cmds.asList();
        }

        cmds = grpAllostatic.check((driveName + "-after-trigger"), Value("None"));
        if (!(cmds.isString() && cmds.asString() == "None")) {
            alloDrive.afterTriggerCmd = *cmds.asList();
        }        

        alloDrive.homeoPort = &to_homeo_rpc;

        alloDrive.inputSensationPort = new BufferedPort<Bottle>;
        string portName = "/" + moduleName + "/" + driveName + "/sensation:i";
        alloDrive.inputSensationPort->open(portName);

        openPorts(driveName);

        string sensationPort = grpAllostatic.check((driveName + "-sensation-port"), Value("None")).asString();
        string pn = "/" + moduleName + "/" + driveName + "/sensation:i";
        while(!Network::connect(sensationPort, pn)) {
            yDebug()<<"Connecting " << sensationPort << " to " << pn;
            yarp::os::Time::delay(0.5);
        }


        // set drive priorities. Default to 1.
        priority = grpAllostatic.check((driveName + "-priority"), Value(1.)).asDouble();
        priority_sum += priority;
        drivePriorities.push_back(priority);

        //Under effects
        string under_port_name = grpAllostatic.check((driveName + "-under-behavior-port"), Value("None")).asString();
        string under_cmd_name = grpAllostatic.check((driveName + "-under-behavior"), Value("None")).asString();

        bool active = false;
        
        if (under_port_name != "None" && under_cmd_name != "None")
        {
            active = true;
            string out_port_name = "/" + moduleName + "/" + driveName + "/under_action:o";
            alloDrive.behaviorUnderPort = new Port();
            alloDrive.behaviorUnderPort->open(out_port_name);
            yDebug() << "trying to connect to" << under_port_name;
            while(!Network::connect(out_port_name,under_port_name))
            {
                
                yarp::os::Time::delay(0.5);
            }
            alloDrive.behaviorUnderCmd = Bottle(under_cmd_name);
        }else{
            yInfo() << "No port name for" << driveName << "under-behavior-port";
        }

        //Over effects
        string over_port_name = grpAllostatic.check((driveName + "-over-behavior-port"), Value("None")).asString();
        string over_cmd_name = grpAllostatic.check((driveName + "-over-behavior"), Value("None")).asString();
        if (over_port_name != "None" && over_cmd_name != "None")
        {
            active=true;
            string out_port_name = "/" + moduleName + "/" + driveName + "/over_action:o";
            alloDrive.behaviorOverPort = new Port();
            alloDrive.behaviorOverPort->open(out_port_name);
            yDebug() << "trying to connect to" << over_port_name;
            while(!Network::connect(out_port_name, over_port_name))
            {
                yarp::os::Time::delay(0.5);
            }
            alloDrive.behaviorOverCmd = Bottle(over_cmd_name);
        } else {
            yInfo() << "No port name for" << driveName << "over-behavior-port";
        }

        alloDrive.active = active;
        allostaticDrives[driveName] = alloDrive;
    }

    if (! Normalize(drivePriorities))
        yDebug() << "Error: Drive priorities sum up to 0.";


    yInfo() << "done.";

}
    bool stereoCalibModule::configure(yarp::os::ResourceFinder &rf)
    {
        Bottle ports_root = rf.findGroup("ports_root");
        moduleName = ports_root.find("name").asString();
        setName(moduleName.c_str());

        //Port names
        Bottle input_port_names = rf.findGroup("input_port_names");
        leftImagePortName = "/" + moduleName + (String) input_port_names.find("left_image_port").asString();
        rightImagePortName = "/" + moduleName + (String) input_port_names.find("right_image_port").asString();
        encodersPortName = "/" + moduleName + (String) input_port_names.find("encoders_port").asString();
        rpcPointRequestPortName = "/" + moduleName + (String) input_port_names.find("rpc_point_request_port").asString();

        Bottle output_port_names = rf.findGroup("output_port_names");
        leftImageOutPortName = "/" + moduleName + (String) output_port_names.find("left_image_out_port").asString();
        rightImageOutPortName = "/" + moduleName + (String) output_port_names.find("right_image_out_port").asString();
        disparityImageOutPortName = "/" + moduleName + (String) output_port_names.find("disparity_image_out_port").asString();

        // open ports
        if (!leftImagePort.open(
                leftImagePortName.c_str()))
        {
            cout << getName() << ": unable to open port"
            << leftImagePortName << endl;
            return false;
        }
        _stereoCalibThread_data.leftImagePort = &leftImagePort;


        if (!rightImagePort.open(
                rightImagePortName.c_str()))
        {
            cout << getName() << ": unable to open port"
            << rightImagePortName << endl;
            return false;
        }
        _stereoCalibThread_data.rightImagePort= &rightImagePort;


        if (!encodersPort.open(
                encodersPortName.c_str()))
        {
            cout << getName() << ": unable to open port"
            << encodersPortName << endl;
            return false;
        }
        _stereoCalibThread_data.encodersPort = &encodersPort;

        if (!rpcPointRequestPort.open(
                rpcPointRequestPortName.c_str()))
        {
            cout << getName() << ": unable to open port"
            << rpcPointRequestPortName << endl;
            return false;
        }
        _worldPointThread_data.rpcPointRequestPort = &rpcPointRequestPort;

        if (!leftImageOutPort.open(
                leftImageOutPortName.c_str()))
        {
            cout << getName() << ": unable to open port"
            << leftImageOutPortName << endl;
            return false;
        }
        _disparityThread_data.leftImageOutPort = &leftImageOutPort;

        if (!rightImageOutPort.open(
                rightImageOutPortName.c_str()))
        {
            cout << getName() << ": unable to open port"
            << rightImageOutPortName << endl;
            return false;
        }
        _disparityThread_data.rightImageOutPort = &rightImageOutPort;//*/

        if (!disparityImageOutPort.open(
                disparityImageOutPortName.c_str()))
        {
            cout << getName() << ": unable to open port"
            << disparityImageOutPortName << endl;
            return false;
        }
        _disparityThread_data.disparityImageOutPort = &disparityImageOutPort;//*/

        // Rate threads period
        _stereoCalibThread_data.threadPeriod = rf.check("threadPeriod", Value(0.001),
            "Control rate thread period key value(double) in seconds ").asDouble();
        _disparityThread_data.threadPeriod = _stereoCalibThread_data.threadPeriod;
        _worldPointThread_data.threadPeriod = _stereoCalibThread_data.threadPeriod;

        //Calibration parameters from left camera
        Bottle left_camera_intrinsics = rf.findGroup("left_camera_intrinsics");
        _stereoCalibThread_data._imagesBase_initial_parameters.left_resx = left_camera_intrinsics.find("resx").asInt();
        _stereoCalibThread_data._imagesBase_initial_parameters.left_resy = left_camera_intrinsics.find("resy").asInt();
        _stereoCalibThread_data._imagesBase_initial_parameters.left_cx = left_camera_intrinsics.find("cx").asDouble();
        _stereoCalibThread_data._imagesBase_initial_parameters.left_cy = left_camera_intrinsics.find("cy").asDouble();
        _stereoCalibThread_data._imagesBase_initial_parameters.left_fx = left_camera_intrinsics.find("fx").asDouble();
        _stereoCalibThread_data._imagesBase_initial_parameters.left_fy = left_camera_intrinsics.find("fy").asDouble();
        _stereoCalibThread_data._imagesBase_initial_parameters.left_k1 = left_camera_intrinsics.find("k1").asDouble();
        _stereoCalibThread_data._imagesBase_initial_parameters.left_k2 = left_camera_intrinsics.find("k2").asDouble();
        _stereoCalibThread_data._imagesBase_initial_parameters.left_k3 = left_camera_intrinsics.find("k3").asDouble();
        _stereoCalibThread_data._imagesBase_initial_parameters.left_k4 = left_camera_intrinsics.find("k4").asDouble();
        _stereoCalibThread_data._imagesBase_initial_parameters.left_k5 = left_camera_intrinsics.find("k5").asDouble();

        //Calibration parameters from right camera
        Bottle right_camera_intrinsics = rf.findGroup("right_camera_intrinsics");
        _stereoCalibThread_data._imagesBase_initial_parameters.right_resx = right_camera_intrinsics.find("resx").asInt();
        _stereoCalibThread_data._imagesBase_initial_parameters.right_resy = right_camera_intrinsics.find("resy").asInt();
        _stereoCalibThread_data._imagesBase_initial_parameters.right_cx = right_camera_intrinsics.find("cx").asDouble();
        _stereoCalibThread_data._imagesBase_initial_parameters.right_cy = right_camera_intrinsics.find("cy").asDouble();
        _stereoCalibThread_data._imagesBase_initial_parameters.right_fx = right_camera_intrinsics.find("fx").asDouble();
        _stereoCalibThread_data._imagesBase_initial_parameters.right_fy = right_camera_intrinsics.find("fy").asDouble();
        _stereoCalibThread_data._imagesBase_initial_parameters.right_k1 = right_camera_intrinsics.find("k1").asDouble();
        _stereoCalibThread_data._imagesBase_initial_parameters.right_k2 = right_camera_intrinsics.find("k2").asDouble();
        _stereoCalibThread_data._imagesBase_initial_parameters.right_k3 = right_camera_intrinsics.find("k3").asDouble();
        _stereoCalibThread_data._imagesBase_initial_parameters.right_k4 = right_camera_intrinsics.find("k4").asDouble();
        _stereoCalibThread_data._imagesBase_initial_parameters.right_k5 = right_camera_intrinsics.find("k5").asDouble();

        _disparityThread_data._imagesBase_initial_parameters = _stereoCalibThread_data._imagesBase_initial_parameters;

        //FIlter parameters
        Bottle filter_parameters = rf.findGroup("filter_parameters");
        _stereoCalibThread_data._calibrationStereoCameras_data.matchingThreshold = filter_parameters.find("matching_threshold").asDouble();
        _stereoCalibThread_data._calibrationStereoCameras_data.minNumberFeatures = filter_parameters.find("minimum_number_of_features").asInt();
        _stereoCalibThread_data._calibrationStereoCameras_data.maxNumberFeatures = filter_parameters.find("maximum_number_of_features").asInt();
        _stereoCalibThread_data._calibrationStereoCameras_data.numFixStateParams = filter_parameters.find("number_of_fixed_state_params").asInt();
        _stereoCalibThread_data._calibrationStereoCameras_data.numMeasurements = filter_parameters.find("number_of_meausurements").asInt();
        _stereoCalibThread_data._calibrationStereoCameras_data.encodersStateNoise = filter_parameters.find("encoders_state_noise").asDouble();
        _stereoCalibThread_data._calibrationStereoCameras_data.encodersTransitionNoise = filter_parameters.find("encoders_transition_noise").asDouble();
        _stereoCalibThread_data._calibrationStereoCameras_data.featuresMeasurementsNoise = filter_parameters.find("features_measurements_noise").asDouble();
        _stereoCalibThread_data._calibrationStereoCameras_data.encodersMeasurementsNoise = filter_parameters.find("encoders_measurements_noise").asDouble();
        _stereoCalibThread_data._calibrationStereoCameras_data.desiredImageWidth = filter_parameters.find("desired_image_w").asInt();
        _stereoCalibThread_data._calibrationStereoCameras_data.desiredImageHeight = filter_parameters.find("desired_image_h").asInt();

        //DIsparity map parameters
        Bottle disparity_parameters = rf.findGroup("disparity_parameters");
        _stereoCalibThread_data._imageDisparity_initial_parameters.numberOfDisparities = disparity_parameters.find("number_of_disparities").asInt();
        _stereoCalibThread_data._imageDisparity_initial_parameters.preFilterCap = disparity_parameters.find("pre_filter_cap").asInt();
        _stereoCalibThread_data._imageDisparity_initial_parameters.SADWindowSize = disparity_parameters.find("SAD_window_size").asInt();
        _stereoCalibThread_data._imageDisparity_initial_parameters.P1 = disparity_parameters.find("P1").asInt();
        _stereoCalibThread_data._imageDisparity_initial_parameters.P2 = disparity_parameters.find("P2").asInt();
        _stereoCalibThread_data._imageDisparity_initial_parameters.minDisparity = disparity_parameters.find("min_disparity").asInt();
        _stereoCalibThread_data._imageDisparity_initial_parameters.uniquenessRatio = disparity_parameters.find("uniqueness_ratio").asInt();
        _stereoCalibThread_data._imageDisparity_initial_parameters.speckleWindowSize = disparity_parameters.find("speckle_window_size").asInt();
        _stereoCalibThread_data._imageDisparity_initial_parameters.speckleRange = disparity_parameters.find("speckle_range").asInt();
        _stereoCalibThread_data._imageDisparity_initial_parameters.disp12MaxDiff = disparity_parameters.find("disp12_max_diff").asInt();

        _disparityThread_data._imageDisparity_initial_parameters = _stereoCalibThread_data._imageDisparity_initial_parameters;

        Bottle baseline_value = rf.findGroup("baseline_value");
        _stereoCalibThread_data.baseline = baseline_value.find("baseline").asDouble();

        /* Create the control rate thread */
        _stereoCalibThread = new stereoCalibThread(_stereoCalibThread_data);
        _disparityThread = new disparityThread(_stereoCalibThread, _disparityThread_data);
        _worldPointThread = new worldPointThread(_disparityThread, _worldPointThread_data);

        /* Starts the thread */
        if (!_stereoCalibThread->start()) {
            delete _stereoCalibThread;
            return false;
        }

        if (!_disparityThread->start()) {
            delete _disparityThread;
            return false;
        }

        if (!_worldPointThread->start()) {
            delete _worldPointThread;
            return false;
        }

        return true;
    }
bool quaternionEKFModule::configure ( yarp::os::ResourceFinder& rf )
{
    if( rf.check("robot") ) {
        robotName = rf.find("robot").asString();
    } else {
        yError("[quaternionEKFModule::configure] Configuration failed! No robot param was found");
        return false;
    }
    
    if( rf.check("rate") && rf.find("rate").asDouble() )
    {
        period = rf.find("rate").asDouble();
    } else {
        yError("[quaternionEKFModule::configure] Configuration failed. No rate was specified. Remember it should be a double.");
        return false;
    }
    
    if( rf.check("local") ) {
        local =  rf.find("local").asString();
    } else {
        yError("[quaternionEKFModule::configure] Configuration failed. No local name was foundd.");
        return false;
    }
    
    if( rf.check("autoconnect") ) {
        autoconnect = rf.find("autoconnect").asBool();
    } else {
        yError("[quaternionEKFModule::configure] Configuration failed. No value for autoconnect was found.");
        return false;
    }
    
    if( rf.check("mode") ) {
        mode = rf.find("mode").asString();
    } else {
        yError("[quaternionEKFModule::configure] Configuration failed. No value for mode was found.");
        return false;
    }
    
    if ( rf.check("usingXSens") ) {
        usingxsens = rf.find("usingXSens").asBool();
    } else {
        yError ("[quaternionEKFModule::configure] Configuration failed. No value for usingXSens was found.");
        return false;
    }
    
    if ( rf.check("verbose") ) {
        verbose = rf.find("verbose").asBool();
    } else {
        yError ("[quaternionEKFModule::configure] Configuration failed. No value for verbose was found.");
        return false;
    }
    
    if ( rf.check("sensorPortName") )  {
        sensorPortName = rf.find("sensorPortName").asString();
    } else {
        yError ("[quaternionEKFModule::configure] Configuration failed. No value for sensorPortName was found");
        return false;
    }
    
    if ( rf.check("usingEKF") ) {
        usingEKF = rf.find("usingEKF").asBool();
    } else {
        yError ("[quaternionEKFModule::configure] Configuration failed. No value for usingEKF was found.");
        return false;
    }
    
    if (rf.check("usingSkin")) {
        usingSkin = rf.find("usingSkin").asBool();
    } else {
        yError ("[quaternionEKFModule::configure] Configuration failed. No value for usingSkin was found.");
        return false;
    }
    
    if (rf.check("inWorldRefFrame")) {
        inWorldRefFrame = rf.find("inWorldRefFrame").asBool();
    } else {
        yError ("[quaternionEKFModule::configure] Configuration failed. No value for inWorldRefFrame was found.");
        return false;
    }
    
    if (rf.check("gravityVec")) {
        gravityVec = rf.find("gravityVec").asDouble();
    } else {
        yError ("[quaternionEKFModule::configure] Configuration failed. No value for gravityVec was found.");
        return false;
    }
    
    if (rf.check("debugGyro")) {
        debugGyro = rf.find("debugGyro").asBool();
    } else {
        yError ("[quaternionEKFModule::configure] Configuration failed. No value for debugGyro was found. ");
        return false;
    }
    
    if (rf.check("debugAcc")) {
        debugAcc = rf.find("debugAcc").asBool();
    } else {
        yError ("[quaternionEKFModule::configure] Configuration failed. No value for debugAcc was found. ");
        return false;
    }
    
    if (rf.check("using2acc")) {
        using2acc = rf.find("using2acc").asBool();
    } else {
        yError ("[quaternionEKFModule::configure] Configuration failed. No value for using2acc was found.");
        return false;
    }
    
    if ( rf.check("calib") ) {
        calib = rf.find("calib").asBool();
    } else {
        yError ("[quaternionEKFModule::configure] Configuration failed. No value for calib was found.");
        return false;
    }
    
    // ------------ IMU PORT ---------------------------------------
    /*TODO This should be configurable! The number of input ports
     depending on the amount of sensor readings.*/
    std::string tmpOffline = "offline";
    std::string tmpOnline  = "online";
    //  If the estimate is done online
    if (!calib) {
        if (!tmpOnline.compare(mode)) {
            yInfo(" [quaternionEKFModule::configure] Online estimation will be performed");
            std::string gyroMeasPortName = "/";
            gyroMeasPortName += local;
            gyroMeasPortName += "/imu:i";
            if (!gyroMeasPort.open(gyroMeasPortName.c_str())) {
                yError("[quaternionEKFModule::configure] Could not open gyroMeasPort");
                return false;
            }
            // If using two accelerometers, open another port for the second reading
            if (using2acc) {
                std::string gyroMeasPortName2 = "/"; gyroMeasPortName2 += local; gyroMeasPortName2 += "/imu2:i";
                if (!gyroMeasPort2.open(gyroMeasPortName2.c_str())) {
                    yError("[quaternionEKFModule::configure] Coult not open gyroMeasPort2");
                    return false;
                }
            }

            // Obtaining filter parameters from configuration file
            yarp::os::Property filterParams;

            // If using Direct method with atan2
            if(!usingEKF)
            {
                if( !rf.check(DIRECT_GROUP_PARAMS_NAME) )  {
                    yError("[quaternionEKFModule::configure] Could not load DIRECT-FILTER-PARAMS group from config file");
                    return false;
                } else   {
                    filterParams.fromString(rf.findGroup(DIRECT_GROUP_PARAMS_NAME).tail().toString());
                    yInfo(" [quaternionEKFModule::configure] Filter parameters are: %s ", filterParams.toString().c_str());
                }
            }
            else
            {
                if( !rf.check(FILTER_GROUP_PARAMS_NAME) )  {
                    yError("[quaternionEKFModule::configure] Could not load EKF-PARAMS group from config file");
                    return false;
                } else   {
                    filterParams.fromString(rf.findGroup(FILTER_GROUP_PARAMS_NAME).tail().toString());
                    yInfo(" [quaternionEKFModule::configure] Filter parameters are: %s ", filterParams.toString().c_str());
                }
            }
            // ----------- THREAD INSTANTIATION AND CALLING -----------------
            quatEKFThread = new quaternionEKFThread(period, local, robotName, autoconnect, usingxsens, usingEKF, inWorldRefFrame, gravityVec, usingSkin, sensorPortName, debugGyro, debugAcc, verbose, filterParams, &gyroMeasPort, &gyroMeasPort2);
            if (!quatEKFThread->start()) {
                yError("Error starting quaternionEKFThread!");
                return false;
            }
            
            yInfo(" [quaternionEKFModule::configure] quaternionEKFThread started");
            
        } else {
            // If the estimate is done offline, read from file with a datadumper format and don't create the thread.
            if(!tmpOffline.compare(mode)) {
                yInfo(" [quaternionEKFModule::configure] Offline batch estimation will be performed");
                
                // **** Initialization
                // Create dataDumper parser
                m_parser = new dataDumperParser(DATAFILE);
                m_parser->parseFile();
                m_parser->countLines();
                
                // Change period of the module thread
                
            } else {
                yError("[quaternionEKFModule::configure] An invalid option was passed to 'mode'. Available options are 'offline' or 'online'.");
                return false;
            }
        }
    } else { 
        std::cout << "Calibrating only... Done bby updateModule()" << std::endl;
    }
  return true;
}
bool TorqueBalancingReferencesGenerator::configure (yarp::os::ResourceFinder &rf)
{
    using namespace yarp::os;
    using namespace yarp::sig;
    using namespace Eigen;

    
    if (!rf.check("wbi_config_file", "Checking wbi configuration file")) {
        std::cout << "No WBI configuration file found.\n";
        return false;
    }
    
    Property wbiProperties;
    if (!wbiProperties.fromConfigFile(rf.findFile("wbi_config_file"))) {
        std::cout << "Not possible to load WBI properties from file.\n";
        return false;
    }
    wbiProperties.fromString(rf.toString(), false);

    //retrieve the joint list
    std::string wbiList = rf.find("wbi_joint_list").asString();

    wbi::IDList iCubMainJoints;
    if (!yarpWbi::loadIdListFromConfig(wbiList, wbiProperties, iCubMainJoints)) {
        std::cout << "Cannot find joint list\n";
        return false;
    }

    size_t actuatedDOFs = iCubMainJoints.size();

    //create an instance of wbi
    m_robot = new yarpWbi::yarpWholeBodyInterface("torqueBalancingReferencesGenerator", wbiProperties);
    if (!m_robot) {
        std::cout << "Could not create wbi object.\n";
        return false;
    }

    m_robot->addJoints(iCubMainJoints);
    if (!m_robot->init()) {
        std::cout << "Could not initialize wbi object.\n";
        return false;
    }
    
    robotName = rf.check("robot", Value("icubSim"), "Looking for robot name").asString();
//     numberOfPostures = rf.check("numberOfPostures", Value(0), "Looking for numberOfPostures").asInt();

    directionOfOscillation.resize(3,0.0);
    frequencyOfOscillation = 0;
    amplitudeOfOscillation = 0;

    if (!loadReferences(rf,postures,comTimeAndSetPoints,actuatedDOFs,changePostural, changeComWithSetPoints,amplitudeOfOscillation,frequencyOfOscillation, directionOfOscillation  ))
    {
        return false;
    }
    loadReferences(rf,postures,comTimeAndSetPoints,actuatedDOFs,changePostural, changeComWithSetPoints,amplitudeOfOscillation,frequencyOfOscillation, directionOfOscillation);
    for(int i=0; i < postures.size(); i++ )
    {
        std::cerr << "[INFO] time_" << i << " = " << postures[i].time << std::endl;
        std::cerr << "[INFO] posture_" << i << " = " << postures[i].qDes.toString()<< std::endl;
    }
    for(int i=0; i < comTimeAndSetPoints.size(); i++ )
    {
        std::cerr << "[INFO] time_" << i << " = " << comTimeAndSetPoints[i].time << std::endl;
        std::cerr << "[INFO] com_" << i << " = " <<  comTimeAndSetPoints[i].comDes.toString()<< std::endl;
    }
//     
    std::cout << "[INFO] Number of DOFs: " << actuatedDOFs << std::endl;
    
    std::cerr << "[INFO] changePostural: " << changePostural << std::endl;
    
    std::cerr << "[INFO] changeComWithSetPoints: " << changeComWithSetPoints << std::endl;
    
    std::cerr << "[INFO] amplitudeOfOscillation: " << amplitudeOfOscillation << std::endl;

    std::cout << "[INFO] frequencyOfOscillation " << frequencyOfOscillation << std::endl;
    
    std::cerr << "[INFO] directionOfOscillation: " << directionOfOscillation.toString() << std::endl;

    q0.resize(actuatedDOFs, 0.0);
    com0.resize(3, 0.0);
    comDes.resize(3, 0.0);
    DcomDes.resize(3, 0.0);
    DDcomDes.resize(3, 0.0);
    m_robot->getEstimates(wbi::ESTIMATE_JOINT_POS, q0.data());
    
    double world2BaseFrameSerialization[16];
    double rotoTranslationVector[7];
    wbi::Frame world2BaseFrame;
    m_robot->getEstimates(wbi::ESTIMATE_BASE_POS, world2BaseFrameSerialization);
    wbi::frameFromSerialization(world2BaseFrameSerialization, world2BaseFrame);
    m_robot->forwardKinematics(q0.data(), world2BaseFrame, wbi::wholeBodyInterface::COM_LINK_ID, rotoTranslationVector);
    com0[0] = rotoTranslationVector[0];    
    com0[1] = rotoTranslationVector[1];
    com0[2] = rotoTranslationVector[2];
    
    timeoutBeforeStreamingRefs = rf.check("timeoutBeforeStreamingRefs", Value(20), "Looking for robot name").asDouble();

    period = rf.check("period", Value(0.01), "Looking for module period").asDouble();
    
    std::cout << "timeoutBeforeStreamingRefs: " << timeoutBeforeStreamingRefs << "\n";

    std::string group_name = "PORTS_INFO";
    if( !rf.check(group_name) )
    {
        std::cerr << "[ERR] no PORTS_INFO group found" << std::endl;
        return true;
    }
    else
    {
        yarp::os::Bottle group_bot = rf.findGroup(group_name);

        if( !group_bot.check("portNameForStreamingQdes") || 
            !group_bot.check("portNameForStreamingComDes") )
        {
             std::cerr << "[ERR] portNameForStreamingQdes or portNameForStreamingComDes not found in config file" << std::endl;
                    return false;
        }

        //Check coupling configuration is well formed
        std::string portNameForStreamingComDes = group_bot.find("portNameForStreamingComDes").asString();
        std::cout << "portNameForStreamingComDes: " << portNameForStreamingComDes << "\n";
        
        std::string portNameForStreamingQdes = group_bot.find("portNameForStreamingQdes").asString();
        std::cout << "portNameForStreamingQdes: " << portNameForStreamingQdes << "\n";

        portForStreamingComDes.open(portNameForStreamingComDes);
        
        portForStreamingQdes.open(portNameForStreamingQdes);
        
        t0 = yarp::os::Time::now();
    return true;
    }
}
示例#17
0
void AdaptiveLayer::configureOPC(yarp::os::ResourceFinder &rf)
{
    //Populate the OPC if required
    cout<<"Populating OPC";
    Bottle grpOPC = rf.findGroup("OPC");
    bool shouldPopulate = grpOPC.find("populateOPC").asInt() == 1;
    if (shouldPopulate)
    {
        Bottle *agentList = grpOPC.find("agent").asList();
        if (agentList)
        {
            for(int d=0; d<agentList->size(); d++)
            {
                string name = agentList->get(d).asString().c_str();
                Agent* agent = iCub->opc->addOrRetrieveEntity<Agent>(name);
                agent->m_present = false;
                iCub->opc->commit(agent);
            }
        }

        Bottle *objectList = grpOPC.find("object").asList();
        if (objectList)
        {
            for(int d=0; d<objectList->size(); d++)
            {
                string name = objectList->get(d).asString().c_str();
                Object* o = iCub->opc->addOrRetrieveEntity<Object>(name);
                o->m_present = false;
                iCub->opc->commit(o);
            }
        }

        Bottle *rtobjectList = grpOPC.find("rtobject").asList();
        if (rtobjectList)
        {
            for(int d=0; d<rtobjectList->size(); d++)
            {
                string name = rtobjectList->get(d).asString().c_str();
                RTObject* o = iCub->opc->addOrRetrieveEntity<RTObject>(name);
                o->m_present = false;
                iCub->opc->commit(o);
            }
        }

        Bottle *adjectiveList = grpOPC.find("adjective").asList();
        if (adjectiveList)
        {
            for(int d=0; d<adjectiveList->size(); d++)
            {
                string name = adjectiveList->get(d).asString().c_str();
                iCub->opc->addOrRetrieveEntity<Adjective>(name);
            }
        }

        Bottle *actionList = grpOPC.find("action").asList();
        if (actionList)
        {
            for(int d=0; d<actionList->size(); d++)
            {
                string name = actionList->get(d).asString().c_str();
                iCub->opc->addOrRetrieveEntity<Action>(name);
            }
        }
    }
    cout<<"done"<<endl;
}
示例#18
0
void ReactiveLayer::configureAllostatic(yarp::os::ResourceFinder &rf)
{
    //Initialise the iCub allostatic model
    cout << "Initializing drives";
    Bottle grpAllostatic = rf.findGroup("ALLOSTATIC");
    Bottle *drivesList = grpAllostatic.find("drives").asList();
    iCub->icubAgent->m_drives.clear();
    if (drivesList)
    {
        for (int d = 0; d<drivesList->size(); d++)
        {
            string driveName = drivesList->get(d).asString().c_str();
            Drive drv;
            drv.name = driveName;
            drv.homeoStasisMin = grpAllostatic.check((driveName + "-homeostasisMin").c_str(), Value(0.25)).asDouble();
            drv.homeoStasisMax = grpAllostatic.check((driveName + "-homeostasisMax").c_str(), Value(0.75)).asDouble();
            drv.decay = grpAllostatic.check((driveName + "-decay").c_str(), Value(0.05)).asDouble();
            drv.value = (drv.homeoStasisMax + drv.homeoStasisMin) / 2.0;
            iCub->icubAgent->m_drives[driveName] = drv;

            //Under effects
            StimulusEmotionalResponse responseUnder;
            Bottle * bSentences = grpAllostatic.find((driveName + "-under-sentences").c_str()).asList();
            for (int s = 0; bSentences && s<bSentences->size(); s++)
            {
                responseUnder.m_sentences.push_back(bSentences->get(s).asString().c_str());
            }
            Bottle *bChore = grpAllostatic.find((driveName + "-under-chore").c_str()).asList();
            for (int sC = 0; bChore && sC<bChore->size(); sC++)
            {
                responseUnder.m_choregraphies.push_back(bChore->get(sC).asString().c_str());
            }
            homeostaticUnderEffects[driveName] = responseUnder;

            //Over effects
            StimulusEmotionalResponse responseOver;
            bSentences = grpAllostatic.find((driveName + "-over-sentences").c_str()).asList();
            for (int s = 0; bSentences&& s<bSentences->size(); s++)
            {
                responseOver.m_sentences.push_back(bSentences->get(s).asString().c_str());
            }
            bChore = grpAllostatic.find((driveName + "-over-chore").c_str()).asList();
            for (int sC = 0; bChore && sC<bChore->size(); sC++)
            {
                responseOver.m_choregraphies.push_back(bChore->get(sC).asString().c_str());
            }
            homeostaticOverEffects[driveName] = responseOver;
        }
    }
    cout << "done" << endl;

    //Initialise the iCub emotional model
    cout << "Initializing emotions...";
    Bottle grpEmotions = rf.findGroup("EMOTIONS");
    Bottle *emotionsList = grpEmotions.find("emotions").asList();
    double emotionalDecay = grpEmotions.check("emotionalDecay", Value(0.1)).asDouble();

    iCub->icubAgent->m_emotions_intrinsic.clear();
    if (emotionsList)
    {
        for (int d = 0; d<emotionsList->size(); d++)
        {
            string emoName = emotionsList->get(d).asString().c_str();
            iCub->icubAgent->m_emotions_intrinsic[emoName] = 0.0;
        }
    }
    cout << "done" << endl;

    faceUpdatePeriod = grpEmotions.check("expressionUpdatePeriod", Value(0.5)).asDouble();

    cout << "Commiting iCubAgent...";
    iCub->commitAgent();
    cout << "done." << endl;

    InternalVariablesDecay* decayThread;
    decayThread = new InternalVariablesDecay(500, emotionalDecay);
    decayThread->start();
}