Add joint type handling to YarpSensorBridge in case of prismatic and revolute joints (#949)

Author: GiulioRomualdi

CHANGELOG.md

@@ -11,6 +11,7 @@ All notable changes to this project are documented in this file.
 - Enable Programmatic Creation of `VariableRegularizationTask` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/934)
 - Check if the setpoint has been set at least once in TSID and IK (https://github.com/ami-iit/bipedal-locomotion-framework/pull/939)
 - Add motor temperature sensor support to `SensorBridge`, `YarpSensorBridge` and `YarpRobotLoggerDevice` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/946)
+- Add the support to the joint type (revolute/prismatic) in `SensorBridge` (https://github.com/ami-iit/bipedal-locomotion-framework/pull/949)
 
 ### Changed
 - Some improvements on the YarpRobotLoggerDevice (https://github.com/ami-iit/bipedal-locomotion-framework/pull/910)

src/RobotInterface/YarpImplementation/include/BipedalLocomotion/RobotInterface/YarpSensorBridgeImpl.h

@@ -187,6 +187,8 @@ struct YarpSensorBridge::Impl
                               */
     bool streamJointAccelerations{true}; /**< flag to enable reading joint accelerations from
                                             encoders */
+    bool allJointsAreRevolutes{false}; /**< flag to indicate if all the joints are revolute */
+    bool allJointsArePrismatics{false}; /**< flag to indicate if all the joints are prismatic */
 
     using SubConfigLoader = bool (YarpSensorBridge::Impl::*)(
         std::weak_ptr<const BipedalLocomotion::ParametersHandler::IParametersHandler>,
@@ -1167,14 +1169,29 @@ struct YarpSensorBridge::Impl
 
         // get the names of all the joints available in the attached remote control board remapper
         std::vector<std::string> controlBoardJoints;
+        std::vector<JointType> controlBoardJointTypes;
         int controlBoardDOFs;
         controlBoardRemapperInterfaces.encoders->getAxes(&controlBoardDOFs);
 
         std::string joint;
+        yarp::dev::JointTypeEnum jType;
         for (int DOF = 0; DOF < controlBoardDOFs; DOF++)
         {
             controlBoardRemapperInterfaces.axis->getAxisName(DOF, joint);
+            controlBoardRemapperInterfaces.axis->getJointType(DOF, jType);
             controlBoardJoints.push_back(joint);
+            if (jType != yarp::dev::VOCAB_JOINTTYPE_REVOLUTE
+                && jType != yarp::dev::VOCAB_JOINTTYPE_PRISMATIC)
+            {
+                log()->error("{} Joint type not supported. Only revolute and prismatic joints are "
+                             "supported.",
+                             logPrefix);
+                return false;
+            }
+
+            controlBoardJointTypes.push_back(yarp::dev::VOCAB_JOINTTYPE_REVOLUTE == jType
+                                                 ? JointType::REVOLUTE
+                                                 : JointType::PRISMATIC);
         }
 
         // if the joint list is empty means that the user did not pass a "joints_list" while the
@@ -1186,6 +1203,17 @@ struct YarpSensorBridge::Impl
             metaData.bridgeOptions.nrJoints = metaData.sensorsList.jointsList.size();
         }
 
+        metaData.sensorsList.jointsTypeList = controlBoardJointTypes;
+
+        this->allJointsArePrismatics
+            = std::all_of(metaData.sensorsList.jointsTypeList.begin(),
+                          metaData.sensorsList.jointsTypeList.end(), //
+                          [](const auto& jointType) { return jointType == JointType::PRISMATIC; });
+        this->allJointsAreRevolutes
+            = std::all_of(metaData.sensorsList.jointsTypeList.begin(),
+                          metaData.sensorsList.jointsTypeList.end(), //
+                          [](const auto& jointType) { return jointType == JointType::REVOLUTE; });
+
         // reset the control board buffers
         this->resetControlBoardBuffers();
 
@@ -1595,21 +1623,69 @@ struct YarpSensorBridge::Impl
                     }
                 }
 
-                // convert from degrees to radians - YARP convention is to store joint positions in
-                // degrees
-                controlBoardRemapperMeasures.jointPositions.noalias()
-                    = controlBoardRemapperMeasures.remappedJointPermutationMatrix
-                      * controlBoardRemapperMeasures.jointPositionsUnordered * M_PI / 180;
+                // Create aliases for clarity
+                auto& measures = controlBoardRemapperMeasures;
+                const auto& perm = measures.remappedJointPermutationMatrix;
 
-                controlBoardRemapperMeasures.jointVelocities.noalias()
-                    = controlBoardRemapperMeasures.remappedJointPermutationMatrix
-                      * controlBoardRemapperMeasures.jointVelocitiesUnordered * M_PI / 180;
+                // Helper lambda for uniform remapping and scaling.
+                auto remapAndScale = [&](auto& target,
+                                         const auto& source,
+                                         std::optional<double> scale = std::nullopt) {
+                    if (!scale.has_value())
+                    {
+                        target.noalias() = perm * source;
+                        return;
+                    }
+                    target.noalias() = perm * source * scale.value();
+                };
 
-                if (streamJointAccelerations)
+                // Helper lambda for mixed joints: remap then convert each revolute joint.
+                auto remapAndConvertMixed = [&](auto& target, const auto& source) {
+                    target.noalias() = perm * source;
+                    for (int i = 0; i < target.size(); ++i)
+                    {
+                        // Convert only revolute joints from degrees to radians.
+                        if (metaData.sensorsList.jointsTypeList[i] == JointType::REVOLUTE)
+                        {
+                            target(i) = deg2rad(target(i));
+                        }
+                    }
+                };
+
+                // Process joint measures based on joint type configuration.
+                if (this->allJointsAreRevolutes)
                 {
-                    controlBoardRemapperMeasures.jointAccelerations.noalias()
-                        = controlBoardRemapperMeasures.remappedJointPermutationMatrix
-                          * controlBoardRemapperMeasures.jointAccelerationsUnordered * M_PI / 180;
+                    remapAndScale(measures.jointPositions,
+                                  measures.jointPositionsUnordered,
+                                  M_PI / 180.0);
+                    remapAndScale(measures.jointVelocities,
+                                  measures.jointVelocitiesUnordered,
+                                  M_PI / 180.0);
+                    if (streamJointAccelerations)
+                    {
+                        remapAndScale(measures.jointAccelerations,
+                                      measures.jointAccelerationsUnordered,
+                                      M_PI / 180.0);
+                    }
+                } else if (this->allJointsArePrismatics)
+                {
+                    remapAndScale(measures.jointPositions, measures.jointPositionsUnordered);
+                    remapAndScale(measures.jointVelocities, measures.jointVelocitiesUnordered);
+                    if (streamJointAccelerations)
+                    {
+                        remapAndScale(measures.jointAccelerations,
+                                      measures.jointAccelerationsUnordered);
+                    }
+                } else // Mixed joints: some revolute and some prismatic.
+                {
+                    remapAndConvertMixed(measures.jointPositions, measures.jointPositionsUnordered);
+                    remapAndConvertMixed(measures.jointVelocities,
+                                         measures.jointVelocitiesUnordered);
+                    if (streamJointAccelerations)
+                    {
+                        remapAndConvertMixed(measures.jointAccelerations,
+                                             measures.jointAccelerationsUnordered);
+                    }
                 }
             } else
             {

src/RobotInterface/include/BipedalLocomotion/RobotInterface/ISensorBridge.h

@@ -24,6 +24,15 @@ namespace BipedalLocomotion
 namespace RobotInterface
 {
 
+/**
+ * Enum class specifying the type of joint
+ */
+enum class JointType
+{
+    REVOLUTE, /**< Revolute joint */
+    PRISMATIC /**< Prismatic joint */
+};
+
 /**
  * Sensor bridge options
  */
@@ -59,6 +68,7 @@ struct SensorBridgeOptions
 struct SensorLists
 {
     std::vector<std::string> jointsList; /**< list of joints attached to the bridge */
+    std::vector<JointType> jointsTypeList; /**< list of joint types */
     std::vector<std::string> IMUsList; /**< list of IMUs attached to the bridge */
     std::vector<std::string> linearAccelerometersList; /**< list of linear accelerometers attached
                                                           to the bridge */