From 7c0de31db804c1b73c9ce86b13242db52e9dc343 Mon Sep 17 00:00:00 2001 From: Ugo Pattacini Date: Wed, 20 Sep 2023 10:41:06 +0200 Subject: [PATCH] update MAIS reading modules to use MAS --- src/libraries/icubmod/imuFilter/ImuFilter.h | 2 +- .../icubmod/imuFilter/PassThroughInertial.cpp | 2 +- .../include/iCub/perception/sensors.h | 41 +++++- .../include/iCub/perception/springyFingers.h | 10 +- .../perceptiveModels/src/sensors.cpp | 45 +++++- .../perceptiveModels/src/springyFingers.cpp | 136 +++++++++--------- src/modules/wholeBodyDynamics/main.cpp | 10 -- .../wholeBodyDynamics/observerThread.h | 2 +- src/tools/depth2kin/include/module.h | 6 +- src/tools/depth2kin/src/module.cpp | 41 +++--- 10 files changed, 186 insertions(+), 109 deletions(-) diff --git a/src/libraries/icubmod/imuFilter/ImuFilter.h b/src/libraries/icubmod/imuFilter/ImuFilter.h index 0c2bd87582..30f3371b32 100644 --- a/src/libraries/icubmod/imuFilter/ImuFilter.h +++ b/src/libraries/icubmod/imuFilter/ImuFilter.h @@ -55,7 +55,7 @@ namespace yarp { This device driver applies filtering to remove gyro bias. \n At startup it tries to attach directly to the IMU devcice of the specified robot -or through a MultipleAnalogSensorClient(using the subdevice mechanism). +or through a MultipleAnalogSensorsClient(using the subdevice mechanism). \section parameters_sec Parameters diff --git a/src/libraries/icubmod/imuFilter/PassThroughInertial.cpp b/src/libraries/icubmod/imuFilter/PassThroughInertial.cpp index c8513b4e76..14f99211e0 100644 --- a/src/libraries/icubmod/imuFilter/PassThroughInertial.cpp +++ b/src/libraries/icubmod/imuFilter/PassThroughInertial.cpp @@ -36,7 +36,7 @@ bool PassThroughInertial::open(yarp::os::Searchable& config) //TODO: still to be auto subdeviceName = config.find("subdevice").asString(); yarp::os::Property masClientProp; masClientProp.fromString(config.toString()); - masClientProp.put("device", subdeviceName); // "multipleanalogsensorclient" + masClientProp.put("device", subdeviceName); // "multipleanalogsensorsclient" masClientProp.put("local", config.find("proxy_local").asString()); masClientProp.put("remote", config.find("proxy_remote").asString()); diff --git a/src/libraries/perceptiveModels/include/iCub/perception/sensors.h b/src/libraries/perceptiveModels/include/iCub/perception/sensors.h index 20ce4ca617..75653cb197 100644 --- a/src/libraries/perceptiveModels/include/iCub/perception/sensors.h +++ b/src/libraries/perceptiveModels/include/iCub/perception/sensors.h @@ -161,7 +161,7 @@ class Sensor /** * @ingroup percmod_Implementations * -* This class implements the reading of motor joints encoders. +* This class implements the reading of joints encoders. */ class SensorEncoders : public Sensor { @@ -185,7 +185,44 @@ class SensorEncoders : public Sensor /** * Retrieve the sensor joint value. - * @param in a value containing the current joint position. + * @param in a value containing the encoder value. + * @return true/false on success/failure. + */ + bool getOutput(yarp::os::Value &in) const; +}; + + +/** +* @ingroup percmod_Implementations +* +* This class implements the reading of encoders through +* MultipleAnalogSensors (MAS) interfaces. +*/ +class SensorEncoderArrays : public Sensor +{ +protected: + int num_arrays; + int index_array; + int index_element; + +public: + /** + * Configure the sensor. + * @param source a pointer to the yarp::dev::IEncoderArrays + * interface. + * @param options a Property containing the configuration + * parameters. Available options are:\n + * name: the name of the sensor.\n + * num_arrays: the number of available arrays.\n + * index_array: the index of the array to be sensed. + * index_element: the index of the element of the array. + * @return true/false on success/failure. + */ + bool configure(void *source, const yarp::os::Property &options); + + /** + * Retrieve the sensor encoder value. + * @param in a value containing the encoder value. * @return true/false on success/failure. */ bool getOutput(yarp::os::Value &in) const; diff --git a/src/libraries/perceptiveModels/include/iCub/perception/springyFingers.h b/src/libraries/perceptiveModels/include/iCub/perception/springyFingers.h index a6313c16ca..60f7789aad 100644 --- a/src/libraries/perceptiveModels/include/iCub/perception/springyFingers.h +++ b/src/libraries/perceptiveModels/include/iCub/perception/springyFingers.h @@ -208,13 +208,13 @@ class SpringyFingersModel : public virtual Model std::string robot; std::string carrier; - SensorEncoders sensEncs[5]; - SensorPort sensPort[12]; - SpringyFinger fingers[5]; + SensorEncoders sensEncs[5]; + SensorEncoderArrays sensEncArrays[12]; + SpringyFinger fingers[5]; bool configured; - yarp::os::BufferedPort *port; - yarp::dev::PolyDriver driver; + yarp::dev::PolyDriver drvEncs; + yarp::dev::PolyDriver drvMAIS; std::mutex mtx; diff --git a/src/libraries/perceptiveModels/src/sensors.cpp b/src/libraries/perceptiveModels/src/sensors.cpp index 9cebabe065..5e81155a86 100644 --- a/src/libraries/perceptiveModels/src/sensors.cpp +++ b/src/libraries/perceptiveModels/src/sensors.cpp @@ -18,6 +18,7 @@ #include #include #include +#include #include #include @@ -59,7 +60,7 @@ bool SensorEncoders::configure(void *source, const Property &options) bool SensorEncoders::getOutput(Value &in) const { if (!configured) - return false; + return false; Vector vect(size); static_cast(source)->getEncoders(vect.data()); @@ -69,6 +70,48 @@ bool SensorEncoders::getOutput(Value &in) const } +/************************************************************************/ +bool SensorEncoderArrays::configure(void *source, const Property &options) +{ + if ((source==NULL) || !options.check("name") || !options.check("num_arrays") || + !options.check("index_array") || !options.check("index_element")) + return false; + + this->source=source; + name=options.find("name").asString(); + num_arrays=options.find("num_arrays").asInt32(); + index_array=options.find("index_array").asInt32(); + index_element=options.find("index_element").asInt32(); + + return configured=true; +} + + +/************************************************************************/ +bool SensorEncoderArrays::getOutput(Value &in) const +{ + auto* iencarray=static_cast(source); + if (!configured) + return false; + + if ((index_array<0) || (index_array>=iencarray->getNrOfEncoderArrays())) + return false; + + if ((index_element<0) || (index_element>=iencarray->getEncoderArraySize(index_array))) + return false; + + if (iencarray->getEncoderArrayStatus(index_array)!=MAS_OK) + return false; + + double stamp; + Vector vect(iencarray->getEncoderArraySize(index_array)); + iencarray->getEncoderArrayMeasure(index_array,vect,stamp); + in=Value(vect[index_element]); + + return true; +} + + /************************************************************************/ bool SensorPort::configure(void *source, const Property &options) { diff --git a/src/libraries/perceptiveModels/src/springyFingers.cpp b/src/libraries/perceptiveModels/src/springyFingers.cpp index c4f294470f..2eef559685 100644 --- a/src/libraries/perceptiveModels/src/springyFingers.cpp +++ b/src/libraries/perceptiveModels/src/springyFingers.cpp @@ -260,7 +260,6 @@ bool SpringyFinger::calibrate(const Property &options) /************************************************************************/ SpringyFingersModel::SpringyFingersModel() { - port=new iCub::perception::Port; configured=false; } @@ -286,26 +285,28 @@ bool SpringyFingersModel::fromProperty(const Property &options) string part_motor=string(type+"_arm"); string part_analog=string(type+"_hand"); - Property prop; - prop.put("device","remote_controlboard"); - prop.put("remote","/"+robot+"/"+part_motor); - prop.put("local","/"+name+"/"+part_motor); - if (!driver.open(prop)) + Property propEncs; + propEncs.put("device","remote_controlboard"); + propEncs.put("remote","/"+robot+"/"+part_motor); + propEncs.put("local","/"+name+"/"+part_motor); + if (!drvEncs.open(propEncs)) return false; - - port->open("/"+name+"/"+part_analog+"/analog:i"); - string analogPortName("/"+robot+"/"+part_analog+"/analog:o"); - if (!Network::connect(analogPortName,port->getName(),carrier)) + + Property propMAIS; + propMAIS.put("device","multipleanalogsensorsclient"); + propMAIS.put("remote","/"+robot+"/"+part_analog+"/MAIS"); + propMAIS.put("local","/"+name+"/"+part_analog+"/MAIS"); + if (!drvMAIS.open(propMAIS)) { - printMessage(log::error,1,"unable to connect to %s",analogPortName.c_str()); + printMessage(log::error,1,"unable to connect to %s",propMAIS.find("remote").asString().c_str()); close(); return false; } - - IEncoders *ienc; driver.view(ienc); + + printMessage(log::info,1,"configuring joint encoders sensors ..."); + IEncoders *ienc; drvEncs.view(ienc); int nAxes; ienc->getAxes(&nAxes); - printMessage(log::info,1,"configuring interface-based sensors ..."); Property propGen; propGen.put("name","In_0"); propGen.put("size",nAxes); @@ -324,33 +325,39 @@ bool SpringyFingersModel::fromProperty(const Property &options) sensors_ok&=sensEncs[3].configure(pEncs,propRing); sensors_ok&=sensEncs[4].configure(pEncs,propLittle); - printMessage(log::info,1,"configuring port-based sensors ..."); - Property thumb_mp( "(name Out_0) (index 1)" ); - Property thumb_ip( "(name Out_1) (index 2)" ); - Property index_mp( "(name Out_0) (index 4)" ); - Property index_ip( "(name Out_1) (index 5)" ); - Property middle_mp( "(name Out_0) (index 7)" ); - Property middle_ip( "(name Out_1) (index 8)" ); - Property ring_mp( "(name Out_0) (index 9)" ); - Property ring_pip( "(name Out_1) (index 10)"); - Property ring_dip( "(name Out_2) (index 11)"); - Property little_mp( "(name Out_0) (index 12)"); - Property little_pip("(name Out_1) (index 13)"); - Property little_dip("(name Out_2) (index 14)"); - - void *pPort=static_cast(port); - sensors_ok&=sensPort[0].configure(pPort,thumb_mp); - sensors_ok&=sensPort[1].configure(pPort,thumb_ip); - sensors_ok&=sensPort[2].configure(pPort,index_mp); - sensors_ok&=sensPort[3].configure(pPort,index_ip); - sensors_ok&=sensPort[4].configure(pPort,middle_mp); - sensors_ok&=sensPort[5].configure(pPort,middle_ip); - sensors_ok&=sensPort[6].configure(pPort,ring_mp); - sensors_ok&=sensPort[7].configure(pPort,ring_pip); - sensors_ok&=sensPort[8].configure(pPort,ring_dip); - sensors_ok&=sensPort[9].configure(pPort,little_mp); - sensors_ok&=sensPort[10].configure(pPort,little_pip); - sensors_ok&=sensPort[11].configure(pPort,little_dip); + printMessage(log::info,1,"configuring analog encoders sensors ..."); + IEncoderArrays *iencarray; drvMAIS.view(iencarray); + + propGen.clear(); + propGen.put("num_arrays",(int)iencarray->getNrOfEncoderArrays()); + propGen.put("index_array",0); + + Property thumb_mp=propGen; thumb_mp.put( "name","Out_0"); thumb_mp.put("index_element","1"); + Property thumb_ip=propGen; thumb_ip.put( "name","Out_1"); thumb_ip.put("index_element","2"); + Property index_mp=propGen; index_mp.put( "name","Out_0"); index_mp.put("index_element","4"); + Property index_ip=propGen; index_ip.put( "name","Out_1"); index_ip.put("index_element","5"); + Property middle_mp=propGen; middle_mp.put( "name","Out_0"); middle_mp.put("index_element","7"); + Property middle_ip=propGen; middle_ip.put( "name","Out_1"); middle_ip.put("index_element","8"); + Property ring_mp=propGen; ring_mp.put( "name","Out_0"); ring_mp.put("index_element","9"); + Property ring_pip=propGen; ring_pip.put( "name","Out_1"); ring_pip.put("index_element","10"); + Property ring_dip=propGen; ring_dip.put( "name","Out_2"); ring_dip.put("index_element","11"); + Property little_mp=propGen; little_mp.put( "name","Out_0"); little_mp.put("index_element","12"); + Property little_pip=propGen; little_pip.put("name","Out_1"); little_pip.put("index_element","13"); + Property little_dip=propGen; little_dip.put("name","Out_2"); little_dip.put("index_element","14"); + + void *pEncArrays=static_cast(iencarray); + sensors_ok&=sensEncArrays[0].configure(pEncArrays,thumb_mp); + sensors_ok&=sensEncArrays[1].configure(pEncArrays,thumb_ip); + sensors_ok&=sensEncArrays[2].configure(pEncArrays,index_mp); + sensors_ok&=sensEncArrays[3].configure(pEncArrays,index_ip); + sensors_ok&=sensEncArrays[4].configure(pEncArrays,middle_mp); + sensors_ok&=sensEncArrays[5].configure(pEncArrays,middle_ip); + sensors_ok&=sensEncArrays[6].configure(pEncArrays,ring_mp); + sensors_ok&=sensEncArrays[7].configure(pEncArrays,ring_pip); + sensors_ok&=sensEncArrays[8].configure(pEncArrays,ring_dip); + sensors_ok&=sensEncArrays[9].configure(pEncArrays,little_mp); + sensors_ok&=sensEncArrays[10].configure(pEncArrays,little_pip); + sensors_ok&=sensEncArrays[11].configure(pEncArrays,little_dip); if (!sensors_ok) { @@ -382,26 +389,26 @@ bool SpringyFingersModel::fromProperty(const Property &options) printMessage(log::info,1,"attaching sensors to fingers ..."); fingers[0].attachSensor(sensEncs[0]); - fingers[0].attachSensor(sensPort[0]); - fingers[0].attachSensor(sensPort[1]); + fingers[0].attachSensor(sensEncArrays[0]); + fingers[0].attachSensor(sensEncArrays[1]); fingers[1].attachSensor(sensEncs[1]); - fingers[1].attachSensor(sensPort[2]); - fingers[1].attachSensor(sensPort[3]); + fingers[1].attachSensor(sensEncArrays[2]); + fingers[1].attachSensor(sensEncArrays[3]); fingers[2].attachSensor(sensEncs[2]); - fingers[2].attachSensor(sensPort[4]); - fingers[2].attachSensor(sensPort[5]); + fingers[2].attachSensor(sensEncArrays[4]); + fingers[2].attachSensor(sensEncArrays[5]); fingers[3].attachSensor(sensEncs[3]); - fingers[3].attachSensor(sensPort[6]); - fingers[3].attachSensor(sensPort[7]); - fingers[3].attachSensor(sensPort[8]); + fingers[3].attachSensor(sensEncArrays[6]); + fingers[3].attachSensor(sensEncArrays[7]); + fingers[3].attachSensor(sensEncArrays[8]); fingers[4].attachSensor(sensEncs[4]); - fingers[4].attachSensor(sensPort[9]); - fingers[4].attachSensor(sensPort[10]); - fingers[4].attachSensor(sensPort[11]); + fingers[4].attachSensor(sensEncArrays[9]); + fingers[4].attachSensor(sensEncArrays[10]); + fingers[4].attachSensor(sensEncArrays[11]); attachNode(fingers[0]); attachNode(fingers[1]); @@ -506,10 +513,10 @@ bool SpringyFingersModel::calibrate(const Property &options) return false; } - IControlMode *imod; driver.view(imod); - IControlLimits *ilim; driver.view(ilim); - IEncoders *ienc; driver.view(ienc); - IPositionControl *ipos; driver.view(ipos); + IControlMode *imod; drvEncs.view(imod); + IControlLimits *ilim; drvEncs.view(ilim); + IEncoders *ienc; drvEncs.view(ienc); + IPositionControl *ipos; drvEncs.view(ipos); int nAxes; ienc->getAxes(&nAxes); Vector qmin(nAxes),qmax(nAxes),vel(nAxes),acc(nAxes); @@ -726,9 +733,9 @@ void SpringyFingersModel::calibrateFinger(SpringyFinger &finger, const int joint double timeout=2.0*(_max-_min)/finger.getCalibVel(); mtx.lock(); - IControlMode *imod; driver.view(imod); - IEncoders *ienc; driver.view(ienc); - IPositionControl *ipos; driver.view(ipos); + IControlMode *imod; drvEncs.view(imod); + IEncoders *ienc; drvEncs.view(ienc); + IPositionControl *ipos; drvEncs.view(ipos); mtx.unlock(); // workaround @@ -812,11 +819,11 @@ void SpringyFingersModel::close() { printMessage(log::info,1,"closing ..."); - if (driver.isValid()) - driver.close(); + if (drvEncs.isValid()) + drvEncs.close(); - if (!port->isClosed()) - port->close(); + if (drvMAIS.isValid()) + drvMAIS.close(); nodes.clear(); @@ -828,7 +835,6 @@ void SpringyFingersModel::close() SpringyFingersModel::~SpringyFingersModel() { close(); - delete port; } diff --git a/src/modules/wholeBodyDynamics/main.cpp b/src/modules/wholeBodyDynamics/main.cpp index 33fe4dbafc..a966329b92 100644 --- a/src/modules/wholeBodyDynamics/main.cpp +++ b/src/modules/wholeBodyDynamics/main.cpp @@ -96,16 +96,6 @@ By launching the following command: wholeBodyDynamics --rate 10 \endcode -the module will create the listening port -/wholeBodyDynamics/right_arm/FT:i for the acquisition of data -vector coming for istance from the right arm analog port. - -Try now the following: - -\code -yarp connect /icub/right_arm/analog:o /wholeBodyDynamics/right_arm/FT:i -\endcode - \author Matteo Fumagalli This file can be edited at src/wholeBodyDynamics/main.cpp. diff --git a/src/modules/wholeBodyDynamics/observerThread.h b/src/modules/wholeBodyDynamics/observerThread.h index 029bb8c254..4a9c38bca1 100644 --- a/src/modules/wholeBodyDynamics/observerThread.h +++ b/src/modules/wholeBodyDynamics/observerThread.h @@ -43,7 +43,7 @@ using namespace std; constexpr int8_t MAX_JN = 12; constexpr int8_t MAX_FILTER_ORDER = 6; -constexpr float_t SKIN_EVENTS_TIMEOUT = 0.2; // max time (in sec) a contact is kept without reading anything from the skin events port +constexpr float_t SKIN_EVENTS_TIMEOUT = 0.2F; // max time (in sec) a contact is kept without reading anything from the skin events port enum thread_status_enum {STATUS_OK=0, STATUS_DISCONNECTED}; enum calib_enum {CALIB_ALL=0, CALIB_ARMS, CALIB_LEGS, CALIB_FEET}; diff --git a/src/tools/depth2kin/include/module.h b/src/tools/depth2kin/include/module.h index 9e3c064d87..a6500ce935 100644 --- a/src/tools/depth2kin/include/module.h +++ b/src/tools/depth2kin/include/module.h @@ -64,15 +64,15 @@ class CalibModule : public RFModule, public depth2kin_IDL ResourceFinder *rf; PolyDriver drvArmL; PolyDriver drvArmR; - PolyDriver drvAnalogL; - PolyDriver drvAnalogR; + PolyDriver drvMAIS_L; + PolyDriver drvMAIS_R; PolyDriver drvCartL; PolyDriver drvCartR; PolyDriver drvGaze; IControlMode *imods; IEncoders *iencs; IPositionControl *iposs; - IAnalogSensor *ianalog; + IEncoderArrays *iencarray; ICartesianControl *iarm; IGazeControl *igaze; diff --git a/src/tools/depth2kin/src/module.cpp b/src/tools/depth2kin/src/module.cpp index 78204c0a09..872f63db88 100644 --- a/src/tools/depth2kin/src/module.cpp +++ b/src/tools/depth2kin/src/module.cpp @@ -551,8 +551,9 @@ void CalibModule::prepareRobot() iencs->getEncoders(encs.data()); poss=encs.subVector(i0,nEncs-1); - Vector analogs; - ianalog->read(analogs); + double stamp; + Vector analogs(iencarray->getEncoderArraySize(0)); + iencarray->getEncoderArrayMeasure(0,analogs,stamp); Vector joints; iCubFinger finger=this->finger; @@ -952,17 +953,17 @@ bool CalibModule::configure(ResourceFinder &rf) if (!drvArmR.open(optionArmR)) yWarning("Position right_arm controller not available!"); - Property optionAnalogL("(device analogsensorclient)"); - optionAnalogL.put("remote","/"+robot+"/left_hand/analog:o"); - optionAnalogL.put("local","/"+name+"/analog/left"); - if (!drvAnalogL.open(optionAnalogL)) - yWarning("Analog left_hand sensor not available!"); + Property optionMAIS_L("(device multipleanalogsensorsclient)"); + optionMAIS_L.put("remote","/"+robot+"/left_hand/MAIS"); + optionMAIS_L.put("local","/"+name+"/MAIS/left"); + if (!drvMAIS_L.open(optionMAIS_L)) + yWarning("MAIS left_hand sensor not available!"); - Property optionAnalogR("(device analogsensorclient)"); - optionAnalogR.put("remote","/"+robot+"/right_hand/analog:o"); - optionAnalogR.put("local","/"+name+"/analog/right"); - if (!drvAnalogR.open(optionAnalogR)) - yWarning("Analog right_hand sensor not available!"); + Property optionMAIS_R("(device multipleanalogsensorsclient)"); + optionMAIS_R.put("remote","/"+robot+"/right_hand/MAIS"); + optionMAIS_R.put("local","/"+name+"/MAIS/right"); + if (!drvMAIS_R.open(optionMAIS_R)) + yWarning("MAIS right_hand sensor not available!"); Property optionCartL("(device cartesiancontrollerclient)"); optionCartL.put("remote","/"+robot+"/cartesianController/left_arm"); @@ -983,8 +984,8 @@ bool CalibModule::configure(ResourceFinder &rf) yWarning("Gaze controller not available!"); // set up some global vars - useArmL=(drvArmL.isValid() && drvAnalogL.isValid() && drvCartL.isValid()); - useArmR=(drvArmR.isValid() && drvAnalogR.isValid() && drvCartR.isValid()); + useArmL=(drvArmL.isValid() && drvMAIS_L.isValid() && drvCartL.isValid()); + useArmR=(drvArmR.isValid() && drvMAIS_R.isValid() && drvCartR.isValid()); selectArmEnabled=(useArmL && useArmR); // quitting condition @@ -1005,7 +1006,7 @@ bool CalibModule::configure(ResourceFinder &rf) (arm=="left")?drvArmL.view(iencs):drvArmR.view(iencs); (arm=="left")?drvArmL.view(iposs):drvArmR.view(iposs); (arm=="left")?drvArmL.view(ilim):drvArmR.view(ilim); - (arm=="left")?drvAnalogL.view(ianalog):drvAnalogR.view(ianalog); + (arm=="left")?drvMAIS_L.view(iencarray):drvMAIS_R.view(iencarray); (arm=="left")?drvCartL.view(iarm):drvCartR.view(iarm); drvGaze.view(igaze); iencs->getAxes(&nEncs); @@ -1368,7 +1369,7 @@ bool CalibModule::setArm(const string &arm) (arm=="left")?drvCartL.view(iarm):drvCartR.view(iarm); (arm=="left")?drvArmL.view(iencs):drvArmR.view(iencs); (arm=="left")?drvArmL.view(iposs):drvArmR.view(iposs); - (arm=="left")?drvAnalogL.view(ianalog):drvAnalogR.view(ianalog); + (arm=="left")?drvMAIS_L.view(iencarray):drvMAIS_R.view(iencarray); finger=iCubFinger(arm+"_index"); return true; } @@ -1799,11 +1800,11 @@ void CalibModule::terminate() if (drvArmR.isValid()) drvArmR.close(); - if (drvAnalogL.isValid()) - drvAnalogL.close(); + if (drvMAIS_L.isValid()) + drvMAIS_L.close(); - if (drvAnalogR.isValid()) - drvAnalogR.close(); + if (drvMAIS_R.isValid()) + drvMAIS_R.close(); if (drvCartL.isValid()) drvCartL.close();