Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Bugfix in YarpRobotControl::Impl::setReferences #271

Merged
merged 5 commits into from
Apr 6, 2021
Merged

Bugfix in YarpRobotControl::Impl::setReferences #271

Show file tree
Hide file tree
Changes from 2 commits
Commits
Show all changes
5 commits
Select commit Hold shift + click to select a range
b5be2e7
Use spdlog in YarpRobotControl class
GiulioRomualdi Apr 3, 2021
317b86f
Call setRefSpeeds() only once in YarpRobotControl
GiulioRomualdi Apr 3, 2021
8659613
YarpRobotControl positionControlRefSpeeds is now resized using only t…
GiulioRomualdi Apr 6, 2021
15ce6e7
Rename maxVelocityInDegPerSecond into minVelocityInDegPerSecond in Ya…
GiulioRomualdi Apr 6, 2021
786fc80
Update the CHANGELOG.md
GiulioRomualdi Apr 6, 2021
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 1 addition & 1 deletion src/RobotInterface/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -11,6 +11,7 @@ add_bipedal_locomotion_library(
SOURCES src/IRobotControl.cpp
PUBLIC_HEADERS ${H_PREFIX}/ISensorBridge.h ${H_PREFIX}/IRobotControl.h
PUBLIC_LINK_LIBRARIES BipedalLocomotion::ParametersHandler Eigen3::Eigen
PRIVATE_LINK_LIBRARIES BipedalLocomotion::TextLogging
SUBDIRECTORIES YarpImplementation tests)
endif()

Expand All @@ -23,4 +24,3 @@ add_bipedal_locomotion_library(
PUBLIC_LINK_LIBRARIES BipedalLocomotion::ParametersHandler ${OpenCV_LIBS} ${PCL_LIBRARIES}
INSTALLATION_FOLDER RobotInterface)
endif()

137 changes: 73 additions & 64 deletions src/RobotInterface/YarpImplementation/src/YarpRobotControl.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -20,6 +20,7 @@
#include <yarp/os/Time.h>

#include <BipedalLocomotion/RobotInterface/YarpRobotControl.h>
#include <BipedalLocomotion/TextLogging/Logger.h>

using namespace BipedalLocomotion::RobotInterface;

Expand Down Expand Up @@ -66,6 +67,8 @@ struct YarpRobotControl::Impl
JointsControlValuesAndMode desiredJointValuesAndMode; /**< Struct containing the information
regarding the desired joint value and
the control mode */
std::vector<double> positionControlRefSpeeds; /**< Vector containing the ref speed in
deg/seconds for the position control joints. */

double positioningDuration{0.0}; /**< Duration of the trajectory generated when the joint is
controlled in position mode */
Expand Down Expand Up @@ -132,11 +135,11 @@ struct YarpRobotControl::Impl

bool getControlModes()
{
constexpr std::string_view errorPrefix = "[YarpRobotControl::Impl::getControlModes] ";
constexpr auto errorPrefix = "[YarpRobotControl::Impl::getControlModes]";

if (this->controlModeInterface == nullptr)
{
std::cerr << errorPrefix << "The control mode I/F is not ready." << std::endl;
log()->error("{} The control mode I/F is not ready.", errorPrefix);
return false;
}

Expand All @@ -146,7 +149,7 @@ struct YarpRobotControl::Impl
{
if (++counter == this->maxReadingAttempts)
{
std::cerr << errorPrefix << "Error while reading the control mode." << std::endl;
log()->error("{} Error while reading the control mode.", errorPrefix);
return false;
}

Expand All @@ -164,11 +167,11 @@ struct YarpRobotControl::Impl

bool setControlModes(const std::vector<IRobotControl::ControlMode>& controlModes)
{
constexpr std::string_view errorPrefix = "[YarpRobotControl::Impl::setControlModes] ";
constexpr auto errorPrefix = "[YarpRobotControl::Impl::setControlModes]";

if (this->controlModeInterface == nullptr)
{
std::cerr << errorPrefix << "The control mode I/F is not ready." << std::endl;
log()->error("{} The control mode I/F is not ready.", errorPrefix);
return false;
}

Expand All @@ -191,7 +194,7 @@ struct YarpRobotControl::Impl
// set the control mode
if (!this->controlModeInterface->setControlModes(this->controlModesYarp.data()))
{
std::cerr << errorPrefix << "Error settings the control mode." << std::endl;
log()->error("{} Error settings the control mode.", errorPrefix);
return false;
}

Expand All @@ -201,16 +204,20 @@ struct YarpRobotControl::Impl
this->desiredJointValuesAndMode.value[mode].resize(indeces.size());
}

// resize the position control reference speed vector
this->positionControlRefSpeeds.resize(
this->desiredJointValuesAndMode.index[IRobotControl::ControlMode::Position].size());

return true;
}

bool getJointPos()
{
constexpr std::string_view errorPrefix = "[YarpRobotControl::Impl::getJointPos] ";
constexpr auto errorPrefix = "[YarpRobotControl::Impl::getJointPos]";

if (this->encodersInterface == nullptr)
{
std::cerr << errorPrefix << "The encoder I/F is not ready." << std::endl;
log()->error("{} The control mode I/F is not ready.", errorPrefix);
return false;
}

Expand All @@ -220,7 +227,7 @@ struct YarpRobotControl::Impl
{
if (++counter == this->maxReadingAttempts)
{
std::cerr << errorPrefix << "Error while reading the encoders." << std::endl;
log()->error("{} Error while reading the encoders.", errorPrefix);
return false;
}

Expand All @@ -236,65 +243,63 @@ struct YarpRobotControl::Impl

bool setDriver(std::shared_ptr<yarp::dev::PolyDriver> robotDevice)
{
constexpr std::string_view errorPrefix = "[YarpRobotControl::Impl::setDriver] ";
constexpr auto errorPrefix = "[YarpRobotControl::Impl::setDriver]";

if (robotDevice == nullptr)
{
std::cerr << errorPrefix
<< "The robotDevice is pointing to an non initialized memory."
<< std::endl;
log()->error("{} The robotDevice is pointing to an non initialized memory.",
errorPrefix);
return false;
}

// obtain the interfaces
if (!robotDevice->view(encodersInterface) || encodersInterface == nullptr)
{
std::cerr << errorPrefix << "Cannot load the IEncodersTimed interface." << std::endl;
log()->error("{} Cannot load the IEncodersTimed interface.", errorPrefix);
return false;
}

if (!robotDevice->view(positionInterface) || positionInterface == nullptr)
{
std::cerr << errorPrefix << "Cannot load the IPositionControl interface." << std::endl;
log()->error("{} Cannot load the IPositionControl interface.", errorPrefix);
return false;
}

if (!robotDevice->view(positionDirectInterface) || positionDirectInterface == nullptr)
{
std::cerr << errorPrefix << "Cannot load the IPositionDirect interface." << std::endl;
log()->error("{} Cannot load the IPositionDirect interface.", errorPrefix);
return false;
}

if (!robotDevice->view(velocityInterface) || velocityInterface == nullptr)
{
std::cerr << errorPrefix << "Cannot load the IVelocityInterface interface."
<< std::endl;
log()->error("{} Cannot load the IVelocityControl interface.", errorPrefix);
return false;
}

if (!robotDevice->view(torqueInterface) || torqueInterface == nullptr)
{
std::cerr << errorPrefix << "Cannot load the ITorqueInterface interface." << std::endl;
log()->error("{} Cannot load the ITorqueControl interface.", errorPrefix);
return false;
}

if (!robotDevice->view(controlModeInterface) || controlModeInterface == nullptr)
{
std::cerr << errorPrefix << "Cannot load the IControlMode interface." << std::endl;
log()->error("{} Cannot load the IControlMode interface.", errorPrefix);
return false;
}

if (!robotDevice->view(axisInfoInterface) || axisInfoInterface == nullptr)
{
std::cerr << errorPrefix << "Cannot load the IAxisInfo interface." << std::endl;
log()->error("{} Cannot load the IAxisInfo interface.", errorPrefix);
return false;
}

// get the number of degree of freedom
int dofs = 0;
if (!encodersInterface->getAxes(&dofs))
{
std::cerr << errorPrefix << "Cannot get the actuated DoFs." << std::endl;
log()->error("{} Cannot get the actuated DoFs.", errorPrefix);
return false;
}
this->actuatedDOFs = dofs;
Expand All @@ -304,7 +309,7 @@ struct YarpRobotControl::Impl
this->controlModes.resize(this->actuatedDOFs);
this->controlModesYarp.resize(this->actuatedDOFs);
this->axesName.resize(this->actuatedDOFs);

this->positionControlRefSpeeds.resize(this->actuatedDOFs);
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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());

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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.


// populate the axesName vector
for (int i = 0; i < this->actuatedDOFs; i++)
Expand All @@ -317,7 +322,7 @@ struct YarpRobotControl::Impl

if (!this->getControlModes())
{
std::cerr << errorPrefix << "Unable to get the control modes." << std::endl;
log()->error("{} Unable to get the control modes.", errorPrefix);
return false;
}

Expand Down Expand Up @@ -407,11 +412,11 @@ struct YarpRobotControl::Impl

bool setReferences(Eigen::Ref<const Eigen::VectorXd> jointValues)
{
constexpr std::string_view errorPrefix = "[YarpRobotControl::Impl::setReferences] ";
constexpr auto errorPrefix = "[YarpRobotControl::Impl::setReferences]";

if(!this->getJointPos())
{
std::cerr << errorPrefix << "Unable to get the joint position." << std::endl;
log()->error("{} Unable to get the joint position.", errorPrefix);
return false;
}

Expand All @@ -420,10 +425,12 @@ struct YarpRobotControl::Impl

if (worstError.second > this->positionDirectMaxAdmissibleError)
{
std::cerr << errorPrefix << "The worst error between the current and the "
<< "desired position of the joint named '" << this->axesName[worstError.first]
<< "' is greater than " << this->positionDirectMaxAdmissibleError
<< " rad. Error = " << worstError.second << " rad." << std::endl;
log()->error("{} The worst error between the current and the desired position of the "
"joint named '{}' is greater than {} deg. Error = {} deg.",
errorPrefix,
this->axesName[worstError.first],
180 / M_PI * this->positionDirectMaxAdmissibleError,
180 / M_PI * worstError.second);
return false;
}

Expand All @@ -438,33 +445,38 @@ struct YarpRobotControl::Impl

else if (mode == IRobotControl::ControlMode::Unknown)
{
std::string error = " The following joints does not have a specified control "
"mode: ";

std::string joints = "";
for (const auto& index : indeces)
error += "'" + this->axesName[index] + "' ";

std::cerr << errorPrefix << error << ". Please set a feasible control mode."
<< std::endl;
joints += " '" + this->axesName[index] + "'";

log()->error("{} The following joints does not have a specified control "
"mode:{}. Please set a feasible control mode.",
errorPrefix,
joints);
return false;

} else if (mode == IRobotControl::ControlMode::Position)
{
std::vector<double> refSpeeds(indeces.size());
for (int i = 0; i < indeces.size(); i++)
{
const auto jointError = std::abs(jointValues[indeces[i]]
- this->positionFeedback[indeces[i]]);

constexpr double scaling = 180 / M_PI;
constexpr double maxVelocityInDegPerSeconds = 3.0;
refSpeeds[i] = std::max(maxVelocityInDegPerSeconds,
scaling * (jointError / this->positioningDuration));
this->positionControlRefSpeeds[i]
= std::max(maxVelocityInDegPerSeconds,
scaling * (jointError / this->positioningDuration));
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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.

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Thank you @S-Dafarra I reneame maxVelocityInDegPerSeconds into minVelocityInDegPerSeconds

}

this->positionInterface->setRefSpeeds(indeces.size(),
indeces.data(),
refSpeeds.data());
if (!this->positionInterface->setRefSpeeds(indeces.size(),
indeces.data(),
this->positionControlRefSpeeds.data()))
{
log()->error("{} Unable to set the reference speed for the position control "
"joints.",
errorPrefix);
return false;
}

this->startPositionControlInstant = yarp::os::Time::now();
Expand All @@ -483,7 +495,7 @@ struct YarpRobotControl::Impl
this->desiredJointValuesAndMode.value[mode].data()))

{
std::cerr << errorPrefix << "Unable to set the desired joint values." << std::endl;
log()->error("{} Unable to set the desired joint values.", errorPrefix);
return false;
}
}
Expand All @@ -505,13 +517,13 @@ bool YarpRobotControl::setDriver(std::shared_ptr<yarp::dev::PolyDriver> robotDev

bool YarpRobotControl::initialize(std::weak_ptr<ParametersHandler::IParametersHandler> handler)
{
constexpr std::string_view errorPrefix = "[YarpRobotControl::initialize] ";
constexpr auto errorPrefix = "[YarpRobotControl::initialize]";

auto ptr = handler.lock();
if (ptr == nullptr)
{
std::cerr << errorPrefix << "The handler is not pointing to an already initialized memory."
<< std ::endl;
log()->error("{} The handler is not pointing to an already initialized memory.",
errorPrefix);
return false;
}

Expand All @@ -522,8 +534,8 @@ bool YarpRobotControl::initialize(std::weak_ptr<ParametersHandler::IParametersHa
// the reading timeout has to be a positive number
if (temp < 0)
{
std::cerr << errorPrefix << "'reading_timeout' parameter has to be a positive number."
<< std ::endl;
log()->error("{} 'reading_timeout' parameter has to be a positive number.",
errorPrefix);
return false;
}
m_pimpl->readingTimeout = temp;
Expand All @@ -534,9 +546,10 @@ bool YarpRobotControl::initialize(std::weak_ptr<ParametersHandler::IParametersHa
// the max_reading_attempts has to be a strictly positive number
if (temp <= 0)
{
std::cerr << errorPrefix
<< "'max_reading_attempts' parameter has to be a strictly positive number."
<< std ::endl;
log()->error("{} 'max_reading_attempts' parameter has to be a strictly positive "
"number.",
errorPrefix);

return false;
}
m_pimpl->maxReadingAttempts = temp;
Expand All @@ -562,9 +575,7 @@ bool YarpRobotControl::setReferences(Eigen::Ref<const Eigen::VectorXd> jointValu
m_pimpl->controlModes = controlModes;
if (!m_pimpl->setControlModes(m_pimpl->controlModes))
{
std::cerr << "[YarpRobotControl::setReferences] Unable to switch in "
"position-direct control mode."
<< std::endl;
log()->error("[YarpRobotControl::setReferences] Unable to set the control modes.");
return false;
}
}
Expand All @@ -584,8 +595,7 @@ bool YarpRobotControl::setReferences(Eigen::Ref<const Eigen::VectorXd> desiredJo
std::fill(m_pimpl->controlModes.begin(), m_pimpl->controlModes.end(), mode);
if (!m_pimpl->setControlModes(m_pimpl->controlModes))
{
std::cerr << "[YarpRobotControl::setReferences] Unable to the desired control mode."
<< std::endl;
log()->error("[YarpRobotControl::setReferences] Unable to set the control modes.");
return false;
}
}
Expand All @@ -594,22 +604,21 @@ bool YarpRobotControl::setReferences(Eigen::Ref<const Eigen::VectorXd> desiredJo
}

bool YarpRobotControl::checkMotionDone(bool& motionDone,
bool& isTimeExpired,
std::vector<std::pair<std::string, double>>& info)
bool& isTimeExpired,
std::vector<std::pair<std::string, double>>& info)
{
constexpr std::string_view errorPrefix = "[YarpRobotControl::checkMotionDone] ";
constexpr auto errorPrefix = "[YarpRobotControl::checkMotionDone]";

if (!m_pimpl->positionInterface->checkMotionDone(&motionDone))
{
std::cerr << errorPrefix
<< "Unable to check if the motion is terminated from the Yarp interface."
<< std::endl;
log()->error("{} Unable to check if the motion is terminated from the Yarp interface.",
errorPrefix);
return false;
}

if (!m_pimpl->getJointPos())
{
std::cerr << errorPrefix << "Unable to get the joint position." << std::endl;
log()->error("{} Unable to get the joint position.", errorPrefix);
return false;
}

Expand Down