Change jointStateCB() to process joint names.
--- /dev/null
+#!/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"
--- /dev/null
+#!/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"
--- /dev/null
+#!/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"
## 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
* Author: dc
*/
- /* Modified on 2017, 2018
- Author: Walter Fetter Lages
+/* Modified on 2017, 2018
+ * Author: Walter Fetter Lages
*/
#define WAM_NODE_SIM_WAM_IMPL_H_
//#include <unistd.h> // usleep
+#include <string>
//#include <boost/ref.hpp>
//#include <boost/bind.hpp>
{
template<size_t DOF>
-Wam<DOF>::Wam(const std::string& sysName)
+Wam<DOF>::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<trajectory_msgs::JointTrajectoryPoint>("command", 1); // wam/command
jointStatesSubscriber_=node_.subscribe("/joint_states",10,&Wam<DOF>::jointStatesCB,this);
template<size_t DOF>
typename Wam<DOF>::jt_type Wam<DOF>::getJointTorques() const
{
- return wamJointEffort_;
+ return jointEffort_;
}
template<size_t DOF>
inline typename Wam<DOF>::jp_type Wam<DOF>::getJointPositions() const
{
- return wamJointPosition_;
+ return jointPosition_;
}
template<size_t DOF>
inline typename Wam<DOF>::jv_type Wam<DOF>::getJointVelocities() const
{
- return wamJointVelocity_;
+ return jointVelocity_;
}
template<size_t DOF>
typename Wam<DOF>::cp_type Wam<DOF>::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)
Eigen::Quaterniond Wam<DOF>::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)
template<size_t DOF>
void Wam<DOF>::jointStatesCB(const sensor_msgs::JointState &jointStates)
{
+ std::vector<std::string> 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);
}
--- /dev/null
+/**
+ * Copyright 2009-2014 Barrett Technology <support@barrett.com>
+ * Copyright (c) 2018 Walter Fetter Lages <w.fetter@ieee.org>
+ *
+ * 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
+ * <http://www.gnu.org/licenses/>.
+ *
+ *
+ * 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 <vector>
+
+#include <Eigen/Core>
+
+#include <sensor_msgs/JointState.h>
+
+#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<double,DOF,1> jt_type;
+ typedef Eigen::Matrix<double,DOF,1> jp_type;
+ typedef Eigen::Matrix<double,DOF,1> jv_type;
+ typedef Eigen::Matrix<double,3,1> cp_type;
+ typedef Eigen::Matrix<double,3,1> cv_type;
+
+ typedef Eigen::Matrix<double,DOF,1> 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<int>& getPrimaryEncoderPosition() const { return primaryEncoder; }
+ /** getSecondaryEncoderPosition Method */
+ const std::vector<int>& 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<int>& 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<TactilePuck*>& 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<int> > encoderTmp;
+ std::vector<int> primaryEncoder, secondaryEncoder;
+ jp_type innerJp, outerJp;
+ std::vector<int> ftt;
+// std::vector<TactilePuck*> 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_ */
--- /dev/null
+/**
+ * Copyright 2009-2014 Barrett Technology <support@barrett.com>
+ *
+ * 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
+ * <http://www.gnu.org/licenses/>.
+ *
+ *
+ * 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 <vector>
+
+#include <Eigen/Core>
+
+#include <barrett/detail/ca_macro.h>
+#include <barrett/units.h>
+#include <barrett/products/abstract/multi_puck_product.h>
+#include <barrett/products/puck.h>
+#include <barrett/products/motor_puck.h>
+#include <barrett/products/tactile_puck.h>
+
+
+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<Puck*>& 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<int>& getPrimaryEncoderPosition() const { return primaryEncoder; }
+ /** getSecondaryEncoderPosition Method */
+ const std::vector<int>& 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<int>& 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<TactilePuck*>& 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<MotorPuck::CombinedPositionParser<int>::result_type> encoderTmp;
+ std::vector<int> primaryEncoder, secondaryEncoder;
+ jp_type innerJp, outerJp;
+ std::vector<int> ftt;
+ std::vector<TactilePuck*> 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_ */
//#include <boost/thread.hpp>
#include <Eigen/Core>
+#include <Eigen/Geometry>
//#include <libconfig.h++>
#include "ros/ros.h"
#include "trajectory_msgs/JointTrajectoryPoint.h"
+#include <sensor_msgs/JointState.h>
#include <kdl/chainfksolverpos_recursive.hpp>
+#include <wam_node_sim/hand.h>
namespace barrett
{
*
* 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
*/
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);
--- /dev/null
+/**
+ * Copyright 2009-2014 Barrett Technology <support@barrett.com>
+ * Copyright (c) 2017, 2018 Walter Fetter Lages <w.fetter@ieee.org>
+ *
+ * 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
+ * <http://www.gnu.org/licenses/>.
+ *
+ *
+ * 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 <stdexcept>
+#include <vector>
+#include <algorithm>
+#include <limits>
+
+#include <ros/ros.h>
+
+#include <wam_node_sim/hand.h>
+
+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<int>::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<Puck::StandardParser>(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<std::string> 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<int>::max();
+ }
+ }
+}
+
+
+}
--- /dev/null
+/**
+ * Copyright 2009-2014 Barrett Technology <support@barrett.com>
+ *
+ * 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
+ * <http://www.gnu.org/licenses/>.
+ *
+ *
+ * Barrett Technology Inc.
+ * 73 Chapel Street
+ * Newton, MA 02458
+ *
+ */
+
+/**
+ * @file hand.cpp
+ * @date 11/09/2010
+ * @author Dan Cody
+ *
+ */
+
+#include <stdexcept>
+#include <vector>
+#include <algorithm>
+#include <limits>
+
+#include <boost/thread/locks.hpp>
+
+#include <barrett/os.h>
+#include <barrett/detail/stl_utils.h>
+#include <barrett/products/abstract/multi_puck_product.h>
+#include <barrett/products/puck.h>
+#include <barrett/products/puck_group.h>
+#include <barrett/products/motor_puck.h>
+#include <barrett/products/tactile_puck.h>
+#include <barrett/products/hand.h>
+
+
+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<Puck*>& _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<thread::Mutex> 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<MotorPuck::CombinedPositionParser<int> >(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<int>::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<Puck::StandardParser>(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);
+ }
+}
+
+
+}
Author: Kyle Maroney
*/
- /* Modified on 2017, 2018
+ /* Modified in 2017, 2018
Author: Walter Fetter Lages
*/
#include <unistd.h>
#include <math.h>
+#include <boost/thread.hpp> // BarrettHand threading
+#include <boost/bind.hpp>
+
+
#include <Eigen/Geometry>
#include "ros/ros.h"
#include <wam_node_sim/rate_limiter.h>
#include <wam_node_sim/wam.h>
+#include <wam_node_sim/hand.h>
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<size_t DOF>
class WamNode
bool jnt_pos_status, cart_pos_status, ortn_pos_status, new_rt_cmd;
double cart_vel_mag, ortn_vel_mag;
barrett::Wam<DOF> &wam;
+ Hand* hand;
jp_type jp, jp_cmd, jp_home;
jp_type rt_jp_cmd, rt_jp_rl;
jv_type rt_jv_cmd;
ros::ServiceServer hand_sprd_vel_srv;
public:
- WamNode(barrett::Wam<DOF> &wam_,bool foundHand);
+ WamNode(barrett::Wam<DOF> &wam_,barrett::Hand *bhand);
~WamNode(void) { }
};
template<size_t DOF>
- WamNode<DOF>::WamNode(barrett::Wam<DOF> &wam_,bool foundHand): wam(wam_)
+ WamNode<DOF>::WamNode(barrett::Wam<DOF> &wam_,barrett::Hand *bhand): wam(wam_)
{
ros::NodeHandle n_("wam"); // WAM specific nodehandle
ros::NodeHandle nh_("bhand"); // BarrettHand specific nodehandle
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
bool WamNode<DOF>::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);
bool WamNode<DOF>::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;
}
bool WamNode<DOF>::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;
}
bool WamNode<DOF>::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;
}
bool WamNode<DOF>::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;
}
{
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;
}
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), Hand::GRASP, false);
return true;
}
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(req.radians), Hand::SPREAD, false);
return true;
}
{
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;
}
bool WamNode<DOF>::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;
}
{
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;
}
template<size_t DOF>
void WamNode<DOF>::publishHand() //systems::PeriodicDataLogger<debug_tuple>& 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();
}
}
{
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();
projects / ufrgs_wam.git / commitdiff