virtual bool configure(ResourceFinder &rf)
{
this->rf=&rf;
if (rf.check("name"))
name=string("/")+rf.find("name").asString();
else
name="/ctpservice";
rpcPort.open(name+string("/")+rf.find("part").asString()+"/rpc");
attach(rpcPort);
Property portProp;
portProp.put("name", name);
portProp.put("robot", rf.find("robot"));
portProp.put("part", rf.find("part"));
if (!posPort.configure(portProp))
{
cerr<<"Error configuring position controller, check parameters"<<endl;
return false;
}
if (!posPort.connect())
{
cerr<<"Error cannot conenct to remote ports"<<endl;
return false;
}
cout << "***** connect to rpc port and type \"help\" for commands list" << endl;
thread.attachPosPort(&posPort);
if (!thread.start())
{
cerr<<"Thread did not start, queue will not work"<<endl;
}
velPort.open(name+string("/")+rf.find("part").asString()+"/vc:o");
velInitPort.open(name+string("/")+rf.find("part").asString()+"/vcInit:o");
velThread.attachVelPort(&velPort);
velThread.attachVelInitPort(&velInitPort);
cout << "Using parameters:" << endl << rf.toString() << endl;
velThread.attachRF(&rf);
cout << "***** starting the thread" << endl;
velThread.start();
return true;
}
//---------------------------------------------------------
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)
{
ConstString cascade;
ConstString nestedCascade;
if(!rf.check("cascade") /*|| !rf.check("nested-cascade")*/)
{
fprintf(stderr, "Could not find the cascade file. \n");
return false;
}
detector->strCascade = rf.getContextPath() + "/" + rf.find("cascade").asString();
printf("cascade: %s\n", detector->strCascade.c_str());
//detector->strNestedCascade= rf.find("nested-cascade").asString();
return detector->open(rf);
}
ObjectFeaturesThread::ObjectFeaturesThread ( int period, ResourceFinder rf ) : RateThread ( period )
{
//_explorationThreadPeriod = 20;
_moduleName = rf.check("moduleName", Value("object-exploration-server"),
"module name (string)").asString().c_str();
}
DispBlobberPort::DispBlobberPort( const string &_moduleName, ResourceFinder &rf)
{
this->moduleName = _moduleName;
moduleRF = &rf;
fprintf(stdout,"Parsing parameters...\n");
int imH = moduleRF->check("imH", Value(240)).asInt();
int imW = moduleRF->check("imW", Value(320)).asInt();
int bufferSize = moduleRF->check("bufferSize", Value(1)).asInt();
int margin = moduleRF->check("margin", Value(20)).asInt();
cropSize = 0;
if (rf.check("cropSize"))
{
Value &vCropSize=rf.find("cropSize");
if (!vCropSize.isString())
{
cropSize = vCropSize.asInt();
margin = 0; // not used in this case
}
}
// threshold of intensity of the image under which info is ignored
int backgroundThresh = moduleRF->check("backgroundThresh", Value(30)).asInt();
int minBlobSize = moduleRF->check("minBlobSize", Value(300)).asInt();
int gaussSize = moduleRF->check("gaussSize", Value(5)).asInt();
int imageThreshRatioLow = moduleRF->check("imageThreshRatioLow", Value(10)).asInt();
int imageThreshRatioHigh = moduleRF->check("imageThreshRatioHigh", Value(20)).asInt();
blobExtractor = NULL;
blobExtractor = new dispBlobber(imH, imW, bufferSize,
margin,
backgroundThresh,
minBlobSize, gaussSize,
imageThreshRatioLow, imageThreshRatioHigh);
}
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;
}
int getNumberUsedJoints(ResourceFinder &rf, int &n)
{
if (!rf.check("joints"))
{
yError("Missing option 'joints' in given config file\n");
return 0;
}
Value& joints = rf.find("joints");
Bottle *pJoints = joints.asList();
if (pJoints == 0)
{
yError("Error in option 'joints'\n");
return 0;
}
n = pJoints->size();
return 1;
}
int main(int argc, char *argv[])
{
ResourceFinder rf;
rf.setVerbose(true);
rf.setDefaultConfigFile( "conf/config.ini" ); //overridden by --from parameter
rf.setDefaultContext( "iKartWirelessDisplay" ); //overridden by --context parameter
rf.configure(argc,argv);
if (rf.check("help"))
{
cout << "Options:" << endl << endl;
cout << "\tNo options at the moment"<< endl;
return 0;
}
CtrlModule mod;
return mod.runModule(rf);
}
/**
* Main function.
*/
int main(int argc, char * argv[])
{
Network yarp;
ResourceFinder moduleRF;
moduleRF.setVerbose(false);
moduleRF.configure(argc,argv);
if (moduleRF.check("help"))
{
yInfo("");
yInfo("Options:");
yInfo(" --verbosity int: verbosity level (default 0).");
yInfo("");
return 0;
}
testSkinDynLib visTacRF;
return visTacRF.runModule(moduleRF);
}
bool configure(ResourceFinder &rf)
{
string mode=rf.check("mode",Value("physical")).asString().c_str();
Property option;
if (mode=="physical")
option.put("device","geomagicdriver");
else
{
option.put("device","hapticdeviceclient");
option.put("remote","/hapticdevice");
option.put("local","/client");
}
if (!driver.open(option))
return false;
driver.view(igeo);
return true;
}
virtual bool configure(ResourceFinder &rf)
{
Time::turboBoost();
//set up the rpc port
name=rf.check("name",Value("randObjSeg")).asString().c_str();
rpcPort = new Port;
string portRpcName="/"+name+"/rpc";
rpcPort->open(portRpcName.c_str());
attach(*rpcPort);
thr=new randObjSegThread(rf);
if (!thr->start())
{
delete thr;
return false;
}
return true;
}
//---------------------------------------------------------
void readBool(ResourceFinder &rf, string name, bool &v, bool vdefault)
{
if(rf.check(name.c_str()))
{
if((rf.find(name.c_str()).asString()=="true")||
(rf.find(name.c_str()).asString()=="yes")||
(rf.find(name.c_str()).asString()=="active")||
(rf.find(name.c_str()).asString()=="on"))
v = true;
else
v = false;
}
else
{
v = vdefault;
cout<<"Could not find value true/false for "<<name<<". "
<<"Setting default "<<((vdefault==true)?"true":"false")<<endl;
}
displayNameValue(name,((v==true)?"true":"false"));
}
int main(int argc, char * argv[])
{
ResourceFinder rf;
rf.setVerbose(true);
rf.setDefaultContext("wholeBodyDynamics");
rf.setDefaultConfigFile("wholeBodyDynamics.ini");
rf.configure(argc,argv);
if (rf.check("help"))
{
cout << "Options:" << endl << endl;
cout << "\t--context context: where to find the called resource (referred to $ICUB_ROOT/app: default wholeBodyDynamics)" << endl;
cout << "\t--from from: the name of the file.ini to be used for calibration" << endl;
cout << "\t--rate rate: the period used by the module. default: 10ms" << endl;
cout << "\t--robot robot: the robot name. default: iCub" << endl;
cout << "\t--local name: the prefix of the ports opened by the module. defualt: wholeBodyDynamics" << endl;
cout << "\t--autoconnect automatically connects the module ports to iCubInterface" << endl;
cout << "\t--no_legs this option disables the dynamics computation for the legs joints" << endl;
cout << "\t--headV2 use the model of the headV2" << endl;
cout << "\t--legsV2 use the model of legsV2" << endl;
cout << "\t--no_left_arm disables the left arm" << endl;
cout << "\t--no_right_arm disables the right arm" << endl;
cout << "\t--no_com disables the com computation" << endl;
cout << "\t--dummy_ft uses fake FT sensors (debug use only)" << endl;
cout << "\t--dumpvel dumps joint velocities and accelerations (debug use only)" << endl;
cout << "\t--experimental_com_vel enables com velocity computation (experimental)" << endl;
cout << "\t--auto_drift_comp enables automatic drift compensation (experimental, under debug)" << endl;
return 0;
}
Network yarp;
if (!yarp.checkNetwork())
{
fprintf(stderr, "Sorry YARP network does not seem to be available, is the yarp server available?\n");
return -1;
}
wholeBodyDynamics obs;
return obs.runModule(rf);
}
/**
* Main function.
*/
int main(int argc, char * argv[])
{
Network yarp;
ResourceFinder rf;
rf.setVerbose(true);
rf.setDefaultContext("periPersonalSpace");
rf.setDefaultConfigFile("virtualContactGeneration.ini");
rf.configure(argc,argv);
if (rf.check("help"))
{
yInfo(" ");
yInfo("Options:");
yInfo(" ");
yInfo(" --context path: where to find the called resource");
yInfo(" --from from: the name of the .ini file.");
yInfo(" --general::name name: the name of the module (default virtualContactGeneration).");
yInfo(" --general::robot robot: the name of the robot. Default icubSim.");
yInfo(" --general::rate rate: the period used by the thread. Default 100ms.");
yInfo(" --general::verbosity int: verbosity level (default 0).");
yInfo(" --general::type type: selection of fake contacts - right now 'random' is the only option available.");
yInfo(" --skin_parts::SKIN_RIGHT_UPPER_ARM on/off: if to enable the right upper arm or not.");
yInfo(" --skin_parts::SKIN_RIGHT_FOREARM on/off: if to enable the right forearm or not.");
yInfo(" --skin_parts::SKIN_RIGHT_HAND on/off: if to enable the right hand or not.");
yInfo(" --skin_parts::SKIN_LEFT_UPPER_ARM on/off: if to enable the left upper arm or not.");
yInfo(" --skin_parts::SKIN_LEFT_FOREARM on/off: if to enable the left forearm or not.");
yInfo(" --skin_parts::SKIN_LEFT_HAND on/off: if to enable the left hand or not.");
yInfo(" ");
return 0;
}
if (!yarp.checkNetwork())
{
printf("No Network!!!\n");
return -1;
}
virtualContactGeneration vCG;
return vCG.runModule(rf);
}
static string configureFinder(int argc, char *argv[],
string &moduleName,
ResourceFinder& finder) {
//string moduleName;
finder.setVerbose();
finder.setDefaultConfigFile("simulator.ini");
finder.setDefaultContext("simConfig");
finder.configure(argc, argv);
if ( finder.check( "name" ) ) {
moduleName = finder.find( "name" ).asString();
moduleName = "/" + moduleName;
cout << "NEW MODULE NAME " << moduleName << endl;
}
else {
moduleName ="/icubSim";
cout << "OLD MODULE NAME " << moduleName << endl;
}
return moduleName;
}
bool KP2JA::configure(ResourceFinder &rf)
{
string name=rf.check("name",Value("kinect2vector")).asString().c_str();
string clientName = name;
portVector.open( ("/" + clientName + "/state:o").c_str() );
clientName += "/kinect";
Property options;
options.put("carrier","tcp");
options.put("remote","kinectServer");
options.put("local",clientName.c_str());
options.put("verbosity", 0);
options.put("noRPC", 1); //To work with the datasetplayer only
if (!client.open(options))
return false;
return true;
}
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;
}
bool configure(ResourceFinder &rf)
{
context=rf.check("context",Value("periPersonalSpace")).asString();
from=rf.check("from",Value("ppsAggregEventsForiCubGui.ini")).asString();
name=rf.check("name",Value("ppsAggregEventsForiCubGui")).asString();
verbosity = rf.check("verbosity",Value(0)).asInt();
autoconnect=rf.check("autoconnect",Value("off")).asString()=="on"?true:false; // on | off
tactile=rf.check("tactile",Value("on")).asString()=="on"?true:false; // on | off
pps=rf.check("pps",Value("on")).asString()=="on"?true:false; // on | off
gain=rf.check("gain",Value(50.0)).asDouble();
yInfo("[ppsAggregEventsForiCubGui] Initial Parameters:");
yInfo("Context: %s \t From: %s \t Name: %s \t Verbosity: %d",
context.c_str(),from.c_str(),name.c_str(),verbosity);
yInfo("Autoconnect : %d \n tactile: %d \n pps: %d \n gain: %f \n",
autoconnect,tactile,pps,gain);
//open ports
if(tactile)
{
aggregSkinEventsInPort.open("/"+name+"/skin_events_aggreg:i");
}
if(pps)
{
aggregPPSeventsInPort.open("/"+name+"/pps_events_aggreg:i");
}
aggregEventsForiCubGuiPort.open("/"+name+"/contacts:o");
if (autoconnect)
{
Network::connect("/skinEventsAggregator/skin_events_aggreg:o",("/"+name+"/skin_events_aggreg:i").c_str());
Network::connect("/visuoTactileRF/pps_events_aggreg:o",("/"+name+"/pps_events_aggreg:i").c_str());
Network::connect(("/"+name+"/contacts:o").c_str(),"/iCubGui/forces");
}
return true;
}
/**
* Main function.
*/
int main(int argc, char * argv[])
{
Network yarp;
ResourceFinder moduleRF;
moduleRF.setVerbose(false);
moduleRF.setDefaultContext("periPersonalSpace");
moduleRF.setDefaultConfigFile("visuoTactileRF.ini");
moduleRF.configure(argc,argv);
if (moduleRF.check("help"))
{
yInfo(" ");
yInfo("Options:");
yInfo(" --context path: where to find the called resource (default periPersonalSpace).");
yInfo(" --from from: the name of the .ini file (default visuoTactileRF.ini).");
yInfo(" --name name: the name of the module (default visuoTactileRF).");
yInfo(" --robot robot: the name of the robot. Default icub.");
yInfo(" --rate rate: the period used by the thread. Default 50ms.");
yInfo(" --verbosity int: verbosity level (default 0).");
yInfo(" --modality string: which modality to use (either 1D or 2D, default 1D).");
yInfo(" --taxelsFile string: the file from which load and save taxels' PPS representations. Defaults:");
yInfo(" 'taxels1D.ini' if modality==1D");
yInfo(" 'taxels2D.ini' if modality==2D");
yInfo(" --rightForeArm, --rightHand, --leftForeArm, --leftHand flag: flag(s) to call if the module");
yInfo(" has to be run with either one of the 4 skin parts available.");
yInfo(" Not using any of them is equal to call all of them at the same time.");
yInfo(" ");
return 0;
}
if (!yarp.checkNetwork())
{
printf("No Network!!!\n");
return -1;
}
visuoTactileRF visTacRF;
return visTacRF.runModule(moduleRF);
}
int main(int argc, char *argv[])
{
Network yarp;
ResourceFinder rf;
rf.setVerbose(true);
rf.configure(argc,argv);
if (rf.check("help"))
{
yInfo() << "Options:";
yInfo() << "\t--name port: service port name (default: /dump)";
yInfo() << "\t--connect port: name of the port to connect the dumper to at launch time";
yInfo() << "\t--dir name: provide explicit name of storage directory";
yInfo() << "\t--overwrite : overwrite pre-existing storage directory";
#ifdef ADD_VIDEO
yInfo() << "\t--type type: type of the data to be dumped [bottle(default), image, image_jpg, video]";
yInfo() << "\t--addVideo : produce video as well (if image is selected)";
yInfo() << "\t--videoType ext: produce video of specified container type [mkv(default), avi]";
#else
yInfo() << "\t--type type: type of the data to be dumped [bottle(default), image, image_jpg]";
#endif
yInfo() << "\t--downsample n: downsample rate (default: 1 => downsample disabled)";
yInfo() << "\t--rxTime : dump the receiver time instead of the sender time";
yInfo() << "\t--txTime : dump the sender time straightaway";
yInfo();
return 0;
}
if (!yarp.checkNetwork())
{
yError()<<"YARP server not available!";
return 1;
}
DumpModule mod;
return mod.runModule(rf);
}
bool configure(ResourceFinder & rf){
string moduleName;
string inPortName;
string outPortName;
moduleName = rf.check("name", Value("FOEFinder"), "module name (String)").asString();
setName(moduleName.c_str());
//open input BufferedPort
inPortName = "/";
inPortName += getName();
inPortName += "/vels:i";
if (!vGrabber.open(inPortName.c_str())){
cerr << getName() << ": Sorry. Unable to open input port" << inPortName << endl;
return false;
}
vGrabber.useCallback();
//open output Port
outPortName = "/";
outPortName += getName();
outPortName += "/FOEMap:o";
if (!outPort.open(outPortName.c_str()) ){
cerr << getName() << "" << outPortName << endl;
return false;
}
foeFinder.setOutPort(&outPort);
inPortName = "/";
inPortName += getName();
inPortName += "/handler";
handlerPort.open(inPortName.c_str());
attach(handlerPort);
ts = 0;
return true;
}
bool configure(ResourceFinder &rf)
{
x = 0;
y = 0;
draw = false;
moduleName = rf.check("name",Value("cameraAlign"), "module name (string)").asString();
setName(moduleName.c_str());
coordPortName = "/" + moduleName + "/coord:i";
coordPort.open( coordPortName.c_str() );
inPortName = "/" + moduleName + "/image:i";
imageInPort.open( inPortName.c_str() );
outPortName = "/" + moduleName + "/image:o";
imageOutPort.open( outPortName.c_str() );
imageInPort.setReader(*this);
return true;
}
int main (int argc, char * argv[])
{
//Creating and preparing the Resource Finder
ResourceFinder rf;
rf.setVerbose(true);
rf.setDefaultConfigFile("default.ini"); //default config file name.
rf.setDefaultContext(DEFAULT_YARP_CONTEXT); //when no parameters are given to the module this is the default context
rf.configure(argc,argv);
if (rf.check("help"))
{
cout<< "Possible parameters" << endl << endl;
cout<< "\t--context :Where to find an user defined .ini file within $ICUB_ROOT/app e.g. /" << DEFAULT_YARP_CONTEXT << "conf" <<endl;
cout<< "\t--from :Name of the file.ini to be used for calibration." <<endl;
cout<< "\t--rate :Period used by the module. Default set to 10ms." <<endl;
cout<< "\t--robot :Robot name. Set to icub by default." <<endl;
cout<< "\t--name :Prefix of the ports opened by the module. Set to the module name by default, i.e. wholeBodyDynamicsTree." <<endl;
cout<< "\t--headV1/headV2 :Version of the head." <<endl;
cout<< "\t--legsV1/legsV2 :Version of the legs." <<endl;
cout<< "\t--feetV1/feetV2 :Version of the feet." <<endl;
cout<< "\t--enable_w0_dw0/disable_w0_dw0 :Enable/disable use of angular velocity and acceleration measured from the IMU (default: disabled)." << endl;
cout<< "\t--autoconnect :Autoconnect torques port for low-level torque feedback. " << endl;
return 0;
}
Network yarp;
if (!yarp.checkNetwork())
{
fprintf(stderr,"Sorry YARP network is not available\n");
return -1;
}
//Creating the module
staticInertiaIdentificationModule module;
return module.runModule(rf);
}
int main(int argc, char *argv[])
{
Network yarp;
YARP_REGISTER_DEVICES(icubmod)
ResourceFinder rf;
rf.setDefaultContext("depth2kin");
rf.setDefaultConfigFile("config.ini");
rf.setDefault("calibrationFile","calibration_data.ini");
rf.configure(argc,argv);
int test=rf.check("test",Value(-1)).asInt();
if (test<0)
{
if (!yarp.checkNetwork())
{
yError("YARP server not available!");
return -1;
}
}
CalibModule mod;
return mod.runModule(rf);
}
int main(int argc, char** argv) {
ResourceFinder rf;
rf.setVerbose(true);
rf.setDefaultContext("cvFaces");
rf.setDefaultConfigFile("cvFaces.ini");
rf.configure(argc, argv);
teo::CvFaces mod;
if(rf.check("help")) {
return mod.runModule(rf);
}
printf("Run \"%s --help\" for options.\n",argv[0]);
printf("%s checking for yarp network... ",argv[0]);
fflush(stdout);
Network yarp;
if (!yarp.checkNetwork()) {
fprintf(stderr,"[fail]\n%s found no yarp network (try running \"yarpserver &\"), bye!\n",argv[0]);
return -1;
} else printf("[ok]\n");
return mod.runModule(rf);
}
bool configure(ResourceFinder &rf)
{
int verbosity=rf.check("verbosity",Value(0)).asInt();
int period=rf.check("period",Value(20)).asInt();
string device=rf.check("device",Value("cartesiancontrollerclient")).asString().c_str();
string name=rf.check("name",Value("d4c_server")).asString().c_str();
string robot=rf.check("robot",Value("icub")).asString().c_str();
string part=rf.check("part",Value("both_arms")).asString().c_str();
Property options;
options.put("verbosity",verbosity);
options.put("period",period);
options.put("device",device.c_str());
options.put("name",name.c_str());
options.put("robot",robot.c_str());
options.put("part",part.c_str());
return server.open(options);
}
void iCubVersionFromRf(ResourceFinder & rf, iCub::iDynTree::iCubTree_version_tag & icub_version)
{
//Checking iCub parts version
/// \todo this part should be replaced by a more general way of accessing robot parameters
/// namely urdf for structure parameters and robotInterface xml (or runtime interface) to get available sensors
icub_version.head_version = 2;
if( rf.check("headV1") ) {
icub_version.head_version = 1;
}
if( rf.check("headV2") ) {
icub_version.head_version = 2;
}
icub_version.legs_version = 2;
if( rf.check("legsV1") ) {
icub_version.legs_version = 1;
}
if( rf.check("legsV2") ) {
icub_version.legs_version = 2;
}
/// \note if feet_version are 2, the presence of FT sensors in the feet is assumed
icub_version.feet_ft = true;
if( rf.check("feetV1") ) {
icub_version.feet_ft = false;
}
if( rf.check("feetV2") ) {
icub_version.feet_ft = true;
}
if( rf.check("urdf") )
{
icub_version.uses_urdf = true;
icub_version.urdf_file = rf.find("urdf").asString().c_str();
}
}
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);
}
bool configure(ResourceFinder &rf)
{
printf(" Starting Configure\n");
this->rf=&rf;
//****************** PARAMETERS ******************
robot = rf.check("robot",Value("icub")).asString().c_str();
name = rf.check("name",Value("demoINNOROBO")).asString().c_str();
arm = rf.check("arm",Value("right_arm")).asString().c_str();
verbosity = rf.check("verbosity",Value(0)).asInt();
rate = rf.check("rate",Value(0.5)).asDouble();
if (arm!="right_arm" && arm!="left_arm")
{
printMessage(0,"ERROR: arm was set to %s, putting it to right_arm\n",arm.c_str());
arm="right_arm";
}
printMessage(1,"Parameters correctly acquired\n");
printMessage(1,"Robot: %s \tName: %s \tArm: %s\n",robot.c_str(),name.c_str(),arm.c_str());
printMessage(1,"Verbosity: %i \t Rate: %g \n",verbosity,rate);
//****************** DETECTOR ******************
certainty = rf.check("certainty", Value(10.0)).asInt();
strCascade = rf.check("cascade", Value("haarcascade_frontalface_alt.xml")).asString().c_str();
detectorL=new Detector();
detectorL->open(certainty,strCascade);
detectorR=new Detector();
detectorR->open(certainty,strCascade);
printMessage(1,"Detectors opened\n");
//****************** DRIVERS ******************
Property optionArm("(device remote_controlboard)");
optionArm.put("remote",("/"+robot+"/"+arm).c_str());
optionArm.put("local",("/"+name+"/"+arm).c_str());
if (!drvArm.open(optionArm))
{
printf("Position controller not available!\n");
return false;
}
Property optionCart("(device cartesiancontrollerclient)");
optionCart.put("remote",("/"+robot+"/cartesianController/"+arm).c_str());
optionCart.put("local",("/"+name+"/cart/").c_str());
if (!drvCart.open(optionCart))
{
printf("Cartesian controller not available!\n");
// return false;
}
Property optionGaze("(device gazecontrollerclient)");
optionGaze.put("remote","/iKinGazeCtrl");
optionGaze.put("local",("/"+name+"/gaze").c_str());
if (!drvGaze.open(optionGaze))
{
printf("Gaze controller not available!\n");
// return false;
}
// quitting conditions
bool andArm=drvArm.isValid()==drvCart.isValid()==drvGaze.isValid();
if (!andArm)
{
printMessage(0,"Something wrong occured while configuring drivers... quitting!\n");
return false;
}
drvArm.view(iencs);
drvArm.view(iposs);
drvArm.view(ictrl);
drvArm.view(iimp);
drvCart.view(iarm);
drvGaze.view(igaze);
iencs->getAxes(&nEncs);
iimp->setImpedance(0, 0.4, 0.03);
iimp->setImpedance(1, 0.35, 0.03);
iimp->setImpedance(2, 0.35, 0.03);
iimp->setImpedance(3, 0.2, 0.02);
iimp->setImpedance(4, 0.2, 0.00);
ictrl -> setImpedancePositionMode(0);
ictrl -> setImpedancePositionMode(1);
ictrl -> setImpedancePositionMode(2);
ictrl -> setImpedancePositionMode(3);
ictrl -> setImpedancePositionMode(2);
ictrl -> setImpedancePositionMode(4);
igaze -> storeContext(&contextGaze);
igaze -> setSaccadesStatus(false);
igaze -> setNeckTrajTime(0.75);
igaze -> setEyesTrajTime(0.5);
iarm -> storeContext(&contextCart);
printMessage(1,"Drivers opened\n");
//****************** PORTS ******************
imagePortInR -> open(("/"+name+"/imageR:i").c_str());
imagePortInL -> open(("/"+name+"/imageL:i").c_str());
imagePortOutR.open(("/"+name+"/imageR:o").c_str());
imagePortOutL.open(("/"+name+"/imageL:o").c_str());
portOutInfo.open(("/"+name+"/info:o").c_str());
if (robot=="icub")
{
Network::connect("/icub/camcalib/left/out",("/"+name+"/imageL:i").c_str());
Network::connect("/icub/camcalib/right/out",("/"+name+"/imageR:i").c_str());
}
else
{
Network::connect("/icubSim/cam/left",("/"+name+"/imageL:i").c_str());
Network::connect("/icubSim/cam/right",("/"+name+"/imageR:i").c_str());
}
Network::connect(("/"+name+"/imageL:o").c_str(),"/demoINN/left");
Network::connect(("/"+name+"/imageR:o").c_str(),"/demoINN/right");
Network::connect(("/"+name+"/info:o").c_str(),"/iSpeak");
rpcClnt.open(("/"+name+"/rpc:o").c_str());
rpcSrvr.open(("/"+name+"/rpc:i").c_str());
attach(rpcSrvr);
printMessage(0,"Configure Finished!\n");
return true;
}
int main(int argc, char *argv[])
{
yarp::os::Network yarp;
ResourceFinder rf;
rf.setVerbose(true);
rf.setDefaultContext("react-control");
rf.setDefaultConfigFile("reactController-sim.ini");
rf.configure(argc,argv);
int verbosity = rf.check("verbosity",Value(0)).asInt();
double sim_time=rf.check("sim-time",Value(10.0)).asDouble();
double motor_tau=rf.check("motor-tau",Value(0.0)).asDouble(); //motor transfer function
string avoidance_type=rf.check("avoidance-type",Value("tactile")).asString(); //none | visuo | tactile
bool visuo_scaling_by_sNorm=rf.check("visuo-scaling-snorm",Value("off")).asString()=="on"?true:false; // on | off
string target_type=rf.check("target-type",Value("moving-circular")).asString(); // moving-circular | static
string obstacle_type=rf.check("obstacle-type",Value("falling")).asString(); //falling | static
yInfo("Starting with the following parameters: \n verbosity: %d \n sim-time: %f \n motor-tau: %f \n avoidance-type: %s \n target-type: %s \n obstacle-type: %s \n",verbosity,sim_time,motor_tau,avoidance_type.c_str(),target_type.c_str(),obstacle_type.c_str());
if (!yarp.checkNetwork())
{
yError("No Network!!!");
return -1;
}
iCubArm arm("left");
iKinChain &chain=*arm.asChain();
chain.releaseLink(0); //releasing torso links that are blocked by default
chain.releaseLink(1);
chain.releaseLink(2);
Vector q0(chain.getDOF(),0.0);
q0[3]=-25.0; q0[4]=20.0; q0[6]=50.0; //setting shoulder position
chain.setAng(CTRL_DEG2RAD*q0);
Matrix lim(chain.getDOF(),2); //joint position limits, in degrees
Matrix v_lim(chain.getDOF(),2); //joint velocity limits, in degrees/s
for (size_t r=0; r<chain.getDOF(); r++)
{
lim(r,0)=CTRL_RAD2DEG*chain(r).getMin();
lim(r,1)=CTRL_RAD2DEG*chain(r).getMax();
v_lim(r,0)=-50.0;
v_lim(r,1)=+50.0;
}
v_lim(1,0)=v_lim(1,1)=0.0; // disable torso roll
Ipopt::SmartPtr<Ipopt::IpoptApplication> app=new Ipopt::IpoptApplication;
app->Options()->SetNumericValue("tol",1e-6);
app->Options()->SetStringValue("mu_strategy","adaptive");
app->Options()->SetIntegerValue("max_iter",10000);
app->Options()->SetNumericValue("max_cpu_time",0.05);
app->Options()->SetStringValue("nlp_scaling_method","gradient-based");
app->Options()->SetStringValue("hessian_approximation","limited-memory");
app->Options()->SetStringValue("derivative_test","none");
app->Options()->SetIntegerValue("print_level",0);
app->Initialize();
Ipopt::SmartPtr<ControllerNLP> nlp=new ControllerNLP(chain);
AvoidanceHandlerAbstract *avhdl;
if (avoidance_type=="none")
avhdl=new AvoidanceHandlerAbstract(arm);
else if (avoidance_type=="visuo")
avhdl=new AvoidanceHandlerVisuo(arm,visuo_scaling_by_sNorm);
else if (avoidance_type=="tactile")
avhdl=new AvoidanceHandlerTactile(arm);
else if (avoidance_type=="visuo-tactile")
avhdl=new AvoidanceHandlerVisuoTactile(arm,visuo_scaling_by_sNorm);
else
{
yError()<<"unrecognized avoidance type! exiting ...";
return 1;
}
yInfo()<<"Avoidance-Handler="<<avhdl->getType();
yInfo()<<"Avoidance Parameters="<<avhdl->getParameters().toString();
double dt=0.01;
double T=1.0;
nlp->set_dt(dt);
Motor motor(q0,lim,motor_tau,dt);
Vector v(chain.getDOF(),0.0);
Vector xee=chain.EndEffPosition();
//actual target
Vector xc(3); //center of target
xc[0]=-0.35;
xc[1]=0.0;
xc[2]=0.1;
double rt=.1; //target will be moving along circular trajectory with this radius
//if (target_type == "moving-circular") double rt=0.1;
if (target_type == "static")
rt=0.0; //static target will be "moving" along a trajectory with 0 radius
minJerkTrajGen target(xee,dt,T); //target for end-effector
Vector xo(3); //obstacle position
Vector vo(3,0.0); //obstacle velocity
Obstacle obstacle(xo,0.07,vo,dt);
if (obstacle_type == "falling"){
xo[0]=-0.3;
xo[1]=0.0;
xo[2]=0.4;
vo[2]=-0.1;
obstacle.setPosition(xo);
obstacle.setVelocity(vo);
}
else if(obstacle_type == "static"){
xo[0]=-0.35;
xo[1]=-0.05;
//xo[2]=0.04;
xo[2]=0.02;
obstacle.setPosition(xo);
obstacle.setRadius(0.04);
}
ofstream fout_param; //log parameters that stay constant during the simulation, but are important for analysis - e.g. joint limits
ofstream fout; //to log data every iteration
fout_param.open("param.log");
fout.open("data.log");
fout_param<<chain.getDOF()<<" ";
for (size_t i=0; i<chain.getDOF(); i++)
{
fout_param<<CTRL_RAD2DEG*chain(i).getMin()<<" ";
fout_param<<CTRL_RAD2DEG*chain(i).getMax()<<" ";
}
for (size_t i=0; i<chain.getDOF(); i++)
{
fout_param<<v_lim(i,0)<<" ";
fout_param<<v_lim(i,1)<<" ";
}
std::signal(SIGINT,signal_handler);
for (double t=0.0; t<sim_time; t+=dt)
{
//printf("\n**************************************\n main loop:t: %f s \n",t);
Vector xd=xc; //target moving along circular trajectory
xd[1]+=rt*cos(2.0*M_PI*0.3*t);
xd[2]+=rt*sin(2.0*M_PI*0.3*t);
target.computeNextValues(xd);
Vector xr=target.getPos(); //target for end-effector - from minJerkTrajGen
xo=obstacle.move();
avhdl->updateCtrlPoints();
Matrix VLIM=avhdl->getVLIM(obstacle,v_lim);
nlp->set_xr(xr);
nlp->set_v_lim(VLIM); //VLIM in deg/s; set_v_lim converts to radians/s
nlp->set_v0(v);
nlp->init();
Ipopt::ApplicationReturnStatus status=app->OptimizeTNLP(GetRawPtr(nlp));
v=nlp->get_result(); //updating the joint velocities with the output from optimizer
xee=chain.EndEffPosition(CTRL_DEG2RAD*motor.move(v));
if (verbosity>0){ //this is probably not the right way to do it
yInfo()<<" t [s] = "<<t;
yInfo()<<" v [deg/s] = ("<<v.toString(3,3)<<")";
yInfo()<<" |xr-xee| [m] = "<<norm(xr-xee);
yInfo()<<"";
}
ostringstream strVLIM;
for (size_t i=0; i<VLIM.rows(); i++)
strVLIM<<VLIM.getRow(i).toString(3,3)<<" ";
ostringstream strCtrlPoints;
deque<Vector> ctrlPoints=avhdl->getCtrlPointsPosition();
for (size_t i=0; i<ctrlPoints.size(); i++)
strCtrlPoints<<ctrlPoints[i].toString(3,3)<<" ";
fout.setf(std::ios::fixed, std::ios::floatfield);
fout<<setprecision(3);
fout<<t<<" "<<
xd.toString(3,3)<<" "<<
obstacle.toString()<<" "<<
xr.toString(3,3)<<" "<<
v.toString(3,3)<<" "<<
(CTRL_RAD2DEG*chain.getAng()).toString(3,3)<<" "<<
strVLIM.str()<<
strCtrlPoints.str()<<
endl;
//in columns on the output for 10 DOF case: 1:time, 2:4 target, 5:8 obstacle, 9:11 end-eff target, 12:21 joint velocities, 22:31 joint pos, 32:51 joint vel limits set by avoidance handler (joint1_min, joint1_max, joint2_min, ...., joint10_min, joint10_max) , 52:end - current control points' (x,y,z) pos in Root FoR for each control point
if (gSignalStatus==SIGINT)
{
yWarning("SIGINT detected: exiting ...");
break;
}
}
fout.close();
delete avhdl;
return 0;
}
