Bugfix in YarpRobotControl::Impl::setReferences

[GiulioRomualdi]

This fixes #270

[traversaro]

I am not sure of this logic, but this is not part of what is changed by the PR.

[S-Dafarra]

Actually, maxVelocityInDegPerSeconds is misleading, as it is instead the minimum velocity. Each joint is supposed to reach the desired position in the time specified by positioningDuration. If the joint is already close to the desired configuration, we don't want the joint to go too slow, so we set the desired velocity to maxVelocityInDegPerSeconds, that should be called minVelocityInDegPerSeconds.

[GiulioRomualdi]

Thank you @S-Dafarra I reneame maxVelocityInDegPerSeconds into minVelocityInDegPerSeconds

[S-Dafarra]

I am just checking the modification, so I might be missing something, but it seems that you are resizing this vector in two different places, in two different ways. Above I see:

this->positionControlRefSpeeds.resize(
        this->desiredJointValuesAndMode.index[IRobotControl::ControlMode::Position].size());

[GiulioRomualdi]

Hi @S-Dafarra, both the resize are required.

Indeed when setDriver() is called the motor control mode for each joint is retrieved.

https://github.com/dic-iit/bipedal-locomotion-framework/blob/317b86fffcbcd2f263bf76785c11269df5aa3ef7/src/RobotInterface/YarpImplementation/src/YarpRobotControl.cpp#L323

Suppose that all the joints are in position when you call

https://github.com/dic-iit/bipedal-locomotion-framework/blob/317b86fffcbcd2f263bf76785c11269df5aa3ef7/src/RobotInterface/YarpImplementation/src/YarpRobotControl.cpp#L586

You will have a segfault if this resize is missing:

https://github.com/dic-iit/bipedal-locomotion-framework/blob/317b86fffcbcd2f263bf76785c11269df5aa3ef7/src/RobotInterface/YarpImplementation/src/YarpRobotControl.cpp#L312

On the other hand

https://github.com/dic-iit/bipedal-locomotion-framework/blob/317b86fffcbcd2f263bf76785c11269df5aa3ef7/src/RobotInterface/YarpImplementation/src/YarpRobotControl.cpp#L208-L209

is required when you ask for a different control mode. (Now the number of joints in position mode may be different)

---

By the way, I think it is more clear to change the resize as

Suggested change

	this->positionControlRefSpeeds.resize(this->actuatedDOFs);
	this->positionControlRefSpeeds.resize(this->desiredJointValuesAndMode.index[IRobotControl::ControlMode::Position].size());

and call it after this line.

[S-Dafarra]

Actually, maxVelocityInDegPerSeconds is misleading, as it is instead the minimum velocity. Each joint is supposed to reach the desired position in the time specified by positioningDuration. If the joint is already close to the desired configuration, we don't want the joint to go too slow, so we set the desired velocity to maxVelocityInDegPerSeconds, that should be called minVelocityInDegPerSeconds.