void testBasics() {
report(0,"testing the basics of RF...");
ResourceFinder rf;
const char *fname0 = "_yarp_regression_test.ini";
const char *fname1 = "_yarp_regression_test_rf1.txt";
const char *fname2 = "_yarp_regression_test_rf2.txt";
// create some test files
{
FILE *fout = fopen(fname0,"w");
yAssert(fout!=NULL);
fprintf(fout,"style capability\n");
fprintf(fout,"capability_directory \".\"\n");
fprintf(fout,"default_capability \".\"\n");
fclose(fout);
fout = NULL;
fout = fopen(fname1,"w");
yAssert(fout!=NULL);
fprintf(fout,"alt %s\n", fname2);
fclose(fout);
fout = NULL;
fout = fopen(fname2,"w");
fprintf(fout,"x 14\n");
fclose(fout);
fout = NULL;
const char *argv[] = { "ignore",
"--_yarp_regression_test", ".",
"--from", fname1,
"--verbose", "0",
NULL };
int argc = 7;
rf.configure(argc,(char **)argv);
ConstString alt = rf.findFile("alt");
checkTrue(alt!="","found ini file");
rf.setDefault("alt2",fname2);
alt = rf.findFile("alt2");
checkTrue(alt!="","default setting worked");
rf.setDefault("alt3","_yarp_nonexistent.txt");
alt = rf.findFile("alt3");
checkTrue(alt=="","cannot find nonexistent files");
rf.setDefault("alt","_yarp_nonexistent.txt");
alt = rf.findFile("alt");
checkTrue(alt!="","default setting is safe");
checkTrue(rf.findPath()!="","existing path found");
alt=rf.findFileByName(fname1);
checkTrue(alt!="","found file by name");
}
}
abmReasoning::abmReasoning(ResourceFinder &rf)
{
iFunction = new abmReasoningFunction(rf);
savefile = rf.findFileByName("saveRequest.txt");
opcNameTable.push_back(EFAA_OPC_ENTITY_TAG);
opcNameTable.push_back(EFAA_OPC_ENTITY_RELATION);
opcNameTable.push_back(EFAA_OPC_ENTITY_OBJECT);
opcNameTable.push_back(EFAA_OPC_ENTITY_RTOBJECT);
opcNameTable.push_back(EFAA_OPC_ENTITY_AGENT);
opcNameTable.push_back(EFAA_OPC_ENTITY_ADJECTIVE);
bReady = false;
}
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 PlaybackLocomotion::configure(ResourceFinder &rf) {
int ptModeMs = rf.check("ptModeMs",yarp::os::Value(DEFAULT_PT_MODE_MS),"PT mode miliseconds").asInt();
CD_INFO("Using ptModeMs: %d (default: %d).\n",ptModeMs,int(DEFAULT_PT_MODE_MS));
playbackThread.hold = rf.check("hold");
//-- Open file for reading.
std::string fileName = rf.check("file",Value(DEFAULT_FILE_NAME),"file name").asString();
CD_INFO("Using file: %s (default: " DEFAULT_FILE_NAME ").\n",fileName.c_str());
playbackThread.ifs.open( fileName.c_str() );
if( ! playbackThread.ifs.is_open() ) {
CD_ERROR("Could not open file: %s.\n",fileName.c_str());
return false;
}
CD_SUCCESS("Opened file: %s.\n",fileName.c_str());
//-- left leg --
std::string leftLegIni = rf.findFileByName("../locomotion/leftLeg.ini");
yarp::os::Property leftLegOptions;
if (! leftLegOptions.fromConfigFile(leftLegIni) ) { //-- Put first because defaults to wiping out.
CD_ERROR("Could not configure from \"leftLeg.ini\".\n");
return false;
}
CD_SUCCESS("Configured left leg from %s.\n",leftLegIni.c_str());
leftLegOptions.put("name","/teo/leftLeg");
leftLegOptions.put("device","CanBusControlboard");
leftLegOptions.put("ptModeMs",ptModeMs);
if (rf.check("home")) leftLegOptions.put("home",1);
if (rf.check("reset")) leftLegOptions.put("reset",1);
leftLegDevice.open(leftLegOptions);
if (!leftLegDevice.isValid()) {
CD_ERROR("leftLegDevice instantiation not worked.\n");
CD_ERROR("Be sure CMake \"ENABLE_LocomotionYarp_CanBusControlboard\" variable is set \"ON\"\n");
CD_ERROR("\"SKIP_CanBusControlboard is set\" --> should be --> \"ENABLE_CanBusControlboard is set\"\n");
// robotDevice.close(); // un-needed?
return false;
}
//-- right leg --
std::string rightLegIni = rf.findFileByName("../locomotion/rightLeg.ini");
yarp::os::Property rightLegOptions;
if (! rightLegOptions.fromConfigFile(rightLegIni) ) { //-- Put first because defaults to wiping out.
CD_ERROR("Could not configure from \"rightLeg.ini\".\n");
return false;
}
CD_SUCCESS("Configured right leg from %s.\n",rightLegIni.c_str());
rightLegOptions.put("name","/teo/rightLeg");
rightLegOptions.put("device","CanBusControlboard");
rightLegOptions.put("ptModeMs",ptModeMs);
if (rf.check("home")) rightLegOptions.put("home",1);
if (rf.check("reset")) rightLegOptions.put("reset",1);
rightLegDevice.open(rightLegOptions);
if (!rightLegDevice.isValid()) {
CD_ERROR("rightLegDevice instantiation not worked.\n");
CD_ERROR("Be sure CMake \"ENABLE_LocomotionYarp_CanBusControlboard\" variable is set \"ON\"\n");
CD_ERROR("\"SKIP_CanBusControlboard is set\" --> should be --> \"ENABLE_CanBusControlboard is set\"\n");
// robotDevice.close(); // un-needed?
return false;
}
//-- Configure the thread.
//-- Obtain manipulator interfaces.
if ( ! leftLegDevice.view( playbackThread.leftLegPos ) ) {
CD_ERROR("Could not obtain leftLegPos.\n");
return false;
}
CD_SUCCESS("Obtained leftLegPos.\n");
if ( ! leftLegDevice.view( playbackThread.leftLegPosDirect ) ) {
CD_ERROR("Could not obtain leftLegPosDirect.\n");
return false;
}
CD_SUCCESS("Obtained leftLegPosDirect.\n");
if ( ! rightLegDevice.view( playbackThread.rightLegPos ) ) {
CD_ERROR("Could not obtain leftLegPos.\n");
return false;
}
CD_SUCCESS("Obtained rightLegPos.\n");
if ( ! rightLegDevice.view( playbackThread.rightLegPosDirect ) ) {
CD_ERROR("Could not obtain rightLegPosDirect.\n");
return false;
}
CD_SUCCESS("Obtained rightLegPosDirect.\n");
//-- Do stuff.
playbackThread.leftLegPos->getAxes( &(playbackThread.leftLegNumMotors) );
playbackThread.rightLegPos->getAxes( &(playbackThread.rightLegNumMotors) );
CD_INFO("setPositionDirectMode...\n");
playbackThread.leftLegPosDirect->setPositionDirectMode();
playbackThread.rightLegPosDirect->setPositionDirectMode();
//-- Start the thread.
CD_INFO("Start thread...\n");
playbackThread.start();
return true;
}
bool speechInteraction::configure(ResourceFinder &rf)
{
moduleName = rf.check("name", Value("speechInteraction"), "module name (string)").asString();
setName(moduleName.c_str());
Property config;
config.fromConfigFile(rf.findFile("from").c_str());
Bottle &nGeneral = config.findGroup("number_of_vocabs");
nVocabs = nGeneral.find("nvocabs").asInt();
Bottle &speechGeneral = config.findGroup("speech_engine");
speechType = speechGeneral.find("engine").asInt();
cout << "Engine type: " << speechType << endl;
Bottle &inputGeneral = config.findGroup("input_vocabs");
string findVocab = "vocab_";
ostringstream convert;
cout << "INPUT VOCABS" << endl;
for( int i = 0; i < nVocabs; i++ )
{
convert << (i+1);
findVocab = findVocab + convert.str();
inputVocabs.push_back(inputGeneral.find(findVocab).asString().c_str());
cout << findVocab << ": " << inputGeneral.find(findVocab).asString().c_str() << endl;
findVocab = "vocab_";
convert.str("");
}
Bottle &outputGeneral = config.findGroup("output_vocabs");
cout << "OUTPUT VOCABS" << endl;
for( int i = 0; i < nVocabs; i++ )
{
convert << (i+1);
findVocab = findVocab + convert.str();
outputVocabs.push_back(outputGeneral.find(findVocab).asString().c_str());
cout << findVocab << ": " << outputGeneral.find(findVocab).asString().c_str() << endl;
findVocab = "vocab_";
convert.str("");
}
Bottle &portsGeneral = config.findGroup("ports");
inputPortName = portsGeneral.find("input_port").asString().c_str();
outputPortName = portsGeneral.find("output_port").asString().c_str();
triggerBehaviourPortName = portsGeneral.find("behaviour_port").asString().c_str();
inputOpen = inputPort.open(inputPortName.c_str());
outputOpen = outputPort.open(outputPortName.c_str());
behaviourPortOpen = triggerBehaviourPort.open(triggerBehaviourPortName.c_str());
if(!outputOpen || !inputOpen || !behaviourPortOpen)
{
cout << "Could not open ports. Exiting" << endl;
return false;
}
if( speechType == 1 )
{
cout << "DEBUG!!!!" << endl;
//moduleName = rf.check("name", Value("speechInteraction"), "module name (string)").asString();
//setName(moduleName.c_str());
GrammarAskNamePerson = rf.findFileByName("GrammarAskNamePerson.xml");
yInfo() << moduleName << " : finding configuration files...";
cout << "Grammar file: " << GrammarAskNamePerson;
//Create an iCub Client and check that all dependencies are here before starting
//bool isRFVerbose = false;
iCub = new ICubClient(moduleName,"speechInteraction","client.ini",true);
iCub->opc->isVerbose = false;
if (!iCub->connect())
{
yInfo() << " iCubClient : Some dependencies are not running...";
Time::delay(1.0);
}
rpc.open(("/" + moduleName +"/rpc").c_str());
attach(rpc);
if (!iCub->getRecogClient())
{
yInfo() << " WARNING SPEECH RECOGNIZER NOT CONNECTED";
}
}
return true;
}
bool PlaybackManipulation::configure(ResourceFinder &rf) {
int ptModeMs = rf.check("ptModeMs",yarp::os::Value(DEFAULT_PT_MODE_MS),"PT mode miliseconds").asInt();
CD_INFO("Using ptModeMs: %d (default: %d).\n",ptModeMs,int(DEFAULT_PT_MODE_MS));
playbackThread.hold = rf.check("hold");
//-- Open file for logging.
if ( rf.check("log") ) {
playbackThread.log = true;
std::string fileName = rf.check("log",yarp::os::Value(DEFAULT_LOG_NAME),"file name").asString();
CD_INFO("Using log file: %s (default: " DEFAULT_LOG_NAME ").\n",fileName.c_str());
playbackThread.logFilePtr = ::fopen (fileName.c_str(),"w");
if( ! playbackThread.logFilePtr ) {
CD_PERROR("Could not open log file: %s.\n",fileName.c_str());
return false;
}
CD_SUCCESS("Opened log file: %s.\n",fileName.c_str());
} else {
playbackThread.log = false;
}
//-- Open file for reading.
std::string fileName = rf.check("file",Value(DEFAULT_FILE_NAME),"file name").asString();
CD_INFO("Using read file: %s (default: " DEFAULT_FILE_NAME ").\n",fileName.c_str());
playbackThread.ifs.open( fileName.c_str() );
if( ! playbackThread.ifs.is_open() ) {
CD_ERROR("Could not open read file: %s.\n",fileName.c_str());
return false;
}
CD_SUCCESS("Opened file: %s.\n",fileName.c_str());
std::string allIni = rf.findFileByName("../launchManipulation/launchManipulation.ini");
yarp::os::Property allOptions;
if (! allOptions.fromConfigFile(allIni) ) { //-- Put first because defaults to wiping out.
CD_ERROR("Could not configure from \"launchManipulation.ini\".\n");
return false;
}
//CD_SUCCESS("Configuring from %s.\n",allOptions.toString().c_str());
//-- /dev/can0 --
yarp::os::Bottle devCan0 = allOptions.findGroup("devCan0");
CD_DEBUG("%s\n",devCan0.toString().c_str());
yarp::os::Property optionsDevCan0;
optionsDevCan0.fromString(devCan0.toString());
optionsDevCan0.put("device","CanBusControlboard");
optionsDevCan0.put("ptModeMs",ptModeMs);
if (rf.check("home")) optionsDevCan0.put("home",1);
if (rf.check("reset")) optionsDevCan0.put("reset",1);
leftArmDevice.open(optionsDevCan0);
if (!leftArmDevice.isValid()) {
CD_ERROR("leftArmDevice instantiation not worked.\n");
CD_ERROR("Be sure CMake \"ENABLE_BodyYarp_CanBusControlboard\" variable is set \"ON\"\n");
CD_ERROR("\"SKIP_CanBusControlboard is set\" --> should be --> \"ENABLE_CanBusControlboard is set\"\n");
// robotDevice.close(); // un-needed?
return false;
}
//-- /dev/can1 --
yarp::os::Bottle devCan1 = allOptions.findGroup("devCan1");
CD_DEBUG("%s\n",devCan1.toString().c_str());
yarp::os::Property optionsDevCan1;
optionsDevCan1.fromString(devCan1.toString());
optionsDevCan1.put("device","CanBusControlboard");
optionsDevCan1.put("ptModeMs",ptModeMs);
if (rf.check("home")) optionsDevCan1.put("home",1);
if (rf.check("reset")) optionsDevCan1.put("reset",1);
rightArmDevice.open(optionsDevCan1);
if (!rightArmDevice.isValid()) {
CD_ERROR("rightArmDevice instantiation not worked.\n");
CD_ERROR("Be sure CMake \"ENABLE_BodyYarp_CanBusControlboard\" variable is set \"ON\"\n");
CD_ERROR("\"SKIP_CanBusControlboard is set\" --> should be --> \"ENABLE_CanBusControlboard is set\"\n");
// robotDevice.close(); // un-needed?
return false;
}
//-- Configure the thread.
//-- Obtain manipulator interfaces.
if ( ! leftArmDevice.view( playbackThread.leftArmPos ) ) {
CD_ERROR("Could not obtain leftArmPos.\n");
return false;
}
CD_SUCCESS("Obtained leftArmPos.\n");
if ( ! leftArmDevice.view( playbackThread.leftArmPosDirect ) ) {
CD_ERROR("Could not obtain leftArmPosDirect.\n");
return false;
}
CD_SUCCESS("Obtained leftArmPosDirect.\n");
if ( ! leftArmDevice.view( playbackThread.leftArmEncTimed ) ) {
CD_ERROR("Could not obtain leftArmEncTimed.\n");
return false;
}
CD_SUCCESS("Obtained leftArmEncTimed.\n");
if ( ! leftArmDevice.view( playbackThread.leftArmTorque ) ) {
CD_ERROR("Could not obtain leftArmTorque.\n");
return false;
}
CD_SUCCESS("Obtained leftArmTorque.\n");
if ( ! rightArmDevice.view( playbackThread.rightArmPos ) ) {
CD_ERROR("Could not obtain leftArmPos.\n");
return false;
}
CD_SUCCESS("Obtained rightArmPos.\n");
if ( ! rightArmDevice.view( playbackThread.rightArmPosDirect ) ) {
CD_ERROR("Could not obtain rightArmPosDirect.\n");
return false;
}
CD_SUCCESS("Obtained rightArmPosDirect.\n");
if ( ! rightArmDevice.view( playbackThread.rightArmEncTimed ) ) {
CD_ERROR("Could not obtain rightArmEncTimed.\n");
return false;
}
CD_SUCCESS("Obtained rightArmEncTimed.\n");
if ( ! rightArmDevice.view( playbackThread.rightArmTorque ) ) {
CD_ERROR("Could not obtain rightArmTorque.\n");
return false;
}
CD_SUCCESS("Obtained rightArmTorque.\n");
//-- Do stuff.
playbackThread.leftArmPos->getAxes( &(playbackThread.leftArmNumMotors) );
playbackThread.rightArmPos->getAxes( &(playbackThread.rightArmNumMotors) );
CD_INFO("setPositionDirectMode...\n");
playbackThread.leftArmPosDirect->setPositionDirectMode();
playbackThread.rightArmPosDirect->setPositionDirectMode();
//-- Start the thread.
CD_INFO("Start thread...\n");
playbackThread.start();
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;
}
