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 (executable)
--- 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 (executable)
--- 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 (executable)
--- 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 (executable)
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 (file)
--- 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 (file)
--- 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 (file)
--- 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);