From: Walter Fetter Lages <w.fetter@ieee.org>
Date: Tue, 8 Jan 2019 22:16:12 +0000 (-0200)
Subject: Fix spread range: 0 is open. M_PI is closed.
X-Git-Url: http://git.ece.ufrgs.br/?a=commitdiff_plain;h=cef8f247ade4f85f9d96beb612da6e05fc0cb92a;p=ufrgs_wam.git

Fix spread range: 0 is open. M_PI is closed.
---

diff --git a/bhand_bringup/scripts/close.sh b/bhand_bringup/scripts/close.sh
index c102578..ddb19ac 100755
--- a/bhand_bringup/scripts/close.sh
+++ b/bhand_bringup/scripts/close.sh
@@ -2,10 +2,5 @@
 
 rostopic pub /bhand_controller/command \
 std_msgs/Float64MultiArray \
-"{layout: {dim: [size: 4, stride: 4]}, data: [0.0, 0.0, 0.0, 0.0]}" \
-"-1"
-
-rostopic pub /bhand_controller/command \
-std_msgs/Float64MultiArray \
-"{layout: {dim: [size: 4, stride: 4]}, data: [2.44, 2.44, 2.44, 0.0]}" \
+"{layout: {dim: [size: 4, stride: 4]}, data: [2.44, 2.44, 2.44, 3.14]}" \
 "-1"
diff --git a/bhand_bringup/scripts/open.sh b/bhand_bringup/scripts/open.sh
index 63bb176..b898bdb 100755
--- a/bhand_bringup/scripts/open.sh
+++ b/bhand_bringup/scripts/open.sh
@@ -4,8 +4,3 @@ rostopic pub /bhand_controller/command \
 std_msgs/Float64MultiArray \
 "{layout: {dim: [size: 4, stride: 4]}, data: [0.0, 0.0, 0.0, 0.0]}" \
 "-1"
-
-rostopic pub /bhand_controller/command \
-std_msgs/Float64MultiArray \
-"{layout: {dim: [size: 4, stride: 4]}, data: [0.0, 0.0, 0.0, 3.14]}" \
-"-1"
diff --git a/bhand_bringup/scripts/zero.sh b/bhand_bringup/scripts/spread_close_grasp_open.sh
similarity index 55%
rename from bhand_bringup/scripts/zero.sh
rename to bhand_bringup/scripts/spread_close_grasp_open.sh
index b898bdb..de0749e 100755
--- a/bhand_bringup/scripts/zero.sh
+++ b/bhand_bringup/scripts/spread_close_grasp_open.sh
@@ -2,5 +2,5 @@
 
 rostopic pub /bhand_controller/command \
 std_msgs/Float64MultiArray \
-"{layout: {dim: [size: 4, stride: 4]}, data: [0.0, 0.0, 0.0, 0.0]}" \
+"{layout: {dim: [size: 4, stride: 4]}, data: [0.0, 0.0, 0.0, 3.14]}" \
 "-1"
diff --git a/bhand_bringup/scripts/spread_open_grasp_close.sh b/bhand_bringup/scripts/spread_open_grasp_close.sh
new file mode 100755
index 0000000..aadda2a
--- /dev/null
+++ b/bhand_bringup/scripts/spread_open_grasp_close.sh
@@ -0,0 +1,6 @@
+#!/bin/bash
+
+rostopic pub /bhand_controller/command \
+std_msgs/Float64MultiArray \
+"{layout: {dim: [size: 4, stride: 4]}, data: [2.44, 2.44, 2.44, 0.0]}" \
+"-1"
diff --git a/bhand_description/xacro/bhand.urdf.xacro b/bhand_description/xacro/bhand.urdf.xacro
index a10c3b9..ed19494 100644
--- a/bhand_description/xacro/bhand.urdf.xacro
+++ b/bhand_description/xacro/bhand.urdf.xacro
@@ -11,56 +11,57 @@
   <link name="bhand_origin"/>
 
   <xacro:bhand_base parent="origin" link="base_link" joint="base_joint"/>
-
-  <xacro:bhand_link1 parent="base_link" link="finger1_link_1" joint="spread" type="revolute">
+ 
+<!--
+	Spread is defined as finger2 joint (and not finger1 joint) to keep the
+	right-hand rule for the spread
+-->
+  <xacro:bhand_link1 parent="base_link" link="finger2_link_1" joint="spread" type="revolute">
   		<inertial_attr>
-  			<inertial>
+ 			<inertial>
 <!--
 	Inertia parameters from http://support.barrett.com/wiki/Hand/280/MassProperties
 	Finger spread frame
 -->
 				<mass value="0.14109"/>
 				<origin xyz="0.030616 -7.3219e-5 -0.011201"/>  
-				<!--inertia ixx="2.0672e-5" ixy="2.6024e-7" ixz="6.3481e-6" iyy="7.4105e-5" iyz="1.7118e-8" izz="6.8207e-5"/-->
+				<!--inertia ixx="2.0672e-5" ixy="2.6024e-7" ixz="6.3481e-6" iyy="7.4105e-5" iyz="1.7118e-8" izz="6.8207e-5"/ -->
 				<inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
 			</inertial>
 		</inertial_attr>
-		<origin xyz="0.02475 0.0 ${0.1325-0.06}" rpy="0.0 0.0 ${M_PI}" /> 
-		<limit effort="30" velocity="1.5" lower="0.0" upper="${M_PI}" />
+		<origin xyz="-0.02475 0.0 ${0.1325-0.06}" rpy="0.0 0.0 0.0" /> 
+		<limit effort="30.0" velocity="1.5" lower="0" upper="${M_PI}" />
 		<mimic_attr/>
   </xacro:bhand_link1>
 
   <xacro:bhand_transmission joint="spread" />
 
-  <xacro:bhand_link1 parent="base_link" link="finger2_link_1" joint="spread_finger2" type="revolute">
+  <xacro:bhand_link1 parent="base_link" link="finger1_link_1" joint="spread_mimic" type="revolute">
   		<inertial_attr>
- 			<inertial>
-
+  			<inertial>
 <!--
 	Inertia parameters from http://support.barrett.com/wiki/Hand/280/MassProperties
 	Finger spread frame
 -->
-
 				<mass value="0.14109"/>
 				<origin xyz="0.030616 -7.3219e-5 -0.011201"/>  
-				<!--inertia ixx="2.0672e-5" ixy="2.6024e-7" ixz="6.3481e-6" iyy="7.4105e-5" iyz="1.7118e-8" izz="6.8207e-5"/ -->
+				<!--inertia ixx="2.0672e-5" ixy="2.6024e-7" ixz="6.3481e-6" iyy="7.4105e-5" iyz="1.7118e-8" izz="6.8207e-5"/-->
 				<inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
 			</inertial>
-		</inertial_attr>
-		<origin xyz="-0.02475 0.0 ${0.1325-0.06}" rpy="0.0 0.0 ${M_PI}" /> 
-		<limit effort="30.0" velocity="1.5" lower="-${M_PI}" upper="0.0" />
+		</inertial_attr>                 
+		<origin xyz="0.02475 0.0 ${0.1325-0.06}" rpy="0.0 0.0 0.0" /> 
+		<limit effort="30" velocity="1.5" lower="-${M_PI}" upper="0" />
 		<mimic_attr>
 			<mimic joint="bhand_spread" multiplier="-1.0" offset="0.0" />
 		</mimic_attr>
-
   </xacro:bhand_link1>
 
-  <xacro:bhand_transmission joint="spread_finger2" />
+  <xacro:bhand_transmission joint="spread_mimic" />
 
   <gazebo>
 	<plugin name="mimic_plugin" filename="libroboticsgroup_gazebo_mimic_joint_plugin.so">
 		<joint>bhand_spread</joint>
-		<mimicJoint>bhand_spread_finger2</mimicJoint>
+		<mimicJoint>bhand_spread_mimic</mimicJoint>
 		<multiplier>-1.0</multiplier>
 		<offset>0.0</offset>
 		<maxEffor>30.0</maxEffor>
@@ -71,13 +72,11 @@
   <xacro:bhand_link1 parent="base_link" link="finger3_link_1" joint="finger3_joint_1" type="fixed">
     		<inertial_attr>
  			<inertial>
-
 <!--
 	Inertia parameters from http://support.barrett.com/wiki/Hand/280/MassProperties
 	Finger spread frame
 	Is this included in base? Fix this...
 -->
-
 				<mass value="0.14109"/>
 				<origin xyz="0.030616 -7.3219e-5 -0.011201"/>  
 				<!--inertia ixx="2.0672e-5" ixy="2.6024e-7" ixz="6.3481e-6" iyy="7.4105e-5" iyz="1.7118e-8" izz="6.8207e-5"/-->
diff --git a/wam_node_sim/src/hand.cpp b/wam_node_sim/src/hand.cpp
index f8292e2..877c4fc 100644
--- a/wam_node_sim/src/hand.cpp
+++ b/wam_node_sim/src/hand.cpp
@@ -179,14 +179,14 @@ void Hand::waitUntilDoneMoving(unsigned int whichDigits, double period_s) const
 /** open Method */
 void Hand::open(unsigned int whichDigits, bool blocking) const {
 	jp_type jp;
-	jp << 0.0, 0.0, 0.0, M_PI;
+	jp << 0.0, 0.0, 0.0, 0.0;
 	trapezoidalMove(jp,whichDigits,blocking);
 	blockIf(blocking, whichDigits);
 }
 /** close Method */
 void Hand::close(unsigned int whichDigits, bool blocking) const {
 	jp_type jp;
-	jp << (140.0*M_PI/180.0), (140.0*M_PI/180.0), (140.0*M_PI/180.0), 0.0;
+	jp << (140.0*M_PI/180.0), (140.0*M_PI/180.0), (140.0*M_PI/180.0), M_PI;
 	trapezoidalMove(jp,whichDigits,blocking);
 	blockIf(blocking, whichDigits);
 }
@@ -348,7 +348,7 @@ void Hand::jointStatesCB(const sensor_msgs::JointState &jointStates)
 	jointNames.push_back("bhand_finger1_joint_3");
 	jointNames.push_back("bhand_finger2_joint_3");
 	jointNames.push_back("bhand_finger3_joint_3");
-	jointNames.push_back("bhand_spread_finger2");
+	jointNames.push_back("bhand_spread_mimic");
 	
 	// Fingers					
 	for(int i=0;i < DOF;i++)
diff --git a/wam_node_test/src/wam_node_test.cpp b/wam_node_test/src/wam_node_test.cpp
index bd0a908..3566463 100644
--- a/wam_node_test/src/wam_node_test.cpp
+++ b/wam_node_test/src/wam_node_test.cpp
@@ -255,7 +255,7 @@ void WamNodeOp::closeSpread(bool block)
 	
 	if(closeSpreadCli_.call(emptySrv)) ROS_INFO_STREAM("Spread closed.");
 	else ROS_ERROR_STREAM("Error closing spread.");
-	if(block) spreadWait(0);
+	if(block) spreadWait(M_PI);
 }
 
 void WamNodeOp::openGrasp(bool block)
@@ -273,7 +273,7 @@ void WamNodeOp::openSpread(bool block)
 	
 	if(openSpreadCli_.call(emptySrv)) ROS_INFO_STREAM("Spread open.");
 	else ROS_ERROR_STREAM("Error opening spread.");
-	if(block) spreadWait(M_PI);
+	if(block) spreadWait(0.0);
 }
 
 void WamNodeOp::goHome(void)
@@ -490,9 +490,14 @@ int main(int argc,char* argv[])
 	ros::NodeHandle node;
 
 	WamNodeOp wam(node,50.0,0.02,0.5,0.005,0.1);
-
+	
 	std::vector<float> jpCmd(7,0);
 	jpCmd[1]=-2.0;
+	jpCmd[3]=3.1-0.4;
+	wam.jointPos(jpCmd);
+	
+	wam.closeSpread();
+	
 	jpCmd[3]=3.1;
 	wam.jointPos(jpCmd);