void partMover::home_click(GtkButton *button, gtkClassData* currentClassData)
{
partMover *currentPart = currentClassData->partPointer;
int * joint = currentClassData->indexPointer;
IPositionControl *ipos = currentPart->pos;
IEncoders *iiencs = currentPart->iencs;
IAmplifierControl *iamp = currentPart->amp;
IPidControl *ipid = currentPart->pid;
IControlCalibration2 *ical = currentPart->cal;
int NUMBER_OF_JOINTS;
ipos->getAxes(&NUMBER_OF_JOINTS);
//fprintf(stderr, "Retrieving finder \n");
ResourceFinder *fnd = currentPart->finder;
//fprintf(stderr, "Retrieved finder: %p \n", fnd);
char buffer1[800];
char buffer2[800];
strcpy(buffer1, currentPart->partLabel);
strcpy(buffer2, strcat(buffer1, "_zero"));
//fprintf(stderr, "Finder retrieved %s\n", buffer2);
if (!fnd->findGroup(buffer2).isNull() && !fnd->isNull())
{
//fprintf(stderr, "Home group was not empty \n");
bool ok = true;
Bottle xtmp;
xtmp = fnd->findGroup(buffer2).findGroup("PositionZero");
ok = ok && (xtmp.size() == NUMBER_OF_JOINTS+1);
double positionZero = xtmp.get(*joint+1).asDouble();
//fprintf(stderr, "%f\n", positionZero);
xtmp = fnd->findGroup(buffer2).findGroup("VelocityZero");
//fprintf(stderr, "VALUE VEL is %d \n", fnd->findGroup(buffer2).find("VelocityZero").toString().c_str());
ok = ok && (xtmp.size() == NUMBER_OF_JOINTS+1);
double velocityZero = xtmp.get(*joint+1).asDouble();
//fprintf(stderr, "%f\n", velocityZero);
if(!ok)
dialog_message(GTK_MESSAGE_ERROR,(char *) "Check the number of entries in the group", buffer2, true);
else
{
ipos->setRefSpeed(*joint, velocityZero);
ipos->positionMove(*joint, positionZero);
}
}
else
{
// currentPart->dialog_message(GTK_MESSAGE_ERROR,"No calib file found", strcat("Define a suitable ", strcat(currentPart->partLabel, "Calib")), true);
dialog_message(GTK_MESSAGE_ERROR,(char *) "No zero group found in the supplied file. Define a suitable", buffer2, true);
}
return;
}
void partMover::home_all(GtkButton *button, partMover* currentPart)
{
IPositionControl *ipos = currentPart->pos;
IEncoders *iiencs = currentPart->iencs;
IAmplifierControl *iamp = currentPart->amp;
IPidControl *ipid = currentPart->pid;
IControlCalibration2 *ical = currentPart->cal;
int NUMBER_OF_JOINTS;
ipos->getAxes(&NUMBER_OF_JOINTS);
//fprintf(stderr, "Retrieving finder \n");
ResourceFinder *fnd = currentPart->finder;
//fprintf(stderr, "Retrieved finder: %p \n", fnd);
char buffer1[800];
char buffer2[800];
strcpy(buffer1, currentPart->partLabel);
strcpy(buffer2, strcat(buffer1, "_zero"));
//fprintf(stderr, "Finder retrieved %s\n", buffer2);
if (!fnd->findGroup(buffer2).isNull() && !fnd->isNull())
{
bool ok = true;
Bottle xtmp, ytmp;
xtmp = fnd->findGroup(buffer2).findGroup("PositionZero");
ok = ok && (xtmp.size() == NUMBER_OF_JOINTS+1);
ytmp = fnd->findGroup(buffer2).findGroup("VelocityZero");
ok = ok && (ytmp.size() == NUMBER_OF_JOINTS+1);
if(ok)
{
for (int joint = 0; joint < NUMBER_OF_JOINTS; joint++)
{
double positionZero = xtmp.get(joint+1).asDouble();
//fprintf(stderr, "%f ", positionZero);
double velocityZero = ytmp.get(joint+1).asDouble();
//fprintf(stderr, "%f ", velocityZero);
ipos->setRefSpeed(joint, velocityZero);
ipos->positionMove(joint, positionZero);
}
}
else
dialog_message(GTK_MESSAGE_ERROR,(char *) "Check the number of entries in the group", buffer2, true);
}
else
{
// currentPart->dialog_message(GTK_MESSAGE_ERROR,"No calib file found", strcat("Define a suitable ", strcat(currentPart->partLabel, "Calib")), true);
dialog_message(GTK_MESSAGE_ERROR,(char *) "No zero group found in the supplied file. Define a suitable", buffer2, true);
}
return;
}
/*
* Initialise the varialbes of the Planner
* input : ressource finder
*/
void abmReasoning::initialisePlanner(ResourceFinder & rf)
{
// TODO : use config file !!
Bottle &bPlanner = rf.findGroup("planner");
//configure PDDL planner variables
plannerPath = (bPlanner.check("plannerPath",
Value("C:/Robot/planner/LPG-td-1.0/"),
"planner path (string)")).asString();
plannerExec = (bPlanner.check("plannerExec1",
Value("lpg-td-1.0"),
"planner exec (string)")).asString();
plannerExec += " ";
plannerOptDom = (bPlanner.check("plannerOptDom",
Value("-o"),
"planner optDom (string)")).asString();
plannerOptDom += " ";
plannerOptProb = (bPlanner.check("plannerOptProb",
Value("-f"),
"planner optProb (string)")).asString();
plannerOptProb += " ";
plannerOptOut = (bPlanner.check("plannerOptOut",
Value("-out"),
"planner optOut (string)")).asString();
plannerOptOut += " ";
plannerOptNb = (bPlanner.check("plannerOptNb",
Value("-n"),
"planner optNb (int)")).asString();
plannerOptNb += " ";
plannerOptCpu = (bPlanner.check("plannerOptCpu",
Value("-cputime"),
"planner optCpu (int)")).asString();
plannerOptCpu += " ";
pddlDomain = (bPlanner.check("pddlDomain", Value("domainEFAA.pddl"))).asString(); //name of the domain pddl file
pddlProblem = (bPlanner.check("pddlProblem", Value("problemEFAA.pddl"))).asString(); //name of the problem pddl file
pddlOut = (bPlanner.check("pddlOut", Value("solutionEFAA"))).asString(); //name of the solution plan file -> will be called solutionEFAA_1.SOL, solutionEFAA_2.SOL, ...
pddlNb = (bPlanner.check("pddlNb", Value(30))).asInt(); //nb max of solution files produced
pddlCpu = (bPlanner.check("pddlCpu", Value(2))).asInt(); //nb max of solution files produced
//pddlDomain = "domainEFAA.pddl"; //name of the domain pddl file
//pddlProblem = "problemEFAA.pddl" ; //name of the problem pddl file
//pddlOut = "solutionEFAA" ; //name of the solution plan file -> will be called solutionEFAA_1.SOL
//there is no last Action => used to update vCurrentActions in findAllSharedPlan
saveEndLastAction = -1;
}
int main(int argc, char *argv[]) {
ResourceFinder robot;
robot.configure(argc,argv);
if (!robot.check("GENERAL")) {
printf("Cannot find needed configuration (try --from config.txt)\n");
return 1;
}
int joints = robot.findGroup("GENERAL").find("Joints").asInt32();
printf("Robot has %d joints\n", joints);
Bottle& maxes = robot.findGroup("LIMITS").findGroup("Max");
printf("Robot has limits: ");
for (int i=1; i<maxes.size(); i++) {
printf("%d ", maxes.get(i).asInt32());
}
printf("\n");
return 0;
}
//---------------------------------------------------------
void readVector(ResourceFinder &rf, string name, Vector &v, int len)
{
v.resize(len,0.0);
if(rf.check(name.c_str()))
{
Bottle &grp = rf.findGroup(name.c_str());
for (int i=0; i<len; i++)
v[i]=grp.get(1+i).asDouble();
}
else
{
cout<<"Could not find parameters for "<<name<<". "
<<"Setting everything to zero by default"<<endl;
}
displayNameVector(name,v);
}
bool configure(ResourceFinder &rf)
{
interrupting=false;
closing=false;
this->rf=&rf;
Bottle &bGeneral=rf.findGroup("general");
if (bGeneral.isNull())
{
yError("group [general] is missing!");
return false;
}
string name=bGeneral.check("name",Value("fingersTuner")).asString().c_str();
setName(name.c_str());
if (Bottle *bParts=bGeneral.find("relevantParts").asList())
{
for (int i=0; (i<bParts->size()) && !interrupting; i++)
{
string part=bParts->get(i).asString().c_str();
tuners[part]=new Tuner;
Tuner *tuner=tuners[part];
if (!tuner->configure(rf,part))
{
dispose();
return false;
}
tuner->sync(Value("*"));
}
}
else
{
yError("\"relevantParts\" option is missing!");
return false;
}
rpcPort.open(("/"+name+"/rpc").c_str());
attach(rpcPort);
// request high resolution scheduling
Time::turboBoost();
return true;
}
bool configure(ResourceFinder &rf)
{
string taxelPosFile="";
string filePath="";
int verbosity = rf.check("verbosity",Value(0)).asInt();
//************* skinTaxels' Resource finder **************
ResourceFinder skinRF;
skinRF.setVerbose(false);
skinRF.setDefaultContext("skinGui"); //overridden by --context parameter
skinRF.setDefaultConfigFile("skinManForearms.ini"); //overridden by --from parameter
skinRF.configure(0,NULL);
Bottle &skinEventsConf = skinRF.findGroup("SKIN_EVENTS");
if(!skinEventsConf.isNull())
{
if(skinEventsConf.check("skinParts"))
{
Bottle* skinPartList = skinEventsConf.find("skinParts").asList();
}
if(skinEventsConf.check("taxelPositionFiles"))
{
Bottle *taxelPosFiles = skinEventsConf.find("taxelPositionFiles").asList();
taxelPosFile = taxelPosFiles->get(1).asString().c_str();
filePath = skinRF.findFile(taxelPosFile.c_str());
}
}
else
{
yError(" No skin configuration files found.");
return 0;
}
// iCub::skinDynLib::skinPart sP;
// sP.setTaxelPosesFromFile(filePath);
// sP.print(1);
iCub::skinDynLib::iCubSkin iCS;
// iCub::skinDynLib::iCubSkin iCS("skinManForearms.ini","skinGui");
iCS.print(verbosity);
return true;
}
bool configure(ResourceFinder &rf)
{
moduleName = rf.find("moduleName").asString().c_str();
string inputPortName = rf.find("inputPortName").asString().c_str();
string outputPortName = rf.find("outputPortName").asString().c_str();
primitives = rf.findGroup("primitives");
grasp = new Grasper(("/"+moduleName+"/"+outputPortName));
if(!rpcPort.open(("/"+moduleName+"/"+inputPortName).c_str()))
{
cout << "Error: unable to open rpc port, closing module" << endl;
return false;
}
// set grasp speeds
Bottle *speeds =NULL;
if (!rf.check("speeds") || (rf.find("speeds").asList())->size()!= 9)
{
for (int i=0; i<9; i++)
speeds->addDouble(0.0);
}
else
speeds = rf.find("speeds").asList();
grasp->setGraspSpeeds(*speeds);
Bottle *offset=NULL;
// set tightening offset
if (!rf.check("offset") || (rf.find("offset").asList())->size()!= 9)
{
for (int i=0; i<9; i++)
offset->addDouble(0.0);
}
else
offset = rf.find("offset").asList();
grasp->setOffset(*offset);
graspSucceeded = false;
cout << "--> module successfully configured" << endl;
bool done = grasp->StartPorts();
if(done)
attach(rpcPort);
return done;
}
bool save()
{
string fileName=rf->getHomeContextPath().c_str();
fileName+="/";
fileName+=rf->find("from").asString().c_str();
ofstream fout;
fout.open(fileName.c_str());
Bottle &bGeneral=rf->findGroup("general");
fout<<"["<<bGeneral.get(0).asString().c_str()<<"]"<<endl;
for (int i=1; i<bGeneral.size(); i++)
fout<<bGeneral.get(i).toString().c_str()<<endl;
fout<<endl;
for (map<string,Tuner*>::iterator it=tuners.begin(); it!=tuners.end(); ++it)
fout<<it->second->toString()<<endl;
fout.close();
return true;
}
stereoCalibThread::stereoCalibThread(ResourceFinder &rf, Port* commPort, const char *imageDir)
{
moduleName=rf.check("name", Value("stereoCalib"),"module name (string)").asString().c_str();
robotName=rf.check("robotName",Value("icub"), "module name (string)").asString().c_str();
this->inputLeftPortName = "/"+moduleName;
this->inputLeftPortName +=rf.check("imgLeft",Value("/cam/left:i"),"Input image port (string)").asString().c_str();
this->inputRightPortName = "/"+moduleName;
this->inputRightPortName += rf.check("imgRight", Value("/cam/right:i"),"Input image port (string)").asString().c_str();
this->outNameRight = "/"+moduleName;
this->outNameRight += rf.check("outRight",Value("/cam/right:o"),"Output image port (string)").asString().c_str();
this->outNameLeft = "/"+moduleName;
this->outNameLeft +=rf.check("outLeft",Value("/cam/left:o"),"Output image port (string)").asString().c_str();
Bottle stereoCalibOpts=rf.findGroup("STEREO_CALIBRATION_CONFIGURATION");
this->boardWidth= stereoCalibOpts.check("boardWidth", Value(8)).asInt();
this->boardHeight= stereoCalibOpts.check("boardHeight", Value(6)).asInt();
this->numOfPairs= stereoCalibOpts.check("numberOfPairs", Value(30)).asInt();
this->squareSize= (float)stereoCalibOpts.check("boardSize", Value(0.09241)).asDouble();
this->boardType= stereoCalibOpts.check("boardType", Value("CHESSBOARD")).asString();
this->commandPort=commPort;
this->imageDir=imageDir;
this->startCalibration=0;
this->currentPathDir=rf.getHomeContextPath().c_str();
int tmp=stereoCalibOpts.check("MonoCalib", Value(0)).asInt();
this->stereo= tmp?false:true;
this->camCalibFile=rf.getHomeContextPath().c_str();
string fileName= "outputCalib.ini"; //rf.find("from").asString().c_str();
this->camCalibFile=this->camCalibFile+"/"+fileName.c_str();
this->mutex=new Semaphore(1);
}
void Reactable2OPC::loadObjectsDatabase(ResourceFinder& rf)
{
int objectsCount = rf.find("objectsCount").asInt();
for(int i=0;i<objectsCount;i++)
{
ostringstream oGrpName; oGrpName<<"object_"<<i;
Bottle cObject = rf.findGroup(oGrpName.str().c_str());
string oName = cObject.find("name").asString().c_str();
int id = cObject.find("id").asInt();
cout<<"Mapping ("<<id<<" , "<<oName<<")"<<endl;
idMap[id] = oName;
//Store the offset of the marker locally
Bottle* bOffset = cObject.find("marker-offset").asList();
idOffsets[id].resize(3);
idOffsets[id][0] = bOffset->get(0).asDouble();
idOffsets[id][1] = bOffset->get(1).asDouble();
idOffsets[id][2] = bOffset->get(2).asDouble();
//Push the object to OPC
RTObject* o = w->addOrRetrieveEntity<RTObject>(oName);
Bottle* bDim = cObject.find("dimensions").asList();
o->m_dimensions[0] = bDim->get(0).asDouble();
o->m_dimensions[1] = bDim->get(1).asDouble();
o->m_dimensions[2] = bDim->get(2).asDouble();
Bottle* bColor = cObject.find("color").asList();
o->m_color[0] = bColor->get(0).asDouble();
o->m_color[1] = bColor->get(1).asDouble();
o->m_color[2] = bColor->get(2).asDouble();
o->m_present = 0.0;
w->commit(o);
//cout<<o->toString()<<endl;
}
}
/* ******* Configure module ********************************************** */
bool RPCControllerModule::configure(ResourceFinder &rf){
using std::string;
using yarp::os::Network;
cout << dbgTag << "Starting. \n";
/* ****** Configure the Module ****** */
// Get resource finder and extract properties
moduleName = rf.check("name", Value("RPCController"), "The module name.").asString().c_str();
period = rf.check("period", 1.0).asDouble();
robotName = rf.check("robot", Value("icub"), "The robot name.").asString().c_str();
string portNameRoot = "/" + moduleName + "/";
/* ******* Ports. ******* */
rpcModule.open((portNameRoot + "cmd:io").c_str());
portExperimentStepsOut.open((portNameRoot + "experiment/status:o").c_str());
/* ******* Read RPC port parameters. ******* */
Bottle parGroup = rf.findGroup("rpc");
if (!parGroup.isNull()) {
if (parGroup.check("rpcServer")) {
string rpcServer = parGroup.find("rpcServer").asString().c_str();
// Connect to RPC server
if (!Network::connect(rpcModule.getName(), rpcServer)) {
cerr << dbgTag << "Could not connect to the specified RPC server: " << rpcServer << ". \n";
return false;
}
} else {
cerr << dbgTag << "Could not find the rpcServer name in the specified configuration file. \n";
return false;
}
} else {
cerr << dbgTag << "Could not find the [rpc] parameter group in the specified configuration file. \n";
return false;
}
/* ******* Read experiment parameters. ******* */
parGroup = rf.findGroup("experiment");
if (!parGroup.isNull()) {
// Number of columns in array
if (parGroup.check("nCols")) {
int nCols = parGroup.find("nCols").asInt();
// Experiment parameters
Bottle *confExpParams = parGroup.find("cmdT").asList();
if (confExpParams != NULL) {
for (int i = 0; i < confExpParams->size(); ++i) {
ExperimentParams tmpPar;
tmpPar.cmd = confExpParams->get(i).asString().c_str();
tmpPar.time = confExpParams->get(i + 1).asDouble();
expParams.push_back(tmpPar);
i++;
}
} else {
cerr << dbgTag << "Could not find the experiment parameter list in the supplied configuration file. \n";
return false;
}
} else {
cerr << dbgTag << "Could not find nCols parameter in supplied configuration file. \n";
return false;
}
} else {
cerr << dbgTag << "Could not find the experiment parameter group [experiment] in the supplied configuration file. \n";
return false;
}
#ifndef NODEBUG
cout << "DEBUG: " << dbgTag << "Experiment parameters are: \n";
for (size_t i = 0; i < expParams.size(); ++i) {
cout << "DEBUG " << dbgTag << expParams[i].cmd << " " << expParams[i].time << "\n";
i++;
}
#endif
cout << dbgTag << "Started correctly. \n";
return true;
}
bool configure(ResourceFinder &rf)
{
name = "visuoTactileRF";
robot = "icub";
modality = "1D";
verbosity = 0; // verbosity
rate = 20; // rate of the vtRFThread
//******************************************************
//********************** CONFIGS ***********************
//******************* NAME ******************
if (rf.check("name"))
{
name = rf.find("name").asString();
yInfo("Module name set to %s", name.c_str());
}
else yInfo("Module name set to default, i.e. %s", name.c_str());
setName(name.c_str());
//******************* ROBOT ******************
if (rf.check("robot"))
{
robot = rf.find("robot").asString();
yInfo("Robot is %s", robot.c_str());
}
else yInfo("Could not find robot option in the config file; using %s as default",robot.c_str());
//***************** MODALITY *****************
if (rf.check("modality"))
{
modality = rf.find("modality").asString();
yInfo("modality is %s", modality.c_str());
}
else yInfo("Could not find modality option in the config file; using %s as default",modality.c_str());
//******************* VERBOSE ******************
if (rf.check("verbosity"))
{
verbosity = rf.find("verbosity").asInt();
yInfo("vtRFThread verbosity set to %i", verbosity);
}
else yInfo("Could not find verbosity option in config file; using %i as default",verbosity);
//****************** rate ******************
if (rf.check("rate"))
{
rate = rf.find("rate").asInt();
yInfo("vtRFThread working at %i ms", rate);
}
else yInfo("Could not find rate in the config file; using %i ms as default",rate);
//************* skinTaxels' Resource finder **************
ResourceFinder skinRF;
skinRF.setVerbose(false);
skinRF.setDefaultContext("skinGui"); //overridden by --context parameter
skinRF.setDefaultConfigFile("skinManForearms.ini"); //overridden by --from parameter
skinRF.configure(0,NULL);
vector<string> filenames;
int partNum=4;
Bottle &skinEventsConf = skinRF.findGroup("SKIN_EVENTS");
if(!skinEventsConf.isNull())
{
yInfo("SKIN_EVENTS section found\n");
if(skinEventsConf.check("skinParts"))
{
Bottle* skinPartList = skinEventsConf.find("skinParts").asList();
partNum=skinPartList->size();
}
if(skinEventsConf.check("taxelPositionFiles"))
{
Bottle *taxelPosFiles = skinEventsConf.find("taxelPositionFiles").asList();
if (rf.check("leftHand") || rf.check("leftForeArm") || rf.check("rightHand") || rf.check("rightForeArm"))
{
if (rf.check("leftHand"))
{
string taxelPosFile = taxelPosFiles->get(0).asString().c_str();
string filePath(skinRF.findFile(taxelPosFile.c_str()));
if (filePath!="")
{
yInfo("[visuoTactileRF] filePath [%i] %s\n",0,filePath.c_str());
filenames.push_back(filePath);
}
}
if (rf.check("leftForeArm"))
{
string taxelPosFile = taxelPosFiles->get(1).asString().c_str();
string filePath(skinRF.findFile(taxelPosFile.c_str()));
if (filePath!="")
{
yInfo("[visuoTactileRF] filePath [%i] %s\n",1,filePath.c_str());
filenames.push_back(filePath);
}
}
if (rf.check("rightHand"))
{
string taxelPosFile = taxelPosFiles->get(2).asString().c_str();
string filePath(skinRF.findFile(taxelPosFile.c_str()));
if (filePath!="")
{
yInfo("[visuoTactileRF] filePath [%i] %s\n",2,filePath.c_str());
filenames.push_back(filePath);
}
}
if (rf.check("rightForeArm"))
{
string taxelPosFile = taxelPosFiles->get(3).asString().c_str();
string filePath(skinRF.findFile(taxelPosFile.c_str()));
if (filePath!="")
{
yInfo("[visuoTactileRF] filePath [%i] %s\n",3,filePath.c_str());
filenames.push_back(filePath);
}
}
}
else
{
for(int i=0;i<partNum;i++) // all of the skinparts
{
string taxelPosFile = taxelPosFiles->get(i).asString().c_str();
string filePath(skinRF.findFile(taxelPosFile.c_str()));
if (filePath!="")
{
yInfo("[visuoTactileRF] filePath [%i] %s\n",i,filePath.c_str());
filenames.push_back(filePath);
}
}
}
}
}
else
{
yError(" No skin's configuration files found.");
return 0;
}
//*************** eyes' Resource finder ****************
ResourceFinder gazeRF;
gazeRF.setVerbose(verbosity!=0);
gazeRF.setDefaultContext("iKinGazeCtrl");
robot=="icub"?gazeRF.setDefaultConfigFile("config.ini"):gazeRF.setDefaultConfigFile("configSim.ini");
gazeRF.configure(0,NULL);
double head_version=gazeRF.check("head_version",Value(1.0)).asDouble();
ResourceFinder eyeAlignRF;
Bottle &camerasGroup = gazeRF.findGroup("cameras");
if(!camerasGroup.isNull())
{
eyeAlignRF.setVerbose(verbosity!=0);
camerasGroup.check("context")?
eyeAlignRF.setDefaultContext(camerasGroup.find("context").asString().c_str()):
eyeAlignRF.setDefaultContext(gazeRF.getContext().c_str());
eyeAlignRF.setDefaultConfigFile(camerasGroup.find("file").asString().c_str());
eyeAlignRF.configure(0,NULL);
}
else
{
yWarning("Did not find camera calibration group into iKinGazeCtrl ResourceFinder!");
}
//******************************************************
//*********************** THREAD **********************
if( filenames.size() > 0 )
{
vtRFThrd = new vtRFThread(rate, name, robot, modality, verbosity, rf,
filenames, head_version, eyeAlignRF);
if (!vtRFThrd -> start())
{
delete vtRFThrd;
vtRFThrd = 0;
yError("vtRFThread wasn't instantiated!!");
return false;
}
yInfo("VISUO TACTILE RECEPTIVE FIELDS: vtRFThread instantiated...");
}
else {
vtRFThrd = 0;
yError("vtRFThread wasn't instantiated. No filenames have been given!");
return false;
}
//******************************************************
//************************ PORTS ***********************
rpcClnt.open(("/"+name+"/rpc:o").c_str());
rpcSrvr.open(("/"+name+"/rpc:i").c_str());
attach(rpcSrvr);
return true;
}
bool learnPrimitive::updateAction(ResourceFinder &rf){
Bottle bOutput;
//1. Primitive
Bottle bAction = rf.findGroup("Action");
if (!bAction.isNull())
{
Bottle * bActionName = bAction.find("ActionName").asList();
Bottle * bActionArg = bAction.find("ActionArg").asList();
if(bActionName->isNull() || bActionArg->isNull()){
yError() << " [updateAction] : one of the primitiveAction conf is null : ActionName, ActionArg" ;
return false ;
}
int actionSize = -1 ;
if(bActionName->size() == bActionArg->size()){
actionSize = bActionName->size() ;
} else {
yError() << " [updateAction] : one of the Action conf has different size!" ;
return false ;
}
if(actionSize == 0) {
yWarning() << " [updateAction] : there is no Action defined at startup!" ;
} else {
for(int i = 0; i < actionSize ; i++) {
//insert protoaction even if already there
Bottle bSubAction;
string currentActionName = bActionName->get(i).asString();
string currentActionArg = bActionArg->get(i).asString();
bSubAction.addString(currentActionName);
bSubAction.addString(currentActionArg);
string concat = currentActionName + "_" + currentActionArg;
// yInfo() << " Looking for " << concat ;
Bottle bCurrent = rf.findGroup(concat);
if(bCurrent.isNull()){
yError() << " [updateAction] : " << concat << "is NOT defined" ;
return false ;
}
Bottle * bListSubAction = bCurrent.find("actionList").asList();
//yInfo() << "Subaction found : " << bListSubAction->toString() ;
if(bListSubAction->isNull()){
yError() << " [updateAction] : " << concat << "is there but is not defined (actionList)" ;
return false ;
}
bSubAction.addList() = *bListSubAction ;
yInfo() << "Complex Action added : (" <<bSubAction.get(0).asString() << ", " << bSubAction.get(1).asString() << ", ( " << bSubAction.get(2).asList()->toString()<< "))" ;
vActionBottle.push_back(bSubAction);
}
}
} else {
yError() << " error in learnPrimitive::updatePrimitive | Primitive_Action is NOT defined in the learnPrimitive.ini";
return false;
}
return true;
}
bool TwoCanBusThreeWrappers::configure(ResourceFinder &rf) {
if(rf.check("help")) {
printf("TwoCanBusThreeWrappers options:\n");
printf("\t--help (this help)\t--from [file.ini]\t--context [path]\n");
CD_DEBUG_NO_HEADER("%s\n",rf.toString().c_str());
return false;
}
//-- /dev/can0 --
Bottle devCan0 = rf.findGroup("devCan0");
CD_DEBUG("%s\n",devCan0.toString().c_str());
Property optionsDevCan0;
optionsDevCan0.fromString(devCan0.toString());
deviceDevCan0.open(optionsDevCan0);
if (!deviceDevCan0.isValid()) {
CD_ERROR("deviceDevCan0 instantiation not worked.\n");
return false;
}
//-- /dev/can1 --
Bottle devCan1 = rf.findGroup("devCan1");
CD_DEBUG("%s\n",devCan1.toString().c_str());
Property optionsDevCan1;
optionsDevCan1.fromString(devCan1.toString());
deviceDevCan1.open(optionsDevCan1);
if (!deviceDevCan1.isValid()) {
CD_ERROR("deviceDevCan1 instantiation not worked.\n");
return false;
}
//-- wrapper0 --
Bottle wrapper0 = rf.findGroup("wrapper0");
CD_DEBUG("%s\n",wrapper0.toString().c_str());
Property optionsWrapper0;
optionsWrapper0.fromString(wrapper0.toString());
deviceWrapper0.open(optionsWrapper0);
if (!deviceWrapper0.isValid()) {
CD_ERROR("deviceWrapper0 instantiation not worked.\n");
return false;
}
//-- wrapper1 --
Bottle wrapper1 = rf.findGroup("wrapper1");
CD_DEBUG("%s\n",wrapper1.toString().c_str());
Property optionsWrapper1;
optionsWrapper1.fromString(wrapper1.toString());
deviceWrapper1.open(optionsWrapper1);
if (!deviceWrapper1.isValid()) {
CD_ERROR("deviceWrapper1 instantiation not worked.\n");
return false;
}
//-- wrapper2 --
Bottle wrapper2 = rf.findGroup("wrapper2");
CD_DEBUG("%s\n",wrapper2.toString().c_str());
Property optionsWrapper2;
optionsWrapper2.fromString(wrapper2.toString());
deviceWrapper2.open(optionsWrapper2);
if (!deviceWrapper2.isValid()) {
CD_ERROR("deviceWrapper2 instantiation not worked.\n");
return false;
}
IMultipleWrapper *iWrapper0, *iWrapper1, *iWrapper2;
deviceWrapper0.view(iWrapper0);
deviceWrapper1.view(iWrapper1);
deviceWrapper2.view(iWrapper2);
PolyDriverList list;
list.push(&deviceDevCan0, "devCan0");
list.push(&deviceDevCan1, "devCan1");
iWrapper0->attachAll(list);
iWrapper1->attachAll(list);
iWrapper2->attachAll(list);
return true;
}
bool configure(ResourceFinder &rf)
{
name = "virtualContactGeneration";
robot = "icubSim";
threadPeriod = 100; //period of the virtContactGenThread in ms
verbosity = 0;
type = "random"; //selection of the
SkinPart partOfSkin;
//******************************************************
//********************** CONFIGS ***********************
//******************* GENERAL GROUP ******************
Bottle &bGeneral=rf.findGroup("general");
bGeneral.setMonitor(rf.getMonitor());
//******************* NAME ******************
if (bGeneral.check("name"))
{
name = bGeneral.find("name").asString();
yInfo("Module name set to %s", name.c_str());
}
else yInfo("Module name set to default, i.e. %s", name.c_str());
setName(name.c_str());
//******************* ROBOT ******************
if (bGeneral.check("robot"))
{
robot = bGeneral.find("robot").asString();
yInfo("Robot is: %s", robot.c_str());
}
else yInfo("Could not find robot option in the config file; using %s as default",robot.c_str());
//****************** rate ******************
if (bGeneral.check("rate"))
{
threadPeriod = bGeneral.find("rate").asInt();
yInfo("virtContactGenThread rateThread working at %i ms.",threadPeriod);
}
else yInfo("Could not find rate in the config file; using %i ms as default period",threadPeriod);
//******************* VERBOSE ******************
if (bGeneral.check("verbosity"))
{
verbosity = bGeneral.find("verbosity").asInt();
yInfo("verbosity set to %i", verbosity);
}
else yInfo("Could not find verbosity option in the config file; using %i as default",verbosity);
//******************* TYPE ******************
if (bGeneral.check("type"))
{
type = bGeneral.find("type").asString();
yInfo("Type is: %s", type.c_str());
}
else yInfo("Could not find type option in the config file; using %s as default",type.c_str());
//*************** ACTIVE SKIN PARTS GROUP ****************
Bottle &bSkinParts=rf.findGroup("skin_parts");
if (!bSkinParts.isNull())
{
bSkinParts.setMonitor(rf.getMonitor());
if (bSkinParts.check("SKIN_LEFT_HAND"))
{
if(bSkinParts.find("SKIN_LEFT_HAND").asString()=="on")
{
partOfSkin = SKIN_LEFT_HAND;
activeSkinPartsNamesVector.push_back(partOfSkin);
yInfo("Adding SKIN_LEFT_HAND to active skin parts.");
}
}
else yInfo("Could not find [skin_parts] SKIN_LEFT_HAND option in the config file; set to default, i.e. off");
if (bSkinParts.check("SKIN_LEFT_FOREARM"))
{
if(bSkinParts.find("SKIN_LEFT_FOREARM").asString()=="on")
{
partOfSkin = SKIN_LEFT_FOREARM;
activeSkinPartsNamesVector.push_back(partOfSkin);
yInfo("Adding SKIN_LEFT_FOREARM to active skin parts.");
}
}
else yInfo("Could not find [skin_parts] SKIN_LEFT_FOREARM option in the config file; set to default, i.e. off");
if (bSkinParts.check("SKIN_LEFT_UPPER_ARM"))
{
if(bSkinParts.find("SKIN_LEFT_UPPER_ARM").asString()=="on")
{
partOfSkin = SKIN_LEFT_UPPER_ARM;
activeSkinPartsNamesVector.push_back(partOfSkin);
yInfo("Adding SKIN_LEFT_UPPER_ARM to active skin parts.");
}
}
else yInfo("Could not find [skin_parts] SKIN_LEFT_UPPER_ARM option in the config file; set to default, i.e. off");
if (bSkinParts.check("SKIN_RIGHT_HAND"))
{
if(bSkinParts.find("SKIN_RIGHT_HAND").asString()=="on")
{
partOfSkin = SKIN_RIGHT_HAND;
activeSkinPartsNamesVector.push_back(partOfSkin);
yInfo("Adding SKIN_RIGHT_HAND to active skin parts.");
}
}
else yInfo("Could not find [skin_parts] SKIN_RIGHT_HAND option in the config file; set to default, i.e. off");
if (bSkinParts.check("SKIN_RIGHT_FOREARM"))
{
if(bSkinParts.find("SKIN_RIGHT_FOREARM").asString()=="on")
{
partOfSkin = SKIN_RIGHT_FOREARM;
activeSkinPartsNamesVector.push_back(partOfSkin);
yInfo("Adding SKIN_RIGHT_FOREARM to active skin parts.");
}
}
else yInfo("Could not find [skin_parts] SKIN_RIGHT_FOREARM option in the config file; set to default, i.e. off");
if (bSkinParts.check("SKIN_RIGHT_UPPER_ARM"))
{
if(bSkinParts.find("SKIN_RIGHT_UPPER_ARM").asString()=="on")
{
partOfSkin = SKIN_RIGHT_UPPER_ARM;
activeSkinPartsNamesVector.push_back(partOfSkin);
yInfo("Adding SKIN_RIGHT_UPPER_ARM to active skin parts.");
}
}
else yInfo("Could not find [skin_parts] SKIN_RIGHT_UPPER_ARM option in the config file; set to default, i.e. off");
}
else{ //bSkinParts.isNull()
yInfo("Could not find [skin_parts] group in the config file; set all to default, i.e. off");
}
//*************** SKIN PARTS FILENAMES FROM SKIN MANAGER INI FILES ****************
ResourceFinder skinRF;
int partNum;
skinRF.setVerbose(false);
skinRF.setDefaultContext("skinGui"); //overridden by --context parameter
skinRF.setDefaultConfigFile("skinManAll.ini"); //overridden by --from parameter
skinRF.setVerbose(true);
skinRF.configure(0,NULL);
Bottle &skinEventsConf = skinRF.findGroup("SKIN_EVENTS");
if(!skinEventsConf.isNull())
{
yInfo("SKIN_EVENTS section found\n");
if(skinEventsConf.check("skinParts"))
{
Bottle* skinPartList = skinEventsConf.find("skinParts").asList();
partNum=skinPartList->size();
}
if(skinEventsConf.check("taxelPositionFiles"))
{
Bottle *taxelPosFiles = skinEventsConf.find("taxelPositionFiles").asList();
for(int i=0;i<partNum;i++) // all of the skinparts
{
string taxelPosFile = taxelPosFiles->get(i).asString().c_str();
string filePath(skinRF.findFile(taxelPosFile.c_str()));
if (filePath!="")
{
if(verbosity>0) yInfo("[skin_event] filePath [%i in bottle] %s; setting under %s in skinPartsPositionsFilePath.\n",i,filePath.c_str(),SkinPart_s[i+1].c_str());
skinPartsPositionsFilePaths[(SkinPart)(i+1)] = filePath; //! Importantly, this is relying on the fact that the skin parts are in the
//right order in the .ini file, matching with SkinPart enum, and starting with 1 for left hand
}
}
}
else
{
yError(" No skin's configuration files found.");
return 0;
}
}
//******************************************************
//*********************** THREAD **********************
virtContactGenThrd = new virtContactGenerationThread(threadPeriod,name,robot,verbosity,type,activeSkinPartsNamesVector,skinPartsPositionsFilePaths);
if (!virtContactGenThrd -> start())
{
delete virtContactGenThrd;
virtContactGenThrd = 0;
yError("virtContactGenThrd wasn't instantiated!!");
return false;
}
yInfo("virtContactGenThrd instantiated...");
if (rf.check("autoconnect"))
{
if (Network::connect(("/"+name+"/virtualContacts:o").c_str(),"/doubleTouch/contacts:i"))
{
yInfo("Autoconnection to doubleTouch port, i.e. /doubleTouch/contacts:i, successful!");
}
}
return true;
}
bool configure(ResourceFinder &rf, const string &part)
{
this->part=part;
Bottle &bGeneral=rf.findGroup("general");
if (bGeneral.isNull())
{
yError("group [general] is missing!");
return false;
}
Bottle &bPart=rf.findGroup(part.c_str());
if (bPart.isNull())
{
yError("group [%s] is missing!",part.c_str());
return false;
}
if (!bPart.check("device"))
{
yError("\"device\" option is missing!");
return false;
}
name=bGeneral.check("name",Value("fingersTuner")).asString().c_str();
robot=bGeneral.check("robot",Value("icub")).asString().c_str();
double ping_robot_tmo=bGeneral.check("ping_robot_tmo",Value(0.0)).asDouble();
device=bPart.find("device").asString().c_str();
if (Bottle *rj=bGeneral.find("relevantJoints").asList())
rJoints=*rj;
else
{
yError("\"relevantJoints\" option is missing!");
return false;
}
int numAlias=bGeneral.check("numAlias",Value(0)).asInt();
for (int i=0; i<numAlias; i++)
{
ostringstream item;
item<<"alias_"<<i;
Bottle &bAlias=bGeneral.findGroup(item.str().c_str());
if (Bottle *joints=bAlias.find("joints").asList())
alias[bAlias.find("tag").asString().c_str()]=*joints;
}
// special wildcard to point to all the joints
alias["*"]=rJoints;
for (int i=0; i<rJoints.size(); i++)
{
int j=rJoints.get(i).asInt();
pids[j]=getPidData(bPart,j);
}
ostringstream portsSuffix;
portsSuffix<<instances<<"/"<<device;
Property option("(device remote_controlboard)");
option.put("remote",("/"+robot+"/"+device).c_str());
option.put("local",("/"+name+"/"+portsSuffix.str()).c_str());
if (ping_robot_tmo>0.0)
driver=waitPart(option,ping_robot_tmo);
else
driver=new PolyDriver(option);
if (!driver->isValid())
{
yError("%s device driver not available!",device.c_str());
return false;
}
return true;
}
/*
* Enable all PID and refresh position sliders
*/
void partMover::calib_all(GtkButton *button, partMover* currentPart)
{
//ask for confirmation
if (!dialog_question("Do you really want to recalibrate the whole part?"))
{
return;
}
IPositionControl *ipos = currentPart->pos;
IControlCalibration2 *ical = currentPart->cal;
int joint;
int NUMBER_OF_JOINTS;
ipos->getAxes(&NUMBER_OF_JOINTS);
ResourceFinder *fnd = currentPart->finder;
//fprintf(stderr, "opening file \n");
char buffer1[800];
char buffer2[800];
strcpy(buffer1, currentPart->partLabel);
strcpy(buffer2, strcat(buffer1, "_calib"));
if (!fnd->findGroup(buffer2).isNull())
{
bool ok = true;
Bottle p1 = fnd->findGroup(buffer2).findGroup("Calibration1");
ok = ok && (p1.size() == NUMBER_OF_JOINTS+1);
Bottle p2 = fnd->findGroup(buffer2).findGroup("Calibration2");
ok = ok && (p2.size() == NUMBER_OF_JOINTS+1);
Bottle p3 = fnd->findGroup(buffer2).findGroup("Calibration3");
ok = ok && (p3.size() == NUMBER_OF_JOINTS+1);
Bottle ty = fnd->findGroup(buffer2).findGroup("CalibrationType");
ok = ok && (ty.size() == NUMBER_OF_JOINTS+1);
if (ok)
{
for (joint=0; joint < NUMBER_OF_JOINTS; joint++)
{
double param1 = p1.get(joint+1).asDouble();
//fprintf(stderr, "%f ", param1);
double param2 = p2.get(joint+1).asDouble();
//fprintf(stderr, "%f ", param2);
double param3 = p3.get(joint+1).asDouble();
//fprintf(stderr, "%f ", param3);
unsigned char type = (unsigned char) ty.get(joint+1).asDouble();
//fprintf(stderr, "%d ", type);
ical->calibrate2(joint, type, param1, param2, param3);
}
}
else
dialog_message(GTK_MESSAGE_ERROR,(char *) "Check number of entries in the file", buffer1, true);
}
else
{
dialog_message(GTK_MESSAGE_ERROR,(char *) "No calib file found. Define a suitable file called:", buffer1, true);
}
return;
}
bool configure(ResourceFinder &rf)
{
name = "skinEventsAggregator";
robot = "icubSim";
threadPeriod = 20; //period of the virtContactGenThread in ms
verbosity = 0;
//******************************************************
//********************** CONFIGS ***********************
//******************* GENERAL GROUP ******************
Bottle &bGeneral=rf.findGroup("general");
bGeneral.setMonitor(rf.getMonitor());
//******************* NAME ******************
if (bGeneral.check("name"))
{
name = bGeneral.find("name").asString();
yInfo("Module name set to %s", name.c_str());
}
else yInfo("Module name set to default, i.e. %s", name.c_str());
setName(name.c_str());
//******************* ROBOT ******************
if (bGeneral.check("robot"))
{
robot = bGeneral.find("robot").asString();
yInfo("Robot is: %s", robot.c_str());
}
else yInfo("Could not find robot option in the config file; using %s as default",robot.c_str());
//****************** rate ******************
if (bGeneral.check("rate"))
{
threadPeriod = bGeneral.find("rate").asInt();
yInfo("skinEventsAggregThread rateThread working at %i ms.",threadPeriod);
}
else yInfo("Could not find rate in the config file; using %i ms as default period",threadPeriod);
//******************* VERBOSE ******************
if (bGeneral.check("verbosity"))
{
verbosity = bGeneral.find("verbosity").asInt();
yInfo("verbosity set to %i", verbosity);
}
else yInfo("Could not find verbosity option in the config file; using %i as default",verbosity);
//******************************************************
//*********************** THREAD **********************
skinEvAggThrd = new skinEventsAggregThread(threadPeriod,name,robot,verbosity);
if (!skinEvAggThrd -> start())
{
delete skinEvAggThrd;
skinEvAggThrd = 0;
yError("skinEventsAggregThread wasn't instantiated!!");
return false;
}
yInfo("skinEventsAggregThread instantiated...");
return true;
}
bool wholeBodyDynamicsModule::configure(ResourceFinder &rf)
{
if( rf.check("robot") ) {
robotName = rf.find("robot").asString();
} else {
std::cerr << "wholeBodyDynamicsModule::configure failed: robot parameter not found. Closing module." << std::endl;
return false;
}
if( rf.check("name") ) {
moduleName = rf.find("name").asString();
setName(moduleName.c_str());
} else {
std::cerr << "wholeBodyDynamicsModule::configure failed: name parameter not found. Closing module." << std::endl;
return false;
}
//Loading thread period
if( rf.check("period") && rf.find("period").isInt() )
{
period = rf.find("period").asInt();
}
// If period is not specified, check also the legacy "rate" option
if( !rf.check("period") && rf.check("rate") && rf.find("rate").isInt() )
{
period = rf.find("rate").asInt();
}
bool fixed_base = false;
bool fixed_base_calibration = false;
std::string fixed_link;
std::string fixed_link_calibration;
if( rf.check("assume_fixed") )
{
fixed_link = rf.find("assume_fixed").asString().c_str();
if( fixed_link != "root_link" &&
fixed_link != "l_sole" &&
fixed_link != "r_sole" &&
fixed_link != "r_foot_dh_frame" &&
fixed_link != "l_foot_dh_frame" )
{
yError() << "assume_fixed option found, but disabled because " << fixed_link << " is not a recognized fixed_link ";
return false;
} else {
yInfo() << "assume_fixed option found, using " << fixed_link << " as fixed link as a kinematic root instead of the imu.";
fixed_base = true;
fixed_base_calibration = true;
fixed_link_calibration = fixed_link;
// \todo TODO workaround for heidelberg
if( fixed_link == "l_sole" )
{
fixed_link = fixed_link_calibration = "l_foot_dh_frame";
}
if( fixed_link == "r_sole" )
{
fixed_link = fixed_link_calibration = "r_foot_dh_frame";
}
}
}
else
{
}
//--------------------------RPC PORT--------------------------------------------
attach(rpcPort);
std::string rpcPortName= "/";
rpcPortName+= getName();
rpcPortName += "/rpc:i";
if (!rpcPort.open(rpcPortName.c_str())) {
std::cerr << getName() << ": Unable to open port " << rpcPortName << std::endl;
return false;
}
//--------------------------WHOLE BODY STATES INTERFACE--------------------------
yarp::os::Property yarpWbiOptions;
//Get wbi options from the canonical file
if( !rf.check("wbi_conf_file") )
{
fprintf(stderr,"[ERR] wholeBodyDynamicsThread: impossible to open wholeBodyInterface: wbi_conf_file option missing");
return false;
}
std::string wbiConfFile = rf.findFile("wbi_conf_file");
yarpWbiOptions.fromConfigFile(wbiConfFile);
//Overwrite the parameters in the wbi_conf_file with stuff from the wbd options file
yarpWbiOptions.fromString(rf.toString(),false);
//List of joints used in the dynamic model of the robot
IDList RobotDynamicModelJoints;
std::string RobotDynamicModelJointsListName = rf.check("torque_estimation_joint_list",
yarp::os::Value("ROBOT_DYNAMIC_MODEL_JOINTS"),
"Name of the list of joint used for torque estimation").asString().c_str();
if( !loadIdListFromConfig(RobotDynamicModelJointsListName,rf,RobotDynamicModelJoints) )
{
if( !loadIdListFromConfig(RobotDynamicModelJointsListName,yarpWbiOptions,RobotDynamicModelJoints) )
{
fprintf(stderr, "[ERR] wholeBodyDynamicsModule: impossible to load wbiId joint list with name %s\n",RobotDynamicModelJointsListName.c_str());
return false;
}
}
//Add to the options some wbd specific stuff
if( fixed_base )
{
yarpWbiOptions.put("fixed_base",fixed_link);
}
if( rf.check("assume_fixed_from_odometry") )
{
yarpWbiOptions.put("assume_fixed_from_odometry","dummy");
}
if( rf.check("IDYNTREE_SKINDYNLIB_LINKS") )
{
yarp::os::Property & prop = yarpWbiOptions.addGroup("IDYNTREE_SKINDYNLIB_LINKS");
prop.fromString(rf.findGroup("IDYNTREE_SKINDYNLIB_LINKS").tail().toString());
}
else
{
fprintf(stderr, "[ERR] wholeBodyDynamicsModule: impossible to load IDYNTREE_SKINDYNLIB_LINKS group, exiting");
}
if( rf.check("WBD_SUBTREES") )
{
yarp::os::Property & prop = yarpWbiOptions.addGroup("WBD_SUBTREES");
prop.fromString(rf.findGroup("WBD_SUBTREES").tail().toString());
}
else
{
fprintf(stderr, "[ERR] wholeBodyDynamicsModule: impossible to load WBD_SUBTREES group, exiting");
}
if( rf.check("WBD_OUTPUT_TORQUE_PORTS") )
{
yarp::os::Property & prop = yarpWbiOptions.addGroup("WBD_OUTPUT_TORQUE_PORTS");
prop.fromString(rf.findGroup("WBD_OUTPUT_TORQUE_PORTS").tail().toString());
}
else
{
fprintf(stderr, "[ERR] wholeBodyDynamicsModule: impossible to load WBD_OUTPUT_TORQUE_PORTS group, exiting");
}
if( rf.check("WBD_OUTPUT_EXTERNAL_WRENCH_PORTS") )
{
yarp::os::Property & prop = yarpWbiOptions.addGroup("WBD_OUTPUT_EXTERNAL_WRENCH_PORTS");
prop.fromString(rf.findGroup("WBD_OUTPUT_EXTERNAL_WRENCH_PORTS").tail().toString());
}
if( rf.check("calibration_support_link") )
{
yarpWbiOptions.put("calibration_support_link",rf.find("calibration_support_link").asString());
}
else
{
yarpWbiOptions.put("calibration_support_link","root_link");
}
sensors = new yarpWholeBodySensors(moduleName.c_str(), yarpWbiOptions);
sensors->addSensors(wbi::SENSOR_ENCODER,RobotDynamicModelJoints);
//estimationInterface->addEstimates(wbi::ESTIMATE_JOINT_VEL,RobotDynamicModelJoints);
//estimationInterface->addEstimates(wbi::ESTIMATE_JOINT_ACC,RobotDynamicModelJoints);
//List of 6-axis Force-Torque sensors in the robot
IDList RobotFTSensors;
std::string RobotFTSensorsListName = "ROBOT_MAIN_FTS";
if( !loadIdListFromConfig(RobotFTSensorsListName,yarpWbiOptions,RobotFTSensors) )
{
yError("wholeBodyDynamicsTree: impossible to load wbiId list with name %s\n",RobotFTSensorsListName.c_str());
}
sensors->addSensors(wbi::SENSOR_FORCE_TORQUE,RobotFTSensors);
//List of IMUs sensors in the robot
IDList RobotIMUSensors;
std::string RobotIMUSensorsListName = "ROBOT_MAIN_IMUS";
if( !loadIdListFromConfig(RobotIMUSensorsListName,yarpWbiOptions,RobotIMUSensors) )
{
yError("wholeBodyDynamicsTree: impossible to load wbiId list with name %s\n",RobotFTSensorsListName.c_str());
}
sensors->addSensors(wbi::SENSOR_IMU,RobotIMUSensors);
if(!sensors->init())
{
yError() << getName() << ": Error while initializing whole body estimator interface.Closing module";
return false;
}
//--------------------------WHOLE BODY DYNAMICS THREAD--------------------------
wbdThread = new wholeBodyDynamicsThread(moduleName,
robotName,
period,
sensors,
yarpWbiOptions,
fixed_base_calibration,
fixed_link_calibration,
rf.check("assume_fixed_from_odometry"));
if(!wbdThread->start())
{
yError() << getName()
<< ": Error while initializing whole body estimator thread."
<< "Closing module";
return false;
}
yInfo() << "wholeBodyDynamicsThread started";
return true;
}
void partMover::calib_click(GtkButton *button, gtkClassData* currentClassData)
{
//ask for confirmation
if (!dialog_question("Do you really want to recalibrate the joint?"))
{
return;
}
partMover *currentPart = currentClassData->partPointer;
int * joint = currentClassData->indexPointer;
IPositionControl *ipos = currentPart->pos;
IEncoders *iiencs = currentPart->iencs;
IAmplifierControl *iamp = currentPart->amp;
IPidControl *ipid = currentPart->pid;
IControlCalibration2 *ical = currentPart->cal;
int NUMBER_OF_JOINTS;
ipos->getAxes(&NUMBER_OF_JOINTS);
ResourceFinder *fnd = currentPart->finder;
//fprintf(stderr, "opening file \n");
char buffer1[800];
char buffer2[800];
strcpy(buffer1, currentPart->partLabel);
strcpy(buffer2, strcat(buffer1, "_calib"));
if (!fnd->findGroup(buffer2).isNull())
{
bool ok = true;
Bottle xtmp;
xtmp.clear();
xtmp = fnd->findGroup(buffer2).findGroup("CalibrationType");
ok = ok && (xtmp.size() == NUMBER_OF_JOINTS+1);
unsigned char type = (unsigned char) xtmp.get(*joint+1).asDouble();
fprintf(stderr, "%d ", type);
xtmp.clear();
xtmp = fnd->findGroup(buffer2).findGroup("Calibration1");
ok = ok && (xtmp.size() == NUMBER_OF_JOINTS+1);
double param1 = xtmp.get(*joint+1).asDouble();
fprintf(stderr, "%f ", param1);
xtmp.clear();
xtmp = fnd->findGroup(buffer2).findGroup("Calibration2");
ok = ok && (xtmp.size() == NUMBER_OF_JOINTS+1);
double param2 = xtmp.get(*joint+1).asDouble();
fprintf(stderr, "%f ", param2);
xtmp.clear();
xtmp = fnd->findGroup(buffer2).findGroup("Calibration3");
ok = ok && (xtmp.size() == NUMBER_OF_JOINTS+1);
double param3 = xtmp.get(*joint+1).asDouble();
fprintf(stderr, "%f \n", param3);
if(!ok)
dialog_message(GTK_MESSAGE_ERROR,(char *)"Check number of calib entries in the group", buffer2, true);
else
ical->calibrate2(*joint, type, param1, param2, param3);
}
else
dialog_message(GTK_MESSAGE_ERROR,(char *)"The supplied file does not conatain a group named:", buffer2, true);
return;
}
bool configure(ResourceFinder &rf)
{
// get configuration flags
if (rf.check("simulation"))
simulationMode=(rf.find("simulation").asString() == "true");
if (rf.check("velocityControlMode"))
velocityControlMode=(rf.find("velocityControlMode").asString()=="true");
if (rf.check("triangulationFromModel"))
useLearnedTriangulationModel=(rf.find("triangulationFromModel").asString() == "true");
if (rf.check("swappedHeadMode"))
useSwappedHead=(rf.find("swappedHeadMode").asString() == "true");
cout << "Configuring resources with simulation="<<simulationMode<<",velMode="<<velocityControlMode<<",triangulationFromModel="<<useLearnedTriangulationModel<<endl;
// parse configuration resources to run
Bottle &config_group=rf.findGroup("configuration_resources");
if(config_group.check("right_arm")) {
runArmConfig[0] = (bool)(config_group.find("right_arm").asInt());
} else {
runArmConfig[0] = false;
}
//cout << "run right arm config: " << runArmConfig[0] << endl;
if(config_group.check("left_arm")) {
runArmConfig[1] = (bool)(config_group.find("left_arm").asInt());;
} else {
runArmConfig[1] = false;
}
//cout << "run left arm config: " << runArmConfig[1] << endl;
if(config_group.check("right_full_arm")) {
runFullArmConfig[0] = (bool)(config_group.find("right_full_arm").asInt());
} else {
runFullArmConfig[0] = false;
}
//cout << "run right full arm config: " << runFullArmConfig[0] << endl;
if(config_group.check("left_full_arm")) {
runFullArmConfig[1] = (bool)(config_group.find("left_full_arm").asInt());;
} else {
runFullArmConfig[1] = false;
}
//cout << "run left full arm config: " << runFullArmConfig[1] << endl;
if(config_group.check("right_leg")) {
runLegConfig[0] = (bool)(config_group.find("right_leg").asInt());
} else {
runLegConfig[0] = false;
}
//cout << "run right leg config: " << runLegConfig[0] << endl;
if(config_group.check("left_leg")) {
runLegConfig[1] = (bool)(config_group.find("left_leg").asInt());;
} else {
runLegConfig[1] = false;
}
//cout << "run left leg config: " << runLegConfig[1] << endl;
if(config_group.check("right_hand")) {
runHandConfig[0] = (bool)(config_group.find("right_hand").asInt());
} else {
runHandConfig[0] = false;
}
//cout << "run right hand config: " << runHandConfig[0] << endl;
if(config_group.check("left_hand")) {
runHandConfig[1] = (bool)(config_group.find("left_hand").asInt());;
} else {
runHandConfig[1] = false;
}
//cout << "run left hand config: " << runHandConfig[1] << endl;
if(config_group.check("head")) {
runHeadConfig = (bool)(config_group.find("head").asInt());
} else {
runHeadConfig = false;
}
//cout << "run head config: " << runHeadConfig << endl;
if(config_group.check("torso")) {
runTorsoConfig = (bool)(config_group.find("torso").asInt());;
} else {
runTorsoConfig = false;
}
//cout << "run torso config: " << runTorsoConfig << endl;
if(config_group.check("eyes-pt")) {
runEyesConfig[0] = (bool)(config_group.find("eyes-pt").asInt());;
} else {
runEyesConfig[0] = false;
}
//cout << "run eyes-pt config: " << runEyesConfig[0] << endl;
if(config_group.check("eyes-ptv")) {
runEyesConfig[1] = (bool)(config_group.find("eyes-ptv").asInt());;
} else {
runEyesConfig[1] = false;
}
//cout << "run eyes-ptv config: " << runEyesConfig[1] << endl;
// parse cartesian position resources
Bottle &cartpos_group=rf.findGroup("cartesianposition_resources");
if(cartpos_group.check("right_arm")) {
runArmPosition[0] = (bool)(cartpos_group.find("right_arm").asInt());
} else {
runArmPosition[0] = false;
}
//cout << "run right arm position: " << runArmPosition[0] << endl;
if(cartpos_group.check("left_arm")) {
runArmPosition[1] = (bool)(cartpos_group.find("left_arm").asInt());;
} else {
runArmPosition[1] = false;
}
//cout << "run left arm position: " << runArmPosition[1] << endl;
if(cartpos_group.check("right_full_arm")) {
runFullArmPosition[0] = (bool)(cartpos_group.find("right_full_arm").asInt());
} else {
runFullArmPosition[0] = false;
}
//cout << "run right full arm position: " << runFullArmPosition[0] << endl;
if(cartpos_group.check("left_full_arm")) {
runFullArmPosition[1] = (bool)(cartpos_group.find("left_full_arm").asInt());;
} else {
runFullArmPosition[1] = false;
}
//cout << "run left full arm position: " << runFullArmPosition[1] << endl;
if(cartpos_group.check("right_leg")) {
runLegPosition[0] = (bool)(cartpos_group.find("right_leg").asInt());
} else {
runLegPosition[0] = false;
}
//cout << "run right leg position: " << runLegPosition[0] << endl;
if(cartpos_group.check("left_leg")) {
runLegPosition[1] = (bool)(cartpos_group.find("left_leg").asInt());;
} else {
runLegPosition[1] = false;
}
//cout << "run left leg position: " << runLegPosition[1] << endl;
if(cartpos_group.check("triangulation")) {
runTriangulation = (bool)(cartpos_group.find("triangulation").asInt());;
} else {
runTriangulation = false;
}
//cout << "run triangulation postion: " << runTriangulation << endl;
// parse cartesian orientation resources
Bottle &cartrot_group=rf.findGroup("cartesianorientation_resources");
if(cartrot_group.check("right_arm")) {
runArmOrientation[0] = (bool)(cartrot_group.find("right_arm").asInt());
} else {
runArmOrientation[0] = false;
}
//cout << "run right arm orientation: " << runArmOrientation[0] << endl;
if(cartrot_group.check("left_arm")) {
runArmOrientation[1] = (bool)(cartrot_group.find("left_arm").asInt());;
} else {
runArmOrientation[1] = false;
}
//cout << "run left arm orientation: " << runArmOrientation[1] << endl;
// parse heading resources
Bottle &heading_group=rf.findGroup("heading_resources");
if(heading_group.check("right_eye")) {
runHeadings[0] = (bool)(heading_group.find("right_eye").asInt());
} else {
runHeadings[0] = false;
}
//cout << "run right heading: " << runHeadings[0] << endl;
if(heading_group.check("left_eye")) {
runHeadings[1] = (bool)(heading_group.find("left_eye").asInt());
} else {
runHeadings[1] = false;
}
//cout << "run left heading: " << runHeadings[1] << endl;
if(heading_group.check("right_blob")) {
runBlobHeadings[0] = (bool)(heading_group.find("right_blob").asInt());
} else {
runBlobHeadings[0] = false;
}
//cout << "run right blob heading: " << runBlobHeadings[0] << endl;
if(heading_group.check("left_blob")) {
runBlobHeadings[1] = (bool)(heading_group.find("left_blob").asInt());
} else {
runBlobHeadings[1] = false;
}
//cout << "run left blob heading: " << runBlobHeadings[1] << endl;
if(heading_group.check("avg_blob")) {
runAvgBlobHeading = (bool)(heading_group.find("avg_blob").asInt());
} else {
runAvgBlobHeading = false;
}
//cout << "run avg blob heading: " << runAvgBlobHeading << endl;
if(heading_group.check("pf_left")) {
runParticleFilterHeadings[CB::ParticleFilterTrackerHeading::LEFT] = (bool)(heading_group.find("pf_left").asInt());
} else {
runParticleFilterHeadings[CB::ParticleFilterTrackerHeading::LEFT] = false;
}
//cout << "run left pf heading: " << runParticleFilterHeadings[LEFT] << endl;
if(heading_group.check("pf_right")) {
runParticleFilterHeadings[CB::ParticleFilterTrackerHeading::RIGHT] = (bool)(heading_group.find("pf_right").asInt());
} else {
runParticleFilterHeadings[CB::ParticleFilterTrackerHeading::RIGHT] = false;
}
//cout << "run right pf heading: " << runParticleFilterHeadings[RIGHT] << endl;
if(heading_group.check("pf_avg")) {
runParticleFilterHeadings[CB::ParticleFilterTrackerHeading::AVG] = (bool)(heading_group.find("pf_avg").asInt());
} else {
runParticleFilterHeadings[CB::ParticleFilterTrackerHeading::AVG] = false;
}
//cout << "run avg pf heading: " << runParticleFilterHeadings[AVG] << endl;
if(heading_group.check("stereo")) {
runStereoHeading = (bool)(heading_group.find("stereo").asInt());
} else {
runStereoHeading = false;
}
//cout << "run stereo heading: " << runStereoHeading << endl;
// parse logpolar heading resources
Bottle &logpolar_heading_group=rf.findGroup("logpolarheading_resources");
if(logpolar_heading_group.check("right_blob")) {
runLogPolarBlobHeadings[0] = (bool)(logpolar_heading_group.find("right_blob").asInt());
} else {
runLogPolarBlobHeadings[0] = false;
}
//cout << "run right logpolar blob heading: " << runLogPolarBlobHeadings[0] << endl;
if(logpolar_heading_group.check("left_blob")) {
runLogPolarBlobHeadings[1] = (bool)(logpolar_heading_group.find("left_blob").asInt());
} else {
runLogPolarBlobHeadings[1] = false;
}
//cout << "run left logpolar blob heading: " << runLogPolarBlobHeadings[1] << endl;
// parse contact set resources
Bottle &contacts_group=rf.findGroup("contactset_resources");
if(contacts_group.check("right_hand")) {
runHandContactSet[0] = (bool)(contacts_group.find("right_hand").asInt());
} else {
runHandContactSet[0] = false;
}
//cout << "run right hand contacts set: " << runHandContactSet[0] << endl;
if(contacts_group.check("left_hand")) {
runHandContactSet[1] = (bool)(contacts_group.find("left_hand").asInt());
} else {
runHandContactSet[1] = false;
}
//cout << "run left hand contacts set: " << runHandContactSet[1] << endl;
// now that we have the config vals, set the params used by resources
setParameters();
// run the resources
startResources();
return true;
}
virtual bool configure(ResourceFinder &rf)
{
Time::turboBoost();
fprintf(stderr, "Getting projections\n");
Matrix PiRight;
Bottle b;
b = rf.findGroup("CAMERA_CALIBRATION_RIGHT");
//fprintf(stderr, "CAMERA_CALIBRATION_RIGHT contains: %s\n", b.toString().c_str());
if (getProjectionMatrix(b, PiRight) == 0)
{
fprintf(stderr, "CAMERA_CALIBRATION_RIGHT was missing some params\n");
return false;
}
else
{
fprintf(stderr, "Working with RightProjection \n");
for (int i=0; i < PiRight.rows(); i++)
fprintf(stderr, "%s\n", PiRight.getRow(i).toString().c_str());
}
Matrix PiLeft;
b = rf.findGroup("CAMERA_CALIBRATION_LEFT");
//fprintf(stderr, "CAMERA_CALIBRATION_LEFT contains: %s\n", b.toString().c_str());
if (getProjectionMatrix(b, PiLeft) == 0)
{
fprintf(stderr, "CAMERA_CALIBRATION_LEFT was missing some params\n");
return false;
}
else
{
fprintf(stderr, "Working with LeftProjection \n");
for (int i=0; i < PiLeft.rows(); i++)
fprintf(stderr, "%s\n", PiLeft.getRow(i).toString().c_str());
}
int period=50;
if (rf.check("period"))
period=rf.find("period").asInt();
bool enableKalman=false;
if (rf.check("kalman"))
enableKalman=true;
string ctrlName=rf.find("name").asString().c_str();
string robotName=rf.find("robot").asString().c_str();
fprintf(stderr, "Initializing eT\n");
eT=new eyeTriangulation(rf, PiLeft, PiRight, enableKalman, period, ctrlName, robotName);
Vector xr(3); xr(0)=PiRight(0,2); xr(1)=PiRight(1,2); xr(2)=1.0;
Vector xl(3); xl(0)=PiLeft(0,2); xl(1)=PiLeft(1,2); xl(2)=1.0;
eT->xInit(xl, xr);
if (rf.check("const"))
eT->xSetConstant(xl, xr);
eT->start();
string rpcPortName="/"+ctrlName+"/rpc";
rpcPort.open(rpcPortName.c_str());
attach(rpcPort);
return true;
}
bool learnPrimitive::updateProtoAction(ResourceFinder &rf){
Bottle bOutput;
//1. Protoaction
Bottle bProtoAction = rf.findGroup("Proto_Action");
if (!bProtoAction.isNull())
{
Bottle * bProtoActionName = bProtoAction.find("protoActionName").asList();
Bottle * bProtoActionEnd = bProtoAction.find("protoActionEnd").asList();
Bottle * bProtoActionSpeed = bProtoAction.find("protoActionSpeed").asList();
if(bProtoActionName->isNull() || bProtoActionEnd->isNull() || bProtoActionSpeed->isNull()){
yError() << " [updateProtoAction] : one of the protoAction conf is null : protoActionName, protoActionEnd, protoActionSpeed" ;
return false ;
}
int protoActionSize = -1 ;
if(bProtoActionName->size() == bProtoActionEnd->size() && bProtoActionEnd->size() == bProtoActionSpeed->size()){
protoActionSize = bProtoActionName->size() ;
} else {
yError() << " [updateProtoAction] : one of the protoAction conf has different size!" ;
return false ;
}
if(protoActionSize == 0) {
yWarning() << " [updateProtoAction] : there is no protoaction defined at startup!" ;
} else {
for(int i = 0; i < protoActionSize ; i++) {
//insert protoaction even if already there
yInfo() << "ProtoAction added : (" << bProtoActionName->get(i).asString() << ", " << bProtoActionEnd->get(i).asInt() << ", " << bProtoActionSpeed->get(i).asDouble() << ")" ;
mProtoActionEnd[bProtoActionName->get(i).asString()] = bProtoActionEnd->get(i).asInt();
mProtoActionSpeed[bProtoActionName->get(i).asString()] = bProtoActionSpeed->get(i).asDouble();
}
}
} else {
yError() << " error in learnPrimitive::updateProtoAction | Proto_Action is NOT defined in the learnPrimitive.ini";
return false;
}
//2. Effect of bodypart
Bottle bBodyPart = rf.findGroup("BodyPart");
if (!bBodyPart.isNull())
{
Bottle * bBodyPartName = bBodyPart.find("bodyPartName").asList();
Bottle * bBodyPartEnd = bBodyPart.find("bodyPartProtoEnd").asList();
Bottle * bBodyPartSpeed = bBodyPart.find("bodyPartProtoSpeed").asList();
//Crash if no match : isnull is not good for that
if(bBodyPartName->isNull() || bBodyPartEnd->isNull() || bBodyPartSpeed->isNull()){
yError() << "[updateProtoAction] : one of the bodyPartProto conf is null : bodyPartName, bodyPartProtoEnd, bodyPartProtoSpeed" ;
return false ;
}
int bodyPartSize = -1 ;
if(bBodyPartName->size() == bBodyPartEnd->size() && bBodyPartEnd->size() == bBodyPartSpeed->size()){
bodyPartSize = bBodyPartName->size() ;
} else {
yError() << "[updateProtoAction] : one of the bodyPartProto conf has different size!" ;
return false ;
}
if(bodyPartSize == 0) {
yWarning() << "[updateProtoAction] : there is no bodyPartProto defined at startup!" ;
} else {
for(int i = 0; i < bodyPartSize ; i++) {
//insert protoaction even if already there
yInfo() << "bodyPartProto added : (" << bBodyPartName->get(i).asString() << ", " << bBodyPartEnd->get(i).asInt() << ", " << bBodyPartSpeed->get(i).asDouble() << ")" ;
mBodyPartEnd[bBodyPartName->get(i).asString()] = bBodyPartEnd->get(i).asInt();
mBodyPartSpeed[bBodyPartName->get(i).asString()] = bBodyPartSpeed->get(i).asDouble();
}
}
} else {
yError() << " error in learnPrimitive::updateProtoAction | BodyPart is NOT defined in the learnPrimitive.ini";
return false;
}
return true;
}
bool TactileLearning::getFingersAndJoints(ResourceFinder& rf)
{
/**
* There are two mutual exclusive ways to specify which fingers and joints are involved in the problem.
* 1. Checking which fingers are active and setting the joints properly.
* 2. Setting by hand the joint vector.
* In both cases checks for consistency have to be done, which higher priority to the fingers vector.
*/
string tmp;
int jointIndex;
if(!rf.findGroup("FINGERS").isNull())
{
// check which fingers are specified in the configuration file and set the activeFingers vector accordingly
for(int i = 0; i < NUM_FINGERS; i++)
{
tmp = rf.findGroup("FINGERS").find(FINGER_TYPES[i].c_str()).asString();
if(tmp.compare("on") == 0) activeFingers[i] = true;
else activeFingers[i] = false;
}
/**
* Set the joints vector so that it is consistent with the fingers vector values. This step requires
* knowledge about hardware implementation of the robot hand.
*/
jointIndex = 0;
//------------------------------------------------------------------------------------------------------------
// WARNING! The joint of the thumb to be ignored is the 9-th in the simulator, but the 8-th in the real robot!
//------------------------------------------------------------------------------------------------------------
// thumb
if(activeFingers[0] == true)
{
for(int i = 0; i < 3; i++)
{
// for simulator: ignore second joint (9-th for the whole arm)
// for real robot: ignore first joint (8-th for the whole arm)
if(i != 0) activeJoints[jointIndex++] = true;
else activeJoints[jointIndex++] = false;
}
}
else
{
for(int i = 0; i < 3; i++) activeJoints[jointIndex++] = false;
}
// index
if(activeFingers[1] == true)
{
for(int i = 0; i < 2; i++) activeJoints[jointIndex++] = true;
}
else
{
for(int i = 0; i < 2; i++) activeJoints[jointIndex++] = false;
}
// middle
if(activeFingers[2] == true)
{
for(int i = 0; i < 2; i++) activeJoints[jointIndex++] = true;
}
else
{
for(int i = 0; i < 2; i++) activeJoints[jointIndex++] = false;
}
// ring + pinky
if(activeFingers[3] == true) activeJoints[jointIndex] = true;
else activeJoints[jointIndex] = false;
}
else if(rf.check("joints"))
{
Bottle joints = rf.findGroup("joints").tail();
// check which joints are specified in the configuration file and set the activeJoints vector accordingly.
// brute force but who cares
for(int i = 0; i < NUM_HAND_JOINTS; i++)
{
activeJoints[i] = false;
for(int j = 0; j < joints.size(); j++)
{
if(i == joints.get(j).asInt() - HAND_JOINTS_OFFSET)
{
activeJoints[i] = true;
break;
}
}
}
/**
* activate each finger if at least a joint related to it has been activated.
* This step requires knowledge about hardware implementation of the robot hand.
*/
// thumb
if(activeJoints[0] || activeJoints[1] || activeJoints[2]) activeFingers[0] = true;
else activeFingers[0] = false;
// index
if(activeJoints[3] || activeJoints[4]) activeFingers[1] = true;
else activeFingers[1] = false;
// middle
if(activeJoints[5] || activeJoints[6]) activeFingers[2] = true;
else activeFingers[2] = false;
// pinky
if(activeJoints[7]) activeFingers[3] = true;
else activeFingers[3] = false;
}
else
{
printf("ERROR: no fingers were set, please check configuration file.\n");
return false;
}
// check for at least one active finger and in this case print both active fingers and active joints
for(int i = 0; i < NUM_FINGERS; i++)
{
if(activeFingers[i])
{
printFingersAndJoints();
return true;
}
}
// if no fingers are active the initialization is not successful
printf("ERROR: no fingers were set, please check configuration file.\n");
return false;
}
bool learnPrimitive::updatePrimitive(ResourceFinder &rf){
Bottle bOutput;
//1. Primitive
Bottle bPrimitiveAction = rf.findGroup("Primitive_Action");
if (!bPrimitiveAction.isNull())
{
Bottle * bPrimitiveActionName = bPrimitiveAction.find("primitiveActionName").asList();
Bottle * bPrimitiveActionArg = bPrimitiveAction.find("primitiveActionArg").asList();
if(bPrimitiveActionName->isNull() || bPrimitiveActionArg->isNull()){
yError() << " [updatePrimitiveAction] : one of the primitiveAction conf is null : primitiveActionName, primitiveActionArg" ;
return false ;
}
int primitiveActionSize = -1 ;
if(bPrimitiveActionName->size() == bPrimitiveActionArg->size()){
primitiveActionSize = bPrimitiveActionName->size() ;
} else {
yError() << " [updatePrimitiveAction] : one of the primitiveAction conf has different size!" ;
return false ;
}
if(primitiveActionSize == 0) {
yWarning() << " [updatePrimitiveAction] : there is no primitiveAction defined at startup!" ;
} else {
for(int i = 0; i < primitiveActionSize ; i++) {
//insert protoaction even if already there
Bottle bPrimitive;
string currentPrimName = bPrimitiveActionName->get(i).asString();
string currentPrimArg = bPrimitiveActionArg->get(i).asString();
bPrimitive.addString(currentPrimName);
bPrimitive.addString(currentPrimArg);
string concat = currentPrimName + "_" + currentPrimArg;
Bottle bCurrentPrim = rf.findGroup(concat);
if(bCurrentPrim.isNull()){
yError() << " [updatePrimitiveAction] : " << concat << "is NOT defined" ;
return false ;
}
Bottle * bListProtoAction = bCurrentPrim.find("actionList").asList();
if(bListProtoAction->isNull()){
yError() << " [updatePrimitiveAction] : " << concat << "is there but is not defined (actionList)" ;
return false ;
}
bPrimitive.addList() = *bListProtoAction ;
yInfo() << "Primitive Action added : (" <<bPrimitive.get(0).asString() << ", " << bPrimitive.get(1).asString() << ", ( " << bPrimitive.get(2).asList()->toString()<< "))" ;
vPrimitiveActionBottle.push_back(bPrimitive);
}
}
} else {
yError() << " error in learnPrimitive::updatePrimitive | Primitive_Action is NOT defined in the learnPrimitive.ini";
return false;
}
return true;
}
bool insituFTSensorCalibrationModule::configure(ResourceFinder &rf)
{
jointInitialized = false;
std::cout << rf.toString() << std::endl;
if( rf.check("robot") )
{
robotName = rf.find("robot").asString().c_str();
}
else
{
robotName = "icub";
}
std::string mode_cfg = rf.find("excitationMode").asString().c_str();
if( mode_cfg == "gridVisit" )
{
mode = GRID_VISIT;
}
else if( mode_cfg == "gridMappingWithReturn" )
{
mode = GRID_MAPPING_WITH_RETURN;
}
else
{
std::cerr << "[ERR] insituFTSensorCalibrationModule: excitationMode " << mode_cfg << "is not available, exiting." << std::endl;
std::cerr << "[ERR] existing modes: random, gridVisit, gridMappingWithReturn" << std::endl;
}
if( rf.check("local") )
{
moduleName = rf.find("local").asString().c_str();
}
else
{
moduleName = "insituFTCalibration";
}
setName(moduleName.c_str());
static_pose_period = rf.check("static_pose_period",1.0).asDouble();
return_point_waiting_period = rf.check("return_point_waiting_period",5.0).asDouble();
elapsed_time = 0.0;
ref_speed = rf.check("ref_speed",3.0).asDouble();
period = rf.check("period",1.0).asDouble();
if ( !rf.check("joints") )
{
fprintf(stderr, "Please specify the name and joints of the robot\n");
fprintf(stderr, "--robot name (e.g. icub)\n");
fprintf(stderr, "--joints ((part_name axis_number lower_limit upper_limit) ... )\n");
return false;
}
yarp::os::Bottle & jnts = rf.findGroup("joints");
if( jnts.isNull() )
{
fprintf(stderr, "Please specify the name and joints of the robot\n");
fprintf(stderr, "--robot name (e.g. icub)\n");
fprintf(stderr, "--joints ((part_name axis_number lower_limit upper_limit) ... )\n");
return false;
}
int nrOfControlledJoints = jnts.size()-1;
std::cout << "Found " << nrOfControlledJoints << " joint to control" << std::endl;
controlledJoints.resize(nrOfControlledJoints);
commandedPositions.resize(nrOfControlledJoints,0.0);
originalPositions.resize(nrOfControlledJoints);
originalRefSpeeds.resize(nrOfControlledJoints);
for(int jnt=0; jnt < nrOfControlledJoints; jnt++ )
{
yarp::os::Bottle * jnt_ptr = jnts.get(jnt+1).asList();
if( jnt_ptr == 0 || jnt_ptr->isNull() || !(jnt_ptr->size() == 4 || jnt_ptr->size() == 5) )
{
fprintf(stderr, "Malformed configuration file (joint %d)\n",jnt);
return false;
}
std::cout << jnt_ptr->toString() << std::endl;
controlledJoint new_joint;
new_joint.part_name = jnt_ptr->get(0).asString().c_str();
new_joint.axis_number = jnt_ptr->get(1).asInt();
new_joint.lower_limit = jnt_ptr->get(2).asDouble();
new_joint.upper_limit = jnt_ptr->get(3).asDouble();
new_joint.delta = jnt_ptr->get(4).asDouble();
controlledJoints[jnt] = new_joint;
}
for(int jnt=0; jnt < nrOfControlledJoints; jnt++ )
{
if( drivers.count(controlledJoints[jnt].part_name) == 1 )
{
//parts controlboard already opened, continue
continue;
}
std::string part_name = controlledJoints[jnt].part_name;
//Open the polydrivers
std::string remotePort="/";
remotePort+=robotName;
remotePort+="/";
remotePort+= part_name;
std::string localPort="/"+moduleName+remotePort;
Property options;
options.put("device", "remote_controlboard");
options.put("local", localPort.c_str()); //local port names
options.put("remote", remotePort.c_str()); //where we connect to
// create a device
PolyDriver * new_driver = new PolyDriver(options);
if( !new_driver->isValid() )
{
fprintf(stderr, "[ERR] Error in opening %s part\n",remotePort.c_str());
close_drivers();
return false;
}
bool ok = true;
IEncoders * new_encs;
IPositionControl * new_poss;
IControlLimits * new_lims;
ok = ok && new_driver->view(new_encs);
ok = ok && new_driver->view(new_poss);
ok = ok && new_driver->view(new_lims);
if(!ok)
{
fprintf(stderr, "Error in opening %s part\n",remotePort.c_str());
close_drivers();
return false;
}
drivers[part_name] = new_driver;
pos[part_name] = new_poss;
encs[part_name] = new_encs;
lims[part_name] = new_lims;
}
for(int jnt=0; jnt < nrOfControlledJoints; jnt++ )
{
std::string part = controlledJoints[jnt].part_name;
int axis = controlledJoints[jnt].axis_number;
encs[part]->getEncoder(axis,originalPositions.data()+jnt);
pos[part]->getRefSpeed(axis,originalRefSpeeds.data()+jnt);
pos[part]->setRefSpeed(axis,ref_speed);
}
jointInitialized = true;
//Compute the list of desired positions
if( mode == GRID_MAPPING_WITH_RETURN )
{
is_desired_point_return_point = false;
listOfDesiredPositions.resize(0,desiredPositions(yarp::sig::Vector(),0.0));
next_desired_position = 0;
//Generate vector of desired positions
if( controlledJoints.size() != 2)
{
std::cerr << "GRID_MAPPING_WITH_RETURN mode available only for two controlled joints" << std::endl;
close_drivers();
return false;
}
yarp::sig::Vector center(2), lower_left(2),lower_right(2),upper_left(2),upper_right(2);
std::vector<int> semi_nr_of_lines(2,0);
lower_left[0] = lower_right[0] = controlledJoints[0].lower_limit;
upper_left[0] = upper_right[0] = controlledJoints[0].upper_limit;
lower_left[1] = upper_left[1] = controlledJoints[1].lower_limit;
lower_right[1] = upper_right[0] = controlledJoints[1].upper_limit;
center[0] = (controlledJoints[0].lower_limit+controlledJoints[0].upper_limit)/2;
center[1] = (controlledJoints[1].lower_limit+controlledJoints[1].upper_limit)/2;
semi_nr_of_lines[0] = ceil((controlledJoints[0].upper_limit-center[0])/controlledJoints[0].delta);
semi_nr_of_lines[1] = ceil((controlledJoints[1].upper_limit-center[1])/controlledJoints[1].delta);
//Start at the center of the workspace
listOfDesiredPositions.push_back(desiredPositions(center,return_point_waiting_period,true));
for(int i=0; i < (int)semi_nr_of_lines[0]; i++ )
{
//Draw upper row
yarp::sig::Vector row_center(2), row_lower(2), row_upper(2);
row_upper[0] = row_lower[0] = row_center[0] = center[0]+i*controlledJoints[0].delta;
row_center[1] = center[1];
row_lower[1] = controlledJoints[1].lower_limit;
row_upper[1] = controlledJoints[1].upper_limit;
listOfDesiredPositions.push_back(desiredPositions(row_center,static_pose_period));
listOfDesiredPositions.push_back(desiredPositions(row_lower,static_pose_period));
listOfDesiredPositions.push_back(desiredPositions(row_upper,static_pose_period));
listOfDesiredPositions.push_back(desiredPositions(row_center,static_pose_period));
if( mode == GRID_MAPPING_WITH_RETURN )
{
listOfDesiredPositions.push_back(desiredPositions(center,return_point_waiting_period,true));
}
//Draw lower row
row_upper[0] = row_lower[0] = row_center[0] = center[0]-i*controlledJoints[0].delta;
row_center[1] = center[1];
row_lower[1] = controlledJoints[1].lower_limit;
row_upper[1] = controlledJoints[1].upper_limit;
listOfDesiredPositions.push_back(desiredPositions(row_center,static_pose_period));
listOfDesiredPositions.push_back(desiredPositions(row_lower,static_pose_period));
listOfDesiredPositions.push_back(desiredPositions(row_upper,static_pose_period));
listOfDesiredPositions.push_back(desiredPositions(row_center,static_pose_period));
if( mode == GRID_MAPPING_WITH_RETURN )
{
listOfDesiredPositions.push_back(desiredPositions(center,return_point_waiting_period));
}
}
for(int j=0; j < (int)semi_nr_of_lines[1]; j++ )
{
//Draw upper row
yarp::sig::Vector row_center(2), row_lower(2), row_upper(2);
row_upper[1] = row_lower[1] = row_center[1] = center[1]+j*controlledJoints[1].delta;
row_center[0] = center[0];
row_lower[0] = controlledJoints[0].lower_limit;
row_upper[0] = controlledJoints[0].upper_limit;
listOfDesiredPositions.push_back(desiredPositions(row_center,static_pose_period));
listOfDesiredPositions.push_back(desiredPositions(row_lower,static_pose_period));
listOfDesiredPositions.push_back(desiredPositions(row_upper,static_pose_period));
listOfDesiredPositions.push_back(desiredPositions(row_center,static_pose_period));
if( mode == GRID_MAPPING_WITH_RETURN )
{
listOfDesiredPositions.push_back(desiredPositions(center,return_point_waiting_period,true));
}
//Draw lower row
row_upper[1] = row_lower[1] = row_center[1] = center[1]-j*controlledJoints[1].delta;
row_center[0] = center[0];
row_lower[0] = controlledJoints[0].lower_limit;
row_upper[0] = controlledJoints[0].upper_limit;
listOfDesiredPositions.push_back(desiredPositions(row_center,static_pose_period));
listOfDesiredPositions.push_back(desiredPositions(row_lower,static_pose_period));
listOfDesiredPositions.push_back(desiredPositions(row_upper,static_pose_period));
listOfDesiredPositions.push_back(desiredPositions(row_center,static_pose_period));
if( mode == GRID_MAPPING_WITH_RETURN )
{
listOfDesiredPositions.push_back(desiredPositions(center,return_point_waiting_period,true));
}
}
}
if( this->mode == GRID_VISIT )
{
is_desired_point_return_point = false;
listOfDesiredPositions.resize(0,desiredPositions(yarp::sig::Vector(),0.0));
next_desired_position = 0;
//Generate vector of desired positions
if( controlledJoints.size() != 2)
{
std::cerr << "GRID_VISIT mode available only for two controlled joints" << std::endl;
close_drivers();
return false;
}
//x is the coordinate on the first controlled joint, y the one of the second
double x,y;
double minx = controlledJoints[0].lower_limit;
double maxx = controlledJoints[0].upper_limit;
double miny = controlledJoints[1].lower_limit;
double maxy = controlledJoints[1].upper_limit;
yarp::sig::Vector desPos(2,0.0);
for(x = minx,
y = miny;
x < maxx;
x += controlledJoints[0].delta,
y = (y == miny) ? maxy : miny )
{
desPos[0] = x;
desPos[1] = y;
listOfDesiredPositions.push_back(desiredPositions(desPos,static_pose_period));
}
for(x = minx,
y = miny;
y < maxy;
x = (x == minx) ? maxx : minx,
y += controlledJoints[1].delta )
{
desPos[0] = x;
desPos[1] = y;
listOfDesiredPositions.push_back(desiredPositions(desPos,static_pose_period));
}
}
///////////////////////////////////////////////
//// RPC PORT
///////////////////////////////////////////////
rpc_handler.yarp().attachAsServer(rpcPort);
std::string rpcPortName= "/";
rpcPortName+= getName();
rpcPortName += "/rpc:i";
if (!rpcPort.open(rpcPortName.c_str())) {
std::cerr << getName() << ": Unable to open port " << rpcPortName << std::endl;
return false;
}
isTheRobotInReturnPoint.open("/"+moduleName+"/isTheRobotInReturnPoint:o");
status = WAITING_NEW_DATASET_START;
///////////////////////////////////////////////
//// LAUNCHING SENSOR READING
///////////////////////////////////////////////
std::string wbi_conf_file;
wbi_conf_file = rf.check("wbi_conf_file",yarp::os::Value("yarpWholeBodyInterface.ini"),"File used for the configuration of the use yarpWholeBodySensors").asString();
yarp::os::Property wbi_prop;
std::string wbi_conf_file_name = rf.findFileByName(wbi_conf_file);
std::cout << wbi_conf_file_name << std::endl;
bool ret = wbi_prop.fromConfigFile(wbi_conf_file_name);
if( rf.check("robot") && rf.find("robot").isString() )
{
wbi_prop.put("robot",rf.find("robot").asString());
}
if( !ret )
{
yError("Failure in opening wbi configuration file");
close_drivers();
return false;
}
sensors = new yarpWbi::yarpWholeBodySensors((moduleName+"sensors").c_str(),wbi_prop);
model = new yarpWbi::yarpWholeBodyModel((moduleName+"model").c_str(),wbi_prop);
//Add sensors
if( rf.findGroup("sensor_to_calibrate").isNull() ||
!(rf.findGroup("sensor_to_calibrate").get(1).isList()) ||
!(rf.findGroup("sensor_to_calibrate").get(1).asList()->check("ft_sensor")) ||
!(rf.findGroup("sensor_to_calibrate").get(1).asList()->find("ft_sensor").isString()) ||
!(
((rf.findGroup("sensor_to_calibrate").get(1).asList()->check("accelerometer")) &&
rf.findGroup("sensor_to_calibrate").get(1).asList()->find("accelerometer").isString()) ||
( rf.findGroup("sensor_to_calibrate").get(1).asList()->check("acceleration_from_geometry") &&
rf.findGroup("sensor_to_calibrate").get(1).asList()->find("acceleration_from_geometry").isString() &&
rf.findGroup("sensor_to_calibrate").get(1).asList()->check("joints_in_geometry") &&
rf.findGroup("sensor_to_calibrate").get(1).asList()->find("joints_in_geometry").isList() )
) )
{
yError("Failure in loading sensors_to_calibrate group");
std::cout << rf.findGroup("sensor_to_calibrate").get(1).toString() << std::endl;
close_drivers();
return false;
}
bool readAccelerationFromSensor;
std::string sensor_frame;
if( rf.findGroup("sensor_to_calibrate").get(1).asList()->check("accelerometer") )
{
readAccelerationFromSensor = true;
}
else if( rf.findGroup("sensor_to_calibrate").get(1).asList()->check("acceleration_from_geometry") )
{
readAccelerationFromSensor = false;
sensor_frame = rf.findGroup("sensor_to_calibrate").get(1).asList()->find("acceleration_from_geometry").asString();
}
else
{
yError("Failure in loading sensors_to_calibrate group");
std::cout << rf.findGroup("sensor_to_calibrate").get(1).toString() << std::endl;
close_drivers();
return false;
}
if( readAccelerationFromSensor )
{
wbi::ID acc_id = rf.findGroup("sensor_to_calibrate").get(1).find("accelerometer").asString().c_str();
ret = ret && sensors->addSensor(wbi::SENSOR_ACCELEROMETER,acc_id);
}
else
{
int nr_of_joints_in_geometry = rf.findGroup("sensor_to_calibrate").get(1).find("joints_in_geometry").asList()->size();
//Add joints in geometry model to the interface
for(int i = 0; i < nr_of_joints_in_geometry; i++ )
{
bool success;
wbi::ID enc_id = rf.findGroup("sensor_to_calibrate").get(1).find("joints_in_geometry").asList()->get(i).asString().c_str();
success = sensors->addSensor(wbi::SENSOR_ENCODER,enc_id);
success = success && model->addJoint(enc_id);
if( !success )
{
yError("Failure in adding joint %s to the wbi", enc_id.toString().c_str());
std::cout << rf.findGroup("sensor_to_calibrate").get(1).toString() << std::endl;
close_drivers();
return false;
}
else
{
yInfo("Success in adding joint %s to the wbi", enc_id.toString().c_str());
}
}
}
wbi::ID FT_id = rf.findGroup("sensor_to_calibrate").get(1).find("ft_sensor").asString().c_str();
ret = ret && sensors->addSensor(wbi::SENSOR_FORCE_TORQUE,FT_id);
if( !ret || !sensors->init() || !model->init() )
{
yError("Failure in opening yarpWholeBodySensors interface");
close_drivers();
return false;
}
///////////////////////////////////////////////
//// LAUNCHING ESTIMATION THREAD
///////////////////////////////////////////////
estimation_thread = new insituFTSensorCalibrationThread(sensors,model,rf,readAccelerationFromSensor,sensor_frame);
if( ! estimation_thread->start() )
{
yError("Failure in starting calibration thread");
close_drivers();
sensors->close();
return false;
}
return true;
}
bool SpeechRecognizerModule::loadGrammarFromRf(ResourceFinder &RF)
{
Bottle &bAgent = RF.findGroup("agent");
Bottle &bAction = RF.findGroup("action");
Bottle &bObject = RF.findGroup("object");
Bottle bMessenger, bReply;
yInfo() << "Agents are: " ;
for (int iBottle = 1 ; iBottle < bAgent.size() ; iBottle++)
{
yInfo() << "\t" << bAgent.get(iBottle).toString();
bMessenger.clear();
bMessenger.addString("add");
bMessenger.addString("#agent");
bMessenger.addString(bAgent.get(iBottle).toString());
handleRGMCmd(bMessenger, bReply);
yInfo() << "\t\t" << bReply.toString() ;
}
yInfo() << "\n" << "Actions are: " ;
for (int iBottle = 1 ; iBottle < bAction.size() ; iBottle++)
{
yInfo() << "\t" << bAction.get(iBottle).toString();
bMessenger.clear();
bMessenger.addString("add");
bMessenger.addString("#action");
bMessenger.addString(bAction.get(iBottle).toString());
handleRGMCmd(bMessenger, bReply);
yInfo() << "\t\t" << bReply.toString() ;
}
yInfo() << "\n" << "Objects are: " ;
for (int iBottle = 1 ; iBottle < bObject.size() ; iBottle++)
{
yInfo() << "\t" << bObject.get(iBottle).toString();
bMessenger.clear();
bMessenger.addString("add");
bMessenger.addString("#object");
bMessenger.addString(bObject.get(iBottle).toString());
handleRGMCmd(bMessenger, bReply);
yInfo() << "\t\t" << bReply.toString() ;
}
Bottle bGrammarMain, bGrammarDef;
bGrammarMain.addString("addGrammar");
bGrammarMain.addString("#agent #action #object");
yInfo() << "\n" << bGrammarMain.toString() ;
// handleAsyncRecognitionCmd(bGrammarMain, bReply);
yInfo() << "\n" << bReply.toString() ;
return true;
}
