Merge pull request #502 from xEnVrE/fix/mais_readout_range

[plugins][maissensor] Fix discrepancy of MAIS analog sensor between simulation and real robot

CHANGELOG.md

@@ -9,6 +9,7 @@ The format of this document is based on [Keep a Changelog](https://keepachangelo
 ### Fixed
 - Fixed wrong measurements feedback used for coupled joints in `gazebo_yarp_controlboard`(https://github.com/robotology/gazebo-yarp-plugins/pull/492).
 - Fixed mismatched behavior, with respect to the real robot, of coupling handler FingersAbductionCouplingHandler in 'gazebo_yarp_controlboard' plugin (https://github.com/robotology/gazebo-yarp-plugins/pull/499)
+- Fixed mismatched behavior, with respect to the real robot, of mais analog sensors in 'gazebo_yarp_maissensor' plugin (https://github.com/robotology/gazebo-yarp-plugins/pull/500)
 
 ### Added
 - Add external wrench smoothing feature for `externalwrench` plugin (https://github.com/robotology/gazebo-yarp-plugins/pull/495)

plugins/maissensor/include/yarp/dev/MaisSensorDriver.h

@@ -92,7 +92,14 @@ class yarp::dev::GazeboYarpMaisSensorDriver:
         JointType_Revolute,
         JointType_Prismatic
     };
-
+
+    struct Range
+    {
+        Range() : min(0), max(0){}
+        double min;
+        double max;
+    };
+
     std::string deviceName;
     gazebo::physics::Model* m_robot;
     gazebo::event::ConnectionPtr m_updateConnection;
@@ -103,7 +110,7 @@ class yarp::dev::GazeboYarpMaisSensorDriver:
 
     yarp::sig::Vector m_positions; /**< joint positions [Degrees] */
     unsigned int m_numberOfJoints; /**< number of joints controlled by the control board */
-
+    std::vector<Range> m_jointPositionLimits;
 
     yarp::os::Stamp m_lastTimestamp; /**< timestamp, updated with simulation time at each onUpdate call */
 
@@ -125,6 +132,7 @@ class yarp::dev::GazeboYarpMaisSensorDriver:
      * Private methods
      */
     bool configureJointType();
+    void configureJointsLimits();
     bool setJointNames();  //WORKS
 
     virtual int read(yarp::sig::Vector &out);

plugins/maissensor/src/MaisSensorDriver.cpp

@@ -55,10 +55,11 @@ bool GazeboYarpMaisSensorDriver::gazebo_init()
 
     if (!setJointNames()) return false;      // this function also fills in the m_jointPointers vector
 
-    m_channels_num = 16;
+    m_channels_num = 15;
     m_numberOfJoints = m_jointNames.size();
 
     m_positions.resize(m_numberOfJoints);
+    m_jointPositionLimits.resize(m_numberOfJoints);
 
     m_jointTypes.resize(m_numberOfJoints);
 
@@ -75,6 +76,8 @@ bool GazeboYarpMaisSensorDriver::gazebo_init()
     if(!configureJointType() )
         return false;
 
+    configureJointsLimits();
+
     this->m_updateConnection =  gazebo::event::Events::ConnectWorldUpdateBegin(boost::bind(&GazeboYarpMaisSensorDriver::onUpdate,  this, _1));
 
     m_gazeboNode = gazebo::transport::NodePtr(new gazebo::transport::Node);
@@ -122,6 +125,20 @@ bool GazeboYarpMaisSensorDriver::configureJointType()
     return ret;
 }
 
+void GazeboYarpMaisSensorDriver::configureJointsLimits()
+{
+    for (size_t i = 0; i < m_numberOfJoints; ++i)
+    {
+#if GAZEBO_MAJOR_VERSION >= 8
+        m_jointPositionLimits[i].max = m_jointPointers[i]->UpperLimit(0);
+        m_jointPositionLimits[i].min = m_jointPointers[i]->LowerLimit(0);
+#else
+        m_jointPositionLimits[i].max = m_jointPointers[i]->GetUpperLimit(0).Radian();
+        m_jointPositionLimits[i].min = m_jointPointers[i]->GetLowerLimit(0).Radian();
+#endif
+    }
+}
+
 void GazeboYarpMaisSensorDriver::onUpdate(const gazebo::common::UpdateInfo& _info)
 {
     std::lock_guard<std::mutex> lock(m_mutex);
@@ -208,7 +225,15 @@ double GazeboYarpMaisSensorDriver::convertGazeboToUser(int joint, double value)
     {
         case JointType_Revolute:
         {
-            newValue = GazeboYarpPlugins::convertRadiansToDegrees(value);
+            // get hw limits
+            const double& limit_min = m_jointPositionLimits[joint].min;
+            const double& limit_max = m_jointPositionLimits[joint].max;
+
+            // evaluate new value in the range 0 - 255 such that
+            // 255 is reached when the joint reading is <= limit_min
+            // 0 is reached when the joints reading is >= limit_max
+            // i.e. rejecting readings having values < limit_min or > limit_max
+            newValue = 255 * (1 - std::min(1.0, std::max(0.0, (value - limit_min) / (limit_max - limit_min))));
             break;
         }