From: Walter Fetter Lages Date: Sun, 9 Dec 2018 06:05:43 +0000 (-0200) Subject: Add trapezoidal move to bhand. X-Git-Url: http://git.ece.ufrgs.br/?a=commitdiff_plain;h=7013e9aba48d4d79836b6524e1f0ce84f1ee9651;p=ufrgs_wam.git Add trapezoidal move to bhand. --- diff --git a/bhand_bringup/scripts/close.sh b/bhand_bringup/scripts/close.sh index 2bd4018..c102578 100755 --- 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 --- 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 --- 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 --- 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 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 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 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 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 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 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 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 --- a/wam_node_sim/src/hand.cpp +++ b/wam_node_sim/src/hand.cpp @@ -45,6 +45,7 @@ #include #include +#include #include #include @@ -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("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 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::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 --- a/wam_node_sim/src/wam_node_sim.cpp +++ b/wam_node_sim/src/wam_node_sim.cpp @@ -230,9 +230,9 @@ template 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 //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 bool WamNode::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 bool WamNode::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; }