From: Walter Fetter Lages Date: Sat, 8 Dec 2018 21:37:14 +0000 (-0200) Subject: Add bhand processing to wam_node_sim. X-Git-Url: http://git.ece.ufrgs.br/?a=commitdiff_plain;h=e6111c0cb23f4dc215d42364cf7513a7d5365351;p=ufrgs_wam.git Add bhand processing to wam_node_sim. Change jointStateCB() to process joint names. --- diff --git a/bhand_bringup/scripts/close.sh b/bhand_bringup/scripts/close.sh new file mode 100755 index 0000000..2bd4018 --- /dev/null +++ b/bhand_bringup/scripts/close.sh @@ -0,0 +1,11 @@ +#!/bin/bash + +rostopic pub /bhand_controller/command \ +std_msgs/Float64MultiArray \ +"{layout: {dim: [size: 4, stride: 1]}, 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]}" \ +"-1" diff --git a/bhand_bringup/scripts/open.sh b/bhand_bringup/scripts/open.sh new file mode 100755 index 0000000..4174d25 --- /dev/null +++ b/bhand_bringup/scripts/open.sh @@ -0,0 +1,11 @@ +#!/bin/bash + +rostopic pub /bhand_controller/command \ +std_msgs/Float64MultiArray \ +"{layout: {dim: [size: 4, stride: 1]}, 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]}" \ +"-1" diff --git a/bhand_bringup/scripts/zero.sh b/bhand_bringup/scripts/zero.sh new file mode 100755 index 0000000..d6ffe42 --- /dev/null +++ b/bhand_bringup/scripts/zero.sh @@ -0,0 +1,6 @@ +#!/bin/bash + +rostopic pub /bhand_controller/command \ +std_msgs/Float64MultiArray \ +"{layout: {dim: [size: 4, stride: 1]}, data: [0.0, 0.0, 0.0, 0.0]}" \ +"-1" diff --git a/wam_node_sim/CMakeLists.txt b/wam_node_sim/CMakeLists.txt index 2d347ad..3e78156 100644 --- a/wam_node_sim/CMakeLists.txt +++ b/wam_node_sim/CMakeLists.txt @@ -144,7 +144,7 @@ include_directories( ## Declare a C++ executable ## With catkin_make all packages are built within a single CMake context ## The recommended prefix ensures that target names across packages don't collide -add_executable(${PROJECT_NAME} src/wam_node_sim.cpp) +add_executable(${PROJECT_NAME} src/wam_node_sim.cpp src/hand.cpp) ## Rename C++ executable without prefix ## The above recommended prefix causes long target names, the following renames the diff --git a/wam_node_sim/include/wam_node_sim/detail/wam-impl.h b/wam_node_sim/include/wam_node_sim/detail/wam-impl.h index 7eba6a0..f955a0f 100644 --- a/wam_node_sim/include/wam_node_sim/detail/wam-impl.h +++ b/wam_node_sim/include/wam_node_sim/detail/wam-impl.h @@ -31,8 +31,8 @@ * Author: dc */ - /* Modified on 2017, 2018 - Author: Walter Fetter Lages +/* Modified on 2017, 2018 + * Author: Walter Fetter Lages */ @@ -40,6 +40,7 @@ #define WAM_NODE_SIM_WAM_IMPL_H_ //#include // usleep +#include //#include //#include @@ -59,9 +60,10 @@ namespace barrett { template -Wam::Wam(const std::string& sysName) +Wam::Wam(Hand *hand,const std::string& sysName) { - ros::NodeHandle node_("wam"); // WAM specific nodehandle + ros::NodeHandle node_(sysName); // WAM specific nodehandle + hand_=hand; wam_cmd_pub=node_.advertise("command", 1); // wam/command jointStatesSubscriber_=node_.subscribe("/joint_states",10,&Wam::jointStatesCB,this); @@ -117,26 +119,26 @@ inline const typename Wam::jp_type& Wam::getHomePosition() const template typename Wam::jt_type Wam::getJointTorques() const { - return wamJointEffort_; + return jointEffort_; } template inline typename Wam::jp_type Wam::getJointPositions() const { - return wamJointPosition_; + return jointPosition_; } template inline typename Wam::jv_type Wam::getJointVelocities() const { - return wamJointVelocity_; + return jointVelocity_; } template typename Wam::cp_type Wam::getToolPosition() const { KDL::JntArray jointPosition(DOF); - for(int i;i < DOF;i++) jointPosition(i)=wamJointPosition_[i]; + for(int i;i < DOF;i++) jointPosition(i)=jointPosition_[i]; KDL::Frame frame; if(fwdKinSolverPos_->JntToCart(jointPosition,frame) < 0) @@ -158,7 +160,7 @@ template Eigen::Quaterniond Wam::getToolOrientation() const { KDL::JntArray jointPosition(DOF); - for(int i;i < DOF;i++) jointPosition(i)=wamJointPosition_[i]; + for(int i;i < DOF;i++) jointPosition(i)=jointPosition_[i]; KDL::Frame frame; if(fwdKinSolverPos_->JntToCart(jointPosition,frame) < 0) @@ -298,12 +300,24 @@ void Wam::idle() template void Wam::jointStatesCB(const sensor_msgs::JointState &jointStates) { + std::vector jointNames; + jointNames.push_back("wam_joint_1"); + jointNames.push_back("wam_joint_2"); + jointNames.push_back("wam_joint_3"); + jointNames.push_back("wam_joint_4"); + jointNames.push_back("wam_joint_5"); + jointNames.push_back("wam_joint_6"); + jointNames.push_back("wam_joint_7"); + for(int i=0;i < DOF;i++) { - wamJointPosition_[i]=jointStates.position[i]; - wamJointVelocity_[i]=jointStates.velocity[i]; - wamJointEffort_[i]=jointStates.effort[i]; + int j=find(jointStates.name.begin(),jointStates.name.end(),jointNames[i]) - jointStates.name.begin(); + jointPosition_[i]=jointStates.position[j]; + jointVelocity_[i]=jointStates.velocity[j]; + jointEffort_[i]=jointStates.effort[j]; } + + if(hand_ != NULL) hand_->jointStatesCB(jointStates); } diff --git a/wam_node_sim/include/wam_node_sim/hand.h b/wam_node_sim/include/wam_node_sim/hand.h new file mode 100644 index 0000000..5336fe3 --- /dev/null +++ b/wam_node_sim/include/wam_node_sim/hand.h @@ -0,0 +1,200 @@ +/** + * Copyright 2009-2014 Barrett Technology + * Copyright (c) 2018 Walter Fetter Lages + * + * The original file is part of libbarrett. + * This file was modified to implement a ROS node simulating the Barrett + * WAM independent from libbarrett. + * + * This version of libbarrett is free software: you can redistribute it + * and/or modify it under the terms of the GNU General Public License as + * published by the Free Software Foundation, either version 3 of the + * License, or (at your option) any later version. + * + * This version of libbarrett is distributed in the hope that it will be + * useful, but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this version of libbarrett. If not, see + * . + * + * + * Barrett Technology Inc. + * 73 Chapel Street + * Newton, MA 02458 + */ + +/* + * @file hand.h + * @date 11/09/2010 + * @author Dan Cody + * + */ + +/* Modified in 2018 + Author: Walter Fetter Lages + */ + + +#ifndef WAM_NODE_SIM_HAND_H_ +#define WAM_NODE_SIM_HAND_H_ + + +#include + +#include + +#include + +#define DISALLOW_COPY_AND_ASSIGN(TypeName) \ + TypeName(const TypeName&); \ + void operator=(const TypeName&) + +namespace barrett { + + +class Hand { +public: + static const size_t DOF = 4; + + typedef Eigen::Matrix jt_type; + typedef Eigen::Matrix jp_type; + typedef Eigen::Matrix jv_type; + typedef Eigen::Matrix cp_type; + typedef Eigen::Matrix cv_type; + + typedef Eigen::Matrix v_type; + + // Constants for referring to specific axes of the Hand + static const unsigned int F1 = 1 << 0; + static const unsigned int F2 = 1 << 1; + static const unsigned int F3 = 1 << 2; + static const unsigned int SPREAD = 1 << 3; + static const unsigned int GRASP = F1 | F2 | F3; + static const unsigned int WHOLE_HAND = GRASP | SPREAD; + + /** Hand Constructor */ + Hand(void); + /** Hand Destructor */ + ~Hand(); + /** initialize Method */ + void initialize() const; + /** idle Method */ + void idle() const { } + /** doneMoving Method */ + bool doneMoving(unsigned int whichDigits = WHOLE_HAND, bool realtime = false) const; + /** waitUntilDoneMoving Method */ + void waitUntilDoneMoving(unsigned int whichDigits = WHOLE_HAND, double period_s = 0.1) const; + + // Basic moves + /** open Method */ + void open(unsigned int whichDigits = WHOLE_HAND, bool blocking = true) const; + /** open Method */ + void open(bool blocking) const { open(WHOLE_HAND, blocking); } + /** close Method */ + void close(unsigned int whichDigits = WHOLE_HAND, bool blocking = true) const; + /** close Method */ + void close(bool blocking) const { close(WHOLE_HAND, blocking); } + + // Preferred: low control-rate moves + /** trapezoidalMove Method */ + void trapezoidalMove(const jp_type& jp, unsigned int whichDigits = WHOLE_HAND, bool blocking = true) const; + /** trapezoidalMove Method */ + void trapezoidalMove(const jp_type& jp, bool blocking) const { trapezoidalMove(jp, WHOLE_HAND, blocking); } + /** velocityMove Method */ + void velocityMove(const jv_type& jv, unsigned int whichDigits = WHOLE_HAND) const; + + // Advanced: high control-rate moves + /** setPositionMode Method */ + void setPositionMode(unsigned int whichDigits = WHOLE_HAND) const; + /** setPositionCommand Method */ + void setPositionCommand(const jp_type& jp, unsigned int whichDigits = WHOLE_HAND) const; + /** setTorqueMode Method */ + void setTorqueMode(unsigned int whichDigits = WHOLE_HAND) const; + /** setTorqueCommand Method */ + void setTorqueCommand(const jt_type& jt, unsigned int whichDigits = WHOLE_HAND) const; + + + // Sensors + static const unsigned int S_POSITION = 1 << 0; + static const unsigned int S_FINGERTIP_TORQUE = 1 << 1; + static const unsigned int S_TACT_FULL = 1 << 2; + static const unsigned int S_ALL = S_POSITION | S_FINGERTIP_TORQUE | S_TACT_FULL; + /** update Method */ + void update(unsigned int sensors = S_ALL, bool realtime = false); + /** getInnerLinkPosition Method */ + const jp_type& getInnerLinkPosition() const { return innerJp; } + /** getOuterLinkPosition Method */ + const jp_type& getOuterLinkPosition() const { return outerJp; } + /** getPrimaryEncoderPosition Method */ + const std::vector& getPrimaryEncoderPosition() const { return primaryEncoder; } + /** getSecondaryEncoderPosition Method */ + const std::vector& getSecondaryEncoderPosition() const { return secondaryEncoder; } + /** enableBreakawayEncoders Method actives or deactivates the Secondary Encoders */ + void enableBreakawayEncoders(bool enable) { useSecondaryEncoders = enable; } // Enabled by default. + /** hasFingertipTorqueSensors Method returns status of installed fingertip torque sensors */ + bool hasFingertipTorqueSensors() const { return hasFtt; } + /** getFingertipTorque Method gets the fingertip torques in torque units */ + const std::vector& getFingertipTorque() const { return ftt; } + /** hasTactSensors Method returns whether or not Tactile Sensors are present on hand. */ + bool hasTactSensors() const { return hasTact; } + /** getTactilePucks Method creates container of pucks to get tactile sensor data from possible locations */ +// const std::vector& getTactilePucks() const { return tactilePucks; } + + void jointStatesCB(const sensor_msgs::JointState &jointStates); + + static const size_t SPREAD_INDEX = 3; + +//protected: +private: + /** */ + bool digitsInclude(unsigned int whichDigits, size_t index) const { return whichDigits & (1 << index); } + /** */ +// void setProperty(unsigned int whichDigits, enum Puck::Property prop, int value) const; + /** */ +// void setProperty(unsigned int whichDigits, enum Puck::Property prop, const v_type& values) const; + /** */ + void blockIf(bool blocking, unsigned int whichDigits) const; + + + static const double J2_RATIO = 125.0; + static const double J2_ENCODER_RATIO = 50.0; + static const double J3_RATIO = 375.0; + static const double SPREAD_RATIO = 17.5; + + + bool hasFtt; + bool hasTact; + bool useSecondaryEncoders; + + int holds[DOF]; + v_type j2pp, j2pt; + mutable v_type pt; + + std::vector< std::vector > encoderTmp; + std::vector primaryEncoder, secondaryEncoder; + jp_type innerJp, outerJp; + std::vector ftt; +// std::vector tactilePucks; + +private: + static const int CMD_HI = 13; + static const int CMD_CLOSE = 18; + static const int CMD_OPEN = 20; +// static const enum Puck::Property props[]; + + jp_type jointPosition_; + + DISALLOW_COPY_AND_ASSIGN(Hand); + +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; + + +} + + +#endif /* WAM_NODE_SIM_HAND_H_ */ diff --git a/wam_node_sim/include/wam_node_sim/hand.h.orig b/wam_node_sim/include/wam_node_sim/hand.h.orig new file mode 100644 index 0000000..b436029 --- /dev/null +++ b/wam_node_sim/include/wam_node_sim/hand.h.orig @@ -0,0 +1,184 @@ +/** + * Copyright 2009-2014 Barrett Technology + * + * This file is part of libbarrett. + * + * This version of libbarrett is free software: you can redistribute it + * and/or modify it under the terms of the GNU General Public License as + * published by the Free Software Foundation, either version 3 of the + * License, or (at your option) any later version. + * + * This version of libbarrett is distributed in the hope that it will be + * useful, but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this version of libbarrett. If not, see + * . + * + * + * Barrett Technology Inc. + * 73 Chapel Street + * Newton, MA 02458 + */ + +/* + * @file hand.h + * @date 11/09/2010 + * @author Dan Cody + * + */ + +#ifndef BARRETT_PRODUCTS_HAND_H_ +#define BARRETT_PRODUCTS_HAND_H_ + + +#include + +#include + +#include +#include +#include +#include +#include +#include + + +namespace barrett { + + +class Hand : public MultiPuckProduct { +public: + static const size_t DOF = 4; + BARRETT_UNITS_TYPEDEFS(DOF); + + + // Constants for referring to specific axes of the Hand + static const unsigned int F1 = 1 << 0; + static const unsigned int F2 = 1 << 1; + static const unsigned int F3 = 1 << 2; + static const unsigned int SPREAD = 1 << 3; + static const unsigned int GRASP = F1 | F2 | F3; + static const unsigned int WHOLE_HAND = GRASP | SPREAD; + + /** Hand Constructor */ + Hand(const std::vector& pucks); + /** Hand Destructor */ + ~Hand(); + /** initialize Method */ + void initialize() const; + /** idle Method */ + void idle() const { group.setProperty(Puck::MODE, MotorPuck::MODE_IDLE); } + /** doneMoving Method */ + bool doneMoving(unsigned int whichDigits = WHOLE_HAND, bool realtime = false) const; + /** waitUntilDoneMoving Method */ + void waitUntilDoneMoving(unsigned int whichDigits = WHOLE_HAND, double period_s = 0.1) const; + + // Basic moves + /** open Method */ + void open(unsigned int whichDigits = WHOLE_HAND, bool blocking = true) const; + /** open Method */ + void open(bool blocking) const { open(WHOLE_HAND, blocking); } + /** close Method */ + void close(unsigned int whichDigits = WHOLE_HAND, bool blocking = true) const; + /** close Method */ + void close(bool blocking) const { close(WHOLE_HAND, blocking); } + + // Preferred: low control-rate moves + /** trapezoidalMove Method */ + void trapezoidalMove(const jp_type& jp, unsigned int whichDigits = WHOLE_HAND, bool blocking = true) const; + /** trapezoidalMove Method */ + void trapezoidalMove(const jp_type& jp, bool blocking) const { trapezoidalMove(jp, WHOLE_HAND, blocking); } + /** velocityMove Method */ + void velocityMove(const jv_type& jv, unsigned int whichDigits = WHOLE_HAND) const; + + // Advanced: high control-rate moves + /** setPositionMode Method */ + void setPositionMode(unsigned int whichDigits = WHOLE_HAND) const; + /** setPositionCommand Method */ + void setPositionCommand(const jp_type& jp, unsigned int whichDigits = WHOLE_HAND) const; + /** setTorqueMode Method */ + void setTorqueMode(unsigned int whichDigits = WHOLE_HAND) const; + /** setTorqueCommand Method */ + void setTorqueCommand(const jt_type& jt, unsigned int whichDigits = WHOLE_HAND) const; + + + // Sensors + static const unsigned int S_POSITION = 1 << 0; + static const unsigned int S_FINGERTIP_TORQUE = 1 << 1; + static const unsigned int S_TACT_FULL = 1 << 2; + static const unsigned int S_ALL = S_POSITION | S_FINGERTIP_TORQUE | S_TACT_FULL; + /** update Method */ + void update(unsigned int sensors = S_ALL, bool realtime = false); + /** getInnerLinkPosition Method */ + const jp_type& getInnerLinkPosition() const { return innerJp; } + /** getOuterLinkPosition Method */ + const jp_type& getOuterLinkPosition() const { return outerJp; } + /** getPrimaryEncoderPosition Method */ + const std::vector& getPrimaryEncoderPosition() const { return primaryEncoder; } + /** getSecondaryEncoderPosition Method */ + const std::vector& getSecondaryEncoderPosition() const { return secondaryEncoder; } + /** enableBreakawayEncoders Method actives or deactivates the Secondary Encoders */ + void enableBreakawayEncoders(bool enable) { useSecondaryEncoders = enable; } // Enabled by default. + /** hasFingertipTorqueSensors Method returns status of installed fingertip torque sensors */ + bool hasFingertipTorqueSensors() const { return hasFtt; } + /** getFingertipTorque Method gets the fingertip torques in torque units */ + const std::vector& getFingertipTorque() const { return ftt; } + /** hasTactSensors Method returns whether or not Tactile Sensors are present on hand. */ + bool hasTactSensors() const { return hasTact; } + /** getTactilePucks Method creates container of pucks to get tactile sensor data from possible locations */ + const std::vector& getTactilePucks() const { return tactilePucks; } + + + static const size_t SPREAD_INDEX = 3; + +protected: + /** */ + bool digitsInclude(unsigned int whichDigits, size_t index) const { return whichDigits & (1 << index); } + /** */ + void setProperty(unsigned int whichDigits, enum Puck::Property prop, int value) const; + /** */ + void setProperty(unsigned int whichDigits, enum Puck::Property prop, const v_type& values) const; + /** */ + void blockIf(bool blocking, unsigned int whichDigits) const; + + + static const double J2_RATIO = 125.0; + static const double J2_ENCODER_RATIO = 50.0; + static const double J3_RATIO = 375.0; + static const double SPREAD_RATIO = 17.5; + + + bool hasFtt; + bool hasTact; + bool useSecondaryEncoders; + + int holds[DOF]; + v_type j2pp, j2pt; + mutable v_type pt; + + std::vector::result_type> encoderTmp; + std::vector primaryEncoder, secondaryEncoder; + jp_type innerJp, outerJp; + std::vector ftt; + std::vector tactilePucks; + +private: + static const int CMD_HI = 13; + static const int CMD_CLOSE = 18; + static const int CMD_OPEN = 20; + static const enum Puck::Property props[]; + + DISALLOW_COPY_AND_ASSIGN(Hand); + +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; + + +} + + +#endif /* BARRETT_PRODUCTS_HAND_H_ */ diff --git a/wam_node_sim/include/wam_node_sim/wam.h b/wam_node_sim/include/wam_node_sim/wam.h index 68ac915..c9b5259 100644 --- a/wam_node_sim/include/wam_node_sim/wam.h +++ b/wam_node_sim/include/wam_node_sim/wam.h @@ -46,12 +46,15 @@ //#include #include +#include //#include #include "ros/ros.h" #include "trajectory_msgs/JointTrajectoryPoint.h" +#include #include +#include namespace barrett { @@ -69,7 +72,7 @@ public: * * GenericPucks must be ordered by joint and must break into torque groups as arranged. */ - Wam(const std::string& sysName = "Wam"); + Wam(Hand *hand=NULL,const std::string& sysName = "wam"); /** Destructor for Wam */ @@ -179,10 +182,11 @@ private: ros::Publisher wam_cmd_pub; // controller command ros::Subscriber jointStatesSubscriber_; - jp_type wamJointPosition_; - jv_type wamJointVelocity_; - jt_type wamJointEffort_; + jp_type jointPosition_; + jv_type jointVelocity_; + jt_type jointEffort_; KDL::ChainFkSolverPos_recursive *fwdKinSolverPos_; + Hand *hand_; void jointStatesCB(const sensor_msgs::JointState &jointStates); diff --git a/wam_node_sim/src/hand.cpp b/wam_node_sim/src/hand.cpp new file mode 100644 index 0000000..dc93388 --- /dev/null +++ b/wam_node_sim/src/hand.cpp @@ -0,0 +1,306 @@ +/** + * Copyright 2009-2014 Barrett Technology + * Copyright (c) 2017, 2018 Walter Fetter Lages + * + * The original file is part of barrett-ros-pkg. + * This file was modified to implement a ROS node simulating the + * Barrett WAM independent from libbarrett. + * + * This version of libbarrett is free software: you can redistribute it + * and/or modify it under the terms of the GNU General Public License as + * published by the Free Software Foundation, either version 3 of the + * License, or (at your option) any later version. + * + * This version of libbarrett is distributed in the hope that it will be + * useful, but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this version of libbarrett. If not, see + * . + * + * + * Barrett Technology Inc. + * 73 Chapel Street + * Newton, MA 02458 + * + */ + +/** + * @file hand.cpp + * @date 11/09/2010 + * @author Dan Cody + * + */ + + /* Modified in 2018 + Author: Walter Fetter Lages + */ + + +#include +#include +#include +#include + +#include + +#include + +namespace barrett { + +// from BarrettHand BH8-Series User Manual Firmware Version 4.4x, section 9.6.1 page 77 +static const double J2_COUNTS_PER_RAD=199111.1/(140*M_PI/180); +static const double SPREAD_COUNTS_PER_RAD=35840/M_PI; + +//const enum Puck::Property Hand::props[] = { Puck::HOLD, Puck::CMD, Puck::MODE, Puck::P, Puck::T, Puck::SG }; + +/** Hand Constructor */ +Hand::Hand(void) : +// MultiPuckProduct(DOF, _pucks, PuckGroup::BGRP_HAND, props, sizeof(props)/sizeof(props[0]), "Hand::Hand()"), + hasFtt(false), hasTact(false), useSecondaryEncoders(true), encoderTmp(DOF), primaryEncoder(DOF, 0), secondaryEncoder(DOF, 0), ftt(DOF, 0) //, tactilePucks() + { + // Check for TACT and FingertipTorque options. + int numFtt = 0; + for (size_t i = 0; i < DOF; ++i) { +// if (pucks[i]->hasOption(Puck::RO_Strain)) { +// ++numFtt; +// hasFtt = true; +// } + } + ROS_INFO(" Found %d Fingertip torque sensors",numFtt); + + bool tactError = false; + for (size_t i = 0; i < DOF; ++i) { +// if (pucks[i]->hasOption(Puck::RO_Tact)) { +// try { + // The TactilePuck ctor might throw if there was an initialization error +// tactilePucks.push_back(new TactilePuck(pucks[i])); +// hasTact = true; +// } catch (std::runtime_error e) { +// tactError = true; +// } +// } + } +// ROS_INFO(" Found %d Tactile arrays",tactilePucks.size()); + ROS_INFO(" Found %d Tactile arrays",0); + if (tactError) { + ROS_WARN(" Initialization error! Disabling Tactile arrays"); + hasTact = false; + } + + // record HOLD values +// group.getProperty(Puck::HOLD, holds); + + + // For the fingers + for (size_t i = 0; i < DOF - 1; ++i) { + j2pp[i] = J2_COUNTS_PER_RAD * J2_RATIO; +// j2pt[i] = motorPucks[i].getIpnm() / J2_RATIO; + } + // For the spread + j2pp[SPREAD_INDEX] = SPREAD_COUNTS_PER_RAD * SPREAD_RATIO; +// j2pt[SPREAD_INDEX] = motorPucks[SPREAD_INDEX].getIpnm() / SPREAD_RATIO; + + for(int i=0;i < DOF;i++) encoderTmp[i].resize(2); +} +/** Hand Destructor */ +Hand::~Hand() +{ +// detail::purge(tactilePucks); +} +/** initialize Method */ +void Hand::initialize() const +{ +// for (size_t i = 0; i < DOF-1; ++i) { +// pucks[i]->setProperty(Puck::CMD, CMD_HI); +// } + waitUntilDoneMoving(); + +// pucks[SPREAD_INDEX]->setProperty(Puck::CMD, CMD_HI); + waitUntilDoneMoving(); +} +/** doneMoving Method */ +bool Hand::doneMoving(unsigned int whichDigits, bool realtime) const +{ + return true; +// int modes[DOF]; + + // TODO(dc): Avoid asking for modes from the Pucks we don't care about. +// group.getProperty(Puck::MODE, modes, realtime); + + for (size_t i = 0; i < DOF; ++i) { + if ( + digitsInclude(whichDigits, i) && +// modes[i] != MotorPuck::MODE_IDLE && +// (modes[i] != MotorPuck::MODE_PID || + holds[i] == 0 //) + ) + { + return false; + } + } + return true; +} +/** waitUntilDoneMoving Method prevents any subsequent actions until finger movement is completed. */ +void Hand::waitUntilDoneMoving(unsigned int whichDigits, double period_s) const +{ + while ( !doneMoving(whichDigits) ) { + usleep(period_s*1e6); + } +} +/** open Method */ +void Hand::open(unsigned int whichDigits, bool blocking) const { + jp_type jp; + jp << 0.0, 0.0, 0.0, M_PI; + 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; + trapezoidalMove(jp,whichDigits,blocking); + blockIf(blocking, whichDigits); +} +/** 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); + blockIf(blocking, whichDigits); +} +/** velocityMove Method */ +void Hand::velocityMove(const jv_type& jv, unsigned int whichDigits) const +{ + // Convert to counts/millisecond +// setProperty(whichDigits, Puck::V, (j2pp.cwise() * jv) / 1000.0); +// setProperty(whichDigits, Puck::MODE, MotorPuck::MODE_VELOCITY); +} +/** setPositionMode Method */ +void Hand::setPositionMode(unsigned int whichDigits) const { +// setProperty(whichDigits, Puck::MODE, MotorPuck::MODE_PID); +} +/** setPositionCommand Method */ +void Hand::setPositionCommand(const jp_type& jp, unsigned int whichDigits) const +{ + trapezoidalMove(jp,whichDigits,false); +} +/** setTorqueMode Method */ +void Hand::setTorqueMode(unsigned int whichDigits) const { +// setProperty(whichDigits, Puck::MODE, MotorPuck::MODE_TORQUE); +} + +/** setTorqueCommand Method */ +void Hand::setTorqueCommand(const jt_type& jt, unsigned int whichDigits) const +{ +// pt = j2pt.cwise() * jt; + if (whichDigits == WHOLE_HAND) { +// MotorPuck::sendPackedTorques(pucks[0]->getBus(), group.getId(), Puck::T, pt.data(), DOF); + } else { +// setProperty(whichDigits, Puck::T, pt); + } +} + +/** update Method */ +void Hand::update(unsigned int sensors, bool realtime) +{ + if (sensors & S_POSITION) { + for (size_t i = 0; i < DOF; ++i) { + primaryEncoder[i] = encoderTmp[i][0]; + secondaryEncoder[i] = encoderTmp[i][1]; + } + + // For the fingers + for (size_t i = 0; i < DOF-1; ++i) { + // If we got a reading from the secondary encoder and it's enabled... + if (useSecondaryEncoders && secondaryEncoder[i] != std::numeric_limits::max()) { + innerJp[i] = secondaryEncoder[i]/J2_COUNTS_PER_RAD / J2_ENCODER_RATIO; + outerJp[i] = primaryEncoder[i]/J2_COUNTS_PER_RAD * (1.0/J2_RATIO + 1.0/J3_RATIO) - innerJp[i]; + } else { + // These calculations are only valid before breakaway! + innerJp[i] = primaryEncoder[i]/J2_COUNTS_PER_RAD / J2_RATIO; + outerJp[i] = innerJp[i] * J2_RATIO / J3_RATIO; + } + } + + // For the spread + innerJp[SPREAD_INDEX] = outerJp[SPREAD_INDEX] = primaryEncoder[SPREAD_INDEX]/SPREAD_COUNTS_PER_RAD / SPREAD_RATIO; + } + if (hasFingertipTorqueSensors() && sensors & S_FINGERTIP_TORQUE) { +// group.receiveGetPropertyReply(group.getPropertyId(Puck::SG), ftt.data(), realtime); + } + if (hasTactSensors() && sensors & S_TACT_FULL) { +// for (size_t i = 0; i < tactilePucks.size(); ++i) { +// tactilePucks[i]->receiveFull(realtime); +// } + } +} + +/** */ +/* +void Hand::setProperty(unsigned int whichDigits, enum Puck::Property prop, int value) const +{ + if (whichDigits == WHOLE_HAND) { + group.setProperty(prop, value); + } else { + for (size_t i = 0; i < DOF; ++i) { + if (digitsInclude(whichDigits, i)) { + pucks[i]->setProperty(prop, value); + } + } + } +} +*/ +/** setProperty Method */ +/* +void Hand::setProperty(unsigned int whichDigits, enum Puck::Property prop, const v_type& values) const +{ + for (size_t i = 0; i < DOF; ++i) { + if (digitsInclude(whichDigits, i)) { + pucks[i]->setProperty(prop, values[i]); + } + } +} +*/ +/** blockIf Method */ +void Hand::blockIf(bool blocking, unsigned int whichDigits) const { + if (blocking) { + waitUntilDoneMoving(whichDigits); + } +} + +void Hand::jointStatesCB(const sensor_msgs::JointState &jointStates) +{ + std::vector jointNames; + jointNames.push_back("bhand_finger1_joint_2"); + jointNames.push_back("bhand_finger2_joint_2"); + jointNames.push_back("bhand_finger3_joint_2"); + jointNames.push_back("bhand_spread"); + 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"); + + 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()) + { + 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; + } + else + { + encoderTmp[i][0]= J2_RATIO*jointStates.position[j]*J2_COUNTS_PER_RAD; + encoderTmp[i][1] = std::numeric_limits::max(); + } + } +} + + +} diff --git a/wam_node_sim/src/hand.cpp.orig b/wam_node_sim/src/hand.cpp.orig new file mode 100644 index 0000000..a0fda8c --- /dev/null +++ b/wam_node_sim/src/hand.cpp.orig @@ -0,0 +1,280 @@ +/** + * Copyright 2009-2014 Barrett Technology + * + * This file is part of libbarrett. + * + * This version of libbarrett is free software: you can redistribute it + * and/or modify it under the terms of the GNU General Public License as + * published by the Free Software Foundation, either version 3 of the + * License, or (at your option) any later version. + * + * This version of libbarrett is distributed in the hope that it will be + * useful, but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this version of libbarrett. If not, see + * . + * + * + * Barrett Technology Inc. + * 73 Chapel Street + * Newton, MA 02458 + * + */ + +/** + * @file hand.cpp + * @date 11/09/2010 + * @author Dan Cody + * + */ + +#include +#include +#include +#include + +#include + +#include +#include +#include +#include +#include +#include +#include +#include + + +namespace barrett { + + +const enum Puck::Property Hand::props[] = { Puck::HOLD, Puck::CMD, Puck::MODE, Puck::P, Puck::T, Puck::SG }; + +/** Hand Constructor */ +Hand::Hand(const std::vector& _pucks) : + MultiPuckProduct(DOF, _pucks, PuckGroup::BGRP_HAND, props, sizeof(props)/sizeof(props[0]), "Hand::Hand()"), + hasFtt(false), hasTact(false), useSecondaryEncoders(true), encoderTmp(DOF), primaryEncoder(DOF, 0), secondaryEncoder(DOF, 0), ftt(DOF, 0), tactilePucks() +{ + // Check for TACT and FingertipTorque options. + int numFtt = 0; + for (size_t i = 0; i < DOF; ++i) { + if (pucks[i]->hasOption(Puck::RO_Strain)) { + ++numFtt; + hasFtt = true; + } + } + logMessage(" Found %d Fingertip torque sensors") % numFtt; + + bool tactError = false; + for (size_t i = 0; i < DOF; ++i) { + if (pucks[i]->hasOption(Puck::RO_Tact)) { + try { + // The TactilePuck ctor might throw if there was an initialization error + tactilePucks.push_back(new TactilePuck(pucks[i])); + hasTact = true; + } catch (std::runtime_error e) { + tactError = true; + } + } + } + logMessage(" Found %d Tactile arrays") % tactilePucks.size(); + if (tactError) { + logMessage(" Initialization error! Disabling Tactile arrays"); + hasTact = false; + } + + // record HOLD values + group.getProperty(Puck::HOLD, holds); + + + // For the fingers + for (size_t i = 0; i < DOF - 1; ++i) { + j2pp[i] = motorPucks[i].getCountsPerRad() * J2_RATIO; + j2pt[i] = motorPucks[i].getIpnm() / J2_RATIO; + } + // For the spread + j2pp[SPREAD_INDEX] = motorPucks[SPREAD_INDEX].getCountsPerRad() * SPREAD_RATIO; + j2pt[SPREAD_INDEX] = motorPucks[SPREAD_INDEX].getIpnm() / SPREAD_RATIO; +} +/** Hand Destructor */ +Hand::~Hand() +{ + detail::purge(tactilePucks); +} +/** initialize Method */ +void Hand::initialize() const +{ + for (size_t i = 0; i < DOF-1; ++i) { + pucks[i]->setProperty(Puck::CMD, CMD_HI); + } + waitUntilDoneMoving(); + + pucks[SPREAD_INDEX]->setProperty(Puck::CMD, CMD_HI); + waitUntilDoneMoving(); +} +/** doneMoving Method */ +bool Hand::doneMoving(unsigned int whichDigits, bool realtime) const +{ + int modes[DOF]; + + // TODO(dc): Avoid asking for modes from the Pucks we don't care about. + group.getProperty(Puck::MODE, modes, realtime); + + for (size_t i = 0; i < DOF; ++i) { + if ( + digitsInclude(whichDigits, i) && + modes[i] != MotorPuck::MODE_IDLE && + (modes[i] != MotorPuck::MODE_PID || holds[i] == 0) + ) + { + return false; + } + } + return true; +} +/** waitUntilDoneMoving Method prevents any subsequent actions until finger movement is completed. */ +void Hand::waitUntilDoneMoving(unsigned int whichDigits, double period_s) const +{ + while ( !doneMoving(whichDigits) ) { + btsleep(period_s); + } +} +/** open Method */ +void Hand::open(unsigned int whichDigits, bool blocking) const { + setProperty(whichDigits, Puck::CMD, CMD_OPEN); + blockIf(blocking, whichDigits); +} +/** close Method */ +void Hand::close(unsigned int whichDigits, bool blocking) const { + setProperty(whichDigits, Puck::CMD, CMD_CLOSE); + blockIf(blocking, whichDigits); +} +/** 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); + blockIf(blocking, whichDigits); +} +/** velocityMove Method */ +void Hand::velocityMove(const jv_type& jv, unsigned int whichDigits) const +{ + // Convert to counts/millisecond + setProperty(whichDigits, Puck::V, (j2pp.cwise() * jv) / 1000.0); + setProperty(whichDigits, Puck::MODE, MotorPuck::MODE_VELOCITY); +} + +/** setPositionMode Method */ +void Hand::setPositionMode(unsigned int whichDigits) const { + setProperty(whichDigits, Puck::MODE, MotorPuck::MODE_PID); +} +/** setPositionCommand Method */ +void Hand::setPositionCommand(const jp_type& jp, unsigned int whichDigits) const +{ + setProperty(whichDigits, Puck::P, j2pp.cwise() * jp); +} +/** setTorqueMode Method */ +void Hand::setTorqueMode(unsigned int whichDigits) const { + setProperty(whichDigits, Puck::MODE, MotorPuck::MODE_TORQUE); +} +/** setTorqueCommand Method */ +void Hand::setTorqueCommand(const jt_type& jt, unsigned int whichDigits) const +{ + pt = j2pt.cwise() * jt; + if (whichDigits == WHOLE_HAND) { + MotorPuck::sendPackedTorques(pucks[0]->getBus(), group.getId(), Puck::T, pt.data(), DOF); + } else { + setProperty(whichDigits, Puck::T, pt); + } +} +/** update Method */ +void Hand::update(unsigned int sensors, bool realtime) +{ + // Do we need to lock? + boost::unique_lock ul(bus.getMutex(), boost::defer_lock); + if (realtime) { + ul.lock(); + } + + + // Send requests + if (sensors & S_POSITION) { + group.sendGetPropertyRequest(group.getPropertyId(Puck::P)); + } + if (hasFingertipTorqueSensors() && sensors & S_FINGERTIP_TORQUE) { + group.sendGetPropertyRequest(group.getPropertyId(Puck::SG)); + } + if (hasTactSensors() && sensors & S_TACT_FULL) { + group.setProperty(Puck::TACT, TactilePuck::FULL_FORMAT); + } + + + // Receive replies + if (sensors & S_POSITION) { + group.receiveGetPropertyReply >(group.getPropertyId(Puck::P), encoderTmp.data(), realtime); + + for (size_t i = 0; i < DOF; ++i) { + primaryEncoder[i] = encoderTmp[i].get<0>(); + secondaryEncoder[i] = encoderTmp[i].get<1>(); + } + + // For the fingers + for (size_t i = 0; i < DOF-1; ++i) { + // If we got a reading from the secondary encoder and it's enabled... + if (useSecondaryEncoders && secondaryEncoder[i] != std::numeric_limits::max()) { + innerJp[i] = motorPucks[i].counts2rad(secondaryEncoder[i]) / J2_ENCODER_RATIO; + outerJp[i] = motorPucks[i].counts2rad(primaryEncoder[i]) * (1.0/J2_RATIO + 1.0/J3_RATIO) - innerJp[i]; + } else { + // These calculations are only valid before breakaway! + innerJp[i] = motorPucks[i].counts2rad(primaryEncoder[i]) / J2_RATIO; + outerJp[i] = innerJp[i] * J2_RATIO / J3_RATIO; + } + } + + // For the spread + innerJp[SPREAD_INDEX] = outerJp[SPREAD_INDEX] = motorPucks[SPREAD_INDEX].counts2rad(primaryEncoder[SPREAD_INDEX]) / SPREAD_RATIO; + } + if (hasFingertipTorqueSensors() && sensors & S_FINGERTIP_TORQUE) { + group.receiveGetPropertyReply(group.getPropertyId(Puck::SG), ftt.data(), realtime); + } + if (hasTactSensors() && sensors & S_TACT_FULL) { + for (size_t i = 0; i < tactilePucks.size(); ++i) { + tactilePucks[i]->receiveFull(realtime); + } + } +} + +/** */ +void Hand::setProperty(unsigned int whichDigits, enum Puck::Property prop, int value) const +{ + if (whichDigits == WHOLE_HAND) { + group.setProperty(prop, value); + } else { + for (size_t i = 0; i < DOF; ++i) { + if (digitsInclude(whichDigits, i)) { + pucks[i]->setProperty(prop, value); + } + } + } +} +/** setProperty Method */ +void Hand::setProperty(unsigned int whichDigits, enum Puck::Property prop, const v_type& values) const +{ + for (size_t i = 0; i < DOF; ++i) { + if (digitsInclude(whichDigits, i)) { + pucks[i]->setProperty(prop, values[i]); + } + } +} +/** blockIf Method */ +void Hand::blockIf(bool blocking, unsigned int whichDigits) const { + if (blocking) { + waitUntilDoneMoving(whichDigits); + } +} + + +} diff --git a/wam_node_sim/src/wam_node_sim.cpp b/wam_node_sim/src/wam_node_sim.cpp index a288f31..b95af1e 100644 --- a/wam_node_sim/src/wam_node_sim.cpp +++ b/wam_node_sim/src/wam_node_sim.cpp @@ -29,7 +29,7 @@ Author: Kyle Maroney */ - /* Modified on 2017, 2018 + /* Modified in 2017, 2018 Author: Walter Fetter Lages */ @@ -37,6 +37,10 @@ #include #include +#include // BarrettHand threading +#include + + #include #include "ros/ros.h" @@ -66,12 +70,15 @@ #include #include +#include static const int PUBLISH_FREQ = 250; // Default Control Loop / Publishing Frequency static const int BHAND_PUBLISH_FREQ = 5; // Publishing Frequency for the BarretHand static const double SPEED = 0.03; // Default Cartesian Velocity +using namespace barrett; + //WamNode Class template class WamNode @@ -85,6 +92,7 @@ template bool jnt_pos_status, cart_pos_status, ortn_pos_status, new_rt_cmd; double cart_vel_mag, ortn_vel_mag; barrett::Wam &wam; + Hand* hand; jp_type jp, jp_cmd, jp_home; jp_type rt_jp_cmd, rt_jp_rl; jv_type rt_jv_cmd; @@ -142,7 +150,7 @@ template ros::ServiceServer hand_sprd_vel_srv; public: - WamNode(barrett::Wam &wam_,bool foundHand); + WamNode(barrett::Wam &wam_,barrett::Hand *bhand); ~WamNode(void) { } @@ -177,7 +185,7 @@ template }; template - WamNode::WamNode(barrett::Wam &wam_,bool foundHand): wam(wam_) + WamNode::WamNode(barrett::Wam &wam_,barrett::Hand *bhand): wam(wam_) { ros::NodeHandle n_("wam"); // WAM specific nodehandle ros::NodeHandle nh_("bhand"); // BarrettHand specific nodehandle @@ -193,10 +201,24 @@ template ROS_INFO(" \n %zu-DOF WAM", DOF); jp_home = wam.getJointPositions(); - if (foundHand) //Does the following only if a BarrettHand is present + if (bhand!=NULL) //Does the following only if a BarrettHand is present { - std::cout << "Barrett Hand" << std::endl; - + std::cout << " Barrett Hand" << std::endl; + hand=bhand; + + // Adjust the torque limits to allow for BarrettHand movements at extents +// pm.getSafetyModule()->setTorqueLimit(3.0); + + // Move j3 in order to give room for hand initialization + jp_type jp_init = wam.getJointPositions(); + jp_init[3] -= 0.35; + usleep(500000); + wam.moveTo(jp_init); + + usleep(500000); + hand->initialize(); + hand->update(); + //Publishing the following topics only if there is a BarrettHand present bhand_joint_state_pub = nh_.advertise < sensor_msgs::JointState > ("joint_states", 1); // bhand/joint_states @@ -271,13 +293,13 @@ template bool WamNode::goHome(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res) { ROS_INFO("Returning to Home Position"); -/* + if (hand != NULL) { hand->open(Hand::GRASP, true); hand->close(Hand::SPREAD, true); } -*/ + for (size_t i = 0; i < DOF; i++) jp_cmd[i] = 0.0; wam.moveTo(jp_cmd, true); @@ -405,7 +427,7 @@ template bool WamNode::handOpenGrasp(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res) { ROS_INFO("Opening the BarrettHand Grasp"); -// hand->open(Hand::GRASP, false); + hand->open(Hand::GRASP, false); return true; } @@ -414,7 +436,7 @@ template bool WamNode::handCloseGrasp(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res) { ROS_INFO("Closing the BarrettHand Grasp"); -// hand->close(Hand::GRASP, false); + hand->close(Hand::GRASP, false); return true; } @@ -423,7 +445,7 @@ template bool WamNode::handOpenSpread(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res) { ROS_INFO("Opening the BarrettHand Spread"); -// hand->open(Hand::SPREAD, false); + hand->open(Hand::SPREAD, false); return true; } @@ -432,7 +454,7 @@ template bool WamNode::handCloseSpread(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res) { ROS_INFO("Closing the BarrettHand Spread"); -// hand->close(Hand::SPREAD, false); + hand->close(Hand::SPREAD, false); return true; } @@ -442,7 +464,7 @@ template { ROS_INFO("Moving BarrettHand to Finger Positions: %.3f, %.3f, %.3f radians", req.radians[0], req.radians[1], req.radians[2]); -// hand->trapezoidalMove(Hand::jp_type(req.radians[0], req.radians[1], req.radians[2], 0.0), Hand::GRASP, false); + hand->trapezoidalMove(Hand::jp_type(req.radians[0], req.radians[1], req.radians[2], 0.0), Hand::GRASP, false); return true; } @@ -451,7 +473,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), Hand::GRASP, false); return true; } @@ -460,7 +482,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(req.radians), Hand::SPREAD, false); return true; } @@ -470,7 +492,7 @@ template { ROS_INFO("Moving BarrettHand Finger Velocities: %.3f, %.3f, %.3f m/s", req.velocity[0], req.velocity[1], req.velocity[2]); -// hand->velocityMove(Hand::jv_type(req.velocity[0], req.velocity[1], req.velocity[2], 0.0), Hand::GRASP); + hand->velocityMove(Hand::jv_type(req.velocity[0], req.velocity[1], req.velocity[2], 0.0), Hand::GRASP); return true; } @@ -479,7 +501,7 @@ template bool WamNode::handGraspVel(wam_srvs::BHandGraspVel::Request &req, wam_srvs::BHandGraspVel::Response &res) { ROS_INFO("Moving BarrettHand Grasp: %.3f m/s", req.velocity); -// hand->velocityMove(Hand::jv_type(req.velocity), Hand::GRASP); + hand->velocityMove(Hand::jv_type(req.velocity), Hand::GRASP); return true; } @@ -489,7 +511,7 @@ template { ROS_INFO("Moving BarrettHand Spread: %.3f m/s", req.velocity); usleep(5000); -// hand->velocityMove(Hand::jv_type(req.velocity), Hand::SPREAD); + hand->velocityMove(Hand::jv_type(req.velocity), Hand::SPREAD); return true; } @@ -620,19 +642,22 @@ template template void WamNode::publishHand() //systems::PeriodicDataLogger& logger { + ros::Rate loop(BHAND_PUBLISH_FREQ); + while (ros::ok()) { -/* hand->update(); // Update the hand sensors + hand->update(); // Update the hand sensors Hand::jp_type hi = hand->getInnerLinkPosition(); // get finger positions information Hand::jp_type ho = hand->getOuterLinkPosition(); for (size_t i = 0; i < 4; i++) // Save finger positions bhand_joint_state.position[i] = hi[i]; for (size_t j = 0; j < 3; j++) bhand_joint_state.position[j + 4] = ho[j]; -*/ + bhand_joint_state.header.stamp = ros::Time::now(); // Set the timestamp bhand_joint_state_pub.publish(bhand_joint_state); // Publish the BarrettHand joint states -// btsleep(1.0 / BHAND_PUBLISH_FREQ); // Sleep according to the specified publishing frequency +// usleep(1.0 / BHAND_PUBLISH_FREQ*1e6); // Sleep according to the specified publishing frequency + loop.sleep(); } } @@ -788,11 +813,15 @@ int main(int argc,char *argv[]) { ros::init(argc,argv,"wam_node_sim"); - barrett::Wam<7> wam; - WamNode<7> wam_node(wam,true); - + bool useHand=true; + Hand *bhand=NULL; + if(useHand) bhand=new Hand(); + Wam<7> wam(bhand); + WamNode<7> wam_node(wam,bhand); ros::Rate loop(PUBLISH_FREQ); + if(useHand) boost::thread handPubThread(&WamNode<7>::publishHand,&wam_node); + while (ros::ok()) { ros::spinOnce();