Skip to content

Commit

Permalink
Merge pull request #204 from EhsanRanjbari/haptic_msg
Browse files Browse the repository at this point in the history 
Fix Haptic glove transmission - GloveDevice
  • Loading branch information
@S-Dafarra
S-Dafarra committed Jun 25, 2024
2 parents 91722ca + 32ff17c commit 6e646b9
Show file tree
Hide file tree
Showing 8 changed files with 124 additions and 31 deletions.
2 changes: 2 additions & 0 deletions .gitignore
View file
Original file line number Diff line number Diff line change
Expand Up @@ -8,3 +8,5 @@ CMakeLists.txt.*
*.autosave
*.original
*.*~
.vscode/c_cpp_properties.json
.vscode/settings.json
2 changes: 1 addition & 1 deletion CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,7 +4,7 @@

cmake_minimum_required(VERSION 3.16)

set(PROJECT_VERSION "1.8.0")
set(PROJECT_VERSION "1.9.0")

set (WEARABLES_PROJECT_NAME Wearables)

Expand Down
12 changes: 6 additions & 6 deletions devices/HapticGlove/conf/HapticGlove.xml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -17,7 +17,7 @@
</device>

<device type="iwear_wrapper" name="LeftHapticGloveWearableDeviceWrapper">
<param extern-name="period" name="period">0.01</param>
<param extern-name="period" name="period">0.1</param>
<param name="dataPortName">/WearableData/HapticGlove/LeftHand/data:o</param>
<param name="rpcPortName">/WearableData/HapticGlove/LeftHand/metadataRpc:o</param>
<action phase="startup" level="5" type="attach">
Expand All @@ -29,8 +29,8 @@
</device>

<device type="iwearactuators_wrapper" name="LeftHapticGloveWearableDeviceActuatorsWrapper">
<param extern-name="period" name="period">0.01</param>
<param name="actuatorCommandInputPortName">/WearableData/HapticGlove/LeftHand/Actuators/input:i</param>
<param extern-name="period" name="period">0.1</param>
<param name="gloveActuatorCommandInputPortName">/WearableData/HapticGlove/LeftHand/Actuators/input:i</param>
<action phase="startup" level="5" type="attach">
<paramlist name="networks">
<elem name="HapticGloveWearableDeviceActuatorWrapperLabel"> LeftHapticGloveWearableDevice </elem>
Expand All @@ -53,7 +53,7 @@
</device>

<device type="iwear_wrapper" name="RightHapticGloveWearableDeviceWrapper">
<param extern-name="period" name="period">0.01</param>
<param extern-name="period" name="period">0.1</param>
<param name="dataPortName">/WearableData/HapticGlove/RightHand/data:o</param>
<param name="rpcPortName">/WearableData/HapticGlove/RightHand/metadataRpc:o</param>
<action phase="startup" level="5" type="attach">
Expand All @@ -65,8 +65,8 @@
</device>

<device type="iwearactuators_wrapper" name="RightHapticGloveWearableDeviceActuatorsWrapper">
<param extern-name="period" name="period">0.01</param>
<param name="actuatorCommandInputPortName">/WearableData/HapticGlove/RightHand/Actuators/input:i</param>
<param extern-name="period" name="period">0.1</param>
<param name="gloveActuatorCommandInputPortName">/WearableData/HapticGlove/RightHand/Actuators/input:i</param>
<action phase="startup" level="5" type="attach">
<paramlist name="networks">
<elem name="HapticGloveWearableDeviceActuatorWrapperLabel"> RightHapticGloveWearableDevice </elem>
Expand Down
38 changes: 30 additions & 8 deletions devices/HapticGlove/src/HapticGlove.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -80,6 +80,7 @@ class HapticGlove::SenseGloveImpl

// Number of actuators
const int nActuators = 11; // humanFingerNames.size()*2 + hand/palm thumper
const int nActuatorsPerGlove = 5; // Number of the actuators per glove

std::unique_ptr<senseGlove::SenseGloveHelper> pGlove; /**< Pointer to the glove object. */

Expand Down Expand Up @@ -305,6 +306,14 @@ bool HapticGlove::open(yarp::os::Searchable& config)

m_pImpl->hapticActuatorPrefix = getWearableName() + actuator::IHaptic::getPrefix();

for (size_t i = 0; i < m_pImpl->gloveData.humanFingerNames.size(); i++) {
m_pImpl->sensegloveHapticActuatorVector.push_back(
std::make_shared<SenseGloveImpl::SenseGloveHapticActuator>(
m_pImpl.get(),
m_pImpl->hapticActuatorPrefix
+ "HapticFeedback"));
}

for (size_t i = 0; i < m_pImpl->gloveData.humanFingerNames.size(); i++) {
m_pImpl->sensegloveHapticActuatorVector.push_back(
std::make_shared<SenseGloveImpl::SenseGloveHapticActuator>(
Expand Down Expand Up @@ -339,6 +348,12 @@ bool HapticGlove::open(yarp::os::Searchable& config)
m_pImpl->gloveData.humanJointSensorNameIdMap.emplace(
std::make_pair(m_pImpl->jointSensorPrefix + m_pImpl->gloveData.humanJointNames[i], i));

for (size_t i = 0; i < m_pImpl->gloveData.humanFingerNames.size(); i++)
m_pImpl->gloveData.humanHapticActuatorNameIdMap.emplace(
std::make_pair(m_pImpl->hapticActuatorPrefix
+ "HapticFeedback",
i));

for (size_t i = 0; i < m_pImpl->gloveData.humanFingerNames.size(); i++)
m_pImpl->gloveData.humanHapticActuatorNameIdMap.emplace(
std::make_pair(m_pImpl->hapticActuatorPrefix + m_pImpl->gloveData.humanFingerNames[i]
Expand Down Expand Up @@ -537,22 +552,29 @@ class HapticGlove::SenseGloveImpl::SenseGloveHapticActuator : public wearable::a

bool setHapticCommand(double& value) const override
{
yError() << LogPrefix << "Wrong method has been called! To set the haptic command please use the setHapticsCommand method.";
return false;
}

std::lock_guard<std::mutex> lock(m_gloveImpl->mutex);
bool setHapticCommands(const std::vector<double>& forceValue, const std::vector<double>& vibrotactileValue) const override
{

if (!m_gloveImpl->isAvailable(m_actuatorName,
m_gloveImpl->gloveData.humanHapticActuatorNameIdMap)) {
yError() << LogPrefix << "The actuator name (" << m_actuatorName
<< ") is not found in the list of actuators.";
std::lock_guard<std::mutex> lock(m_gloveImpl->mutex);

if (forceValue.size() != m_gloveImpl->nActuatorsPerGlove || vibrotactileValue.size() != m_gloveImpl->nActuatorsPerGlove)
{
yError() << "The sizes of the forceValue and the vibrotactileValue vectors are not correct!";
return false;
}
m_gloveImpl->gloveData.fingersHapticFeedback
[m_gloveImpl->gloveData.humanHapticActuatorNameIdMap[m_actuatorName]] = value;

for (size_t i = 0; i < m_gloveImpl->nFingers; i++)
{
m_gloveImpl->gloveData.fingersHapticFeedback[i] = forceValue[i];
m_gloveImpl->gloveData.fingersHapticFeedback[i + 5] = vibrotactileValue[i];
}
return true;
}


inline void setStatus(const actuator::ActuatorStatus& status) { m_status = status; }

private:
Expand Down
2 changes: 2 additions & 0 deletions interfaces/IWear/include/Wearable/IWear/Actuators/IHaptic.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -27,6 +27,8 @@ class wearable::actuator::IHaptic : public wearable::actuator::IActuator
inline static const std::string getPrefix();

virtual bool setHapticCommand(double& value) const = 0;

virtual bool setHapticCommands(const std::vector<double>& forceValue, const std::vector<double>& vibrotactileValue) const = 0;
};

inline const std::string wearable::actuator::IHaptic::getPrefix()
Expand Down
10 changes: 10 additions & 0 deletions msgs/thrift/WearableActuators.thrift
View file
Original file line number Diff line number Diff line change
Expand Up @@ -27,3 +27,13 @@ struct WearableActuatorCommand {
2: double value;
3: double duration;
}

// ==========================
// Glove Actuator Command data type
// ==========================

struct GloveActuatorCommand {
1: ActuatorInfo info;
2: list<double> forceValue;
3: list<double> vibroTactileValue;
}
3 changes: 3 additions & 0 deletions wrappers/IWearActuators/include/IWearActuatorsWrapper.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -12,6 +12,7 @@
#include <yarp/os/TypedReaderCallback.h>

#include "thrift/WearableActuatorCommand.h"
#include "thrift/GloveActuatorCommand.h"

namespace wearable {
namespace wrappers {
Expand All @@ -25,6 +26,7 @@ class wearable::wrappers::IWearActuatorsWrapper
, public yarp::dev::IMultipleWrapper
, public yarp::os::PeriodicThread
, public yarp::os::TypedReaderCallback<msg::WearableActuatorCommand>
, public yarp::os::TypedReaderCallback<msg::GloveActuatorCommand>
{
private:
class impl;
Expand All @@ -40,6 +42,7 @@ class wearable::wrappers::IWearActuatorsWrapper

// TypedReaderCallback
void onRead(msg::WearableActuatorCommand& wearableActuatorCommand) override;
void onRead(msg::GloveActuatorCommand& gloveActuatorCommand) override;

// PeriodicThread
void run() override;
Expand Down
86 changes: 70 additions & 16 deletions wrappers/IWearActuators/src/IWearActuatorsWrapper.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -23,11 +23,14 @@ class IWearActuatorsWrapper::impl : public wearable::msg::WearableActuatorComman
std::string attachedWearableDeviceKey = "defaultIWearActuatorsWrapperDevice";

std::string actuatorCommandInputPortName;
std::string gloveActuatorCommandInputPortName;
yarp::os::BufferedPort<wearable::msg::WearableActuatorCommand> actuatorCommandInputPort;
yarp::os::BufferedPort<wearable::msg::GloveActuatorCommand> gloveActuatorCommandInputPort;

std::unordered_map<std::string, wearable::ElementPtr<const actuator::IActuator>> actuatorsMap;

msg::WearableActuatorCommand wearableActuatorCommand;
msg::GloveActuatorCommand gloveActuatorCommand;

wearable::IWear* iWear = nullptr;
};
Expand All @@ -54,11 +57,43 @@ void IWearActuatorsWrapper::run()

bool IWearActuatorsWrapper::open(yarp::os::Searchable& config)
{
if (!config.check("actuatorCommandInputPortName") || !config.find("actuatorCommandInputPortName").isString()) {
yError() << LogPrefix << "actuatorCommandInputPortName parameter not found";
if (!config.check("actuatorCommandInputPortName") || !config.check("gloveActuatorCommandInputPortName"))
{
yError() << LogPrefix << "No actuator command input ports found.";
return false;
}

// Find and configure yarp port related to the actuator commands
if (!config.find("actuatorCommandInputPortName").isString()) {
yWarning() << LogPrefix << "actuatorCommandInputPortName parameter not found. Haptic feedback will not be available.";
}
else
{
pImpl->actuatorCommandInputPortName = config.find("actuatorCommandInputPortName").asString();
if(!pImpl->actuatorCommandInputPort.open(pImpl->actuatorCommandInputPortName))
{
yError() << "Failed to open " << pImpl->actuatorCommandInputPortName << " yarp port";
return false;
}

// Set the callback to use onRead() method of this device
pImpl->actuatorCommandInputPort.useCallback(*this);

}
if (!config.check("gloveActuatorCommandInputPortName") || !config.find("gloveActuatorCommandInputPortName").isString()) {
yWarning() << LogPrefix << "gloveActuatorCommandInputPortName parameter not found! The port will not be opened.";
}
else
{
pImpl->gloveActuatorCommandInputPortName = config.find("gloveActuatorCommandInputPortName").asString();
if(!pImpl->gloveActuatorCommandInputPort.open(pImpl->gloveActuatorCommandInputPortName))
{
yError() << "Failed to open " << pImpl->gloveActuatorCommandInputPortName << " yarp port";
return false;
}
// Set the callback to use onRead() method of this device
pImpl->gloveActuatorCommandInputPort.useCallback(*this);
}

if (!config.check("period")) {
yInfo() << LogPrefix << "Using default period: " << DefaultPeriod << "s";
Expand All @@ -69,20 +104,6 @@ bool IWearActuatorsWrapper::open(yarp::os::Searchable& config)
const double period = config.check("period", yarp::os::Value(DefaultPeriod)).asFloat64();
setPeriod(period);

pImpl->actuatorCommandInputPortName = config.find("actuatorCommandInputPortName").asString();


// Configure yarp ports

if(!pImpl->actuatorCommandInputPort.open(pImpl->actuatorCommandInputPortName))
{
yError() << "Failed to open " << pImpl->actuatorCommandInputPortName << " yarp port";
return false;
}

// Set the callback to use onRead() method of this device
pImpl->actuatorCommandInputPort.useCallback(*this);

return true;
}

Expand Down Expand Up @@ -136,6 +157,39 @@ void IWearActuatorsWrapper::onRead(msg::WearableActuatorCommand& wearableActuato
}
}

void IWearActuatorsWrapper::onRead(msg::GloveActuatorCommand& gloveActuatorCommand)
{
// Unpack the actuator in from incoming command
wearable::msg::ActuatorInfo info = gloveActuatorCommand.info;

// Check if the commanded actuator name is available
if (pImpl->actuatorsMap.find(info.name) == pImpl->actuatorsMap.end())
{
yWarning() << "Requested actuator with name " << info.name << " is not available in " << pImpl->attachedWearableDeviceKey << " wearable device \n \t Ignoring wearable actuation command.";
}
else // process the wearable actuator command
{
wearable::actuator::ActuatorType aType = pImpl->actuatorsMap[info.name]->getActuatorType();
if (aType == wearable::actuator::ActuatorType::Haptic)
{
// Check if the actuator type in the wearable command is correct
if(info.type == wearable::msg::ActuatorType::HAPTIC)
{
// Get haptic actuator
wearable::ElementPtr<const wearable::actuator::IHaptic> castActuator = std::static_pointer_cast<const wearable::actuator::IHaptic>(pImpl->actuatorsMap[info.name]);

// Send haptic command
castActuator->setHapticCommands(gloveActuatorCommand.forceValue, gloveActuatorCommand.vibroTactileValue);
}
}
else
{
yError() << "Actuator type is not correct!";
return;
}
}
}

bool IWearActuatorsWrapper::close()
{
pImpl->actuatorCommandInputPort.close();
Expand Down

0 comments on commit 6e646b9

Please sign in to comment.