Fix processing of spread encoder.

diff --git a/wam_node_sim/src/hand.cpp b/wam_node_sim/src/hand.cpp
index d21b073..ccad25a 100644 (file)
--- a/wam_node_sim/src/hand.cpp
+++ b/wam_node_sim/src/hand.cpp
@@ -349,14 +349,20 @@ void Hand::jointStatesCB(const sensor_msgs::JointState &jointStates)
        jointNames.push_back("bhand_finger2_joint_3");
        jointNames.push_back("bhand_finger3_joint_3");
        jointNames.push_back("bhand_spread_finger2");
-                                               
+       
+       // Fingers                                      
        for(int i=0;i < DOF;i++)
        {
                int j=find(jointStates.name.begin(),jointStates.name.end(),jointNames[i])-jointStates.name.begin();
                if(j == jointStates.name.size()) continue; 
                int k=find(jointStates.name.begin(),jointStates.name.end(),jointNames[i+SPREAD_INDEX+1])-jointStates.name.begin();
 
-               if(useSecondaryEncoders && k < jointStates.name.size())
+               if(i == SPREAD_INDEX)
+               {
+                       encoderTmp[i][0]= SPREAD_RATIO*jointStates.position[j]*SPREAD_COUNTS_PER_RAD;
+                       encoderTmp[i][1] = std::numeric_limits<int>::max();
+               }
+               else if(useSecondaryEncoders && k < jointStates.name.size())
                {
                        encoderTmp[i][0] = (jointStates.position[k]+jointStates.position[j])/(1.0/J2_RATIO+1.0/J3_RATIO)*J2_COUNTS_PER_RAD;
                        encoderTmp[i][1] = J2_ENCODER_RATIO*jointStates.position[j]*J2_COUNTS_PER_RAD;