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