Update Paexo wearable device with motor actuator initialization, upda…

…te config file Update Paexo config file IWearActuatorsWrapper Cleanup
robotology · Jan 15, 2021 · 89fced9 · 89fced9
1 parent e793ccd
commit 89fced9
Show file tree

Hide file tree

Showing 3 changed files with 41 additions and 8 deletions.
diff --git a/devices/Paexo/conf/Paexo.xml b/devices/Paexo/conf/Paexo.xml
@@ -53,4 +53,15 @@
         <action phase="shutdown" level="4" type="detach"/>
     </device>
 
+    <device type="iwearactuators_wrapper" name="PaexoWearableDeviceActuatorsWrapper">
+        <param name="period">0.01</param>
+        <param name="actuatorCommandInputPortName">/Paexo/WearableActuatorsData/input:i</param>
+        <action phase="startup" level="5" type="attach">
+            <paramlist name="networks">
+                <elem name="PaexoWearableDeviceActuatorsWrapperLabel"> PaexoWearableDevice </elem>
+            </paramlist>
+        </action>
+        <action phase="shutdown" level="4" type="detach"/>
+    </device>
+
 </robot>
diff --git 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> 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<PaexoImpl::PaexoVirtualJointKinSensor>{std::make_shared<PaexoImpl::PaexoVirtualJointKinSensor>(pImpl.get(),
                                                                                                                                        pImpl->jointSensorPrefix + pImpl->jointSensorName)};
 
-    pImpl->forceSensorPrefix = getWearableName() + wearable::Separator + sensor::IForce3DSensor::getPrefix();
+    pImpl->forceSensorPrefix = getWearableName() + sensor::IForce3DSensor::getPrefix();
     pImpl->paexoForceSensor = SensorPtr<PaexoImpl::PaexoForce3DSensor>{std::make_shared<PaexoImpl::PaexoForce3DSensor>(pImpl.get(),
                                                                                                                       pImpl->forceSensorPrefix + pImpl->forceSensorName)};
 
-    pImpl->torqueSensorPrefix = getWearableName() + wearable::Separator + sensor::ITorque3DSensor::getPrefix();
+    pImpl->torqueSensorPrefix = getWearableName() + sensor::ITorque3DSensor::getPrefix();
     pImpl->paexoTorqueSensor = SensorPtr<PaexoImpl::PaexoTorque3DSensor>{std::make_shared<PaexoImpl::PaexoTorque3DSensor>(pImpl.get(), pImpl->torqueSensorPrefix + pImpl->torqueSensorName)};
 
+    // Initialize wearable actuators
+    pImpl->motorActuatorPrefix = getWearableName() + actuator::IMotor::getPrefix();
+    pImpl->paexoMotorActuator = DevicePtr<PaexoImpl::PaexoMotorActuator>{std::make_shared<PaexoImpl::PaexoMotorActuator>(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<wearable::SensorPtr<const wearable::sensor::ITorque3DSensor>&>(
         *pImpl->paexoTorqueSensor);
 }
+
+// ---------------
+// MOTORO Actuator
+// ---------------
+wearable::DevicePtr<const actuator::IMotor>
+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<wearable::DevicePtr<const wearable::actuator::IMotor>&>(
+        *pImpl->paexoMotorActuator);
+}
diff --git 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