[plugins][controlboard] Change behavior of FingersAbductionCouplingHa…

…ndler such that abduction is minimum when the associated DoF is commanded to 60.0 and maximum when commanded to 0.0 as on the real robot
robotology · Jul 17, 2020 · 7585bcb · 7585bcb
1 parent ca8f854
commit 7585bcb
Showing 1 changed file with 12 additions and 12 deletions.
diff --git a/plugins/controlboard/src/ControlBoardDriverCoupling.cpp b/plugins/controlboard/src/ControlBoardDriverCoupling.cpp
@@ -462,22 +462,22 @@ FingersAbductionCouplingHandler::FingersAbductionCouplingHandler(gazebo::physics
 bool FingersAbductionCouplingHandler::decouplePos (yarp::sig::Vector& current_pos)
 {
     if (m_coupledJoints.size()!=m_couplingSize) return false;
-    current_pos[m_coupledJoints[0]] = current_pos[m_coupledJoints[3]];
+    current_pos[m_coupledJoints[0]] = (20.0 - current_pos[m_coupledJoints[2]])*3;
     return true;
 }
 
 bool FingersAbductionCouplingHandler::decoupleVel (yarp::sig::Vector& current_vel)
 {
     if (m_coupledJoints.size()!=m_couplingSize) return false;
-    current_vel[m_coupledJoints[0]] = current_vel[m_coupledJoints[3]];
+    current_vel[m_coupledJoints[0]] = -current_vel[m_coupledJoints[2]]*3;
     return true;
 }
 
 bool FingersAbductionCouplingHandler::decoupleAcc (yarp::sig::Vector& current_acc)
 {
 
     if (m_coupledJoints.size()!=m_couplingSize) return false;
-    current_acc[m_coupledJoints[0]] = current_acc[m_coupledJoints[3]];
+    current_acc[m_coupledJoints[0]] = -current_acc[m_coupledJoints[2]]*3;
     return true;
 }
 
@@ -491,32 +491,32 @@ yarp::sig::Vector FingersAbductionCouplingHandler::decoupleRefPos (yarp::sig::Ve
 {
     yarp::sig::Vector out = pos_ref;
     if (m_coupledJoints.size()!=m_couplingSize) {yError() << "FingersAbductionCouplingHandler: Invalid coupling vector"; return out;}
-    out[m_coupledJoints[0]] = -pos_ref[m_coupledJoints[0]]/2;
+    out[m_coupledJoints[0]] = -(20.0 - pos_ref[m_coupledJoints[0]]/3);
     out[m_coupledJoints[1]] = 0.0;
-    out[m_coupledJoints[2]] = pos_ref[m_coupledJoints[0]]/2;
-    out[m_coupledJoints[3]] = pos_ref[m_coupledJoints[0]];
+    out[m_coupledJoints[2]] = 20.0 - pos_ref[m_coupledJoints[0]]/3;
+    out[m_coupledJoints[3]] = 20.0 - pos_ref[m_coupledJoints[0]]/3;
     return out;
 }
 
 yarp::sig::Vector FingersAbductionCouplingHandler::decoupleRefVel (yarp::sig::Vector& vel_ref)
 {
     yarp::sig::Vector out = vel_ref;
     if (m_coupledJoints.size()!=m_couplingSize) {yError() << "FingersAbductionCouplingHandler: Invalid coupling vector"; return out;}
-    out[m_coupledJoints[0]] = -vel_ref[m_coupledJoints[0]]/2;
+    out[m_coupledJoints[0]] = vel_ref[m_coupledJoints[0]]/3;
     out[m_coupledJoints[1]] = 0.0;
-    out[m_coupledJoints[2]] = vel_ref[m_coupledJoints[0]]/2;
-    out[m_coupledJoints[3]] = vel_ref[m_coupledJoints[0]];
+    out[m_coupledJoints[2]] = -vel_ref[m_coupledJoints[0]]/3;
+    out[m_coupledJoints[3]] = -vel_ref[m_coupledJoints[0]]/3;
     return out;
 }
 
 yarp::sig::Vector FingersAbductionCouplingHandler::decoupleRefTrq (yarp::sig::Vector& trq_ref)
 {
     yarp::sig::Vector out =trq_ref;
     if (m_coupledJoints.size()!=m_couplingSize) {yError() << "FingersAbductionCouplingHandler: Invalid coupling vector"; return out;}
-    out[m_coupledJoints[0]] = -trq_ref[m_coupledJoints[0]]/2;
+    out[m_coupledJoints[0]] = trq_ref[m_coupledJoints[0]]/3;
     out[m_coupledJoints[1]] = 0.0;
-    out[m_coupledJoints[2]] = trq_ref[m_coupledJoints[0]]/2;
-    out[m_coupledJoints[3]] = trq_ref[m_coupledJoints[0]];
+    out[m_coupledJoints[2]] = -trq_ref[m_coupledJoints[0]]/3;
+    out[m_coupledJoints[3]] = -trq_ref[m_coupledJoints[0]]/3;
     return out;
 }