bool embObjMultiEnc::fromConfig(yarp::os::Searchable &_config)
{
yDebug()<< "configurazione: ";;
yDebug() << _config.toString();
Bottle general = _config.findGroup("JOINTS");
if(general.isNull())
{
yError() << "embObjMultiEnc cannot find general group";
return false;
}
Bottle jointsbottle = general.findGroup("listofjoints");
if (jointsbottle.isNull())
{
yError() << "embObjMultiEnc cannot find listofjoints param";
return false;
}
Bottle encsbottle = general.findGroup("encoderConversionFactor");
if (encsbottle.isNull())
{
yError() << "embObjMultiEnc cannot find encoderConversionFactor param";
return false;
}
//jointsbottle contains: "listofjoints 0 1 2 3. So the num of joints is jointsbottle->size() -1 "
numofjoints = jointsbottle.size() -1;
listofjoints.clear();
for (int i = 1; i < jointsbottle.size(); i++) listofjoints.push_back(jointsbottle.get(i).asInt());
yDebug()<< " embObjMultiEnc List of joints: " << numofjoints;
for(int i=0; i<numofjoints; i++) yDebug() << "pos="<< i << "val="<< listofjoints[i];
analogdata.resize(numofencperjoint*numofjoints, 0.0);
encoderConversionFactor.resize(numofencperjoint*numofjoints, 1.0);
if (numofencperjoint*numofjoints!=encsbottle.size()-1)
{
yError() << "embObjMultiEnc invalid size of encoderConversionFactor param";
return false;
}
for (int i=0; i<encsbottle.size()-1; i++)
{
encoderConversionFactor[i]=encsbottle.get(i+1).asDouble();
}
return true;
}
void Bottle::copy(const Bottle& alt, int first, int len)
{
implementation->edit();
if (alt.isNull()) {
clear();
implementation->invalid = true;
return;
}
implementation->copyRange(alt.implementation, first, len);
}
static bool checkParam(const Bottle& input, RGBDSensorParamParser::RGBDParam& param, bool& found)
{
bool ret = false;
Bottle bt=input.findGroup(param.name).tail(); // the first element is the name of the parameter
if (!bt.isNull())
{
Bottle* b;
if (param.size>1 && bt.size()==1)
{
b = bt.get(0).asList();
}
else
b = &bt;
if (b->isNull())
{
yError()<<"RGBDSensorParamParser: check"<<param.name<<"in config file";
return false;
}
if (b->size() != param.size)
{
yError() << "RGBDSensorParamParser: parameter" << param.name << "size should be" << param.size;
return false;
}
param.val.resize(param.size);
for (size_t i=0;i<b->size();i++)
{
ret = true;
param.val[i] = b->get(i);
found = true;
}
}
else
{
ret = true;
found = false;
}
return ret;
}
Vector ReachManager::Solve(const Vector &xd, string partName, string &resultPart)
{
double minNorm2;
Bottle *resultQBottle;
Vector resultQ(7);
if(partName == "both")
{
//ask to resolve for some xyz position
Bottle cmd, replyLeft, replyRight;
cmd.clear();
cout << "SOLVING WITH RIGHT ARM" << endl;
iCubArmCartesianSolver::addTargetOption(cmd,xd);
iKinPorts["right"]->out.write(cmd);
iKinPorts["left"]->out.write(cmd);
iKinPorts["right"]->in.wait(replyRight);
iKinPorts["left"]->in.wait(replyLeft);
if(replyRight.isNull() || replyLeft.isNull())
{
cout << "\n\nERROR ! \n\n" << endl;
resultPart = "none";
return resultQ;
}
Bottle *xdBottleRight = CartesianSolver::getTargetOption(replyRight);
Bottle *xBottleRight = CartesianSolver::getEndEffectorPoseOption(replyRight);
Bottle *qBottleRight = CartesianSolver::getJointsOption(replyRight);
if (!xdBottleRight || !xBottleRight || !qBottleRight)
{
cout << "\n\nERROR ! \n\n" << endl;
resultPart = "none";
return resultQ;
}
if (xdBottleRight->isNull() || xBottleRight->isNull() || qBottleRight->isNull())
{
cout << "\n\nERROR ! \n\n" << endl;
resultPart = "none";
return resultQ;
}
Vector deltaRight(3);
deltaRight[0] = xBottleRight->get(0).asDouble() - xdBottleRight->get(0).asDouble();
deltaRight[1] = xBottleRight->get(1).asDouble() - xdBottleRight->get(1).asDouble();
deltaRight[2] = xBottleRight->get(2).asDouble() - xdBottleRight->get(2).asDouble();
double deltaRightNorm2 = deltaRight[0]*deltaRight[0] + deltaRight[1]*deltaRight[1] + deltaRight[2]*deltaRight[2];
cout << "right error : " << sqrt(deltaRightNorm2) << endl;
cout<<"xd ="<<xdBottleRight->toString()<<endl;
cout<<"x ="<<xBottleRight->toString()<<endl;
cout<<"q [rad] ="<<qBottleRight->toString()<<endl;
cout<<endl;
cout << "SOLVING WITH LEFT ARM" << endl;
Bottle *xdBottleLeft = CartesianSolver::getTargetOption(replyLeft);
Bottle *xBottleLeft = CartesianSolver::getEndEffectorPoseOption(replyLeft);
Bottle *qBottleLeft = CartesianSolver::getJointsOption(replyLeft);
if (!xdBottleLeft || !xBottleLeft || !qBottleLeft)
{
cout << "\n\nERROR ! \n\n" << endl;
resultPart = "none";
return resultQ;
}
if (xdBottleLeft->isNull() || xBottleLeft->isNull() || qBottleLeft->isNull())
{
cout << "\n\nERROR ! \n\n" << endl;
resultPart = "none";
return resultQ;
}
Vector deltaLeft(3);
deltaLeft[0] = xBottleLeft->get(0).asDouble() - xdBottleLeft->get(0).asDouble();
deltaLeft[1] = xBottleLeft->get(1).asDouble() - xdBottleLeft->get(1).asDouble();
deltaLeft[2] = xBottleLeft->get(2).asDouble() - xdBottleLeft->get(2).asDouble();
double deltaLeftNorm2 = deltaLeft[0]*deltaLeft[0] + deltaLeft[1]*deltaLeft[1] + deltaLeft[2]*deltaLeft[2];
cout << "left error : " << sqrt(deltaLeftNorm2) << endl;
cout<<"xd ="<<xdBottleLeft->toString()<<endl;
cout<<"x ="<<xBottleLeft->toString()<<endl;
cout<<"q [rad] ="<<qBottleLeft->toString()<<endl;
cout<<endl;
if(deltaLeftNorm2<deltaRightNorm2)
{
resultPart = "left";
resultQBottle = qBottleLeft;
}
else
{
resultPart = "right";
resultQBottle = qBottleRight;
}
minNorm2 = min(deltaLeftNorm2, deltaRightNorm2);
if(minNorm2>(pow(parameters["max_error"]->asDouble(),2)))
{
cout << "TOO BIG ERROR !!! \n\n" << endl;
resultPart = "none";
return resultQ;
}
if(resultPart == "left" && deltaLeft[0] < 0)
{
cout << "REACHING TOO FAR !!! " << endl;
resultPart = "none";
return resultQ;
}
if(resultPart == "right" && deltaRight[0] < 0)
{
cout << "REACHING TOO FAR !!! " << endl;
resultPart = "none";
return resultQ;
}
for(int i =0; i<7; ++i)
resultQ[i] = resultQBottle->get(i).asDouble() * M_PI/180;
}
else
{
resultPart = partName;
Bottle cmd, reply;
cmd.clear();
cout << "SOLVING WITH " << partName << " ARM" << endl;
iCubArmCartesianSolver::addTargetOption(cmd,xd);
iKinPorts[partName]->out.write(cmd);
iKinPorts[partName]->in.wait(reply);
if(reply.isNull())
{
cout << "\n\nERROR ! \n\n" << endl;
resultPart = "none";
return resultQ;
}
Bottle *xdBottle = CartesianSolver::getTargetOption(reply);
Bottle *xBottle = CartesianSolver::getEndEffectorPoseOption(reply);
Bottle *qBottle = CartesianSolver::getJointsOption(reply);
if (!xdBottle || !xBottle || !qBottle)
{
cout << "\n\nERROR ! \n\n" << endl;
resultPart = "none";
return resultQ;
}
if (xdBottle->isNull() || xBottle->isNull() || qBottle->isNull())
{
cout << "\n\nERROR ! \n\n" << endl;
resultPart = "none";
return resultQ;
}
Vector delta(3);
delta[0] = xBottle->get(0).asDouble() - xdBottle->get(0).asDouble();
delta[1] = xBottle->get(1).asDouble() - xdBottle->get(1).asDouble();
delta[2] = xBottle->get(2).asDouble() - xdBottle->get(2).asDouble();
double deltaNorm2 = delta[0]*delta[0] + delta[1]*delta[1] + delta[2]*delta[2];
cout << "error : " << sqrt(deltaNorm2) << endl;
cout << "xd =" << xdBottle->toString()<<endl;
cout << "x =" << xBottle->toString()<<endl;
cout << "q [rad] =" << qBottle->toString()<<endl;
cout << endl;
if(delta[0] < 0)
{
cout << "REACHING TOO FAR !!! " << endl;
resultPart = "none";
return resultQ;
}
if(deltaNorm2>(pow(parameters["max_error"]->asDouble(),2)))
{
cout << "TOO BIG ERROR !!! \n\n" << endl;
resultPart = "none";
return resultQ;
}
for(int i =0; i<7; ++i)
resultQ[i] = qBottle->get(i).asDouble() * M_PI/180;
}
//cout << "MAX ERROR " << parameters["max_error"].asDouble() << endl;
return resultQ;
}
bool SimToolLoaderModule::configure(yarp::os::ResourceFinder &rf) {
Bottle table;
Bottle temp;
string objectName = "obj";
/* module name */
moduleName = rf.check("name", Value("simtoolloader"),
"Module name (string)").asString();
setName(moduleName.c_str());
/* Hand used */
hand=rf.find("hand").asString().c_str();
if ((hand!="left") && (hand!="right"))
hand="right";
/* port names */
simToolLoaderSimOutputPortName = "/";
simToolLoaderSimOutputPortName += getName( rf.check("simToolLoaderSimOutputPort",
Value("/sim:rpc"),
"Loader output port(string)")
.asString() );
simToolLoaderCmdInputPortName = "/";
simToolLoaderCmdInputPortName += getName( rf.check("simToolLoaderCmdInputPort",
Value("/cmd:i"),
"Loader input port(string)")
.asString() );
/* open ports */
if (!simToolLoaderSimOutputPort.open(
simToolLoaderSimOutputPortName.c_str())) {
cout << getName() << ": unable to open port"
<< simToolLoaderSimOutputPortName << endl;
return false;
}
if (!simToolLoaderCmdInputPort.open(
simToolLoaderCmdInputPortName.c_str())) {
cout << getName() << ": unable to open port"
<< simToolLoaderCmdInputPortName << endl;
return false;
}
/* Rate thread period */
threadPeriod = rf.check("threadPeriod", Value(0.5),
"Control rate thread period key value(double) in seconds ").asDouble();
/* Read Table Configuration */
table = rf.findGroup("table");
/* Read the Objects configurations */
vector<Bottle> object;
cout << "Loading objects to buffer" << endl;
bool noMoreModels = false;
int n =0;
while (!noMoreModels){ // read until there are no more objects.
stringstream s;
s.str("");
s << objectName << n;
string objNameNum = s.str();
temp = rf.findGroup("objects").findGroup(objNameNum);
if(!temp.isNull()) {
cout << "object" << n << ", id: " << objNameNum;
cout << ", model: " << temp.get(2).asString() << endl;
object.push_back(temp);
temp.clear();
n++;
}else {
noMoreModels = true;
}
}
numberObjs = n;
cout << "Loaded " << object.size() << " objects " << endl;
/* Create the control rate thread */
ctrlThread = new CtrlThread(&simToolLoaderSimOutputPort,
&simToolLoaderCmdInputPort,
threadPeriod,
table, object,hand);
/* Starts the thread */
if (!ctrlThread->start()) {
delete ctrlThread;
return false;
}
return true;
}
bool embObjAnalogSensor::open(yarp::os::Searchable &config)
{
std::string str;
if(config.findGroup("GENERAL").find("verbose").asBool())
str=config.toString().c_str();
else
str="\n";
yTrace() << str;
// Read stuff from config file
if(!fromConfig(config))
{
yError() << "embObjAnalogSensor missing some configuration parameter. Check logs and your config file.";
return false;
}
// Tmp variables
Bottle groupEth;
ACE_TCHAR address[64];
ACE_UINT16 port;
bool ret;
Bottle groupTransceiver = Bottle(config.findGroup("TRANSCEIVER"));
if(groupTransceiver.isNull())
{
yError() << "embObjAnalogSensor::open(): Can't find TRANSCEIVER group in xml config files";
return false;
}
Bottle groupProtocol = Bottle(config.findGroup("PROTOCOL"));
if(groupProtocol.isNull())
{
yError() << "embObjAnalogSensor::open(): Can't find PROTOCOL group in xml config files";
return false;
}
// Get both PC104 and EMS ip addresses and port from config file
groupEth = Bottle(config.findGroup("ETH"));
Bottle parameter1( groupEth.find("PC104IpAddress").asString() ); // .findGroup("IpAddress");
port = groupEth.find("CmdPort").asInt(); // .get(1).asInt();
snprintf(_fId.pc104IPaddr.string, sizeof(_fId.pc104IPaddr.string), "%s", parameter1.toString().c_str());
_fId.pc104IPaddr.port = port;
Bottle parameter2( groupEth.find("IpAddress").asString() ); // .findGroup("IpAddress");
snprintf(_fId.boardIPaddr.string, sizeof(_fId.boardIPaddr.string), "%s", parameter2.toString().c_str());
_fId.boardIPaddr.port = port;
sscanf(_fId.boardIPaddr.string,"\"%d.%d.%d.%d", &_fId.boardIPaddr.ip1, &_fId.boardIPaddr.ip2, &_fId.boardIPaddr.ip3, &_fId.boardIPaddr.ip4);
sscanf(_fId.pc104IPaddr.string,"\"%d.%d.%d.%d", &_fId.pc104IPaddr.ip1, &_fId.pc104IPaddr.ip2, &_fId.pc104IPaddr.ip3, &_fId.pc104IPaddr.ip4);
snprintf(_fId.boardIPaddr.string, sizeof(_fId.boardIPaddr.string), "%u.%u.%u.%u:%u", _fId.boardIPaddr.ip1, _fId.boardIPaddr.ip2, _fId.boardIPaddr.ip3, _fId.boardIPaddr.ip4, _fId.boardIPaddr.port);
snprintf(_fId.pc104IPaddr.string, sizeof(_fId.pc104IPaddr.string), "%u.%u.%u.%u:%u", _fId.pc104IPaddr.ip1, _fId.pc104IPaddr.ip2, _fId.pc104IPaddr.ip3, _fId.pc104IPaddr.ip4, _fId.pc104IPaddr.port);
// debug info
memset(info, 0x00, sizeof(info));
snprintf(info, sizeof(info), "embObjAnalogSensor: referred to EMS: %d at address %s\n", _fId.boardNumber, address);
snprintf(_fId.name, sizeof(_fId.name), "%s", info); // Saving User Friendly Id
// Set dummy values
_fId.boardNumber = FEAT_boardnumber_dummy;
Value val = config.findGroup("ETH").check("Ems", Value(1), "Board number");
if(val.isInt())
_fId.boardNumber = val.asInt();
else
{
yError () << "embObjAnalogSensor: EMS Board number identifier not found for IP" << _fId.pc104IPaddr.string;
return false;
}
ethManager = TheEthManager::instance();
if(NULL == ethManager)
{
yFatal() << "embObjAnalogSensor::open() cannot instantiate ethManager";
return false;
}
// N.B.: use a dynamic_cast<> to extract correct interface when using this pointer
_fId.interface = this;
_fId.endpoint = eoprot_endpoint_analogsensors;
if(AS_STRAIN == _as_type)
_fId.type = ethFeatType_AnalogStrain;
else if(AS_MAIS == _as_type)
_fId.type = ethFeatType_AnalogMais;
/* Once I'm ok, ask for resources, through the _fId struct I'll give the ip addr, port and
* and boardNum to the ethManager in order to create the ethResource requested.
*/
res = ethManager->requestResource(config, groupTransceiver, groupProtocol, _fId);
if(NULL == res)
{
yError() << "embObjAnalogSensor::open() fails because could not instantiate the ethResource board" << _fId.boardNumber << " ... unable to continue";
return false;
}
if(false == res->isEPmanaged(eoprot_endpoint_analogsensors))
{
yError() << "embObjAnalogSensor::open() detected that EMS "<< _fId.boardNumber << " does not support analog sensors";
cleanup();
return false;
}
if(false == res->verifyBoard(groupProtocol))
{
yError() << "embObjAnalogSensor::open() fails in function verifyBoard() for board " << _fId.boardNumber << ": CANNOT PROCEED ANY FURTHER";
cleanup();
return false;
}
else
{
if(verbosewhenok)
{
yDebug() << "embObjAnalogSensor::open() has succesfully verified that board "<< _fId.boardNumber << " is communicating correctly";
}
}
if(!res->verifyEPprotocol(groupProtocol, eoprot_endpoint_analogsensors))
{
yError() << "embObjAnalogSensor::open() fails in function verifyEPprotocol() for board "<< _fId.boardNumber << ": either the board cannot be reached or it does not have the same eoprot_endpoint_management and/or eoprot_endpoint_analogsensors protocol version: DO A FW UPGRADE";
cleanup();
return false;
}
else
{
if(verbosewhenok)
{
yDebug() << "embObjAnalogSensor::open() has succesfully verified that board "<< _fId.boardNumber << " has same protocol version for analogsensors as robotInterface";
}
}
// // marco.accame on 04 sept 2014: we could add a verification about the entities of analog ... MAYBE in the future
//
// uint8_t numberofmaisboards = eoprot_entity_numberof_get(featIdBoardNum2nvBoardNum(_fId.boardNumber), eoprot_endpoint_analogsensors, eoprot_entity_as_mais);
// if(false == res->verifyENTITYnumber(groupProtocol, eoprot_endpoint_analogsensors, eoprot_entity_as_mais, numberofmaisboards))
// { // JUST AN EXAMPLE OF HOW TO DO IT FOR THE MAIS.
// yError() << "embObjAnalogSensor::init() fails in function verifyENTITYnumber() for board "<< _fId.boardNumber << " and entity eoprot_entity_as_mais: VERIFY their number in board, and in XML files";
// return false;
// }
// uint8_t numberofstrainboards = eoprot_entity_numberof_get(featIdBoardNum2nvBoardNum(_fId.boardNumber), eoprot_endpoint_analogsensors, eoprot_entity_as_strain);
// if(false == res->verifyENTITYnumber(groupProtocol, eoprot_endpoint_analogsensors, eoprot_entity_as_strain, numberofstrainboards))
// { // JUST AN EXAMPLE OF HOW TO DO IT FOR THE STRAIN.
// yError() << "embObjAnalogSensor::init() fails in function verifyENTITYnumber() for board "<< _fId.boardNumber << " and entity eoprot_entity_as_strain: VERIFY their number in board, and in XML files";
// return false;
// }
data = new AnalogData(_channels, _channels+1);
scaleFactor = new double[_channels];
int i=0;
for (i=0; i<_channels; i++) scaleFactor[i]=1;
// Real values will be read from the sensor itself during its initalization hereafter
for(int i=0; i<_channels; i++)
{
scaleFactor[i]=1;
}
//#warning --> marco.accame on 04sept14: both embObjAnalogSensors and embObjMotionControl initialises the mais board. but potentially w/ different params (mc uses: datarate = 10 and mode = eoas_maismode_txdatacontinuously).
switch(_as_type)
{
case AS_MAIS:
{
ret = sendConfig2Mais();
} break;
case AS_STRAIN:
{
ret = sendConfig2Strain();
} break;
default:
{
//i should not be here. if AS_NONE then i should get error in fromConfig function
ret = false;
}
}
if(!ret)
{
cleanup();
return false;
}
// Set variable to be signalled
if(! init())
{
cleanup();
return false;
}
if(false == res->goToRun())
{
yError() << "embObjAnalogSensor::open() fails to start control loop of board" << _fId.boardNumber << ": cannot continue";
cleanup();
return false;
}
else
{
if(verbosewhenok)
{
yDebug() << "embObjAnalogSensor::open() correctly activated control loop of BOARD" << _fId.boardNumber;
}
}
opened = true;
return true;
}
bool MainWindow::initGuiStatus(){
Bottle reply = portThread.sendRpcCommand(true, get_binarization);
if(string(reply.toString().c_str()).compare("on") == 0){
ui->btnBinarization->setEnabled(true);
ui->btnBinarization->setText("ON");
ui->btnBinarization->setChecked(true);
}else{
ui->btnBinarization->setEnabled(true);
ui->btnBinarization->setText("OFF");
ui->btnBinarization->setChecked(false);
}
reply = portThread.sendRpcCommand(true, get_smooth_filter);
if(string(reply.toString().c_str()).compare("on") == 0){
ui->btnSmooth->setText("ON");
ui->sliderContainer->setEnabled(true);
ui->btnSmooth->setChecked(true);
}else{
ui->btnSmooth->setText("OFF");
ui->sliderContainer->setEnabled(false);
ui->btnSmooth->setChecked(false);
}
reply = portThread.sendRpcCommand(true, get_smooth_factor);
currentSmoothFactor = reply.get(0).asDouble();
ui->sliderScaleSmooth->setValue(currentSmoothFactor * 10);
onSmoothValueChanged(currentSmoothFactor * 10);
//gtk_adjustment_set_value(scaleSmooth->range.adjustment, currentSmoothFactor);
reply = portThread.sendRpcCommand(true, get_threshold);
if(reply.isNull() || reply.size()==0 || !reply.get(0).isInt()){
printLog("Error while getting the safety threshold");
return false;
}else{
currentThreshold = reply.get(0).asInt();
ui->spinThreashold->setValue(currentThreshold);
}
reply = portThread.sendRpcCommand(true, get_gain);
if(reply.isNull() || reply.size()==0 || (!reply.get(0).isDouble() && !reply.get(0).isInt())){
printLog("Error while getting the compensation gain");
return false;
}else{
currentCompGain = reply.get(0).asDouble();
ui->spinCompGain->setValue(currentCompGain);
}
reply = portThread.sendRpcCommand(true,get_cont_gain);
if(reply.isNull() || reply.size()==0 || (!reply.get(0).isDouble() && !reply.get(0).isInt())){
printLog("Error while getting the contact compensation gain");
return false;
}else{
currentContCompGain = reply.get(0).asDouble();
ui->spinCompContactGain->setValue(currentContCompGain);
}
reply = portThread.sendRpcCommand(true,get_max_neigh_dist);
if(reply.isNull() || reply.size()==0 || (!reply.get(0).isDouble() && !reply.get(0).isInt())){
printLog("Error while getting the max neighbor distance");
return false;
}else{
currentMaxNeighDist = reply.get(0).asDouble();
ui->spinNeighbor->setValue(currentMaxNeighDist * 100);
//gtk_adjustment_set_value(spinMaxNeighDist->adjustment, 1e2*currentMaxNeighDist);
}
// get module information
reply = portThread.sendRpcCommand(true, get_info);
if(reply.isNull() || reply.size()!=3){
printLog("Error while reading the module information");
//gtk_label_set_text(lblInfo, reply.toString().c_str());
return false;
}
stringstream ss;
ss<< reply.get(0).toString().c_str()<< endl;
ss<< reply.get(1).toString().c_str()<< "\nInput ports:";
Bottle* portList = reply.get(2).asList();
portNames.resize(portList->size()/2);
portDim.resize(portList->size()/2);
//int numTaxels = 0;
for(unsigned int i=0;i<portDim.size();i++){
portNames[i] = portList->get(i*2).toString().c_str();
portDim[i] = portList->get(i*2+1).asInt();
//numTaxels += portDim[i];
ss<< "\n - "<< portNames[i]<< " ("<< portDim[i]<< " taxels)";
}
ui->infoPanel->setPlainText(QString("%1").arg(ss.str().c_str()));
//gtk_label_set_text(lblInfo, ss.str().c_str());
// check whether the skin calibration is in process
reply = portThread.sendRpcCommand(true, is_calibrating);
if(string(reply.toString().c_str()).compare("yes")==0){
loadingWidget.start();
calibratingTimer.start();
//gtk_widget_show(GTK_WIDGET(progBarCalib));
//g_timeout_add(100, progressbar_calibration, NULL);
//gtk_widget_set_sensitive(GTK_WIDGET(btnCalibration), false);
}
ui->controlsWidget->setEnabled(true);
ui->sampleFreqContainer->setEnabled(true);
ui->treeCompensation->setEnabled(true);
return true;
}
bool learnPrimitive::updateAction(ResourceFinder &rf){
Bottle bOutput;
//1. Primitive
Bottle bAction = rf.findGroup("Action");
if (!bAction.isNull())
{
Bottle * bActionName = bAction.find("ActionName").asList();
Bottle * bActionArg = bAction.find("ActionArg").asList();
if(bActionName->isNull() || bActionArg->isNull()){
yError() << " [updateAction] : one of the primitiveAction conf is null : ActionName, ActionArg" ;
return false ;
}
int actionSize = -1 ;
if(bActionName->size() == bActionArg->size()){
actionSize = bActionName->size() ;
} else {
yError() << " [updateAction] : one of the Action conf has different size!" ;
return false ;
}
if(actionSize == 0) {
yWarning() << " [updateAction] : there is no Action defined at startup!" ;
} else {
for(int i = 0; i < actionSize ; i++) {
//insert protoaction even if already there
Bottle bSubAction;
string currentActionName = bActionName->get(i).asString();
string currentActionArg = bActionArg->get(i).asString();
bSubAction.addString(currentActionName);
bSubAction.addString(currentActionArg);
string concat = currentActionName + "_" + currentActionArg;
// yInfo() << " Looking for " << concat ;
Bottle bCurrent = rf.findGroup(concat);
if(bCurrent.isNull()){
yError() << " [updateAction] : " << concat << "is NOT defined" ;
return false ;
}
Bottle * bListSubAction = bCurrent.find("actionList").asList();
//yInfo() << "Subaction found : " << bListSubAction->toString() ;
if(bListSubAction->isNull()){
yError() << " [updateAction] : " << concat << "is there but is not defined (actionList)" ;
return false ;
}
bSubAction.addList() = *bListSubAction ;
yInfo() << "Complex Action added : (" <<bSubAction.get(0).asString() << ", " << bSubAction.get(1).asString() << ", ( " << bSubAction.get(2).asList()->toString()<< "))" ;
vActionBottle.push_back(bSubAction);
}
}
} else {
yError() << " error in learnPrimitive::updatePrimitive | Primitive_Action is NOT defined in the learnPrimitive.ini";
return false;
}
return true;
}
bool learnPrimitive::updatePrimitive(ResourceFinder &rf){
Bottle bOutput;
//1. Primitive
Bottle bPrimitiveAction = rf.findGroup("Primitive_Action");
if (!bPrimitiveAction.isNull())
{
Bottle * bPrimitiveActionName = bPrimitiveAction.find("primitiveActionName").asList();
Bottle * bPrimitiveActionArg = bPrimitiveAction.find("primitiveActionArg").asList();
if(bPrimitiveActionName->isNull() || bPrimitiveActionArg->isNull()){
yError() << " [updatePrimitiveAction] : one of the primitiveAction conf is null : primitiveActionName, primitiveActionArg" ;
return false ;
}
int primitiveActionSize = -1 ;
if(bPrimitiveActionName->size() == bPrimitiveActionArg->size()){
primitiveActionSize = bPrimitiveActionName->size() ;
} else {
yError() << " [updatePrimitiveAction] : one of the primitiveAction conf has different size!" ;
return false ;
}
if(primitiveActionSize == 0) {
yWarning() << " [updatePrimitiveAction] : there is no primitiveAction defined at startup!" ;
} else {
for(int i = 0; i < primitiveActionSize ; i++) {
//insert protoaction even if already there
Bottle bPrimitive;
string currentPrimName = bPrimitiveActionName->get(i).asString();
string currentPrimArg = bPrimitiveActionArg->get(i).asString();
bPrimitive.addString(currentPrimName);
bPrimitive.addString(currentPrimArg);
string concat = currentPrimName + "_" + currentPrimArg;
Bottle bCurrentPrim = rf.findGroup(concat);
if(bCurrentPrim.isNull()){
yError() << " [updatePrimitiveAction] : " << concat << "is NOT defined" ;
return false ;
}
Bottle * bListProtoAction = bCurrentPrim.find("actionList").asList();
if(bListProtoAction->isNull()){
yError() << " [updatePrimitiveAction] : " << concat << "is there but is not defined (actionList)" ;
return false ;
}
bPrimitive.addList() = *bListProtoAction ;
yInfo() << "Primitive Action added : (" <<bPrimitive.get(0).asString() << ", " << bPrimitive.get(1).asString() << ", ( " << bPrimitive.get(2).asList()->toString()<< "))" ;
vPrimitiveActionBottle.push_back(bPrimitive);
}
}
} else {
yError() << " error in learnPrimitive::updatePrimitive | Primitive_Action is NOT defined in the learnPrimitive.ini";
return false;
}
return true;
}
bool learnPrimitive::updateProtoAction(ResourceFinder &rf){
Bottle bOutput;
//1. Protoaction
Bottle bProtoAction = rf.findGroup("Proto_Action");
if (!bProtoAction.isNull())
{
Bottle * bProtoActionName = bProtoAction.find("protoActionName").asList();
Bottle * bProtoActionEnd = bProtoAction.find("protoActionEnd").asList();
Bottle * bProtoActionSpeed = bProtoAction.find("protoActionSpeed").asList();
if(bProtoActionName->isNull() || bProtoActionEnd->isNull() || bProtoActionSpeed->isNull()){
yError() << " [updateProtoAction] : one of the protoAction conf is null : protoActionName, protoActionEnd, protoActionSpeed" ;
return false ;
}
int protoActionSize = -1 ;
if(bProtoActionName->size() == bProtoActionEnd->size() && bProtoActionEnd->size() == bProtoActionSpeed->size()){
protoActionSize = bProtoActionName->size() ;
} else {
yError() << " [updateProtoAction] : one of the protoAction conf has different size!" ;
return false ;
}
if(protoActionSize == 0) {
yWarning() << " [updateProtoAction] : there is no protoaction defined at startup!" ;
} else {
for(int i = 0; i < protoActionSize ; i++) {
//insert protoaction even if already there
yInfo() << "ProtoAction added : (" << bProtoActionName->get(i).asString() << ", " << bProtoActionEnd->get(i).asInt() << ", " << bProtoActionSpeed->get(i).asDouble() << ")" ;
mProtoActionEnd[bProtoActionName->get(i).asString()] = bProtoActionEnd->get(i).asInt();
mProtoActionSpeed[bProtoActionName->get(i).asString()] = bProtoActionSpeed->get(i).asDouble();
}
}
} else {
yError() << " error in learnPrimitive::updateProtoAction | Proto_Action is NOT defined in the learnPrimitive.ini";
return false;
}
//2. Effect of bodypart
Bottle bBodyPart = rf.findGroup("BodyPart");
if (!bBodyPart.isNull())
{
Bottle * bBodyPartName = bBodyPart.find("bodyPartName").asList();
Bottle * bBodyPartEnd = bBodyPart.find("bodyPartProtoEnd").asList();
Bottle * bBodyPartSpeed = bBodyPart.find("bodyPartProtoSpeed").asList();
//Crash if no match : isnull is not good for that
if(bBodyPartName->isNull() || bBodyPartEnd->isNull() || bBodyPartSpeed->isNull()){
yError() << "[updateProtoAction] : one of the bodyPartProto conf is null : bodyPartName, bodyPartProtoEnd, bodyPartProtoSpeed" ;
return false ;
}
int bodyPartSize = -1 ;
if(bBodyPartName->size() == bBodyPartEnd->size() && bBodyPartEnd->size() == bBodyPartSpeed->size()){
bodyPartSize = bBodyPartName->size() ;
} else {
yError() << "[updateProtoAction] : one of the bodyPartProto conf has different size!" ;
return false ;
}
if(bodyPartSize == 0) {
yWarning() << "[updateProtoAction] : there is no bodyPartProto defined at startup!" ;
} else {
for(int i = 0; i < bodyPartSize ; i++) {
//insert protoaction even if already there
yInfo() << "bodyPartProto added : (" << bBodyPartName->get(i).asString() << ", " << bBodyPartEnd->get(i).asInt() << ", " << bBodyPartSpeed->get(i).asDouble() << ")" ;
mBodyPartEnd[bBodyPartName->get(i).asString()] = bBodyPartEnd->get(i).asInt();
mBodyPartSpeed[bBodyPartName->get(i).asString()] = bBodyPartSpeed->get(i).asDouble();
}
}
} else {
yError() << " error in learnPrimitive::updateProtoAction | BodyPart is NOT defined in the learnPrimitive.ini";
return false;
}
return true;
}
/* ******* Configure module ********************************************** */
bool RPCControllerModule::configure(ResourceFinder &rf){
using std::string;
using yarp::os::Network;
cout << dbgTag << "Starting. \n";
/* ****** Configure the Module ****** */
// Get resource finder and extract properties
moduleName = rf.check("name", Value("RPCController"), "The module name.").asString().c_str();
period = rf.check("period", 1.0).asDouble();
robotName = rf.check("robot", Value("icub"), "The robot name.").asString().c_str();
string portNameRoot = "/" + moduleName + "/";
/* ******* Ports. ******* */
rpcModule.open((portNameRoot + "cmd:io").c_str());
portExperimentStepsOut.open((portNameRoot + "experiment/status:o").c_str());
/* ******* Read RPC port parameters. ******* */
Bottle parGroup = rf.findGroup("rpc");
if (!parGroup.isNull()) {
if (parGroup.check("rpcServer")) {
string rpcServer = parGroup.find("rpcServer").asString().c_str();
// Connect to RPC server
if (!Network::connect(rpcModule.getName(), rpcServer)) {
cerr << dbgTag << "Could not connect to the specified RPC server: " << rpcServer << ". \n";
return false;
}
} else {
cerr << dbgTag << "Could not find the rpcServer name in the specified configuration file. \n";
return false;
}
} else {
cerr << dbgTag << "Could not find the [rpc] parameter group in the specified configuration file. \n";
return false;
}
/* ******* Read experiment parameters. ******* */
parGroup = rf.findGroup("experiment");
if (!parGroup.isNull()) {
// Number of columns in array
if (parGroup.check("nCols")) {
int nCols = parGroup.find("nCols").asInt();
// Experiment parameters
Bottle *confExpParams = parGroup.find("cmdT").asList();
if (confExpParams != NULL) {
for (int i = 0; i < confExpParams->size(); ++i) {
ExperimentParams tmpPar;
tmpPar.cmd = confExpParams->get(i).asString().c_str();
tmpPar.time = confExpParams->get(i + 1).asDouble();
expParams.push_back(tmpPar);
i++;
}
} else {
cerr << dbgTag << "Could not find the experiment parameter list in the supplied configuration file. \n";
return false;
}
} else {
cerr << dbgTag << "Could not find nCols parameter in supplied configuration file. \n";
return false;
}
} else {
cerr << dbgTag << "Could not find the experiment parameter group [experiment] in the supplied configuration file. \n";
return false;
}
#ifndef NODEBUG
cout << "DEBUG: " << dbgTag << "Experiment parameters are: \n";
for (size_t i = 0; i < expParams.size(); ++i) {
cout << "DEBUG " << dbgTag << expParams[i].cmd << " " << expParams[i].time << "\n";
i++;
}
#endif
cout << dbgTag << "Started correctly. \n";
return true;
}
bool embObjVirtualAnalogSensor::open(yarp::os::Searchable &config)
{
std::string str;
if(config.findGroup("GENERAL").find("verbose").asBool())
{
str=config.toString().c_str();
_verbose = true;
}
else
str=" ";
yTrace() << str;
// Read stuff from config file
if(!fromConfig(config))
{
yError() << "embObjAnalogSensor missing some configuration parameter. Check logs and your config file.";
return false;
}
// Tmp variables
Bottle groupEth;
ACE_TCHAR address[64];
ACE_UINT16 port;
Bottle groupTransceiver = Bottle(config.findGroup("TRANSCEIVER"));
if(groupTransceiver.isNull())
{
yError() << "embObjVirtualAnalogSensor: Can't find TRANSCEIVER group in config files";
return false;
}
Bottle groupProtocol = Bottle(config.findGroup("PROTOCOL"));
if(groupProtocol.isNull())
{
yError() << "embObjVirtualAnalogSensor: Can't find PROTOCOL group in config files";
return false;
}
// Get both PC104 and EMS ip addresses and port from config file
groupEth = Bottle(config.findGroup("ETH"));
Bottle parameter1( groupEth.find("PC104IpAddress").asString() ); // .findGroup("IpAddress");
port = groupEth.find("CmdPort").asInt(); // .get(1).asInt();
snprintf(_fId.pc104IPaddr.string, sizeof(_fId.pc104IPaddr.string), "%s", parameter1.toString().c_str());
_fId.pc104IPaddr.port = port;
Bottle parameter2( groupEth.find("IpAddress").asString() ); // .findGroup("IpAddress");
snprintf(_fId.boardIPaddr.string, sizeof(_fId.boardIPaddr.string), "%s", parameter2.toString().c_str());
_fId.boardIPaddr.port = port;
sscanf(_fId.boardIPaddr.string,"\"%d.%d.%d.%d", &_fId.boardIPaddr.ip1, &_fId.boardIPaddr.ip2, &_fId.boardIPaddr.ip3, &_fId.boardIPaddr.ip4);
sscanf(_fId.pc104IPaddr.string,"\"%d.%d.%d.%d", &_fId.pc104IPaddr.ip1, &_fId.pc104IPaddr.ip2, &_fId.pc104IPaddr.ip3, &_fId.pc104IPaddr.ip4);
snprintf(_fId.boardIPaddr.string, sizeof(_fId.boardIPaddr.string), "%u.%u.%u.%u:%u", _fId.boardIPaddr.ip1, _fId.boardIPaddr.ip2, _fId.boardIPaddr.ip3, _fId.boardIPaddr.ip4, _fId.boardIPaddr.port);
snprintf(_fId.pc104IPaddr.string, sizeof(_fId.pc104IPaddr.string), "%u.%u.%u.%u:%u", _fId.pc104IPaddr.ip1, _fId.pc104IPaddr.ip2, _fId.pc104IPaddr.ip3, _fId.pc104IPaddr.ip4, _fId.pc104IPaddr.port);
// Debug info
snprintf(_fId.name, sizeof(_fId.name), "embObjAnalogSensor: referred to EMS: %d at address %s\n", _fId.boardNumber, address); // Saving User Friendly Id
Value val =config.findGroup("ETH").check("Ems",Value(1), "Board number");
if(val.isInt())
_fId.boardNumber =val.asInt();
else
{
yError () << "embObjAnalogSensor: EMS Board number identifier not found for IP" << _fId.pc104IPaddr.string;
return false;
}
ethManager = TheEthManager::instance();
if(NULL == ethManager)
{
yError() << "Unable to instantiate ethManager";
return false;
}
// N.B.: use a dynamic_cast to extract correct interface when using this pointer
_fId.interface = this;
_fId.type = ethFeatType_AnalogVirtual;
/* Once I'm ok, ask for resources, through the _fId struct I'll give the ip addr, port and
* and boradNum to the ethManagerin order to create the ethResource requested.
* I'll Get back the very same sturct filled with other data useful for future handling
* like the EPvector and EPhash_function */
res = ethManager->requestResource(config, groupTransceiver, groupProtocol, _fId);
if(NULL == res)
{
yError() << "EMS device not instantiated... unable to continue";
cleanup();
return false;
}
/*IMPORTANT: implement isEpManagedByBoard like every embObj obj when virtaulAnalogSensor will be exist in eo proto!!!!*/
// if(!isEpManagedByBoard())
// {
// yError() << "EMS "<< _fId.boardNumber << "is not connected to virtual analog sensor";
// return false;
// }
// if(!res->verifyProtocol(groupProtocol, eoprot_endpoint_???))
// {
// yError() << "embObjVirtualAnalogSensor and board "<< _fId.boardNumber << "dont not have the same eoprot_endpoint_??? protocol version: DO A FW UPGRADE";
// return false;
// }
yTrace() << "EmbObj Virtual Analog Sensor for board "<< _fId.boardNumber << "instantiated correctly";
opened = true;
return true;
}
bool icubFinger::calibrateIndexMiddle(){
// Open the finger
Finger::open();
// Set the proximal to 0 and distal to 180
setAngle(_proximalJointIndex, 0, 30);
setAngle(_distalJointIndex, 180, 30);
while(!checkMotionDone()){
;
}
// TODO: remove this for the simulation. There is not such data
// available during simulation
Bottle *fingerEnc;
for(int i = 0; i <= _fingerEncoders->getPendingReads(); i++)
fingerEnc = _fingerEncoders->read();
if(!fingerEnc->isNull())
{
_maxProximal = fingerEnc->get(_proximalEncoderIndex).asDouble();
_minMiddle = fingerEnc->get(_middleEncoderIndex).asDouble();
_minDistal = fingerEnc->get(_distalEncoderIndex).asDouble();
}
setAngle(_distalJointIndex, 0, 30);
while(!checkMotionDone()){
;
}
setAngle(_proximalJointIndex, 90, 30);
while(!checkMotionDone()){
;
}
// TODO: remove this for the simulation. There is not such data
// available during simulation
for(int i = 0; i < _fingerEncoders->getPendingReads(); i++)
fingerEnc = _fingerEncoders->read();
if(!fingerEnc->isNull())
{
_minProximal = fingerEnc->get(_proximalEncoderIndex).asDouble();
_maxMiddle = fingerEnc->get(_middleEncoderIndex).asDouble();
_maxDistal = fingerEnc->get(_distalEncoderIndex).asDouble();
}
setAngle(_proximalJointIndex, 0);
while(!checkMotionDone()){
;
}
checkMinMax(_minDistal, _maxDistal);
checkMinMax(_minMiddle, _minMiddle);
checkMinMax(_minProximal, _minProximal);
cout << "Calibration mins:" <<
_minProximal << "\t" <<
_minMiddle << "\t" <<
_minDistal << "\t" << endl;
cout << "Calibration maxes:" <<
_maxProximal << "\t" <<
_maxMiddle << "\t" <<
_maxDistal << "\t" << endl;
}
/*
* Send a rpc command to ABM to obtain the kinematicStructure of part of the body (using the joint info)
* Send a rpc command to kinematicStructure if no kinematicStructure were provided
* input: joint of the bodypart to be moved (e.g. m_joint_number) + body part type (e.g. left_arm, right_arm, ...)
*/
Bottle proactiveTagging::assignKinematicStructureByJoint(int BPjoint, std::string sBodyPartType, bool forcingKS) {
//1. extract instance from singleJointAction for joint BPjoint, from ABM
Bottle bResult, bOutput;
ostringstream osRequest;
osRequest << "SELECT max(main.instance) FROM main, contentarg WHERE main.instance = contentarg.instance AND activityname = 'singleJointBabbling' AND main.begin = TRUE AND contentarg.role = 'limb' AND contentarg.argument = '" << BPjoint << "' ;";
yInfo() << "assignKinematicStructureByJoint request: " << osRequest.str();
bResult = iCub->getABMClient()->requestFromString(osRequest.str().c_str());
if (bResult.toString() == "NULL") {
yError() << " error in proactiveTagging::assignKinematicStructureByJoint | for joint " << BPjoint << " | No instance corresponding to singleJointBabbling for this part" ;
bOutput.addString("error");
bOutput.addString("No instance corresponding to singleJointBabbling for this part");
return bOutput;
}
int ksInstance = atoi(bResult.get(0).asList()->get(0).asString().c_str());
Bottle bResultCheckKS = checkForKinematicStructure(ksInstance, forcingKS);
if(bResultCheckKS.get(0).asString() == "error") {
return bResultCheckKS;
}
yInfo() << " [assignKinematicStructureByJoint] | for joint " << BPjoint << " | instance found : " << ksInstance;
//WRITE IN OPC
iCub->opc->checkout();
list<Entity*> lEntities = iCub->opc->EntitiesCache();
Bottle bListEntChanged;
for (auto& entity: lEntities) //go through all entity
{
yInfo() << "Checking if entity " << entity->name() << " has entitytype = bodypart : ----> " << entity->entity_type() ;
if (entity->entity_type() == "bodypart") //check bodypart entity
{
//pb with the casting: BPtemp is empty
Bodypart* BPtemp = dynamic_cast<Bodypart*>(entity);
if(!BPtemp) {
yDebug() << "Could not cast " << entity->name() << " to Bodypart";
continue;
}
if(BPtemp->m_joint_number == BPjoint) { //if corresponding joint : change it
//BPtemp->m_kinStruct_instance = ksInstance;
bListEntChanged.addString(BPtemp->name());
yInfo() << "Change" << BPtemp->name() << "to kinematic instance" << ksInstance;
iCub->opc->commit(BPtemp);
break;
}
}
}
yInfo() << "Out of the loop for checking entity in OPC, number of entityChanged : " << bListEntChanged.size() ;
if(bListEntChanged.isNull()){
yWarning() << "assignKinematicStructureByJoint | for joint " << BPjoint << " | no bodypart has been found with this joint!";
bOutput.addString("warning");
bOutput.addString("no bodypart has been found with this joint!");
return bOutput;
}
bOutput.addString("ack");
bOutput.addInt(ksInstance);
bOutput.addList() = bListEntChanged;
return bOutput;
}
