Skip to content
Open
Show file tree
Hide file tree
Changes from all commits
Commits
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
12 changes: 6 additions & 6 deletions src/JoypadModule/src/Module.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -131,7 +131,7 @@ bool JoypadModule::configure(yarp::os::ResourceFinder &rf)
yError() << "[configure] Unable to get a string from searchable";
return false;
}
yarp::os::Network::connect(m_rpcClientPortName, m_rpcServerPortName);
yarp::os::Network::connect(m_rpcClientPortName, m_rpcServerPortName, "shmem");

if (!YarpUtilities::getStringFromSearchable(rf, "robotGoalOutputPort_name", portName))
{
Expand All @@ -146,7 +146,7 @@ bool JoypadModule::configure(yarp::os::ResourceFinder &rf)
yError() << "[configure] Unable to get a string from searchable";
return false;
}
yarp::os::Network::connect(m_robotGoalOutputPortName, m_robotGoalInputPortName);
yarp::os::Network::connect(m_robotGoalOutputPortName, m_robotGoalInputPortName, "shmem");

return true;
}
Expand Down Expand Up @@ -260,21 +260,21 @@ bool JoypadModule::updateModule()
{
yInfo() << "[updateModule] Connecting both ports";
if (!yarp::os::Network::isConnected(m_rpcClientPortName, m_rpcServerPortName))
yarp::os::Network::connect(m_rpcClientPortName, m_rpcServerPortName);
yarp::os::Network::connect(m_rpcClientPortName, m_rpcServerPortName, "shmem");
if (!yarp::os::Network::isConnected(m_robotGoalOutputPortName, m_robotGoalInputPortName))
yarp::os::Network::connect(m_robotGoalOutputPortName, m_robotGoalInputPortName);
yarp::os::Network::connect(m_robotGoalOutputPortName, m_robotGoalInputPortName, "shmem");
}
else if (m_connectPortsSeparately && connectGoal > 0)
{
yInfo() << "[updateModule] Connecting goal port";
if (!yarp::os::Network::isConnected(m_robotGoalOutputPortName, m_robotGoalInputPortName))
yarp::os::Network::connect(m_robotGoalOutputPortName, m_robotGoalInputPortName);
yarp::os::Network::connect(m_robotGoalOutputPortName, m_robotGoalInputPortName, "shmem");
}
else if (m_connectPortsSeparately && connectRpc > 0)
{
yInfo() << "[updateModule] Connecting RPC port";
if (!yarp::os::Network::isConnected(m_rpcClientPortName, m_rpcServerPortName))
yarp::os::Network::connect(m_rpcClientPortName, m_rpcServerPortName);
yarp::os::Network::connect(m_rpcClientPortName, m_rpcServerPortName, "shmem");
}
// disconnect the ports
else if (disconnect > 0)
Expand Down
4 changes: 2 additions & 2 deletions src/RobotInterface/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -4,7 +4,7 @@

add_walking_controllers_library(
NAME RobotInterface
SOURCES src/Helper.cpp src/PIDHandler.cpp
PUBLIC_HEADERS include/WalkingControllers/RobotInterface/Helper.h include/WalkingControllers/RobotInterface/PIDHandler.h
SOURCES src/Helper.cpp src/PIDHandler.cpp src/MotorTemperatureChecker.cpp
PUBLIC_HEADERS include/WalkingControllers/RobotInterface/Helper.h include/WalkingControllers/RobotInterface/PIDHandler.h include/WalkingControllers/RobotInterface/MotorTemperatureChecker.h
PUBLIC_LINK_LIBRARIES WalkingControllers::YarpUtilities WalkingControllers::iDynTreeUtilities ctrlLib
PRIVATE_LINK_LIBRARIES Eigen3::Eigen)
Original file line number Diff line number Diff line change
Expand Up @@ -16,6 +16,8 @@
#include <yarp/dev/IPositionDirect.h>
#include <yarp/dev/IVelocityControl.h>
#include <yarp/dev/IInteractionMode.h>
#include <yarp/dev/IMotor.h>

#include <yarp/sig/Vector.h>
#include <yarp/os/Timer.h>

Expand Down Expand Up @@ -48,6 +50,7 @@ namespace WalkingControllers
yarp::dev::IControlMode *m_controlModeInterface{nullptr}; /**< Control mode interface. */
yarp::dev::IControlLimits *m_limitsInterface{nullptr}; /**< Encorders interface. */
yarp::dev::IInteractionMode *m_interactionInterface{nullptr}; /**< Stiff/compliant mode interface. */
yarp::dev::IMotor *m_motorInterface{nullptr}; /**< Motor interface. */

std::unique_ptr<WalkingPIDHandler> m_PIDHandler; /**< Pointer to the PID handler object. */

Expand All @@ -66,6 +69,13 @@ namespace WalkingControllers
iDynTree::VectorDynSize m_jointVelocitiesBounds; /**< Joint Velocity bounds [rad/s]. */
iDynTree::VectorDynSize m_jointPositionsUpperBounds; /**< Joint Position upper bound [rad]. */
iDynTree::VectorDynSize m_jointPositionsLowerBounds; /**< Joint Position lower bound [rad]. */

iDynTree::VectorDynSize m_motorTemperatures; /**< Motor temperature [Celsius]. */
mutable std::mutex m_motorTemperatureMutex; /**< Mutex for the motor temperature. */
std::atomic_bool m_motorTemperatureTreadIsRunning; /**< True if the motor temperature is updated. */
double m_motorTemperatureDt{0.5};
std::thread m_motorTemperatureThread; /**< Thread for the motor temperature. */

// yarp::sig::Vector m_positionFeedbackDegFiltered;
yarp::sig::Vector m_velocityFeedbackDegFiltered; /**< Vector containing the filtered joint velocity [deg/s]. */
std::unique_ptr<iCub::ctrl::FirstOrderLowPassFilter> m_positionFilter; /**< Joint position low pass filter .*/
Expand Down Expand Up @@ -123,10 +133,13 @@ namespace WalkingControllers
bool configureForceTorqueSensor(const std::string& portPrefix,
const std::string& portInputName,
const std::string& wholeBodyDynamicsPortName,
const std::string& carrier,
const double& samplingTime,
bool useWrenchFilter,
double cutFrequency,
MeasuredWrench& measuredWrench);

void readMotorTemperature();
public:

/**
Expand Down Expand Up @@ -205,6 +218,9 @@ namespace WalkingControllers
*/
const iDynTree::VectorDynSize& getJointVelocity() const;


iDynTree::VectorDynSize getMotorTemperature() const;

/**
* Get the joint upper limit
* @return the joint upper bound in radiants
Expand Down
Original file line number Diff line number Diff line change
@@ -0,0 +1,33 @@
// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT)
// SPDX-License-Identifier: BSD-3-Clause

#ifndef WALKING_CONTROLLERS_MOTOR_TEMPERATURE_CHECKER
#define WALKING_CONTROLLERS_MOTOR_TEMPERATURE_CHECKER

// std
#include <vector>

#include <iDynTree/VectorDynSize.h>

#include <yarp/os/Searchable.h>

namespace WalkingControllers
{
class MotorsTemperatureChecker
{
private:
std::vector<double> m_maxTemperature;
std::vector<unsigned int> m_samplesAboveTheLimit;
iDynTree::VectorDynSize m_limits;
int m_maxNumberOfSampleAboveTheLimits;

public:

const std::vector<double>& getMaxTemperature() const;
std::vector<unsigned int> getMotorsOverLimit() const;
bool isThereAMotorOverLimit() const;
bool setMotorTemperatures(const iDynTree::VectorDynSize& temperature);
bool configure(const yarp::os::Searchable& config, int dofs);
};
};
#endif
49 changes: 48 additions & 1 deletion src/RobotInterface/src/Helper.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -269,6 +269,7 @@ bool RobotInterface::configureRobot(const yarp::os::Searchable& config)
options.put("localPortPrefix", "/" + name + "/remoteControlBoard");
yarp::os::Property& remoteControlBoardsOpts = options.addGroup("REMOTE_CONTROLBOARD_OPTIONS");
remoteControlBoardsOpts.put("writeStrict", "on");
remoteControlBoardsOpts.put("carrier", "shmem");

// get the actuated DoFs
m_actuatedDOFs = m_axesList.size();
Expand Down Expand Up @@ -313,6 +314,8 @@ bool RobotInterface::configureRobot(const yarp::os::Searchable& config)
}
}

m_motorTemperatures.resize(m_actuatedDOFs);

// open the device
if(!m_robotDevice.open(options))
{
Expand Down Expand Up @@ -363,6 +366,12 @@ bool RobotInterface::configureRobot(const yarp::os::Searchable& config)
return false;
}

if (!m_robotDevice.view(m_motorInterface) || !m_motorInterface)
{
yError() << "[configureRobot] Cannot obtain IMotor interface";
return false;
}

// resize the buffers
m_positionFeedbackDeg.resize(m_actuatedDOFs, 0.0);
m_velocityFeedbackDeg.resize(m_actuatedDOFs, 0.0);
Expand Down Expand Up @@ -482,12 +491,35 @@ bool RobotInterface::configureRobot(const yarp::os::Searchable& config)
return false;
}

m_motorTemperatureThread = std::thread(&RobotInterface::readMotorTemperature, this);

return true;
}

void RobotInterface::readMotorTemperature()
{
m_motorTemperatureTreadIsRunning = true;

while(m_motorTemperatureTreadIsRunning)
{
{
std::lock_guard<std::mutex> lock(m_motorTemperatureMutex);

if(!m_motorInterface->getTemperatures(m_motorTemperatures.data()))
{
yError() << "[RobotInterface::readMotorTemperature] Unable to get the motor temperatures.";
}
}

// TODO GR you should load the time required for the read and close it
yarp::os::Time::delay(m_motorTemperatureDt);
}
}

bool RobotInterface::configureForceTorqueSensor(const std::string& portPrefix,
const std::string& portInputName,
const std::string& wholeBodyDynamicsPortName,
const std::string& carrier,
const double& samplingTime,
bool useWrenchFilter,
double cutFrequency,
Expand All @@ -497,7 +529,7 @@ bool RobotInterface::configureForceTorqueSensor(const std::string& portPrefix,
measuredWrench.port = std::make_unique<yarp::os::BufferedPort<yarp::sig::Vector>>();
measuredWrench.port->open("/" + portPrefix + portInputName);
// connect port
if(!yarp::os::Network::connect(wholeBodyDynamicsPortName, "/" + portPrefix + portInputName))
if(!yarp::os::Network::connect(wholeBodyDynamicsPortName, "/" + portPrefix + portInputName, carrier))
{
yError() << "[RobotInterface::configureForceTorqueSensors] Unable to connect to port "
<< wholeBodyDynamicsPortName << " to " << "/" + portPrefix + portInputName;
Expand Down Expand Up @@ -643,6 +675,7 @@ bool RobotInterface::configureForceTorqueSensors(const yarp::os::Searchable& con
ok = ok && this->configureForceTorqueSensor(portPrefix,
leftFootWrenchInputPorts[i],
leftFootWrenchOutputPorts[i],
"shmem",
samplingTime,
useWrenchFilter, cutFrequency,
m_leftFootMeasuredWrench[i]);
Expand All @@ -653,6 +686,7 @@ bool RobotInterface::configureForceTorqueSensors(const yarp::os::Searchable& con
ok = ok && this->configureForceTorqueSensor(portPrefix,
rightFootWrenchInputPorts[i],
rightFootWrenchOutputPorts[i],
"shmem",
samplingTime,
useWrenchFilter, cutFrequency,
m_rightFootMeasuredWrench[i]);
Expand Down Expand Up @@ -1096,6 +1130,12 @@ bool RobotInterface::setVelocityReferences(const iDynTree::VectorDynSize& desire

bool RobotInterface::close()
{
m_motorTemperatureTreadIsRunning = false;
if (m_motorTemperatureThread.joinable())
{
m_motorTemperatureThread.join();
}

// close all the ports
for(auto& wrench : m_leftFootMeasuredWrench)
wrench.port->close();
Expand All @@ -1119,11 +1159,18 @@ const iDynTree::VectorDynSize& RobotInterface::getJointPosition() const
{
return m_positionFeedbackRad;
}

const iDynTree::VectorDynSize& RobotInterface::getJointVelocity() const
{
return m_velocityFeedbackRad;
}

iDynTree::VectorDynSize RobotInterface::getMotorTemperature() const
{
std::lock_guard lock(m_motorTemperatureMutex);
return m_motorTemperatures;
}

const iDynTree::Wrench& RobotInterface::getLeftWrench() const
{
return m_leftWrench;
Expand Down
116 changes: 116 additions & 0 deletions src/RobotInterface/src/MotorTemperatureChecker.cpp
Original file line number Diff line number Diff line change
@@ -0,0 +1,116 @@
// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT)
// SPDX-License-Identifier: BSD-3-Clause

#include <yarp/os/LogStream.h>

#include <WalkingControllers/RobotInterface/MotorTemperatureChecker.h>
#include <WalkingControllers/YarpUtilities/Helper.h>

using namespace WalkingControllers;

bool MotorsTemperatureChecker::configure(const yarp::os::Searchable& config, int dofs)
{
m_limits.resize(dofs);
if(!YarpUtilities::getVectorFromSearchable(config, "temperature_limits", m_limits))
{
yError() << "[MotorsTemperatureChecker::configure] Unable to get the parameter temperature limits";
return false;
}

m_samplesAboveTheLimit = std::vector<unsigned int>(dofs, 0);

if(!YarpUtilities::getNumberFromSearchable(config, "max_samples_above_the_limits", m_maxNumberOfSampleAboveTheLimits))
{
yError() << "[MotorsTemperatureChecker::configure] Unable to get the parameter 'max_samples_above_the_limits'";
return false;
}

m_maxTemperature.resize(dofs);
for (int i = 0; i < dofs; i++)
{
m_maxTemperature[i] = 0;
}

return true;
}

bool MotorsTemperatureChecker::isThereAMotorOverLimit() const
{
if (m_maxNumberOfSampleAboveTheLimits < 0)
{
return false;
}

for (const auto sample : m_samplesAboveTheLimit)
{
if (sample > m_maxNumberOfSampleAboveTheLimits)
{
return true;
}
}
return false;
}

std::vector<unsigned int> MotorsTemperatureChecker::getMotorsOverLimit() const
{
std::vector<unsigned int> indeces;

if (m_maxNumberOfSampleAboveTheLimits < 0)
{
return indeces;
}

for (int i =0; i < m_samplesAboveTheLimit.size(); i++)
{
if (m_samplesAboveTheLimit[i] > m_maxNumberOfSampleAboveTheLimits)
{
indeces.push_back(i);
}
}
return indeces;
}

bool MotorsTemperatureChecker::setMotorTemperatures(const iDynTree::VectorDynSize& temperature)
{
if (temperature.size() != m_limits.size())
{
yError() << "[MotorsTemperatureChecker::setMotorTemperatures] Unexpected size of the temperature vector"
<< "provided: " << temperature.size() << " expected: " << m_limits.size();
return false;
}

if (m_maxNumberOfSampleAboveTheLimits < 0)
{
return true;
}

for (int i = 0; i < temperature.size(); i++)
{

if (m_limits(i) < 0)
{
continue;
}

if (temperature(i) > m_limits(i))
{
m_samplesAboveTheLimit[i]++;
}
else
{
m_samplesAboveTheLimit[i] = 0;
}

if (temperature(i) > m_maxTemperature[i])
{
m_maxTemperature[i] = temperature(i);
}
}

return true;
}

const std::vector<double>& MotorsTemperatureChecker::getMaxTemperature() const
{
return m_maxTemperature;
}
Loading