Add trapezoidal move to bhand.

diff --git a/bhand_bringup/scripts/close.sh b/bhand_bringup/scripts/close.sh
index 2bd4018..c102578 100755 (executable)
--- a/bhand_bringup/scripts/close.sh
+++ b/bhand_bringup/scripts/close.sh
@@ -2,10 +2,10 @@
 
 rostopic pub /bhand_controller/command \
 std_msgs/Float64MultiArray \
-"{layout: {dim: [size: 4, stride: 1]}, data: [0.0, 0.0, 0.0, 0.0]}" \
+"{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: 1]}, data: [2.44, 2.44, 2.44, 0.0]}" \
+"{layout: {dim: [size: 4, stride: 4]}, data: [2.44, 2.44, 2.44, 0.0]}" \
 "-1"

diff --git a/bhand_bringup/scripts/open.sh b/bhand_bringup/scripts/open.sh
index 4174d25..63bb176 100755 (executable)
--- a/bhand_bringup/scripts/open.sh
+++ b/bhand_bringup/scripts/open.sh
@@ -2,10 +2,10 @@
 
 rostopic pub /bhand_controller/command \
 std_msgs/Float64MultiArray \
-"{layout: {dim: [size: 4, stride: 1]}, data: [0.0, 0.0, 0.0, 0.0]}" \
+"{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: 1]}, data: [0.0, 0.0, 0.0, 3.14]}" \
+"{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/zero.sh
index d6ffe42..b898bdb 100755 (executable)
--- a/bhand_bringup/scripts/zero.sh
+++ b/bhand_bringup/scripts/zero.sh
@@ -2,5 +2,5 @@
 
 rostopic pub /bhand_controller/command \
 std_msgs/Float64MultiArray \
-"{layout: {dim: [size: 4, stride: 1]}, data: [0.0, 0.0, 0.0, 0.0]}" \
+"{layout: {dim: [size: 4, stride: 4]}, data: [0.0, 0.0, 0.0, 0.0]}" \
 "-1"

diff --git a/wam_node_sim/include/wam_node_sim/hand.h b/wam_node_sim/include/wam_node_sim/hand.h
index d15fca7..a837d85 100644 (file)
--- a/wam_node_sim/include/wam_node_sim/hand.h
+++ b/wam_node_sim/include/wam_node_sim/hand.h
@@ -187,7 +187,7 @@ private:
 //     static const enum Puck::Property props[];
 
        ros::Publisher commandPub_; // controller command
-
+       jp_type jointPosition_;         // Necessary to keep trapezoidalMove const
 
        DISALLOW_COPY_AND_ASSIGN(Hand);
 

diff --git a/wam_node_sim/scripts/close_grasp.sh b/wam_node_sim/scripts/close_grasp.sh
new file mode 100755 (executable)
index 0000000..2271890
--- /dev/null
+++ b/wam_node_sim/scripts/close_grasp.sh
@@ -0,0 +1,3 @@
+#!/bin/bash
+
+rosservice call /bhand/close_grasp "{}"

diff --git a/wam_node_sim/scripts/close_spread.sh b/wam_node_sim/scripts/close_spread.sh
new file mode 100755 (executable)
index 0000000..35cad79
--- /dev/null
+++ b/wam_node_sim/scripts/close_spread.sh
@@ -0,0 +1,3 @@
+#!/bin/bash
+
+rosservice call /bhand/close_spread "{}"

diff --git a/wam_node_sim/scripts/finger_pos.sh b/wam_node_sim/scripts/finger_pos.sh
new file mode 100755 (executable)
index 0000000..268854f
--- /dev/null
+++ b/wam_node_sim/scripts/finger_pos.sh
@@ -0,0 +1,7 @@
+#!/bin/bash
+
+if [ "$#" -ne 3 ]; then
+       echo "Error: There should be 3 parameters."
+       exit -1;
+fi;
+rosservice call /bhand/finger_pos "[$1, $2, $3]"

diff --git a/wam_node_sim/scripts/grasp_pos.sh b/wam_node_sim/scripts/grasp_pos.sh
new file mode 100755 (executable)
index 0000000..c7cd193
--- /dev/null
+++ b/wam_node_sim/scripts/grasp_pos.sh
@@ -0,0 +1,7 @@
+#!/bin/bash
+
+if [ "$#" -ne 1 ]; then
+       echo "Error: There should be 1 parameter."
+       exit -1;
+fi;
+rosservice call /bhand/grasp_pos "$1"

diff --git a/wam_node_sim/scripts/open_grasp.sh b/wam_node_sim/scripts/open_grasp.sh
new file mode 100755 (executable)
index 0000000..3883b60
--- /dev/null
+++ b/wam_node_sim/scripts/open_grasp.sh
@@ -0,0 +1,3 @@
+#!/bin/bash
+
+rosservice call /bhand/open_grasp "{}"

diff --git a/wam_node_sim/scripts/open_spread.sh b/wam_node_sim/scripts/open_spread.sh
new file mode 100755 (executable)
index 0000000..618e361
--- /dev/null
+++ b/wam_node_sim/scripts/open_spread.sh
@@ -0,0 +1,3 @@
+#!/bin/bash
+
+rosservice call /bhand/open_spread "{}"

diff --git a/wam_node_sim/scripts/spread_pos.sh b/wam_node_sim/scripts/spread_pos.sh
new file mode 100755 (executable)
index 0000000..259e776
--- /dev/null
+++ b/wam_node_sim/scripts/spread_pos.sh
@@ -0,0 +1,7 @@
+#!/bin/bash
+
+if [ "$#" -ne 1 ]; then
+       echo "Error: There should be 1 parameter."
+       exit -1;
+fi;
+rosservice call /bhand/spread_pos "$1"

diff --git a/wam_node_sim/src/hand.cpp b/wam_node_sim/src/hand.cpp
index 391e5be..d21b073 100644 (file)
--- a/wam_node_sim/src/hand.cpp
+++ b/wam_node_sim/src/hand.cpp
@@ -45,6 +45,7 @@
 #include <limits>
 
 #include <ros/ros.h>
+#include <kdl/velocityprofile_trap.hpp>
 #include <std_msgs/Float64MultiArray.h>
 
 #include <wam_node_sim/hand.h>
@@ -55,6 +56,23 @@ namespace barrett {
 static const double J2_COUNTS_PER_RAD=199111.1/(140*M_PI/180);
 static const double SPREAD_COUNTS_PER_RAD=35840/M_PI;
 
+static const double BHAND_PUBLISH_FREQ = 5.0; // Publishing Frequency for the BarretHand
+
+// from BarrettHand BH8-Series User Manual Firmware Version 4.4x, section 5.3.1 page 31
+// unit (counts/ms) from BH8-280 Quick Referencd Commands cheatsheet
+static const int J2_MAX_COUNTS_PER_MS=100;
+static const int SPREAD_MAX_COUNTS_PER_MS=60;
+
+// from BarrettHand BH8-Series User Manual Firmware Version 4.4x, section 5.3.4 page 34
+// unit (counts/ms^2) from BH8-280 Quick Referencd Commands cheatsheet
+static const int J2_MAX_COUNTS_PER_MS2=4;
+static const int SPREAD_MAX_COUNTS_PER_MS2=2;
+
+static const double J2_MAX_VEL=J2_MAX_COUNTS_PER_MS*1e3/J2_COUNTS_PER_RAD;
+static const double SPREAD_MAX_VEL=SPREAD_MAX_COUNTS_PER_MS*1e3/SPREAD_COUNTS_PER_RAD;
+static const double J2_MAX_ACCEL=J2_MAX_COUNTS_PER_MS2*1e3/J2_COUNTS_PER_RAD;
+static const double SPREAD_MAX_ACCEL=SPREAD_MAX_COUNTS_PER_MS2*1e3/SPREAD_COUNTS_PER_RAD;
+
 //const enum Puck::Property Hand::props[] = { Puck::HOLD, Puck::CMD, Puck::MODE, Puck::P, Puck::T, Puck::SG };
 
 /** Hand Constructor */
@@ -105,9 +123,11 @@ Hand::Hand(const std::string &nodeName) :
 //     j2pt[SPREAD_INDEX] = motorPucks[SPREAD_INDEX].getIpnm() / SPREAD_RATIO;
 
        for(int i=0;i < DOF;i++) encoderTmp[i].resize(2);
+       jointPosition_.Zero();
 
        ros::NodeHandle node_(nodeName); // bhand specific nodehandle   
        commandPub_=node_.advertise<std_msgs::Float64MultiArray>("command", 1); // bhand/command
+
 }
 /** Hand Destructor */
 Hand::~Hand()
@@ -173,8 +193,49 @@ void Hand::close(unsigned int whichDigits, bool blocking) const {
 /** trapezoidalMove Method */
 void Hand::trapezoidalMove(const jp_type& jp, unsigned int whichDigits, bool blocking) const
 {
-//     setProperty(whichDigits, Puck::E, j2pp.cwise() * jp);
-//     setProperty(whichDigits, Puck::MODE, MotorPuck::MODE_TRAPEZOIDAL);
+       jp_type finalJp;
+       for(int i=0;i < DOF;i++) finalJp[i]=digitsInclude(whichDigits,i)? jp[i]:jointPosition_[i];
+       
+       int longest;
+       (jp-finalJp).cwiseAbs().maxCoeff(&longest);
+       
+       std::vector<KDL::VelocityProfile_Trap> trajectory(DOF);
+
+       if(longest!=SPREAD_INDEX) trajectory[longest].SetMax(J2_MAX_VEL,J2_MAX_ACCEL);
+               else trajectory[longest].SetMax(SPREAD_MAX_VEL,SPREAD_MAX_ACCEL);
+       trajectory[longest].SetProfile(jointPosition_[longest],finalJp[longest]);
+       double tf=trajectory[longest].Duration();
+
+       for(int i=0;i < DOF;i++)
+       {
+               if(i == longest) continue;
+               if(i!=SPREAD_INDEX) trajectory[i].SetMax(J2_MAX_VEL,J2_MAX_ACCEL);
+                       else trajectory[i].SetMax(SPREAD_MAX_VEL,SPREAD_MAX_ACCEL);
+               trajectory[i].SetMax(0,0);
+               trajectory[i].SetProfileDuration(jointPosition_[i],finalJp[i],tf);
+       }
+       
+       std_msgs::Float64MultiArray commandMsg;
+       commandMsg.layout.dim.resize(1);
+       commandMsg.layout.dim[0].label="bhand/command";
+       commandMsg.layout.dim[0].size=DOF;
+       commandMsg.layout.dim[0].stride=DOF;
+       commandMsg.layout.data_offset=0;
+
+       ros::Rate loop(BHAND_PUBLISH_FREQ);
+       double t=0.0;
+       ros::Time t0=ros::Time::now();
+       while(ros::ok() && (t=(ros::Time::now()-t0).toSec()) < tf)
+       {
+               for(size_t i=0; i < DOF;i++) commandMsg.data.push_back(trajectory[i].Pos(t));
+               commandPub_.publish(commandMsg);
+                       
+               ros::spinOnce();
+               loop.sleep();
+       }
+       for(size_t i=0; i < DOF;i++) commandMsg.data.push_back(finalJp[i]);
+       commandPub_.publish(commandMsg);
+
        blockIf(blocking, whichDigits);
 }
 /** velocityMove Method */
@@ -305,6 +366,7 @@ void Hand::jointStatesCB(const sensor_msgs::JointState &jointStates)
                        encoderTmp[i][0]= J2_RATIO*jointStates.position[j]*J2_COUNTS_PER_RAD;
                        encoderTmp[i][1] = std::numeric_limits<int>::max();
                }
+               jointPosition_[i]=jointStates.position[j];
        }
 }
 

diff --git a/wam_node_sim/src/wam_node_sim.cpp b/wam_node_sim/src/wam_node_sim.cpp
index 2bfa79f..095647e 100644 (file)
--- a/wam_node_sim/src/wam_node_sim.cpp
+++ b/wam_node_sim/src/wam_node_sim.cpp
@@ -230,9 +230,9 @@ template<size_t DOF>
       hand_fngr_pos_srv = nh_.advertiseService("finger_pos", &WamNode::handFingerPos, this); // bhand/finger_pos
       hand_grsp_pos_srv = nh_.advertiseService("grasp_pos", &WamNode::handGraspPos, this); // bhand/grasp_pos
       hand_sprd_pos_srv = nh_.advertiseService("spread_pos", &WamNode::handSpreadPos, this); // bhand/spread_pos
-      hand_fngr_vel_srv = nh_.advertiseService("finger_vel", &WamNode::handFingerVel, this); // bhand/finger_vel
-      hand_grsp_vel_srv = nh_.advertiseService("grasp_vel", &WamNode::handGraspVel, this); // bhand/grasp_vel
-      hand_sprd_vel_srv = nh_.advertiseService("spread_vel", &WamNode::handSpreadVel, this); // bhand/spread_vel
+//      hand_fngr_vel_srv = nh_.advertiseService("finger_vel", &WamNode::handFingerVel, this); // bhand/finger_vel
+//      hand_grsp_vel_srv = nh_.advertiseService("grasp_vel", &WamNode::handGraspVel, this); // bhand/grasp_vel
+//      hand_sprd_vel_srv = nh_.advertiseService("spread_vel", &WamNode::handSpreadVel, this); // bhand/spread_vel
 
       //Set up the BarrettHand joint state publisher
 //      const char* bhand_jnts[] = {"inner_f1", "inner_f2", "inner_f3", "spread", "outer_f1", "outer_f2", "outer_f3"};
@@ -268,15 +268,15 @@ template<size_t DOF>
 
 
     //Advertising the following rosservices
-    gravity_srv = n_.advertiseService("gravity_comp", &WamNode::gravity, this); // wam/gravity_comp
-    go_home_srv = n_.advertiseService("go_home", &WamNode::goHome, this); // wam/go_home
+//    gravity_srv = n_.advertiseService("gravity_comp", &WamNode::gravity, this); // wam/gravity_comp
+//    go_home_srv = n_.advertiseService("go_home", &WamNode::goHome, this); // wam/go_home
     hold_jpos_srv = n_.advertiseService("hold_joint_pos", &WamNode::holdJPos, this); // wam/hold_joint_pos
     hold_cpos_srv = n_.advertiseService("hold_cart_pos", &WamNode::holdCPos, this); // wam/hold_cart_pos
     hold_ortn_srv = n_.advertiseService("hold_ortn", &WamNode::holdOrtn, this); // wam/hold_ortn
-    joint_move_srv = n_.advertiseService("joint_move", &WamNode::jointMove, this); // wam/joint_move
-    pose_move_srv = n_.advertiseService("pose_move", &WamNode::poseMove, this); // wam/pose_move
-    cart_move_srv = n_.advertiseService("cart_move", &WamNode::cartMove, this); // wam/cart_pos_move
-    ortn_move_srv = n_.advertiseService("ortn_move", &WamNode::ortnMove, this); // wam/ortn_move
+//    joint_move_srv = n_.advertiseService("joint_move", &WamNode::jointMove, this); // wam/joint_move
+//    pose_move_srv = n_.advertiseService("pose_move", &WamNode::poseMove, this); // wam/pose_move
+//    cart_move_srv = n_.advertiseService("cart_move", &WamNode::cartMove, this); // wam/cart_pos_move
+//    ortn_move_srv = n_.advertiseService("ortn_move", &WamNode::ortnMove, this); // wam/ortn_move
 
   }
 
@@ -475,7 +475,7 @@ template<size_t DOF>
   bool WamNode<DOF>::handGraspPos(wam_srvs::BHandGraspPos::Request &req, wam_srvs::BHandGraspPos::Response &res)
   {
     ROS_INFO("Moving BarrettHand Grasp: %.3f radians", req.radians);
-    hand->trapezoidalMove(Hand::jp_type(req.radians), Hand::GRASP, false);
+    hand->trapezoidalMove(Hand::jp_type(req.radians,req.radians,req.radians,0.0), Hand::GRASP, false);
     return true;
   }
 
@@ -484,7 +484,7 @@ template<size_t DOF>
   bool WamNode<DOF>::handSpreadPos(wam_srvs::BHandSpreadPos::Request &req, wam_srvs::BHandSpreadPos::Response &res)
   {
     ROS_INFO("Moving BarrettHand Spread: %.3f radians", req.radians);
-    hand->trapezoidalMove(Hand::jp_type(req.radians), Hand::SPREAD, false);
+    hand->trapezoidalMove(Hand::jp_type(0.0,0.0,0.0,req.radians), Hand::SPREAD, false);
     return true;
   }