Merge pull request #492 from xEnVrE/fix/wrong_feedback_coupled_joints

[plugins][controlboard] Fix wrong feedback measurements for coupled joints
robotology · May 26, 2020 · 81f7dc1 · 81f7dc1
2 parents 6275d40 + c6da62c
commit 81f7dc1
Show file tree

Hide file tree

Showing 3 changed files with 14 additions and 7 deletions.
diff --git a/CHANGELOG.md b/CHANGELOG.md
@@ -6,6 +6,9 @@ The format of this document is based on [Keep a Changelog](https://keepachangelo
 
 ## [Unreleased]
 
+### Fixed
+- Fixed wrong measurements feedback used for coupled joints in `gazebo_yarp_controlboard`(https://github.com/robotology/gazebo-yarp-plugins/pull/492).
+
 ## [3.4.0] - 2020-05-19
 
 ### Added

diff --git a/plugins/controlboard/include/yarp/dev/ControlBoardDriver.h b/plugins/controlboard/include/yarp/dev/ControlBoardDriver.h
@@ -385,6 +385,7 @@ class yarp::dev::GazeboYarpControlBoardDriver:
 
     yarp::sig::Vector m_positions;          /**< joint positions [Degrees] */
     yarp::sig::Vector m_motPositions;      /**< motor positions [Degrees] */
+    yarp::sig::Vector m_motVelocities;      /**< motor velocities [Degrees/Seconds] */
     yarp::sig::Vector m_velocities;         /**< joint velocities [Degrees/Seconds] */
     yarp::sig::Vector m_torques;            /**< joint torques [Netwon Meters] */
     yarp::sig::Vector m_measTorques;        /**< joint torques from virtual analog sensor [Newton Meters] */

diff --git a/plugins/controlboard/src/ControlBoardDriver.cpp b/plugins/controlboard/src/ControlBoardDriver.cpp
@@ -62,6 +62,7 @@ bool GazeboYarpControlBoardDriver::gazebo_init()
     m_zeroPosition.resize(m_numberOfJoints);
     m_jntReferenceVelocities.resize(m_numberOfJoints);
     m_velocities.resize(m_numberOfJoints);
+    m_motVelocities.resize(m_numberOfJoints);
     m_amp.resize(m_numberOfJoints);
     m_torques.resize(m_numberOfJoints); m_torques.zero();
     m_measTorques.resize(m_numberOfJoints); m_measTorques.zero();
@@ -101,6 +102,7 @@ bool GazeboYarpControlBoardDriver::gazebo_init()
     m_motPositions.zero();
     m_zeroPosition.zero();
     m_velocities.zero();
+    m_motVelocities.zero();
     m_motReferencePositions.zero();
     m_motReferenceVelocities.zero();
     m_motReferenceTorques.zero();
@@ -479,6 +481,7 @@ void GazeboYarpControlBoardDriver::onUpdate(const gazebo::common::UpdateInfo& _i
     }
 
     m_motPositions=m_positions;
+    m_motVelocities=m_velocities;
     //measurements decoupling
     for (size_t cpl_cnt = 0; cpl_cnt < m_coupling_handler.size(); cpl_cnt++)
     {
@@ -574,30 +577,30 @@ void GazeboYarpControlBoardDriver::onUpdate(const gazebo::common::UpdateInfo& _i
         }
         else if ((m_controlMode[j] == VOCAB_CM_POSITION || m_controlMode[j] == VOCAB_CM_POSITION_DIRECT) && (m_interactionMode[j] == VOCAB_IM_COMPLIANT))
         {
-            double q = m_positions[j] - m_zeroPosition[j];
-            forceReference  = -m_impedancePosPDs[j].GetPGain() * (q - m_motReferencePositions[j]) - m_impedancePosPDs[j].GetDGain() * m_velocities[j] + m_torqueOffset[j];
+            double q = m_motPositions[j] - m_zeroPosition[j];
+            forceReference  = -m_impedancePosPDs[j].GetPGain() * (q - m_motReferencePositions[j]) - m_impedancePosPDs[j].GetDGain() * m_motVelocities[j] + m_torqueOffset[j];
         }
         else if ((m_controlMode[j] == VOCAB_CM_VELOCITY) && (m_interactionMode[j] == VOCAB_IM_STIFF))
         {
             gazebo::common::PID &pid = m_pids[VOCAB_PIDTYPE_VELOCITY][j];
-            forceReference = pid.Update(convertUserToGazebo(j, m_velocities[j]) - convertUserToGazebo(j, m_motReferenceVelocities[j]), stepTime);
+            forceReference = pid.Update(convertUserToGazebo(j, m_motVelocities[j]) - convertUserToGazebo(j, m_motReferenceVelocities[j]), stepTime);
         }
         else if ((m_controlMode[j] == VOCAB_CM_VELOCITY) && (m_interactionMode[j] == VOCAB_IM_COMPLIANT))
         {
             gazebo::common::PID &pid = m_pids[VOCAB_PIDTYPE_VELOCITY][j];
-            forceReference = pid.Update(convertUserToGazebo(j, m_velocities[j]) - convertUserToGazebo(j, m_motReferenceVelocities[j]), stepTime);
+            forceReference = pid.Update(convertUserToGazebo(j, m_motVelocities[j]) - convertUserToGazebo(j, m_motReferenceVelocities[j]), stepTime);
             yWarning("Compliant velocity control not yet implemented");
         }
         else if ((m_controlMode[j] == VOCAB_CM_MIXED) && (m_interactionMode[j] == VOCAB_IM_STIFF))
         {
             gazebo::common::PID &pid = m_pids[VOCAB_PIDTYPE_POSITION][j];
-            forceReference = pid.Update(convertUserToGazebo(j, m_positions[j]) - convertUserToGazebo(j, m_motReferencePositions[j]), stepTime);
+            forceReference = pid.Update(convertUserToGazebo(j, m_motPositions[j]) - convertUserToGazebo(j, m_motReferencePositions[j]), stepTime);
 
         }
         else if ((m_controlMode[j] == VOCAB_CM_MIXED) && (m_interactionMode[j] == VOCAB_IM_COMPLIANT))
         {
-            double q = m_positions[j] - m_zeroPosition[j];
-            forceReference  = -m_impedancePosPDs[j].GetPGain() * (q - m_motReferencePositions[j]) - m_impedancePosPDs[j].GetDGain() * m_velocities[j] + m_torqueOffset[j];
+            double q = m_motPositions[j] - m_zeroPosition[j];
+            forceReference  = -m_impedancePosPDs[j].GetPGain() * (q - m_motReferencePositions[j]) - m_impedancePosPDs[j].GetDGain() * m_motVelocities[j] + m_torqueOffset[j];
         }
         else if (m_controlMode[j] == VOCAB_CM_TORQUE)
         {