search

ami-iit / bipedal-locomotion-framework

Suite of libraries for achieving bipedal locomotion on humanoid robots
https://ami-iit.github.io/bipedal-locomotion-framework/
BSD 3-Clause "New" or "Revised" License
155 stars 38 forks source link

YarpSensorBridge::getJointPositions() returns an empty vector if jointPositions size is zero #185

Open GiulioRomualdi opened 3 years ago

GiulioRomualdi commented 3 years ago

If jointPositions size is 0 YarpSensorBridge::getJointPositions() return an empty vector https://github.com/dic-iit/bipedal-locomotion-framework/blob/e2385fc708e4bbb2d4b2553601f3ffdd735d976d/src/RobotInterface/YarpImplementation/src/YarpSensorBridge.cpp#L235-L242

The code does not crash if compiled in release, if it's compiled in debug the following exception is thrown

blf-cartesian-trajectory-player: /usr/include/eigen3/Eigen/src/Core/DenseBase.h:257: void Eigen::DenseBase<Derived>::resize(Eigen::Index, Eigen::Index) [with Derived = Eigen::Ref<Eigen::Matrix<double, -1, 1> >; Eigen::Index = long int]: Assertion `rows == this->rows() && cols == this->cols() && "DenseBase::resize() does not actually allow one to resize."' failed.

apparently the operator=() do not resize the eigen::ref

@prashanthr05, @S-Dafarra do you have any clue>

GiulioRomualdi commented 3 years ago

Related problem: https://stackoverflow.com/questions/46339455/workaround-for-resizing-eigenref

S-Dafarra commented 3 years ago

Ref, like map or span cannot resize the underlying container because it does not own the memory. This is the reason why GenericContainer::Vector was designed in the first place 😁

GiulioRomualdi commented 3 years ago

For the time being, we can add a check into getJointPositions and similar functions

prashanthr05 commented 3 years ago

If I remember right, we switched from the usage of GenericContainer to Eigen::Ref and added this warning saying that the user should pass a resized vector.

https://github.com/dic-iit/bipedal-locomotion-framework/blob/e2385fc708e4bbb2d4b2553601f3ffdd735d976d/src/RobotInterface/YarpImplementation/include/BipedalLocomotion/RobotInterface/YarpSensorBridge.h#L209-L211

and looks like the addition of checks were not done in the YarpSensorBridge implementation. So yes, we have to update the code in YarpSensorBridge.

S-Dafarra commented 3 years ago

I think that this is strongly related to https://github.com/dic-iit/bipedal-locomotion-framework/issues/95. I think it is necessary to clearly define a guideline.

GiulioRomualdi commented 3 years ago

Otherwise we may change 

 bool YarpSensorBridge::getJointPositions(Eigen::Ref<Eigen::VectorXd> jointPositions, 
                                          double* receiveTimeInSeconds)

into 

Eigen::Ref<const Eigen::VectorXd> YarpSensorBridge::getJointPositions(double* receiveTimeInSeconds)

or 

GenericVector<const double> YarpSensorBridge::getJointPositions(double* receiveTimeInSeconds)
S-Dafarra commented 3 years ago

Otherwise we may change

 bool YarpSensorBridge::getJointPositions(Eigen::Ref<Eigen::VectorXd> jointPositions, 
                                          double* receiveTimeInSeconds)

into

Eigen::Ref<const Eigen::VectorXd> YarpSensorBridge::getJointPositions(double* receiveTimeInSeconds)

or

GenericVector<const double> YarpSensorBridge::getJointPositions(double* receiveTimeInSeconds)

See https://github.com/dic-iit/bipedal-locomotion-framework/issues/124