From: Walter Fetter Lages <w.fetter@ieee.org>
Date: Mon, 10 Dec 2018 08:56:52 +0000 (-0200)
Subject: Fix processing of spread encoder.
X-Git-Url: http://git.ece.ufrgs.br/?a=commitdiff_plain;h=28bf8302f8415a6f7bc4efe27d7cc0bccad4de21;p=ufrgs_wam.git

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
--- 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;