update robot interface and related classes

modules/HapticGlove_module/CMakeLists.txt

@@ -42,7 +42,6 @@ set(${EXE_TARGET_NAME}_SRC
   src/main.cpp
   src/RobotController_hapticGlove.cpp
   src/RobotControlInterface_hapticGlove.cpp
-  src/RobotControlHelper_hapticGlove.cpp
   src/HapticGloveModule.cpp
   src/GloveControlHelper.cpp
   src/KalmanFilter.cpp
@@ -60,7 +59,6 @@ set(${EXE_TARGET_NAME}_SRC
 set(${EXE_TARGET_NAME}_HDR
   include/RobotController_hapticGlove.hpp
   include/RobotControlInterface_hapticGlove.hpp
-  include/RobotControlHelper_hapticGlove.hpp
   include/HapticGloveModule.hpp
   include/GloveControlHelper.hpp
   include/KalmanFilter.hpp

modules/HapticGlove_module/include/RobotControlInterface_hapticGlove.hpp

@@ -11,6 +11,7 @@
 
 // std
 #include <memory>
+#include <vector>
 
 // YARP
 #include <yarp/dev/IAnalogSensor.h>
@@ -32,7 +33,7 @@
 
 namespace HapticGlove
 {
-class RobotControlInterface;
+class RobotInterface;
 struct axisSensorData;
 } // namespace HapticGlove
 
@@ -50,10 +51,11 @@ struct HapticGlove::axisSensorData
 /**
  * RobotControlInterface is an helper class for controlling the robot.
  */
-class HapticGlove::RobotControlInterface
+class HapticGlove::RobotInterface
 {
-    yarp::dev::PolyDriver m_robotDevice; /**< Main robot device. */
-    yarp::dev::PolyDriver m_analogDevice; /**< Analog device. */
+    std::string m_logPrefix; /**< the log prefix*/
+
+    bool m_rightHand; /**< if the right hand is used, the variable is true*/
 
     int m_noActuatedAxis; /**< Number of the actuated DoF */
     size_t m_noAnalogSensor; /**< Number of the joints ( associated with the analog sensors) */
@@ -71,14 +73,17 @@ class HapticGlove::RobotControlInterface
         m_allJointNames; /**< Vector containing the name of the controlled axes. */
 
     std::vector<std::string>
-        m_actuatedJointList; /**< Vector containing the names of the activated joints (activated
+        m_actuatedJointList; /**< Vector containing the names of the actuated joints (activated
                                 joints are related to the axis that we are using). */
 
     size_t m_noActuatedJoints;
     std::vector<axisSensorData>
         m_axisInfoList; /**< Vector containing the data structure for controlled axis and the
                            associated sensor list. */
 
+    yarp::dev::PolyDriver m_robotDevice; /**< Main robot device. */
+    yarp::dev::PolyDriver m_analogDevice; /**< Analog device. */
+
     yarp::dev::IPreciselyTimed* m_timedInterface{nullptr};
     yarp::dev::IEncodersTimed* m_encodersInterface{nullptr}; /**< Encorders interface. */
     yarp::dev::IPositionDirect* m_positionDirectInterface{nullptr}; /**< Direct position control
@@ -87,7 +92,7 @@ class HapticGlove::RobotControlInterface
     yarp::dev::IVelocityControl* m_velocityInterface{nullptr}; /**< Velocity control interface. */
     yarp::dev::IControlMode* m_controlModeInterface{nullptr}; /**< Control mode interface. */
     yarp::dev::IControlLimits* m_limitsInterface{nullptr}; /**< Encorders interface. */
-    yarp::dev::IAnalogSensor* m_AnalogSensorInterface{nullptr}; /**< Sensor interface */
+    yarp::dev::IAnalogSensor* m_analogSensorInterface{nullptr}; /**< Sensor interface */
     yarp::dev::ICurrentControl* m_currentInterface{nullptr}; /**< current control interface */
     yarp::dev::IPWMControl* m_pwmInterface{nullptr}; /**< PWM control interface*/
     yarp::dev::IPidControl* m_pidInterface{nullptr}; /**< pid control interface*/
@@ -100,7 +105,6 @@ class HapticGlove::RobotControlInterface
     yarp::sig::Vector m_analogSensorFeedbackRaw; /**< sensor feedback [raw]*/
     yarp::sig::Vector m_analogSensorFeedbackInDegrees; /**< sensor feedback [deg]*/
     yarp::sig::Vector m_analogSensorFeedbackInRadians; /**< sensor feedback [rad]*/
-    yarp::sig::Vector m_analogSensorFeedbackSelected; /**< sensor info to read*/
     yarp::sig::Vector m_SensorActuatedJointFeedbackInRadians; /**< all the interested sensor info to
                                                                  read (analog+encoders)*/
     yarp::sig::Vector m_currentFeedback; /**< motor current feedbacks*/
@@ -178,7 +182,10 @@ class HapticGlove::RobotControlInterface
      * problem in the configuration phase
      * @return true / false in case of success / failure
      */
-    bool configure(const yarp::os::Searchable& config, const std::string& name, bool isMandatory);
+    bool configure(const yarp::os::Searchable& config,
+                   const std::string& name,
+                   const bool& rightHand,
+                   const bool& isMandatory);
 
     /**
      * Update the time stamp
@@ -251,14 +258,14 @@ class HapticGlove::RobotControlInterface
     yarp::os::Stamp& timeStamp();
 
     /**
-     * Get the joint encoders value
-     * @return the joint encoder value
+     * Get the axis encider values
+     * @return the axis encoder value
      */
     const yarp::sig::Vector& axisFeedbacks() const;
 
     /**
-     * Get the joint encoders value
-     * @return the joint encoder value
+     * Get the axis encoders value
+     * @param axisFeedbacks the axis encoder values
      */
     void axisFeedbacks(std::vector<double>& axisFeedbacks) const;
 
@@ -288,19 +295,19 @@ class HapticGlove::RobotControlInterface
 
     /**
      * Get all the intersted joints sensors value (including analog+encoders)
-     * @return all the intersted sensor values
+     * @param jointsFeedbacks all the intersted sensor values
      */
     void allSensors(std::vector<double>& jointsFeedbacks) const;
 
     /**
-     * Get all the intersted motor current values
+     * Get all the actuated motor current values
      * @return all the motor current values
      */
     const yarp::sig::Vector& motorCurrents() const;
 
     /**
-     * Get all the intersted motor current values
-     * @return all the motor current values
+     * Get all the actuated motor current values
+     * @param  motorCurrents the motor current values
      */
     void motorCurrents(std::vector<double>& motorCurrents) const;
 
@@ -336,7 +343,7 @@ class HapticGlove::RobotControlInterface
 
     /**
      * Get all the actuated motor low level pid outputs
-     * @return all the motor pid outputs
+     * @param motorPidOutputs the motor pid outputs
      */
     void motorPidOutputs(std::vector<double>& motorPidOutputs) const;
 
@@ -348,32 +355,56 @@ class HapticGlove::RobotControlInterface
 
     /**
      * Get all the actuated motor PWM References
-     * @return all the motor PWM References
+     * @param motorPwmReference the motor PWM References
      */
     void motorPwmReference(std::vector<double>& motorPwmReference) const;
 
     /**
-     * Get the number of actuated degree of freedom (motors)
-     * @return the number of actuated DoF
+     * Get the number of actuated axis/motors
+     * @return the number of actuated axis/motors
      */
     const int getNumberOfActuatedAxis() const;
 
+    /**
+     * Get the number of all axis/motors related to the used parts
+     * @return the number of all axis/motors
+     */
     const int getNumberOfAllAxis() const;
 
     /**
-     * Get the number of joints
-     * @return the number of joints
+     * Get the number of all the joints related to the robot used parts
+     * @return the number of all the joints
      */
     const int getNumberOfAllJoints() const;
 
+    /**
+     * Get the number of actuated joints of the used parts
+     * @return the number of actuated joints
+     */
     const int getNumberOfActuatedJoints() const;
 
+    /**
+     * Get the name of the actuated joints
+     * @param names the names of the actuated joints
+     */
     void getActuatedJointNames(std::vector<std::string>& names) const;
 
+    /**
+     * Get the name of the all the joints related to the used parts
+     * @param names the names of the all joints
+     */
     void getAllJointNames(std::vector<std::string>& names) const;
 
+    /**
+     * Get the name of the actuated axes
+     * @param names the names of the actuated axes
+     */
     void getActuatedAxisNames(std::vector<std::string>& names) const;
 
+    /**
+     * Get the name of the all the axes related to the used parts
+     * @param names the names of the all axis
+     */
     void getAllAxisNames(std::vector<std::string>& names) const;
 
     /**

modules/HapticGlove_module/include/RobotController_hapticGlove.hpp

@@ -50,11 +50,7 @@ class HapticGlove::RobotController
     size_t m_numAllJoints;
     size_t m_numActuatedJoints;
 
-    // TO CHECK: maybe delete later
-    //    std::unique_ptr<iCub::ctrl::Integrator> m_fingerIntegrator{nullptr}; /**< Velocity
-    //    integrator */
-
-    std::unique_ptr<RobotControlInterface> m_robotControlInterface; /**< robot control interface */
+    std::unique_ptr<RobotInterface> m_robotInterface; /**< robot control interface */
 
     CtrlHelper::Eigen_Mat m_A; /**< Coupling Matrix from the motors to the joints; Dimension <n,m>
                             n: number of joints, m: number of motors; we have q= m_A x m + m_Bias
@@ -213,12 +209,6 @@ class HapticGlove::RobotController
      */
     bool updateFeedback(void);
 
-    //    /**
-    //     * Find the coupling relationship bwtween the motors and joitns of the robots
-    //     * @return true if it could open the logger
-    //     */
-    //    bool LogDataToCalibrateRobotMotorsJointsCouplingRandom(const bool generateRandomVelocity);
-
     /**
      * Find the coupling relationship bwtween the motors and joitns of the robots using sin input
      * function
@@ -300,13 +290,13 @@ class HapticGlove::RobotController
      * Expose the contolHelper interface (const)
      * @return control helper interface
      */
-    const std::unique_ptr<RobotControlInterface>& controlHelper() const;
+    const std::unique_ptr<RobotInterface>& controlHelper() const;
 
     /**
      * Expose the contolHelper interface
      * @return control helper interface
      */
-    std::unique_ptr<RobotControlInterface>& controlHelper();
+    std::unique_ptr<RobotInterface>& controlHelper();
 };
 
 #endif

modules/HapticGlove_module/src/ControlHelper.cpp

@@ -24,8 +24,11 @@ void CtrlHelper::toStdVector(Eigen::VectorXd& vecEigen, std::vector<double>& vec
 
 void CtrlHelper::toStdVector(yarp::sig::Vector& vecYarp, std::vector<double>& vecStd)
 {
-    Eigen::VectorXd tmpVec = CtrlHelper::toEigenVector(vecYarp);
-    CtrlHelper::toStdVector(tmpVec, vecStd);
+    if (vecStd.size() != vecYarp.size())
+    {
+        vecStd.resize(vecYarp.size());
+    }
+    CtrlHelper::toEigenVector(vecStd) = CtrlHelper::toEigenVector(vecYarp);
 }
 
 void CtrlHelper::toYarpVector(Eigen::VectorXd& vecEigen, yarp::sig::Vector& vecYarp)
@@ -39,6 +42,9 @@ void CtrlHelper::toYarpVector(Eigen::VectorXd& vecEigen, yarp::sig::Vector& vecY
 
 void CtrlHelper::toYarpVector(std::vector<double>& vecStd, yarp::sig::Vector& vecYarp)
 {
-    Eigen::VectorXd tmpVec = CtrlHelper::toEigenVector(vecStd);
-    CtrlHelper::toYarpVector(tmpVec, vecYarp);
+    if (vecYarp.size() != vecStd.size())
+    {
+        vecYarp.resize(vecStd.size());
+    }
+    CtrlHelper::toEigenVector(vecYarp) = CtrlHelper::toEigenVector(vecStd);
 }

modules/HapticGlove_module/src/GloveWearable.cpp

@@ -61,9 +61,7 @@ bool GloveWearableImpl::configure(const yarp::os::Searchable& config,
     if (!YarpHelper::yarpListToStringVector(jointListYarp, m_humanJointNameList))
     {
         yError() << m_logPrefix
-                 << "unable to convert "
-                    "human_joint_list list into a "
-                    "vector of strings.";
+                 << "unable to convert human_joint_list list into a vector of strings.";
         return false;
     }
 
@@ -78,17 +76,13 @@ bool GloveWearableImpl::configure(const yarp::os::Searchable& config,
     yarp::os::Value* fingerListYarp;
     if (!config.check("human_finger_list", fingerListYarp))
     {
-        yError() << m_logPrefix
-                 << "unable to find "
-                    "human_finger_list into config file.";
+        yError() << m_logPrefix << "unable to find human_finger_list into config file.";
         return false;
     }
     if (!YarpHelper::yarpListToStringVector(fingerListYarp, m_humanFingerNameList))
     {
         yError() << m_logPrefix
-                 << "unable to convert "
-                    "human_finger_list list into a "
-                    "vector of strings.";
+                 << "unable to convert human_finger_list list into a vector of strings.";
         return false;
     }