virtual bool configure(ResourceFinder &rf)
{
string name=rf.find("name").asString().c_str();
setName(name.c_str());
string partUsed=rf.find("part").asString().c_str();
if ((partUsed!="left_arm") && (partUsed!="right_arm"))
{
cout<<"Invalid part requested!"<<endl;
return false;
}
Property config;
config.fromConfigFile(rf.findFile("from").c_str());
Bottle &bGeneral=config.findGroup("general");
if (bGeneral.isNull())
{
cout<<"Error: group general is missing!"<<endl;
return false;
}
// parsing general config options
Property option;
for (int i=1; i<bGeneral.size(); i++)
{
Bottle *pB=bGeneral.get(i).asList();
if (pB->size()==2)
option.put(pB->get(0).asString().c_str(),pB->get(1));
else
{
cout<<"Error: invalid option!"<<endl;
return false;
}
}
option.put("local",name.c_str());
option.put("part",rf.find("part").asString().c_str());
option.put("grasp_model_type",rf.find("grasp_model_type").asString().c_str());
option.put("grasp_model_file",rf.findFile("grasp_model_file").c_str());
option.put("hand_sequences_file",rf.findFile("hand_sequences_file").c_str());
// parsing arm dependent config options
Bottle &bArm=config.findGroup("arm_dependent");
getArmDependentOptions(bArm,graspOrien,graspDisp,dOffs,dLift,home_x);
cout<<"***** Instantiating primitives for "<<partUsed<<endl;
action=new AFFACTIONPRIMITIVESLAYER(option);
if (!action->isValid())
{
delete action;
return false;
}
deque<string> q=action->getHandSeqList();
cout<<"***** List of available hand sequence keys:"<<endl;
for (size_t i=0; i<q.size(); i++)
cout<<q[i]<<endl;
string fwslash="/";
inPort.open((fwslash+name+"/in").c_str());
rpcPort.open((fwslash+name+"/rpc").c_str());
attach(rpcPort);
openPorts=true;
// check whether the grasp model is calibrated,
// otherwise calibrate it and save the results
Model *model;
action->getGraspModel(model);
if (!model->isCalibrated())
{
Property prop("(finger all)");
model->calibrate(prop);
ofstream fout;
fout.open(option.find("grasp_model_file").asString().c_str());
model->toStream(fout);
fout.close();
}
return true;
}
int getDataToDump(ResourceFinder &rf, std::string *listOfData, int n, bool *needDebug)
{
std::string availableDataToDump[NUMBER_OF_AVAILABLE_STANDARD_DATA_TO_DUMP];
std::string availableDebugDataToDump[NUMBER_OF_AVAILABLE_DEBUG_DATA_TO_DUMP];
int j;
// standard
availableDataToDump[0] = "getEncoders";
availableDataToDump[1] = "getEncoderSpeeds";
availableDataToDump[2] = "getEncoderAccelerations";
availableDataToDump[3] = "getPositionErrors";
availableDataToDump[4] = "getOutputs";
availableDataToDump[5] = "getCurrents";
availableDataToDump[6] = "getTorques";
availableDataToDump[7] = "getTorqueErrors";
availableDataToDump[8] = "getPosPidReferences";
availableDataToDump[9] = "getTrqPidReferences";
availableDataToDump[10] = "getControlModes";
availableDataToDump[11] = "getInteractionModes";
availableDataToDump[12] = "getMotorEncoders";
availableDataToDump[13] = "getMotorSpeeds";
availableDataToDump[14] = "getMotorAccelerations";
availableDataToDump[15] = "getTemperatures";
availableDataToDump[16] = "getMotorsPwm";
// debug
availableDebugDataToDump[0] = "getRotorPositions";
availableDebugDataToDump[1] = "getRotorSpeeds";
availableDebugDataToDump[2] = "getRotorAccelerations";
if (rf.check("dataToDumpAll"))
{
for (int i = 0; i < n; i++)
listOfData[i] = availableDataToDump[i];
return 1;
}
if (!rf.check("dataToDump"))
{
yError("Missing option 'dataToDump' in given config file\n");
return 0;
}
Value& list = rf.find("dataToDump");
Bottle *pList = list.asList();
for (int i = 0; i < n; i++)
{
listOfData[i] = pList->get(i).toString();
// check if the requested data is a standard one
for (j = 0; j< NUMBER_OF_AVAILABLE_STANDARD_DATA_TO_DUMP; j++)
{
if (listOfData[i] == (availableDataToDump[j]))
break;
}
// check if the requested data is a debug one
for (j = 0; j< NUMBER_OF_AVAILABLE_DEBUG_DATA_TO_DUMP; j++)
{
if (listOfData[i] == (availableDebugDataToDump[j]))
{
*needDebug = true;
break;
}
}
// if not found
if (j == (NUMBER_OF_AVAILABLE_STANDARD_DATA_TO_DUMP + NUMBER_OF_AVAILABLE_DEBUG_DATA_TO_DUMP))
{
yError("Illegal values for 'dataToDump': %s does not exist!\n",listOfData[i].c_str());
return 0;
}
}
return 1;
}
bool readMatrix(const string &tag, Matrix &matrix, const int &dimension, ResourceFinder &rf)
{
string tag_x=tag+"_x";
string tag_y=tag+"_y";
bool check_x;
if(tag=="x0")
{
if (Bottle *b=rf.find(tag.c_str()).asList())
{
Vector col;
if (b->size()>=dimension)
{
for(size_t i=0; i<b->size();i++)
col.push_back(b->get(i).asDouble());
matrix.setCol(0, col);
}
return true;
}
}
else
{
if(tag=="bounds")
{
cout<<"in boudn "<<endl;
tag_x=tag+"_l";
tag_y=tag+"_u";
}
if (Bottle *b=rf.find(tag_x.c_str()).asList())
{
cout<<"in boudn 2"<<endl;
Vector col;
if (b->size()>=dimension)
{
cout<<"in boudn 3"<<endl;
for(size_t i=0; i<b->size();i++)
col.push_back(b->get(i).asDouble());
matrix.setCol(0, col);
}
check_x=true;
}
if (Bottle *b=rf.find(tag_y.c_str()).asList())
{
Vector col;
if (b->size()>=dimension)
{
cout<<"in boudn 4"<<endl;
for(size_t i=0; i<b->size();i++)
col.push_back(b->get(i).asDouble());
matrix.setCol(1, col);
}
cout<<"matrix "<<matrix.toString().c_str()<<endl;
if(check_x==true)
return true;
}
}
return false;
}
int main(int argc, char *argv[])
{
ResourceFinder rf;
rf.configure(argc,argv);
double t,t0;
string nameFileOut, nameFileSolution;
string mu_strategy,nlp_scaling_method;
double tol, sum;
int acceptable_iter,max_iter;
int file_provided;
pointClouds pC;
nameFileOut=rf.find("nameFileOut").asString().c_str();
if(rf.find("nameFileOut").isNull())
nameFileOut="test";
nameFileSolution=rf.find("nameFileSolution").asString().c_str();
if(rf.find("nameFileSolution").isNull())
nameFileSolution="solution.txt";
tol=rf.find("tol").asDouble();
if(rf.find("tol").isNull())
tol=1e-4;
acceptable_iter=rf.find("acceptable_iter").asInt();
if(rf.find("acceptable_iter").isNull())
acceptable_iter=0;
max_iter=rf.find("max_iter").asInt();
if(rf.find("max_iter").isNull())
max_iter=2e19;
mu_strategy=rf.find("mu_strategy").asString().c_str();
if(rf.find("mu_strategy").isNull())
mu_strategy="monotone";
nlp_scaling_method=rf.find("nlp_scaling_method").asString().c_str();
if(rf.find("nlp_scaling_method").isNull())
nlp_scaling_method="none";
pC.init(rf);
file_provided=rf.find("point cloud file").asInt();
if(rf.find("point cloud file").isNull())
file_provided=1;
pC.acquirePoints(file_provided,rf);
//algorithm settings
Ipopt::SmartPtr<Ipopt::IpoptApplication> app=new Ipopt::IpoptApplication;
app->Options()->SetNumericValue("tol",tol);
app->Options()->SetIntegerValue("acceptable_iter",acceptable_iter);
app->Options()->SetStringValue("mu_strategy",mu_strategy);
app->Options()->SetIntegerValue("max_iter",max_iter);
app->Options()->SetStringValue("nlp_scaling_method",nlp_scaling_method);
//app->Options()->SetNumericValue("nlp_scaling_max_gradient",10);
//app->Options()->SetNumericValue("nlp_scaling_min_value",1e-2);
app->Options()->SetStringValue("hessian_approximation","limited-memory");
//app->Options()->SetStringValue("derivative_test","first-order");
//app->Options()->SetStringValue("derivative_test_print_all","yes");
app->Options()->SetStringValue("output_file",nameFileOut+".out");
//app->Options()->SetStringValue("print_user_options","yes");
//app->Options()->SetStringValue("print_options_documentation","no");
app->Options()->SetIntegerValue("print_level",0);
app->Initialize();
t0=Time::now();
Ipopt::SmartPtr<SuperQuadric_NLP> superQ_nlp= new SuperQuadric_NLP;
superQ_nlp->init(pC);
superQ_nlp->configure(rf);
Ipopt::ApplicationReturnStatus status=app->OptimizeTNLP(GetRawPtr(superQ_nlp));
t=Time::now()-t0;
sum=0.0;
for(size_t i=0;i<superQ_nlp->t.size(); i++)
{
sum=sum+superQ_nlp->t[i];
}
if(status==Ipopt::Solve_Succeeded)
{
Vector sol;
sol=superQ_nlp->get_result();
cout<<"The optimal solution is: "<<sol.toString().c_str()<<endl;
ofstream fout(nameFileSolution.c_str());
if(fout.is_open())
{
fout<<"x: "<<endl<<endl;
fout<<sol.toString().c_str()<<endl<<endl;
fout<<"t for ipopt: "<<endl<<endl;
fout<<t<<endl<<endl;
fout<<"average time for each gradient step: "<<endl<<endl;
fout<<sum/superQ_nlp->t.size()<<endl<<endl;
fout<<"method to compute gradient:"<<endl<<endl;
fout<<superQ_nlp->gradient_comp<<endl;
fout<<"point cloud: "<<endl;
fout<<pC.pointCloudFileName<<endl;
}
}
else
cout<<"Problem not solved!"<<endl;
cout<<"average time to compute grad: "<<endl;
cout<<sum/superQ_nlp->t.size()<<endl;
cout<<"t "<<t<<endl;
return 0;
}
bool saliencyBlobFinderModule::configure(ResourceFinder &rf) {
/* Process all parameters from both command-line and .ini file */
if(rf.check("help")) {
printf("HELP \n");
printf("====== \n");
printf("--name : changes the rootname of the module ports \n");
printf("--robot : changes the name of the robot where the module interfaces to \n");
printf("--name : rootname for all the connection of the module \n");
printf("--camerasContext : context where camera parameters are stored \n");
printf("--camerasFile : file of parameters of the camera in the context \n");
printf(" \n");
printf("press CTRL-C to stop... \n");
return true;
}
/* get the module name which will form the stem of all module port names */
moduleName = rf.check("name",
Value("/blobFinder/icub/left_cam"),
"module name (string)").asString();
/*
* before continuing, set the module name before getting any other parameters,
* specifically the port names which are dependent on the module name
*/
setName(moduleName.c_str());
/*
* get the thread rate which will define the period of the processing thread
*/
threadRate = rf.check("ratethread",
Value(33),
"processing ratethread (int)").asInt();
cout << "Module started with the parameter ratethread: " << threadRate << endl;
/*
* gets the minBounding area for blob neighbours definition
*/
minBoundingArea = rf.check("minBoundingArea",
Value(225),
"minBoundingArea (int)").asInt();
cout << "Module started with min bounding area " << minBoundingArea << endl;
if (!cmdPort.open(getName())) {
cout << getName() << ": Unable to open port " << endl;
return false;
}
attach(cmdPort); // attach to port
/*
if (rf.check("config")) {
configFile=rf.findFile(rf.find("config").asString().c_str());
if (configFile=="") {
return false;
}
}
else {
configFile.clear();
}
*/
if (rf.check("camerasFile")) {
if (rf.check("camerasContext")) {
printf("found a new context %s \n", rf.find("camerasContext").asString().c_str());
// rf.find("camerasContext").asString().c_str()
rf.setDefaultContext("cameraCalibration/conf");
}
printf("got the cameraContext %s \n", rf.getContext().c_str());
camerasFile=rf.findFile(rf.find("camerasFile").asString().c_str());
if (camerasFile==""){
return false;
}
else {
printf("found the camerasFile %s \n", camerasFile.c_str());
}
}
else {
camerasFile.clear();
}
printf("configFile: %s \n", camerasFile.c_str());
//initialization of the main thread
blobFinder = new blobFinderThread(threadRate,camerasFile);
blobFinder->setName(this->getName().c_str());
blobFinder->setMinBoundingArea(minBoundingArea);
blobFinder->start();
cout << "waiting for connection of the input port" << endl;
return true;
}
bool BehaviorModule::configure(ResourceFinder &rf) {
robotName = rf.find("robot").asString().c_str();
std::cout << robotName << " is the name of our robot" << std::endl;
action_left = NULL;
action_right = NULL;
options_left.put("device", "remote_controlboard");
options_left.put("local", "/pos_ctrl_left_arm");
options_left.put("remote", ("/" + robotName + "/left_arm").c_str());
options_right.put("device", "remote_controlboard");
options_right.put("local", "/pos_ctrl_right_arm");
options_right.put("remote", ("/" + robotName + "/right_arm").c_str());
driver_left.open(options_left);
driver_right.open(options_right);
options_head.put("device", "remote_controlboard");
options_head.put("local", "/pos_ctrl_head");
options_head.put("remote", ("/" + robotName + "/head").c_str());
driver_head.open(options_head);
options_torso.put("device", "remote_controlboard");
options_torso.put("local", "/pos_ctrl_torso");
options_torso.put("remote", ("/" + robotName + "/torso").c_str());
driver_torso.open(options_torso);
emotP.open("/local/emoInt");
Network::connect("/local/emoInt", "/icub/face/emotions/in");
// become happy
happy();
if (!driver_left.isValid() || !driver_right.isValid() || !driver_head.isValid() || !driver_torso.isValid()) {
cerr << "A device is not available. Here are the known devices:"
<< endl;
cerr << Drivers::factory().toString().c_str() << endl;
exit(-1);
}
bool ok = true;
ok = ok && driver_left.view(pos_ctrl_left);
ok = ok && driver_left.view(encoders_left);
ok = ok && driver_right.view(pos_ctrl_right);
ok = ok && driver_right.view(encoders_right);
ok = ok && driver_head.view(pos_ctrl_head);
ok = ok && driver_head.view(encoders_head);
ok = ok && driver_torso.view(pos_ctrl_torso);
ok = ok && driver_torso.view(encoders_torso);
ok = ok && driver_left.view(ictrl_left);
ok = ok && driver_left.view(iimp_left);
ok = ok && driver_left.view(itrq_left);
ok = ok && driver_right.view(ictrl_right);
ok = ok && driver_right.view(iimp_right);
ok = ok && driver_right.view(itrq_right);
chosen_arm = "left";
// needed for impedance controller
//ok = ok && driver_right.view(ictrl);
//ok = ok && driver_right.view(trq_ctrl_left);
//ok = ok && driver_right.view(trq_ctrl_right);
options_gaze.put("device", "gazecontrollerclient");
options_gaze.put("remote", "/iKinGazeCtrl");
options_gaze.put("local", "/client/gaze");
driver_gaze.open(options_gaze);
driver_gaze.view(igaze);
if (driver_gaze.isValid()) {
ok = ok && driver_gaze.view(igaze);
}
igaze->setTrackingMode(false);
igaze->setEyesTrajTime(1.0);
igaze->setNeckTrajTime(2.0);
igaze->bindNeckPitch(-38, 0);
igaze->bindNeckRoll(-5, 5);
igaze->bindNeckYaw(-30.0, 35.0);
if (!ok) {
cerr << "error getting interfaces" << std::endl;
exit(-1);
}
int n_jnts = 0;
pos_ctrl_left->getAxes(&n_jnts);
positions_left_cmd.resize(n_jnts);
positions_left_enc.resize(n_jnts);
pos_ctrl_right->getAxes(&n_jnts);
positions_right_cmd.resize(n_jnts);
positions_right_enc.resize(n_jnts);
pos_ctrl_head->getAxes(&n_jnts);
positions_head_enc.resize(n_jnts);
positions_head_cmd.resize(n_jnts);
pos_ctrl_torso->getAxes(&n_jnts);
positions_torso_enc.resize(n_jnts);
positions_torso_cmd.resize(n_jnts);
openPorts = false;
firstRun = true;
sim = false;
string name = rf.find("name").asString().c_str();
// setName(name.c_str());
string sim_or_real = rf.find("sim").asString().c_str();
if (sim_or_real == "on")
sim = true;
else
sim = false;
Property config;
config.fromConfigFile(rf.findFile("from").c_str());
Bottle &bGeneral = config.findGroup("general");
if (bGeneral.isNull()) {
cout << "Error: group general is missing!" << endl;
return false;
}
// parsing general config options
Property option;
for (int i = 1; i < bGeneral.size(); i++) {
Bottle *pB = bGeneral.get(i).asList();
if (pB->size() == 2)
option.put(pB->get(0).asString().c_str(), pB->get(1));
else {
cout << "Error: invalid option!" << endl;
return false;
}
}
option.put("local", name.c_str());
option.put("grasp_model_type",
rf.find("grasp_model_type").asString().c_str());
option.put("grasp_model_file", rf.findFile("grasp_model_file").c_str());
option.put("hand_sequences_file",
rf.findFile("hand_sequences_file").c_str());
printf("%s\n", option.toString().c_str());
// parsing arm dependent config options
Bottle &bArm = config.findGroup("arm_dependent");
graspOrien.resize(4);
graspDisp.resize(4);
dOffs.resize(3);
dLift.resize(3);
home_x.resize(3);
getArmDependentOptions(bArm, graspOrien, graspDisp, dOffs, dLift, home_x);
option.put("part", "right_arm");
printf("Options for right arm \n %s\n", option.toString().c_str());
cout << "***** Instantiating primitives for right_arm" << endl;
action_right = new ActionPrimitivesLayer2(option);
option.put("part", "left_arm");
printf("Options for left arm \n %s\n", option.toString().c_str());
cout << "***** Instantiating primitives for left_arm"<< endl;
action_left = new ActionPrimitivesLayer2(option);
if (!action_left->isValid()) {
delete action_left;
cout << "Action_left is not valid" << endl;
return false;
}
if (!action_right->isValid()) {
delete action_right;
cout << "Action_right is not valid" << endl;
return false;
}
deque<string> q = action_right->getHandSeqList();
cout << "***** List of available for right hand sequence keys:" << endl;
for (size_t i = 0; i < q.size(); i++)
cout << q[i] << endl;
q = action_left->getHandSeqList();
cout << "***** List of available for left hand sequence keys:" << endl;
for (size_t i = 0; i < q.size(); i++)
cout << q[i] << endl;
return true;
}
bool AwareTouch::configure(ResourceFinder &rf)
{
// Defining module
string moduleName = rf.check("name", Value("awareTouch")).asString().c_str(); // Check name of the module
setName(moduleName.c_str()); // Assign this name for ports
cout<< "|| Starting config "<< moduleName <<endl;
printf("Naming the module \n");
string robot=rf.check("robot",Value("icubSim")).asString().c_str(); //type of robot
cout<< "|| Robot :"<< robot <<endl;
// port for skin data
string rpcName="/" ;
rpcName += getName ( rf.check("inPort",Value("/skin_contacts:i")).asString() ); //input port name
// port for output events
string eventsName="/" ;
eventsName += getName ( rf.check("eventPort",Value("/events:o")).asString() ); //events port name
cout<< "|| Opening port :"<< eventsName <<endl;
eventsPort.open(eventsName.c_str());
string skinManagerName = rf.check("skinManagerPort", Value("/skinManager/skin_events:o")).asString().c_str(); // Check name of the module
string opcName = rf.check("opcName", Value("OPC")).asString().c_str();
//Generating a copy of the world from OPC
world = new OPCClient("OPCTouch");
while(!world->connect(opcName)) {
cout<< "Trying to connect OPC Server"<<endl;
}
cout<< "|| Connected OPC :" <<endl;
// Generating type of gestures
cout<< "|| Reading files of gesture types ... :" <<endl;
Bottle * gestureTypes=rf.find("gestureTypes").asList();
cout<< "|| Gesture types ("<< gestureTypes->size()<<") "<<endl;
//Populating the world with gestures and subjects
gestureSet.clear();
string gestureStr;
for (int iGesture=0; iGesture<gestureTypes->size(); iGesture++) {
gestureStr=(gestureTypes->get(iGesture).asString().c_str() );
gestureSet.push_back(gestureStr);
cout<<gestureStr<<endl;
world->addOrRetrieveEntity<Adjective>(gestureStr);
}
world->addOrRetrieveEntity<Agent>("icub");
touchLocation = world->addOrRetrieveEntity<Object>("touchLocation");
touchLocation->m_present = 0.0;
world->commit(touchLocation);
world->addOrRetrieveEntity<Action>("is");
world->addOrRetrieveEntity<Adjective>("none");
// holding time in OPC
recordingPeriod=rf.check("recordingPeriod",Value(3.0)).asDouble(); //type of robot
cout << "|| Recording Period is " << recordingPeriod << endl ;
string pathG(rf.getHomeContextPath().c_str());
cout<< "|| Creating Touch Thread:" <<endl;
estimationThread= new TouchEstimationThread(skinManagerName, rpcName, pathG, gestureSet, 50);
cout<< "|| Starting Touch :" <<endl;
estimationThread->start();
cout<< "|| Started Touch :" <<endl;
return true;
}
bool configure(ResourceFinder &rf)
{
string name=rf.find("name").asString().c_str();
string robot=rf.find("robot").asString().c_str();
string hand=rf.find("hand").asString().c_str();
string modelType=rf.find("modelType").asString().c_str();
fingerName=rf.find("finger").asString().c_str();
if (fingerName=="thumb")
joint=10;
else if (fingerName=="index")
joint=12;
else if (fingerName=="middle")
joint=14;
else if (fingerName=="ring")
joint=15;
else if (fingerName=="little")
joint=15;
else
{
fprintf(stdout,"unknown finger!\n");
return false;
}
Property driverOpt("(device remote_controlboard)");
driverOpt.put("remote",("/"+robot+"/"+hand+"_arm").c_str());
driverOpt.put("local",("/"+name).c_str());
if (!driver.open(driverOpt))
return false;
driver.view(ipos);
driver.view(ienc);
IControlLimits *ilim;
driver.view(ilim);
ilim->getLimits(joint,&min,&max);
double margin=0.1*(max-min);
min=min+margin;
max=max-margin;
val=&min;
Property genOpt;
genOpt.put("name",(name+"/"+modelType).c_str());
genOpt.put("robot",robot.c_str());
genOpt.put("type",hand.c_str());
genOpt.put("verbose",1);
string general(genOpt.toString().c_str());
string thumb( "(thumb (name thumb))");
string index( "(index (name index))");
string middle("(middle (name middle))");
string ring( "(ring (name ring))");
string little("(little (name little))");
Property options((general+" "+thumb+" "+index+" "+middle+" "+ring+" "+little).c_str());
fprintf(stdout,"configuring options: %s\n",options.toString().c_str());
if (modelType=="springy")
model=new SpringyFingersModel;
else if (modelType=="tactile")
model=new TactileFingersModel;
else
{
fprintf(stdout,"unknown model type!\n");
return false;
}
if (model->fromProperty(options))
return true;
else
{
delete model;
return false;
}
}
bool configure(ResourceFinder &rf) override
{
portName=rf.check("name",Value("/dump")).asString();
if (portName[0]!='/')
portName="/"+portName;
bool saveData=true;
bool videoOn=false;
string videoType=rf.check("videoType",Value("mkv")).asString();
if (rf.check("type"))
{
string optTypeName=rf.find("type").asString();
if (optTypeName=="bottle")
type=bottle;
else if (optTypeName=="image")
{
type=image;
#ifdef ADD_VIDEO
if (rf.check("addVideo"))
videoOn=true;
#endif
}
else if (optTypeName == "image_jpg")
{
type=image;
save_jpeg = true;
}
#ifdef ADD_VIDEO
else if (optTypeName=="video")
{
type=image;
videoOn=true;
saveData=false;
}
#endif
else
{
yError() << "Error: invalid type";
return false;
}
}
else
type=bottle;
dwnsample=rf.check("downsample",Value(1)).asInt32();
rxTime=rf.check("rxTime");
txTime=rf.check("txTime");
string templateDirName=rf.check("dir")?rf.find("dir").asString():portName;
if (templateDirName[0]!='/')
templateDirName="/"+templateDirName;
string dirName;
if (rf.check("overwrite"))
dirName="."+templateDirName;
else
{
// look for a proper directory
int i=0;
do
{
ostringstream checkDirName;
if (i>0)
checkDirName << "." << templateDirName << "_" << setw(5) << setfill('0') << i;
else
checkDirName << "." << templateDirName;
dirName=checkDirName.str();
i++;
}
while (!yarp::os::stat(dirName.c_str()));
}
yarp::os::mkdir_p(dirName.c_str());
q=new DumpQueue();
t=new DumpThread(type,*q,dirName,100,saveData,videoOn,videoType);
if (!t->start())
{
delete t;
delete q;
return false;
}
reporter.setThread(t);
if (type==bottle)
{
p_bottle=new DumpPort<Bottle>(*q,dwnsample,rxTime,txTime);
p_bottle->useCallback();
p_bottle->open(portName);
p_bottle->setStrict();
p_bottle->setReporter(reporter);
}
else
{
p_image=new DumpPort<Image>(*q,dwnsample,rxTime,txTime);
p_image->useCallback();
p_image->open(portName);
p_image->setStrict();
p_image->setReporter(reporter);
}
if (rf.check("connect"))
{
string srcPort=rf.find("connect").asString();
bool ok=Network::connect(srcPort.c_str(),
(type==bottle)?p_bottle->getName().c_str():
p_image->getName().c_str(),"tcp");
ostringstream msg;
msg << "Connection to " << srcPort << " " << (ok?"successful":"failed");
if (ok)
yInfo() << msg.str();
else
yWarning() << msg.str();
}
// this port serves to handle the "quit" rpc command
rpcPort.open(portName+"/rpc");
attach(rpcPort);
yInfo() << "Service yarp port: " << portName;
yInfo() << "Data stored in : " << dirName;
return true;
}
//Gathering resources for initialization
bool configure(ResourceFinder &rf) {
string name = rf.find("name").asString().c_str();
setName(name.c_str());
Property config; config.fromConfigFile(rf.findFile("from").c_str());
Bottle &bGeneral = config.findGroup("general");
if (bGeneral.isNull()) {
cout << "Error: group general is missing!" << endl;
return false;
}
// parsing general config options from config.ini file
Property option(bGeneral.toString().c_str());
option.put("local", name.c_str());
option.put("part", "left_arm");
option.put("grasp_model_type", rf.find("grasp_model_type").asString().c_str());
option.put("grasp_model_file", rf.findFile("grasp_model_file").c_str());
option.put("hand_sequences_file", rf.findFile("hand_sequences_file").c_str());
// parsing arm dependent config options from config.ini file
Bottle &bArm = config.findGroup("arm_dependent");
getArmDependentOptions(bArm, graspOrien, graspDisp, dOffs, dLift, home_x);
cout << "***** Instantiating primitives for left arm" << endl;
action = new AFFACTIONPRIMITIVESLAYER(option);
if (!action->isValid()) {
delete action;
return false;
}
//createing YARP ports and connections
deque <string> q = action->getHandSeqList();
cout << "***** List of available hand sequence keys:" << endl;
for (size_t i = 0; i < q.size(); i++)
cout << q[i] << endl;
string fwslash = "/";
inPort.open((fwslash + name + "/in").c_str());
rpcPort.open((fwslash + name + "/rpc").c_str());
attach(rpcPort);
// check for graspCalibration
Model *model; action->getGraspModel(model);
if (model != NULL) {
if (!model->isCalibrated()) {
Property prop("(finger all_parallel)");
model->calibrate(prop);
string fileName = rf.getHomeContextPath();
fileName += "/";
fileName += option.find("grasp_model_file").asString().c_str();
ofstream fout;
fout.open(fileName.c_str());
model->toStream(fout);
fout.close();
}
}
return true;
}
int main(int argc, char **argv) {
using namespace yarp::os;
ResourceFinder resourceFinder = ResourceFinder::getResourceFinderSingleton();
resourceFinder.configure(argc, argv);
if (resourceFinder.check("help")) {
std::cout<< "Possible parameters" << std::endl << std::endl;
std::cout<< "\t--context :Where to find a user defined .ini file within $ICUB_ROOT/app e.g. /adaptiveControl/conf" << std::endl;
std::cout<< "\t--robotURDFFile :URDF file name" << std::endl;
std::cout<< "\t--comDes :Desired CoM of the robot." << std::endl;
std::cout<< "\t--qDes :Desired joint positions of the robot." << std::endl;
std::cout<< "\t--feetInSupport :left, right or both" << std::endl;
std::cout<< "\t--jointMapping :[optional]ordered list of joints name which should be used in the optimization. Size must match size of qDes. If missing all joints are assumed" << std::endl;
return 0;
}
//read model file name
std::string filename = resourceFinder.check("robotURDFFile", Value("model.urdf"), "Checking for model URDF file").asString();
std::string filepath = resourceFinder.findFileByName(filename);
yInfo() << "Robot model found in " << filepath;
//read desired CoM
if (!resourceFinder.check("comDes", "Checking desired CoM parameter")) {
yError("Parameter comDes is required");
return -1;
}
Value &comDes = resourceFinder.find("comDes");
//Start checking: it should be a list of 3
if (!comDes.isList() || comDes.asList()->size() != 3) {
yError("Number of elements in comDes parameter is wrong. Expecting 3 values");
return -2;
}
yarp::sig::Vector desiredCoM(3);
Bottle* comList = comDes.asList();
desiredCoM[0] = comList->get(0).asDouble();
desiredCoM[1] = comList->get(1).asDouble();
desiredCoM[2] = comList->get(2).asDouble();
yInfo() << "Desired CoM is: " << desiredCoM.toString();
//read desired Joints configuration
if (!resourceFinder.check("qDes", "Checking desired joint configuration parameter")) {
yError("Parameter qDes is required");
return -1;
}
Value &qDes = resourceFinder.find("qDes");
if (!qDes.isList()) {
yError("qDes parameter should be a list");
return -2;
}
Bottle *qDesBottle = qDes.asList();
yarp::sig::Vector desiredJoints(qDesBottle->size());
for (unsigned i = 0; i < qDesBottle->size(); i++) {
desiredJoints[i] = qDesBottle->get(i).asDouble();
}
using namespace yarp::math;
yInfo() << "Desired joint configuration is (rad): " << desiredJoints.toString();
yInfo() << "Desired joint configuration is (deg): " << (desiredJoints * (180.0/M_PI)).toString();
//read initial Joints configuration
Value &qInit = resourceFinder.find("qInit");
Bottle *qInitBottle = 0;
yarp::sig::Vector initialJoints(0);
if (!qInit.isNull()) {
if (!qInit.isList()) {
yError("qInit parameter should be a list");
return -2;
}
qInitBottle = qInit.asList();
initialJoints.resize(qInitBottle->size());
for (unsigned i = 0; i < qInitBottle->size(); i++) {
initialJoints[i] = qInitBottle->get(i).asDouble();
}
yInfo() << "Initial joint configuration is: " << initialJoints.toString();
}
//read which foot/feet is in support
if (!resourceFinder.check("feetInSupport", "Checking feet in support parameter")) {
yError("Parameter feetInSupport is required");
return -1;
}
Value &feet = resourceFinder.find("feetInSupport");
std::string feetInSupport = feet.asString();
yInfo() << "Feet in support is: " << feetInSupport;
std::vector<std::string> mapping;
if (resourceFinder.check("jointMapping", "Checking joint mapping parameter")) {
Bottle *mappingBottle = resourceFinder.find("jointMapping").asList();
if (!mappingBottle || mappingBottle->size() != desiredJoints.size()) {
yError("jointMapping parameter list has wrong size.");
return -2;
}
mapping.reserve(mappingBottle->size());
for (int i = 0; i < mappingBottle->size(); i++) {
mapping.push_back(mappingBottle->get(i).asString());
}
yInfo() << "Joint mapping is (index => joint name): " << mapping;
} else {
yInfo("Joint mapping not specified");
}
OptimProblem problem;
if (!problem.initializeModel(filepath, mapping)) {
yError("Error initializing the robot model");
return -2;
}
problem.solveOptimization(desiredCoM, desiredJoints, feetInSupport);
return 0;
}
bool gOcraControllerModule::configure(ResourceFinder &rf)
{
//--------------------------READ FROM CONFIGURATION----------------------
if( rf.check("robot") )
{
robotName = rf.find("robot").asString().c_str();
std::cout <<"robot name is " << robotName << std::endl;
}
if( rf.check("local") )
{
moduleName = rf.find("local").asString().c_str();
}
if( rf.check("replay") )
{
replayJointAnglesPath = rf.find("replay").asString().c_str();
}
yarp::os::Property yarpWbiOptions;
// Get wbi options from the canonical file
if ( !rf.check("wbi_conf_file") )
{
fprintf(stderr, "[ERR] gOcraController: Impossible to open wholeBodyInterface: wbi_conf_file option missing");
}
std::string wbiConfFile = rf.findFile("wbi_conf_file");
yarpWbiOptions.fromConfigFile(wbiConfFile);
// Overwrite the robot parameter that could be present in wbi_conf_file
yarpWbiOptions.put("robot", robotName);
robotInterface = new yarpWholeBodyInterface(moduleName.c_str(), yarpWbiOptions);
IDList robotJoints;
std::string robotJointsListName = "ROBOT_MAIN_JOINTS";
if(!loadIdListFromConfig(robotJointsListName, yarpWbiOptions, robotJoints))
{
fprintf(stderr, "[ERR] gOcraController: Impossible to load wbiId joint list with name %s\n", robotJointsListName.c_str());
}
robotInterface->addJoints(robotJoints);
if( rf.check("uses_external_torque_control") )
{
/*
if(yarp::os::NetworkBase::exists(string("/jtc/info:o").c_str()))
printf ("The module jointTorqueControl is running. Proceeding with configuration of the interface...\n");
else{
printf ("ERROR [mdlStart] >> The jointTorqueControl module is not running... \n");
return false;
}
yarp::os::Value trueValue;
trueValue.fromString ("true");
( (yarpWholeBodyInterface*) robotInterface)->setActuactorConfigurationParameter (icubWholeBodyActuators::icubWholeBodyActuatorsUseExternalTorqueModule, trueValue);
( (yarpWholeBodyInterface*) robotInterface)->setActuactorConfigurationParameter (icubWholeBodyActuators::icubWholeBodyActuatorsExternalTorqueModuleAutoconnect, trueValue);
( (yarpWholeBodyInterface*) robotInterface)->setActuactorConfigurationParameter (icubWholeBodyActuators::icubWholeBodyActuatorsExternalTorqueModuleName, Value ("jtc"));
*/
}
// Make sure all the add* functions are done before the "init"
if(!robotInterface->init())
{
fprintf(stderr, "Error while initializing whole body interface. Closing module\n"); return false;
}
//--------------------------CTRL THREAD--------------------------
yarp::os::Property controller_options;
//If the printPeriod is found in the options, send it to the controller
if( rf.check("printPeriod") && rf.find("printPeriod").isDouble() )
{
controller_options.put("printPeriod",rf.find("printPeriod").asDouble());
}
ctrlThread = new gOcraControllerThread(moduleName, robotName, period, robotInterface, controller_options, replayJointAnglesPath);
if(!ctrlThread->start()){ fprintf(stderr, "Error while initializing control thread. Closing module.\n"); return false; }
fprintf(stderr,"gOcraController thread started\n");
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;
}
