diff --git a/src/libraries/skinDynLib/CMakeLists.txt b/src/libraries/skinDynLib/CMakeLists.txt index f43cfca6ab..40002e623a 100644 --- a/src/libraries/skinDynLib/CMakeLists.txt +++ b/src/libraries/skinDynLib/CMakeLists.txt @@ -25,7 +25,7 @@ set(folder_header include/iCub/skinDynLib/skinContact.h include/iCub/skinDynLib/dynContact.h include/iCub/skinDynLib/dynContactList.h include/iCub/skinDynLib/common.h - include/iCub/skinDynLib/rpcSkinManager.h + include/iCub/skinDynLib/rpcSkinManager.h include/iCub/skinDynLib/utils.h include/iCub/skinDynLib/Taxel.h include/iCub/skinDynLib/skinPart.h @@ -47,4 +47,4 @@ icub_export_library(${PROJECTNAME} INTERNAL_INCLUDE_DIRS ${PROJECT_SOURCE_DIR}/i DESTINATION include/iCub/skinDynLib FILES ${folder_header}) - + diff --git a/src/modules/skinManager/include/iCub/skinManager/compensator.h b/src/modules/skinManager/include/iCub/skinManager/compensator.h index 35b251683c..411bb562ed 100644 --- a/src/modules/skinManager/include/iCub/skinManager/compensator.h +++ b/src/modules/skinManager/include/iCub/skinManager/compensator.h @@ -40,6 +40,7 @@ #include "iCub/skinDynLib/skinContact.h" #include "iCub/skinDynLib/skinContactList.h" #include "iCub/skinDynLib/rpcSkinManager.h" +#include "iCub/skinDynLib/utils.h" using namespace std; using namespace yarp::os; @@ -153,6 +154,7 @@ class Compensator bool setContactCompensationGain(double gain); bool setMaxNeighborDistance(double d); bool setTaxelPosesFromFile(const char *filePath); + bool setTaxelPosesFromFileOld(const char *filePath); bool setTaxelPoses(const vector &poses); bool setTaxelPose(unsigned int taxelId, const Vector &pose); bool setTaxelPositions(const Vector &positions); diff --git a/src/modules/skinManager/src/compensationThread.cpp b/src/modules/skinManager/src/compensationThread.cpp index d112d5ec36..369dc5009e 100644 --- a/src/modules/skinManager/src/compensationThread.cpp +++ b/src/modules/skinManager/src/compensationThread.cpp @@ -137,7 +137,7 @@ bool CompensationThread::threadInit() skinEventsOn = false; Bottle &skinEventsConf = rf->findGroup("SKIN_EVENTS"); if(!skinEventsConf.isNull()){ - cout<< "SKIN_EVENTS section found\n"; + yDebug("SKIN_EVENTS section found"); string eventPortName = "/" + moduleName + "/skin_events:o"; // output skin events if(!skinEventsPort.open(eventPortName.c_str())) sendErrorMsg("Unable to open port "+eventPortName); @@ -154,7 +154,7 @@ bool CompensationThread::threadInit() }else{ FOR_ALL_PORTS(i){ // cout<< "Skin part "<< SkinPart_s[skinPartList->get(i).asInt()]<< endl; - yInfo("Skin part %s",SkinPart_s[skinPartList->get(i).asInt()].c_str()); + yDebug("Skin part %s",SkinPart_s[skinPartList->get(i).asInt()].c_str()); compensators[i]->setSkinPart((SkinPart)skinPartList->get(i).asInt()); } } @@ -173,7 +173,7 @@ bool CompensationThread::threadInit() else{ maxNeighDist = skinEventsConf.check("maxNeighborDist", Value(MAX_NEIGHBOR_DISTANCE)).asDouble(); - printf("Max neighbor distance: %f m\n", maxNeighDist); + yInfo("Max neighbor distance: %f m\n", maxNeighDist); FOR_ALL_PORTS(i){ if(compWorking[i]){ string taxelPosFile = taxelPosFiles->get(i).asString().c_str(); diff --git a/src/modules/skinManager/src/compensator.cpp b/src/modules/skinManager/src/compensator.cpp index 287a211197..4167b51b0c 100644 --- a/src/modules/skinManager/src/compensator.cpp +++ b/src/modules/skinManager/src/compensator.cpp @@ -419,8 +419,8 @@ void Compensator::updateBaseline(){ baselines[j] += change; mean_change += change; -// if(j<5) -// printf("%d touch detected %s, change %.6f, gain: %.3f\n", j, touchDetected[j]?"yes":"no", change, touchDetected[j]?contactCompensationGain:compensationGain); + // if(j<5) + // printf("%d touch detected %s, change %.6f, gain: %.3f\n", j, touchDetected[j]?"yes":"no", change, touchDetected[j]?contactCompensationGain:compensationGain); if(baselines[j]<0){ char* temp = new char[300]; @@ -707,6 +707,46 @@ bool Compensator::setMaxNeighborDistance(double d){ } bool Compensator::setTaxelPosesFromFile(const char *filePath){ + yarp::os::ResourceFinder rf; + rf.setVerbose(false); + rf.setDefaultContext("skinGui"); //overridden by --context parameter + rf.setDefaultConfigFile(filePath); //overridden by --from parameter + rf.configure(0,NULL); + rf.setVerbose(true); + + yarp::os::Bottle &calibration = rf.findGroup("calibration"); + if (calibration.isNull()) + { + return setTaxelPosesFromFileOld(filePath); + } + else + { + poseSem.wait(); + int size=calibration.size()-1; + skinDim=size; + taxelPos.resize(skinDim, zeros(3)); + taxelOri.resize(skinDim, zeros(3)); + taxelPoseConfidence.resize(skinDim, 0.0); + for (unsigned int i = 0; i < size; ++i) + { + if (i0.0) + taxelPoseConfidence[i] = 1.0; + } + computeNeighbors(); + poseSem.post(); + } + + return true; +} + +bool Compensator::setTaxelPosesFromFileOld(const char *filePath){ ifstream posFile; posFile.open(filePath); if (!posFile.is_open())