diff --git a/devices/Paexo/conf/Paexo.xml b/devices/Paexo/conf/Paexo.xml
index 3d4776b0..0644319e 100644
--- a/devices/Paexo/conf/Paexo.xml
+++ b/devices/Paexo/conf/Paexo.xml
@@ -53,4 +53,15 @@
+
+ 0.01
+ /Paexo/WearableActuatorsData/input:i
+
+
+ PaexoWearableDevice
+
+
+
+
+
diff --git a/devices/Paexo/src/Paexo.cpp b/devices/Paexo/src/Paexo.cpp
index 1238d19a..fd172d75 100644
--- a/devices/Paexo/src/Paexo.cpp
+++ b/devices/Paexo/src/Paexo.cpp
@@ -81,7 +81,7 @@ class Paexo::PaexoImpl
// Motor Actuator
std::string motorActuatorPrefix;
- const std::string motorActuatorName = "Motor";
+ const std::string motorActuatorName = "Actuator";
class PaexoMotorActuator;
DevicePtr paexoMotorActuator;
@@ -215,17 +215,22 @@ bool Paexo::open(yarp::os::Searchable& config)
pImpl->rpcPort.setReader(*pImpl->cmdPro);
// Intialize wearable sensors
- pImpl->jointSensorPrefix = getWearableName() + wearable::Separator + sensor::IVirtualJointKinSensor::getPrefix();
+ pImpl->jointSensorPrefix = getWearableName() + sensor::IVirtualJointKinSensor::getPrefix();
pImpl->paexoJointSensor = SensorPtr{std::make_shared(pImpl.get(),
pImpl->jointSensorPrefix + pImpl->jointSensorName)};
- pImpl->forceSensorPrefix = getWearableName() + wearable::Separator + sensor::IForce3DSensor::getPrefix();
+ pImpl->forceSensorPrefix = getWearableName() + sensor::IForce3DSensor::getPrefix();
pImpl->paexoForceSensor = SensorPtr{std::make_shared(pImpl.get(),
pImpl->forceSensorPrefix + pImpl->forceSensorName)};
- pImpl->torqueSensorPrefix = getWearableName() + wearable::Separator + sensor::ITorque3DSensor::getPrefix();
+ pImpl->torqueSensorPrefix = getWearableName() + sensor::ITorque3DSensor::getPrefix();
pImpl->paexoTorqueSensor = SensorPtr{std::make_shared(pImpl.get(), pImpl->torqueSensorPrefix + pImpl->torqueSensorName)};
+ // Initialize wearable actuators
+ pImpl->motorActuatorPrefix = getWearableName() + actuator::IMotor::getPrefix();
+ pImpl->paexoMotorActuator = DevicePtr{std::make_shared(pImpl.get(),
+ pImpl->motorActuatorPrefix + pImpl->motorActuatorName)};
+
// Initialize paexo data buffer
pImpl->paexoData.angle = 0.0;
pImpl->paexoData.force = 0.0;
@@ -676,3 +681,20 @@ Paexo::getTorque3DSensor(const wearable::sensor::SensorName name) const
return dynamic_cast&>(
*pImpl->paexoTorqueSensor);
}
+
+// ---------------
+// MOTORO Actuator
+// ---------------
+wearable::DevicePtr
+Paexo::getMotorActuator(const actuator::ActuatorName name) const
+{
+ // Check if user-provided name corresponds to an available actuator
+ if (name == pImpl->motorActuatorPrefix + pImpl->motorActuatorName) {
+ yError() << LogPrefix << "Invalid actuator name " << name;
+ return nullptr;
+ }
+
+ // Return a shared point to the required sensor
+ return dynamic_cast&>(
+ *pImpl->paexoMotorActuator);
+}
diff --git a/wrappers/IWearActuators/src/IWearActuatorsWrapper.cpp b/wrappers/IWearActuators/src/IWearActuatorsWrapper.cpp
index dfd5e244..fcd9e1f8 100644
--- a/wrappers/IWearActuators/src/IWearActuatorsWrapper.cpp
+++ b/wrappers/IWearActuators/src/IWearActuatorsWrapper.cpp
@@ -147,24 +147,24 @@ bool IWearActuatorsWrapper::attach(yarp::dev::PolyDriver* poly)
// Check and add all the available actuators
- yInfo() << LogPrefix << "Finding available actuators from " << pImpl->attachedWearableDeviceName << " wearable deive";
+ yInfo() << LogPrefix << "Finding available actuators from " << pImpl->attachedWearableDeviceName << " wearable deive ...";
for (const auto& a : pImpl->iWear->getHapticActuators())
{
pImpl->actuatorsMap[a->getActuatorName()] = a;
- yInfo() << LogPrefix << "Found Actuator : " << a->getActuatorName();
+ yInfo() << LogPrefix << "Adding actuator" << a->getActuatorName();
}
for (const auto& a : pImpl->iWear->getMotorActuators())
{
pImpl->actuatorsMap[a->getActuatorName()] = a;
- yInfo() << LogPrefix << "Found Actuator : " << a->getActuatorName();
+ yInfo() << LogPrefix << "Adding actuator" << a->getActuatorName();
}
for (const auto& a : pImpl->iWear->getHeaterActuators())
{
pImpl->actuatorsMap[a->getActuatorName()] = a;
- yInfo() << LogPrefix << "Found Actuator : " << a->getActuatorName();
+ yInfo() << LogPrefix << "Adding actuator" << a->getActuatorName();
}
// Start the PeriodicThread loop