Update Paexo wearable device with motor control through yarp

devices/Paexo/conf/Paexo.xml

@@ -7,7 +7,7 @@
     <device type="serialport" name="SerialportDevice">
         <param name="verbose"> 0 </param>
         <!--param name="comport"> COM2 </param-->        <!--  //windows     -->
-        <param name="comport">  /dev/ttyACM2 </param>   <!--    //linux     -->
+        <param name="comport">  /dev/ttyACM0 </param>   <!--    //linux     -->
         <param name="baudrate"> 115200 </param>
         <param name="xonlim"> 0 </param>
         <param name="xofflim"> 0 </param>
@@ -72,9 +72,9 @@
         <action phase="shutdown" level="4" type="detach"/>
     </device>
 
-    <!-- <device type="iwear_logger" name="ProducerLoggerDevice">
+    <device type="iwear_logger" name="ProducerLoggerDevice">
         <param name="period">0.01</param>
-
+        <param name="LoggerLevel">(yarp)</param>
         <param name="logAllQuantities">true</param>
         <param name="logAccelerometers">false</param>
         <param name="logEMGSensors">false</param>
@@ -107,6 +107,6 @@
             </paramlist>
         </action>
         <action phase="shutdown" level="5" type="detach"/>
-    </device> -->
+    </device>
 
 </robot>

devices/Paexo/conf/paexo-validation-motor-control.sh

@@ -1,15 +1,15 @@
 #!/bin/bash
 # set n to 1
-min_motor_pos=27
-max_motor_pos=55
+min_motor_pos=30
+max_motor_pos=50
 step_size=2
 
 ## Paexo initialization
 echo "start" | yarp rpc /wearable/paexo/rpc:i
 sleep 0.5
 echo "en_bc_data" | yarp rpc /wearable/paexo/rpc:i
 sleep 0.5
-echo "init:r" | yarp rpc /wearable/paexo/rpc:i
+echo "init:l" | yarp rpc /wearable/paexo/rpc:i
 
 # Set max position as initial motor position
 current_pos=${max_motor_pos}
@@ -19,6 +19,6 @@ while [ ${min_motor_pos} -le ${current_pos} ]
 do
 	read -s -n1 key
 	#sleep 0.1
-  echo "move:r:${current_pos}" | yarp rpc /wearable/paexo/rpc:i
+  echo "move:l:${current_pos}" | yarp rpc /wearable/paexo/rpc:i
   current_pos=$((current_pos - step_size))
 done

devices/Paexo/src/Paexo.cpp

@@ -12,6 +12,8 @@
 
 #include <yarp/os/BufferedPort.h>
 #include <yarp/os/RpcServer.h>
+#include <yarp/os/TypedReaderCallback.h>
+
 
 #include <mutex>
 #include <string>
@@ -31,6 +33,8 @@ double period = 0.01;
 using namespace wearable;
 using namespace wearable::devices;
 
+using YarpBufferedPort = yarp::os::BufferedPort<yarp::os::Bottle>;
+
 const std::string EOL = "\n"; //EOL character
 const int MAX_LINE_LENGTH = 5000; // Maximum line length to read from serial port
 
@@ -93,9 +97,54 @@ class Paexo::PaexoImpl
 
     // Motor Actuator
     std::string motorActuatorPrefix;
-    const std::string motorActuatorName = "Actuator";
+    const std::string motorActuatorName = "Motor";
     class PaexoMotorActuator;
-    ElementPtr<PaexoMotorActuator> paexoMotorActuator;
+    ElementPtr<PaexoMotorActuator> paexoLeftMotorActuator;
+    ElementPtr<PaexoMotorActuator> paexoRightMotorActuator;
+
+    // Motor actuator yarp port control
+    class PaexoMotorControlPort;
+    std::unique_ptr<PaexoMotorControlPort> paexoLeftMotorControlPort;
+    std::unique_ptr<PaexoMotorControlPort> paexoRightMotorControlPort;
+
+    // Helper function to split wearable element name
+    // TODO: To be moved to wearables utilities
+    inline std::vector<std::string> split(const std::string& s, const std::string& delimiter)
+    {
+        std::size_t pos_start = 0, pos_end, delim_len = delimiter.length();
+        std::string token;
+        std::vector<std::string> res;
+
+        while ((pos_end = s.find(delimiter, pos_start)) != std::string::npos) {
+            token = s.substr(pos_start, pos_end - pos_start);
+            pos_start = pos_end + delim_len;
+            res.push_back(token);
+        }
+
+        res.push_back(s.substr(pos_start));
+        return res;
+    }
+
+    inline std::string getValidYarpName(const std::string& actuatorName)
+    {
+        std::string portName;
+
+        // Get valid port name from actuator name
+        auto vecStr = split(actuatorName, wearable::Separator);
+
+        for (auto& str : vecStr) {
+            if (portName.empty()) {
+                portName = str;
+            }
+            else {
+                portName = portName + '/' + str;
+            }
+        }
+
+        portName = "/" + portName + ":i";
+
+        return portName;
+    }
 
     // Number of sensors
     const int nSensors = 3; // Hardcoded for Paexo
@@ -187,6 +236,29 @@ Paexo::Paexo()
 // Destructor
 Paexo::~Paexo() = default;
 
+class Paexo::PaexoImpl::PaexoMotorControlPort : public YarpBufferedPort
+{
+public:
+
+    std::string portName;
+    ElementPtr<Paexo::PaexoImpl::PaexoMotorActuator> paexoMotorActuator = nullptr;
+
+    void onRead(yarp::os::Bottle& motorCommand) override;
+
+    // Constructor
+    PaexoMotorControlPort(const std::string& name, ElementPtr<Paexo::PaexoImpl::PaexoMotorActuator>& actuator)
+    {
+        portName = name;
+        // Set the actutor
+        if (actuator == nullptr)
+        {
+            yError() << LogPrefix << "Actuator passing error for motor control port " << portName;
+        }
+
+        paexoMotorActuator = actuator;
+    }
+};
+
 bool Paexo::open(yarp::os::Searchable& config)
 {
     // ==================================
@@ -346,10 +418,48 @@ bool Paexo::open(yarp::os::Searchable& config)
                                                                                                                                         pImpl->ftSensorPrefix + "Right" + pImpl->ftSensorName)};
 #endif
 
-    // Initialize wearable actuators
+    // Initialize wearable actuators for left and right motor
     pImpl->motorActuatorPrefix = getWearableName() + actuator::IMotor::getPrefix();
-    pImpl->paexoMotorActuator = ElementPtr<PaexoImpl::PaexoMotorActuator>{std::make_shared<PaexoImpl::PaexoMotorActuator>(pImpl.get(),
-                                                                                                                         pImpl->motorActuatorPrefix + pImpl->motorActuatorName)};
+    const std::string leftActuatorName = pImpl->motorActuatorPrefix + "Left" + pImpl->motorActuatorName;
+    pImpl->paexoLeftMotorActuator = ElementPtr<PaexoImpl::PaexoMotorActuator>{std::make_shared<PaexoImpl::PaexoMotorActuator>(pImpl.get(), leftActuatorName)};
+
+    const std::string rightActuatorName = pImpl->motorActuatorPrefix + "Right" + pImpl->motorActuatorName;
+    pImpl->paexoRightMotorActuator = ElementPtr<PaexoImpl::PaexoMotorActuator>{std::make_shared<PaexoImpl::PaexoMotorActuator>(pImpl.get(), rightActuatorName)};
+
+    // Initialize yarp control ports
+
+    // Check yarp network initialization
+    if (!yarp::os::Network::isNetworkInitialized()) {
+        yInfo() << LogPrefix << "Initializing yarp network";
+        yarp::os::Network();
+    }
+
+    yInfo() << LogPrefix << "Initiailizing PaexoMotorControlPort for " << leftActuatorName;
+
+    const std::string leftActatorName = pImpl->getValidYarpName(leftActuatorName);
+    pImpl->paexoLeftMotorControlPort = std::make_unique<PaexoImpl::PaexoMotorControlPort>(leftActuatorName, pImpl->paexoLeftMotorActuator);
+    pImpl->paexoLeftMotorControlPort->useCallback();
+
+
+    if (!pImpl->paexoLeftMotorControlPort->open(leftActatorName))
+    {
+        yError() << LogPrefix << "Failed to open paexo motor control port " << leftActatorName;
+        return false;
+    }
+
+    yInfo() << LogPrefix << "Initiailizing PaexoMotorControlPort for " << rightActuatorName;
+
+    const std::string rightActatorName = pImpl->getValidYarpName(rightActuatorName);
+    pImpl->paexoRightMotorControlPort = std::make_unique<PaexoImpl::PaexoMotorControlPort>(rightActuatorName, pImpl->paexoRightMotorActuator);
+    pImpl->paexoRightMotorControlPort->useCallback();
+
+
+    if (!pImpl->paexoRightMotorControlPort->open(rightActatorName))
+    {
+        yError() << LogPrefix << "Failed to open paexo motor control port " << rightActatorName;
+        return false;
+    }
+
 
     // Initialize paexo data buffer
     pImpl->paexoData.angle    = 0.0;
@@ -552,7 +662,6 @@ class Paexo::PaexoImpl::PaexoForceTorque6DSensor : public wearable::sensor::IFor
 // =======================================
 // Paexo implementation of Motor actutator
 // =======================================
-//TODO: Check if the paexo needs left and right actuators
 class Paexo::PaexoImpl::PaexoMotorActuator : public wearable::actuator::IMotor
 {
 public:
@@ -570,18 +679,43 @@ class Paexo::PaexoImpl::PaexoMotorActuator : public wearable::actuator::IMotor
     bool setMotorPosition(double& value) const override
     {
         // Prepare the move command
-        std::string motorCommand = "move::" + std::to_string(value);
+        std::string motorCommand = {};
+
+        if (this->getActuatorName().find("Left") != std::string::npos)
+        {
+            motorCommand = "move:l:" + std::to_string(value);
+        }
+
+        if (this->getActuatorName().find("Right") != std::string::npos)
+        {
+            motorCommand = "move:r:" + std::to_string(value);
+        }
+
+        motorCommand += EOL;
+
         char c[motorCommand.length() + 1];
         std::strcpy(c, motorCommand.c_str());
 
         // Set the commanded value to the serial write
         // TODO: Check for serial write failure
+        paexoImpl->iSerialDevice->flush();
         paexoImpl->iSerialDevice->send(c, motorCommand.length());
 
         return true;
     }
 };
 
+void Paexo::PaexoImpl::PaexoMotorControlPort::onRead(yarp::os::Bottle& motorCommand)
+{
+    // TODO: Check if mutex is needed
+    // NOTE: Assuming the associated port received a vector of one double as motor command
+    assert(paexoMotorActuator != nullptr);
+    //yInfo() << LogPrefix << "Data received on " << portName << motorCommand.toString().c_str();
+
+    double cmd = motorCommand.get(0).asDouble();
+    paexoMotorActuator.get()->setMotorPosition(cmd);
+}
+
 void Paexo::run()
 {
     // Send commands to BLE central serial port
@@ -626,7 +760,7 @@ void Paexo::run()
 
         }
         else if(!isdigit(msg[0])) {
-            yInfo() << LogPrefix << msg;
+            //yInfo() << LogPrefix << msg;
         }
     }
 
@@ -822,7 +956,8 @@ Paexo::getActuators(const wearable::actuator::ActuatorType aType) const
 
     switch (aType) {
         case wearable::actuator::ActuatorType::Motor: {
-            outVec.push_back(static_cast<ElementPtr<actuator::IActuator>>(pImpl->paexoMotorActuator));
+            outVec.push_back(static_cast<ElementPtr<actuator::IActuator>>(pImpl->paexoLeftMotorActuator));
+            outVec.push_back(static_cast<ElementPtr<actuator::IActuator>>(pImpl->paexoRightMotorActuator));
             break;
         }
         default: {
@@ -927,7 +1062,9 @@ Paexo::getMotorActuator(const actuator::ActuatorName name) const
         return nullptr;
     }
 
-    // Return a shared point to the required sensor
+    // Return a shared point to the required actuator
+    return dynamic_cast<wearable::ElementPtr<const wearable::actuator::IMotor>&>(
+        *pImpl->paexoLeftMotorActuator);
     return dynamic_cast<wearable::ElementPtr<const wearable::actuator::IMotor>&>(
-        *pImpl->paexoMotorActuator);
+        *pImpl->paexoRightMotorActuator);
 }