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"; }
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; }
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; } } }
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; } }
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; } }
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"); }
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; } } }
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; }
/** * 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; }
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; }
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; }
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; } }
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; }
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(); }