<?xml version="1.0"?>
<package>
<name>ufrgs_wam</name>
- <version>2.0.0</version>
+ <version>3.0.0</version>
<description>The ufrgs_wam package</description>
<!-- One maintainer tag required, multiple allowed, one person per tag -->
<buildtool_depend>catkin</buildtool_depend>
<run_depend>wam_description</run_depend>
- <run_depend>wam_gazebo_ros_control</run_depend>
- <run_depend>wam_controllers</run_depend>
+ <!--run_depend>wam_gazebo_ros_control</run_depend-->
+ <run_depend>pid_plus_gravity_controller</run_depend>
+ <run_depend>computed_torque_controller</run_depend>
+ <run_depend>wam_simulation</run_depend>
+ <run_depend>wam_node_sim</run_depend>
<!-- The export tag contains other, unspecified, tags -->
<export>
--- /dev/null
+cmake_minimum_required(VERSION 2.8.3)
+project(wam_node_sim)
+
+## Compile as C++11, supported in ROS Kinetic and newer
+# add_compile_options(-std=c++11)
+
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+find_package(catkin REQUIRED COMPONENTS
+# bullet
+ geometry_msgs
+ roscpp
+ std_msgs
+ std_srvs
+ wam_msgs
+ wam_srvs
+ trajectory_msgs
+ orocos_kdl
+ kdl_parser
+)
+
+find_package(cmake_modules REQUIRED)
+
+## System dependencies are found with CMake's conventions
+# find_package(Boost REQUIRED COMPONENTS system)
+
+find_package(Eigen REQUIRED)
+
+## Uncomment this if the package has a setup.py. This macro ensures
+## modules and global scripts declared therein get installed
+## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
+# catkin_python_setup()
+
+################################################
+## Declare ROS messages, services and actions ##
+################################################
+
+## To declare and build messages, services or actions from within this
+## package, follow these steps:
+## * Let MSG_DEP_SET be the set of packages whose message types you use in
+## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
+## * In the file package.xml:
+## * add a build_depend tag for "message_generation"
+## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET
+## * If MSG_DEP_SET isn't empty the following dependency has been pulled in
+## but can be declared for certainty nonetheless:
+## * add a run_depend tag for "message_runtime"
+## * In this file (CMakeLists.txt):
+## * add "message_generation" and every package in MSG_DEP_SET to
+## find_package(catkin REQUIRED COMPONENTS ...)
+## * add "message_runtime" and every package in MSG_DEP_SET to
+## catkin_package(CATKIN_DEPENDS ...)
+## * uncomment the add_*_files sections below as needed
+## and list every .msg/.srv/.action file to be processed
+## * uncomment the generate_messages entry below
+## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
+
+## Generate messages in the 'msg' folder
+# add_message_files(
+# FILES
+# Message1.msg
+# Message2.msg
+# )
+
+## Generate services in the 'srv' folder
+# add_service_files(
+# FILES
+# Service1.srv
+# Service2.srv
+# )
+
+## Generate actions in the 'action' folder
+# add_action_files(
+# FILES
+# Action1.action
+# Action2.action
+# )
+
+## Generate added messages and services with any dependencies listed here
+# generate_messages(
+# DEPENDENCIES
+# geometry_msgs# std_msgs# wam_msgs
+# )
+
+################################################
+## Declare ROS dynamic reconfigure parameters ##
+################################################
+
+## To declare and build dynamic reconfigure parameters within this
+## package, follow these steps:
+## * In the file package.xml:
+## * add a build_depend and a run_depend tag for "dynamic_reconfigure"
+## * In this file (CMakeLists.txt):
+## * add "dynamic_reconfigure" to
+## find_package(catkin REQUIRED COMPONENTS ...)
+## * uncomment the "generate_dynamic_reconfigure_options" section below
+## and list every .cfg file to be processed
+
+## Generate dynamic reconfigure parameters in the 'cfg' folder
+# generate_dynamic_reconfigure_options(
+# cfg/DynReconf1.cfg
+# cfg/DynReconf2.cfg
+# )
+
+###################################
+## catkin specific configuration ##
+###################################
+## The catkin_package macro generates cmake config files for your package
+## Declare things to be passed to dependent projects
+## INCLUDE_DIRS: uncomment this if you package contains header files
+## LIBRARIES: libraries you create in this project that dependent projects also need
+## CATKIN_DEPENDS: catkin_packages dependent projects also need
+## DEPENDS: system dependencies of this project that dependent projects also need
+catkin_package(
+# INCLUDE_DIRS include
+# LIBRARIES wam_node_sim
+# CATKIN_DEPENDS bullet geometry_msgs roscpp std_msgs std_srvs wam_msgs wam_srvs geometry_msgs
+# DEPENDS system_lib
+)
+
+###########
+## Build ##
+###########
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+include_directories(
+ include
+ ${catkin_INCLUDE_DIRS}
+ ${Eigen_INCLUDE_DIRS}
+)
+
+## Declare a C++ library
+# add_library(${PROJECT_NAME}
+# src/${PROJECT_NAME}/wam_node_sim.cpp
+# )
+
+## Add cmake target dependencies of the library
+## as an example, code may need to be generated before libraries
+## either from message generation or dynamic reconfigure
+# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## 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)
+
+## Rename C++ executable without prefix
+## The above recommended prefix causes long target names, the following renames the
+## target back to the shorter version for ease of user use
+## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
+# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
+
+## Add cmake target dependencies of the executable
+## same as for the library above
+# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Specify libraries to link a library or executable target against
+target_link_libraries(${PROJECT_NAME}
+ ${catkin_LIBRARIES}
+)
+
+#############
+## Install ##
+#############
+
+# all install targets should use catkin DESTINATION variables
+# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
+
+## Mark executable scripts (Python etc.) for installation
+## in contrast to setup.py, you can choose the destination
+# install(PROGRAMS
+# scripts/my_python_script
+# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark executables and/or libraries for installation
+# install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node
+# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark cpp header files for installation
+# install(DIRECTORY include/${PROJECT_NAME}/
+# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+# FILES_MATCHING PATTERN "*.h"
+# PATTERN ".svn" EXCLUDE
+# )
+
+## Mark other files for installation (e.g. launch and bag files, etc.)
+# install(FILES
+# # myfile1
+# # myfile2
+# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+# )
+
+#############
+## Testing ##
+#############
+
+## Add gtest based cpp test target and link libraries
+# catkin_add_gtest(${PROJECT_NAME}-test test/test_wam_node_sim.cpp)
+# if(TARGET ${PROJECT_NAME}-test)
+# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
+# endif()
+
+## Add folders to be run by python nosetests
+# catkin_add_nosetests(test)
--- /dev/null
+/*
+ Copyright 2012 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/>.
+
+ Further, non-binding information about licensing is available at:
+ <http://wiki.barrett.com/libbarrett/wiki/LicenseNotes>
+*/
+
+/*
+ * rate_limiter-inl.h
+ *
+ * Created on: Apr 12, 2012
+ * Author: dc
+ */
+
+
+#ifndef WAM_NODE_SIM_RATE_LIMITER_IMPL_H_
+#define WAM_NODE_SIM_RATE_LIMITER_IMPL_H_
+
+#include <algorithm>
+#include <cassert>
+
+namespace wam_node_sim {
+
+template <typename T> int sgn(T val)
+{
+ return (T(0) < val) - (val < T(0));
+}
+
+template<typename T>
+void RateLimiter<T>::setLimit(const T& newLimit)
+{
+ // Limit must be non-negative. Zero is a special value meaning "don't limit".
+ for(int i=0;i < newLimit.rows();i++)
+ assert(newLimit[i] == fabs(newLimit[i]));
+ limit = newLimit;
+ maxDelta = T_s * limit;
+}
+
+template<typename T>
+void RateLimiter<T>::setCurVal(const T& newPos)
+{
+ data = newPos;
+}
+
+template<typename T>
+T &RateLimiter<T>::getLimit(const T &x)
+{
+ delta = x-data;
+ for(int i;i < x.rows();i++)
+ if(limit[i] == 0.0) data[i]=x[i];
+ else data[i] += sgn(delta[i])*std::min(fabs(delta[i]),maxDelta[i]);
+
+ return data;
+}
+
+
+}
+#endif
--- /dev/null
+/*
+ Copyright 2009, 2010, 2011, 2012 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/>.
+
+ Further, non-binding information about licensing is available at:
+ <http://wiki.barrett.com/libbarrett/wiki/LicenseNotes>
+*/
+
+/*
+ * wam-inl.h
+ *
+ * Created on: Sep 25, 2009
+ * Author: dc
+ */
+
+#ifndef WAM_NODE_SIM_WAM_IMPL_H_
+#define WAM_NODE_SIM_WAM_IMPL_H_
+
+//#include <unistd.h> // usleep
+
+//#include <boost/ref.hpp>
+//#include <boost/bind.hpp>
+//#include <boost/thread.hpp>
+
+//#define EIGEN_USE_NEW_STDVECTOR
+//#include <Eigen/StdVector>
+#include <Eigen/Core>
+//#include <Eigen/Geometry>
+
+//#include <libconfig.h>
+
+#include <kdl/frames.hpp>
+#include <kdl_parser/kdl_parser.hpp>
+
+namespace barrett
+{
+
+template<size_t DOF>
+Wam<DOF>::Wam(const std::string& sysName)
+{
+ ros::NodeHandle node_("wam"); // WAM specific nodehandle
+ wam_cmd_pub=node_.advertise<trajectory_msgs::JointTrajectoryPoint>("command", 1); // wam/command
+ jointStatesSubscriber_=node_.subscribe("/joint_states",10,&Wam<DOF>::jointStatesCB,this);
+
+ std::string robotDescription;
+ if(!node_.getParam("/robot_description",robotDescription))
+ ROS_ERROR("Could not find 'robot_description'.");
+
+ KDL::Tree tree;
+ if (!kdl_parser::treeFromString(robotDescription,tree))
+ ROS_ERROR("Failed to construct KDL tree.");
+
+
+ KDL::SegmentMap::const_iterator s=tree.getRootSegment();
+ KDL::Chain chain;
+ if (!tree.getChain(GetTreeElementSegment(s->second).getName(),"wam_tool_plate",chain))
+ ROS_ERROR("Failed to get chain from KDL tree.");
+
+ fwdKinSolverPos_=new KDL::ChainFkSolverPos_recursive(chain);
+}
+
+template<size_t DOF>
+Wam<DOF>::~Wam()
+{
+ wam_cmd_pub.shutdown();
+ jointStatesSubscriber_.shutdown();
+ delete fwdKinSolverPos_;
+}
+
+template<size_t DOF>
+void Wam<DOF>::trackReferenceSignal(jp_type &referenceSignal)
+{
+ trajectory_msgs::JointTrajectoryPoint jointRef;
+
+ for(size_t i=0; i < DOF;i++)
+ {
+ jointRef.positions.push_back(referenceSignal[i]);
+ jointRef.velocities.push_back(0.0);
+ jointRef.accelerations.push_back(0.0);
+ }
+
+ wam_cmd_pub.publish(jointRef);
+}
+
+
+template<size_t DOF>
+inline const typename Wam<DOF>::jp_type& Wam<DOF>::getHomePosition() const
+{
+ static jp_type home={0.0, -2.0, 0.0, 3.1, 0.0, 0.0, 0.0};
+ return home;
+}
+
+
+template<size_t DOF>
+typename Wam<DOF>::jt_type Wam<DOF>::getJointTorques() const
+{
+ return wamJointEffort_;
+}
+
+template<size_t DOF>
+inline typename Wam<DOF>::jp_type Wam<DOF>::getJointPositions() const
+{
+ return wamJointPosition_;
+}
+
+template<size_t DOF>
+inline typename Wam<DOF>::jv_type Wam<DOF>::getJointVelocities() const
+{
+ return wamJointVelocity_;
+}
+
+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];
+
+ KDL::Frame frame;
+ if(fwdKinSolverPos_->JntToCart(jointPosition,frame) < 0)
+ ROS_ERROR("Failed to compute forward kinematics.");
+
+ cp_type cp;
+ for(int i=0;i < 3;i++) cp[i]=frame.p.data[i];
+
+ return cp;
+}
+
+template<size_t DOF>
+typename Wam<DOF>::cv_type Wam<DOF>::getToolVelocity() const
+{
+ return cv_type();
+}
+
+template<size_t DOF>
+Eigen::Quaterniond Wam<DOF>::getToolOrientation() const
+{
+ KDL::JntArray jointPosition(DOF);
+ for(int i;i < DOF;i++) jointPosition(i)=wamJointPosition_[i];
+
+ KDL::Frame frame;
+ if(fwdKinSolverPos_->JntToCart(jointPosition,frame) < 0)
+ ROS_ERROR("Failed to compute forward kinematics.");
+
+ double x,y,z,w;
+ frame.M.GetQuaternion(x,y,z,w);
+ return Eigen::Quaterniond(w,x,y,z);
+}
+/*
+template<size_t DOF>
+inline typename Wam<DOF>::pose_type Wam<DOF>::getToolPose() const
+{
+ return boost::make_tuple(getToolPosition(), getToolOrientation());
+}
+*/
+
+/*
+template<size_t DOF>
+inline math::Matrix<6,DOF> Wam<DOF>::getToolJacobian() const
+{
+ kin.eval(getJointPositions(), getJointVelocities());
+ return math::Matrix<6,DOF>(kin.impl->tool_jacobian);
+}
+*/
+
+template<size_t DOF>
+void Wam<DOF>::gravityCompensate(bool compensate)
+{
+
+}
+
+/*
+template<size_t DOF>
+bool Wam<DOF>::updateGravity(double val)
+{
+ return (gravity.setGravity(val));
+}
+*/
+
+template<size_t DOF>
+inline bool Wam<DOF>::isGravityCompensated()
+{
+ return true;
+}
+
+template<size_t DOF>
+inline void Wam<DOF>::moveHome(bool blocking)
+{
+ moveTo(getHomePosition(), blocking);
+}
+
+template<size_t DOF>
+inline void Wam<DOF>::moveHome(bool blocking, double velocity)
+{
+ moveTo(getHomePosition(), blocking, velocity);
+}
+
+template<size_t DOF>
+inline void Wam<DOF>::moveHome(bool blocking, double velocity, double acceleration)
+{
+ moveTo(getHomePosition(), blocking, velocity, acceleration);
+}
+
+template<size_t DOF>
+inline void Wam<DOF>::moveTo(const jp_type& destination, bool blocking, double velocity, double acceleration)
+{
+// moveTo(currentPosHelper(getJointPositions()), getJointVelocities(), destination, blocking, velocity, acceleration);
+// moveTo(currentPosHelper(getJointPositions()), /*jv_type(0.0),*/ destination, blocking, velocity, acceleration);
+}
+
+template<size_t DOF>
+inline void Wam<DOF>::moveTo(const cp_type& destination, bool blocking, double velocity, double acceleration)
+{
+// moveTo(currentPosHelper(getToolPosition()), /*cv_type(0.0),*/ destination, blocking, velocity, acceleration);
+}
+
+template<size_t DOF>
+inline void Wam<DOF>::moveTo(const Eigen::Quaterniond& destination, bool blocking, double velocity, double acceleration)
+{
+// moveTo(currentPosHelper(getToolOrientation()), destination, blocking, velocity, acceleration);
+}
+
+/*
+template<size_t DOF>
+inline void Wam<DOF>::moveTo(const pose_type& destination, bool blocking, double velocity, double acceleration)
+{
+// moveTo(currentPosHelper(getToolPose()), destination, blocking, velocity, acceleration);
+}
+*/
+
+template<size_t DOF>
+template<typename T>
+void Wam<DOF>::moveTo(const T& currentPos, /*const typename T::unitless_type& currentVel,*/ const T& destination, bool blocking, double velocity, double acceleration)
+{
+ bool started = false;
+// boost::promise<boost::thread*> threadPtrPromise;
+// boost::shared_future<boost::thread*> threadPtrFuture(threadPtrPromise.get_future());
+// boost::thread* threadPtr = new boost::thread(&Wam<DOF>::moveToThread<T>, this, boost::ref(currentPos), /*currentVel,*/ boost::ref(destination), velocity, acceleration, &started, threadPtrFuture);
+// mtThreadGroup.add_thread(threadPtr);
+// threadPtrPromise.set_value(threadPtr);
+
+
+ // wait until move starts
+ while ( !started ) {
+// btsleep(0.001);
+ }
+
+ if (blocking) {
+ while (!moveIsDone()) {
+// btsleep(0.01);
+ }
+ }
+}
+
+template<size_t DOF>
+bool Wam<DOF>::moveIsDone() const
+{
+ return doneMoving;
+}
+
+template<size_t DOF>
+void Wam<DOF>::idle()
+{
+// supervisoryController.disconnectInput();
+}
+
+
+//template<size_t DOF>
+//template<typename T>
+//void Wam<DOF>::moveToThread(const T& currentPos, /*const typename T::unitless_type& currentVel,*/ const T& destination, double velocity, double acceleration, bool* started, boost::shared_future<boost::thread*> threadPtrFuture)
+//{
+
+
+//}
+
+template<size_t DOF>
+void Wam<DOF>::jointStatesCB(const sensor_msgs::JointState &jointStates)
+{
+ for(int i=0;i < DOF;i++)
+ {
+ wamJointPosition_[i]=jointStates.position[i];
+ wamJointVelocity_[i]=jointStates.velocity[i];
+ wamJointEffort_[i]=jointStates.effort[i];
+ }
+}
+
+
+}
+
+#endif
--- /dev/null
+/*
+ Copyright 2012 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/>.
+
+ Further, non-binding information about licensing is available at:
+ <http://wiki.barrett.com/libbarrett/wiki/LicenseNotes>
+*/
+
+/*
+ * rate_limiter.h
+ *
+ * Created on: Apr 12, 2012
+ * Author: dc
+ */
+
+#ifndef WAM_NODE_SIM_RATE_LIMITER_H_
+#define WAM_NODE_SIM_RATE_LIMITER_H_
+
+#include <Eigen/Core>
+
+namespace wam_node_sim
+{
+
+template<typename T>
+class RateLimiter
+{
+ public:
+ RateLimiter(double Ts=1.0/250.0)
+ {
+ T limit;
+ limit.setZero();
+ T_s=Ts;
+ setLimit(limit);
+ }
+
+ RateLimiter(const T &limit,double Ts=1.0/250.0)
+ {
+ T_s=Ts;
+ setLimit(limit);
+ }
+
+ ~RateLimiter(void)
+ {
+
+ }
+
+ void setLimit(const T& newLimit);
+
+ void setCurVal(const T& newPos);
+
+ T &getLimit(const T &x);
+
+ private:
+
+ double T_s;
+ T limit;
+ T maxDelta;
+ T data;
+
+ T delta;
+};
+
+
+}
+
+
+// include template definitions
+#include <wam_node_sim/detail/rate_limiter-impl.h>
+
+
+#endif /* WAM_NODE_SIM_RATE_LIMITER_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
+ *
+ */
+
+/** Defines systems::Wam.
+ *
+ * @file systems/wam.h
+ * @date 09/25/2009
+ * @author Dan Cody
+ */
+
+
+#ifndef WAM_NODE_SIM_WAM_H_
+#define WAM_NODE_SIM_WAM_H_
+
+
+//#include <vector>
+
+//#include <boost/thread.hpp>
+#include <Eigen/Core>
+//#include <libconfig.h++>
+
+#include "ros/ros.h"
+#include "trajectory_msgs/JointTrajectoryPoint.h"
+
+#include <kdl/chainfksolverpos_recursive.hpp>
+
+namespace barrett
+{
+
+template<size_t DOF>
+class Wam {
+ 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;
+
+public:
+ /** Constructor for Wam
+ *
+ * GenericPucks must be ordered by joint and must break into torque groups as arranged.
+ */
+ Wam(const std::string& sysName = "Wam");
+
+ /** Destructor for Wam
+ */
+ ~Wam();
+
+ /** trackReferenceSignal() used for following updating input. (any barrett input units for control)
+ */
+ void trackReferenceSignal(jp_type & referenceSignal); //NOLINT: non-const reference for syntax
+
+ /** getHomePosition() returns home postion of individual joints in Radians
+ */
+ const jp_type& getHomePosition() const;
+
+ /** getJointTorques() returns joint torques in Newtons per meter
+ */
+ jt_type getJointTorques() const;
+
+ /** getJointPositions() returns joint Position values in Radians
+ */
+ jp_type getJointPositions() const;
+
+ /** getJointVelocities() returns joint velocity values in meters per second
+ */
+ jv_type getJointVelocities() const;
+
+ /** getToolPosition() returns Tool Position in Cartesian Space meters
+ */
+ cp_type getToolPosition() const;
+
+ /** getToolVelocity() returns Tool velocity in meters per second
+ */
+ cv_type getToolVelocity() const;
+
+ /** getToolOrientation() returns Tool Orientation in Quaternions
+ */
+ Eigen::Quaterniond getToolOrientation() const;
+
+ /** getToolPose() returns Tool Pose as a combination of
+ */
+// pose_type getToolPose() const;
+
+ /** getToolJacobian() returns matrix of first order partial derivatives.
+ */
+// math::Matrix<6,DOF> getToolJacobian() const;
+
+ /** gravityCompensate() method activates Gravity Compensation for WAM
+ */
+ void gravityCompensate(bool compensate = true);
+
+ /** updateGravity() method updates the value of Gravity applied for WAM
+ */
+ bool updateGravity(double val = -9.8);
+
+ /** isGravityCompensated() returns flag as to the status of Gravity Compensation
+ */
+ bool isGravityCompensated();
+
+ /** moveHome(bool blocking, double velocity, double acceleration) method sends WAM to stored Home Position
+ *
+ * Blocking Allows/Disallows other functions to happen while Movement is under way
+ * Velocity Speed value in m/s
+ * Acceleration Acceleration value in m/s^2
+ * Function is overloaded to allow multiple types of calls.
+ */
+ void moveHome(bool blocking = true);
+ void moveHome(bool blocking, double velocity);
+ void moveHome(bool blocking, double velocity, double acceleration);
+
+ /** moveTo() method sends WAM to desired point based on input type.
+ *
+ * Function is overloaded to allow moves any type of Barrett Unit Types input.
+ * Destination Based on type cartesian(xyz), joint(radians), quaternoind(orientation), pose
+ * blocking Determines whether program should wait for move to finish before continuing
+ * velocity Speed at which to move
+ * acceleration value in radians per second
+ */
+ void moveTo(const jp_type& destination, bool blocking = true, double velocity = 0.5, double acceleration = 0.5);
+ void moveTo(const cp_type& destination, bool blocking = true, double velocity = 0.1, double acceleration = 0.2);
+ void moveTo(const Eigen::Quaterniond& destination, bool blocking = true, double velocity = 0.5, double acceleration = 0.5);
+// void moveTo(const pose_type& destination, bool blocking = true, double velocity = 0.1, double acceleration = 0.2);
+ template<typename T> void moveTo(const T& currentPos, /*const typename T::unitless_type& currentVel,*/ const T& destination, bool blocking, double velocity, double acceleration);
+
+ /** moveIsDone() method returns false while the trajectory controller for the most recent moveTo() command is still active.
+ *
+ * Only useful if the moveTo() is non-blocking.
+ */
+ bool moveIsDone() const;
+
+ /** idle() method Terminates the position controller (if active).
+ *
+ * To prevent uncontrolled falling, you should ensure you have set gravityCompensate(true) before calling idle().
+ */
+ void idle();
+
+ /** getEmMutex() method allows access to ExecutionManagers Mutex in LowLevelWam Class.
+ */
+// thread::Mutex& getEmMutex() const { return llww.getEmMutex(); }
+
+
+private:
+
+ bool doneMoving;
+// boost::thread_group mtThreadGroup;
+
+ // Used to calculate TP and TO if the values aren't already being calculated in the control loop.
+// mutable math::Kinematics<DOF> kin;
+
+ ros::Publisher wam_cmd_pub; // controller command
+ ros::Subscriber jointStatesSubscriber_;
+ jp_type wamJointPosition_;
+ jv_type wamJointVelocity_;
+ jt_type wamJointEffort_;
+ KDL::ChainFkSolverPos_recursive *fwdKinSolverPos_;
+
+ void jointStatesCB(const sensor_msgs::JointState &jointStates);
+
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+};
+
+
+}
+
+// include template definitions
+#include <wam_node_sim/detail/wam-impl.h>
+
+#endif /* WAM_NODE_SIM_WAM_H_ */
--- /dev/null
+<launch>
+ <arg name="paused" default="true"/>
+ <arg name="headless" default="false"/>
+ <arg name="use_sim_time" default="true"/>
+ <arg name="table" default="true"/>
+ <arg name="bhand" default="false"/>
+
+ <!-- This is the default Barrett WAM Controller, used by libbarrett -->
+ <arg name="controller" default="pid_plus_gravity_controller"/>
+ <arg name="config" default="$(find wam_bringup)/config/$(arg controller).yaml"/>
+
+ <include file="$(find wam_bringup)/launch/gazebo.launch" >
+ <arg name="paused" value="$(arg paused)"/>
+ <arg name="headless" value="$(arg headless)"/>
+ <arg name="use_sim_time" value="$(arg use_sim_time)"/>
+ <arg name="table" value="$(arg table)"/>
+ <arg name="bhand" value="$(arg bhand)"/>
+ <arg name="controller" value="$(arg controller)" />
+ <arg name="config" value="$(arg config)" />
+ </include>
+
+ <remap from="wam/command" to="controller/command" />
+
+ <include file="$(find wam_node_sim)/launch/wam_node_sim.launch" />
+ <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher">
+ <remap from="/joint_states" to="wam/joint_states" />
+ </node>
+</launch>
--- /dev/null
+<launch>
+ <node name="wam_node_sim" pkg="wam_node_sim" type="wam_node_sim" output="screen" />
+</launch>
--- /dev/null
+<?xml version="1.0"?>
+<package>
+ <name>wam_node_sim</name>
+ <version>0.0.0</version>
+ <description>ROS node to simulate the wam_node, which runs the actual Barrett WAM.</description>
+
+ <!-- One maintainer tag required, multiple allowed, one person per tag -->
+ <!-- Example: -->
+ <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+ <maintainer email="fetter@ece.ufrgs.br">Walter Fetter Lages</maintainer>
+
+
+ <!-- One license tag required, multiple allowed, one license per tag -->
+ <!-- Commonly used license strings: -->
+ <!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+ <license>GPLv2</license>
+
+
+ <!-- Url tags are optional, but multiple are allowed, one per tag -->
+ <!-- Optional attribute type can be: website, bugtracker, or repository -->
+ <!-- Example: -->
+ <!-- <url type="website">http://wiki.ros.org/wam_node_sim</url> -->
+
+
+ <!-- Author tags are optional, multiple are allowed, one per tag -->
+ <!-- Authors do not have to be maintainers, but could be -->
+ <!-- Example: -->
+ <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+ <author email="fetter@ece.ufrgs.br">Walter Fetter Lages</author>
+
+
+ <!-- The *_depend tags are used to specify dependencies -->
+ <!-- Dependencies can be catkin packages or system dependencies -->
+ <!-- Examples: -->
+ <!-- Use build_depend for packages you need at compile time: -->
+ <!-- <build_depend>message_generation</build_depend> -->
+ <!-- Use buildtool_depend for build tool packages: -->
+ <!-- <buildtool_depend>catkin</buildtool_depend> -->
+ <!-- Use run_depend for packages you need at runtime: -->
+ <!-- <run_depend>message_runtime</run_depend> -->
+ <!-- Use test_depend for packages you need only for testing: -->
+ <!-- <test_depend>gtest</test_depend> -->
+ <buildtool_depend>catkin</buildtool_depend>
+ <build_depend>Eigen</build_depend>
+ <build_depend>bullet</build_depend>
+ <build_depend>geometry_msgs</build_depend>
+ <build_depend>roscpp</build_depend>
+ <build_depend>std_msgs</build_depend>
+ <build_depend>std_srvs</build_depend>
+ <build_depend>wam_msgs</build_depend>
+ <build_depend>wam_srvs</build_depend>
+ <build_depend>trajetory_msgs</build_depend>
+ <build_depend>orocos_kdl</build_depend>
+ <build_depend>kdl_parser</build_depend>
+ <run_depend>Eigen</run_depend>
+ <run_depend>bullet</run_depend>
+ <run_depend>geometry_msgs</run_depend>
+ <run_depend>roscpp</run_depend>
+ <run_depend>std_msgs</run_depend>
+ <run_depend>std_srvs</run_depend>
+ <run_depend>wam_msgs</run_depend>
+ <run_depend>wam_srvs</run_depend>
+ <run_depend>trajetory_msgs</run_depend>
+ <run_depend>orocos_kdl</run_depend>
+ <run_depend>kdl_parser</run_depend>
+
+
+
+ <!-- The export tag contains other, unspecified, tags -->
+ <export>
+ <!-- Other tools can request additional information be placed here -->
+
+ </export>
+</package>
--- /dev/null
+/*
+ Copyright 2012 Barrett Technology <support@barrett.com>
+
+ This file is part of barrett-ros-pkg.
+
+ This version of barrett-ros-pkg 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 barrett-ros-pkg 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 barrett-ros-pkg. If not, see
+ <http://www.gnu.org/licenses/>.
+
+ Barrett Technology holds all copyrights on barrett-ros-pkg. As the sole
+ copyright holder, Barrett reserves the right to release future versions
+ of barrett-ros-pkg under a different license.
+
+ File: wam_node.cpp
+ Date: 5 June, 2012
+ Author: Kyle Maroney
+ */
+
+#include <unistd.h>
+#include <math.h>
+
+#include <boost/thread.hpp> // BarrettHand threading
+#include <boost/bind.hpp>
+
+#include "ros/ros.h"
+#include "tf/transform_datatypes.h"
+
+#include "wam_msgs/RTJointPos.h"
+#include "wam_msgs/RTJointVel.h"
+#include "wam_msgs/RTCartPos.h"
+#include "wam_msgs/RTCartVel.h"
+#include "wam_msgs/RTOrtnPos.h"
+#include "wam_msgs/RTOrtnVel.h"
+#include "wam_srvs/GravityComp.h"
+#include "wam_srvs/Hold.h"
+#include "wam_srvs/JointMove.h"
+#include "wam_srvs/PoseMove.h"
+#include "wam_srvs/CartPosMove.h"
+#include "wam_srvs/OrtnMove.h"
+#include "wam_srvs/BHandFingerPos.h"
+#include "wam_srvs/BHandGraspPos.h"
+#include "wam_srvs/BHandSpreadPos.h"
+#include "wam_srvs/BHandFingerVel.h"
+#include "wam_srvs/BHandGraspVel.h"
+#include "wam_srvs/BHandSpreadVel.h"
+#include "std_srvs/Empty.h"
+#include "sensor_msgs/JointState.h"
+#include "geometry_msgs/PoseStamped.h"
+
+#include <barrett/math.h>
+#include <barrett/units.h>
+#include <barrett/systems.h>
+#include <barrett/products/product_manager.h>
+#include <barrett/standard_main_function.h>
+#include <barrett/systems/wam.h>
+#include <barrett/detail/stl_utils.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;
+
+//Creating a templated multiplier for our real-time computation
+template<typename T1, typename T2, typename OutputType>
+ class Multiplier : public systems::System, public systems::SingleOutput<OutputType>
+ {
+ public:
+ Input<T1> input1;
+ public:
+ Input<T2> input2;
+
+ public:
+ Multiplier(std::string sysName = "Multiplier") :
+ systems::System(sysName), systems::SingleOutput<OutputType>(this), input1(this), input2(this)
+ {
+ }
+ virtual ~Multiplier()
+ {
+ mandatoryCleanUp();
+ }
+
+ protected:
+ OutputType data;
+ virtual void operate()
+ {
+ data = input1.getValue() * input2.getValue();
+ this->outputValue->setData(&data);
+ }
+
+ private:
+ DISALLOW_COPY_AND_ASSIGN(Multiplier);
+
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ };
+
+//Creating a templated converter from Roll, Pitch, Yaw to Quaternion for real-time computation
+class ToQuaternion : public systems::SingleIO<math::Vector<3>::type, Eigen::Quaterniond>
+{
+public:
+ Eigen::Quaterniond outputQuat;
+
+public:
+ ToQuaternion(std::string sysName = "ToQuaternion") :
+ systems::SingleIO<math::Vector<3>::type, Eigen::Quaterniond>(sysName)
+ {
+ }
+ virtual ~ToQuaternion()
+ {
+ mandatoryCleanUp();
+ }
+
+protected:
+ btQuaternion q;
+ virtual void operate()
+ {
+ const math::Vector<3>::type &inputRPY = input.getValue();
+ q.setEulerZYX(inputRPY[2], inputRPY[1], inputRPY[0]);
+ outputQuat.x() = q.getX();
+ outputQuat.y() = q.getY();
+ outputQuat.z() = q.getZ();
+ outputQuat.w() = q.getW();
+ this->outputValue->setData(&outputQuat);
+ }
+
+private:
+ DISALLOW_COPY_AND_ASSIGN(ToQuaternion);
+
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+};
+
+//Simple Function for converting Quaternion to RPY
+math::Vector<3>::type toRPY(Eigen::Quaterniond inquat)
+{
+ math::Vector<3>::type newRPY;
+ btQuaternion q(inquat.x(), inquat.y(), inquat.z(), inquat.w());
+ btMatrix3x3(q).getEulerZYX(newRPY[2], newRPY[1], newRPY[0]);
+ return newRPY;
+}
+
+//WamNode Class
+template<size_t DOF>
+ class WamNode
+ {
+ BARRETT_UNITS_TEMPLATE_TYPEDEFS(DOF);
+ protected:
+ bool cart_vel_status, ortn_vel_status, jnt_vel_status;
+ bool jnt_pos_status, cart_pos_status, ortn_pos_status, new_rt_cmd;
+ double cart_vel_mag, ortn_vel_mag;
+ systems::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;
+ cp_type cp_cmd, rt_cv_cmd;
+ cp_type rt_cp_cmd, rt_cp_rl;
+ Eigen::Quaterniond ortn_cmd, rt_op_cmd, rt_op_rl;
+ pose_type pose_cmd;
+ math::Vector<3>::type rt_ortn_cmd;
+ systems::ExposedOutput<Eigen::Quaterniond> orientationSetPoint, current_ortn;
+ systems::ExposedOutput<cp_type> cart_dir, current_cart_pos, cp_track;
+ systems::ExposedOutput<math::Vector<3>::type> rpy_cmd, current_rpy_ortn;
+ systems::ExposedOutput<jv_type> jv_track;
+ systems::ExposedOutput<jp_type> jp_track;
+ systems::TupleGrouper<cp_type, Eigen::Quaterniond> rt_pose_cmd;
+ systems::Summer<cp_type> cart_pos_sum;
+ systems::Summer<math::Vector<3>::type> ortn_cmd_sum;
+ systems::Ramp ramp;
+ systems::RateLimiter<jp_type> jp_rl;
+ systems::RateLimiter<cp_type> cp_rl;
+ Multiplier<double, cp_type, cp_type> mult_linear;
+ Multiplier<double, math::Vector<3>::type, math::Vector<3>::type> mult_angular;
+ ToQuaternion to_quat, to_quat_print;
+ Eigen::Quaterniond ortn_print;
+ ros::Time last_cart_vel_msg_time, last_ortn_vel_msg_time, last_jnt_vel_msg_time;
+ ros::Time last_jnt_pos_msg_time, last_cart_pos_msg_time, last_ortn_pos_msg_time;
+ ros::Duration rt_msg_timeout;
+
+ //Subscribed Topics
+ wam_msgs::RTCartVel cart_vel_cmd;
+ wam_msgs::RTOrtnVel ortn_vel_cmd;
+
+ //Subscribers
+ ros::Subscriber cart_vel_sub;
+ ros::Subscriber ortn_vel_sub;
+ ros::Subscriber jnt_vel_sub;
+ ros::Subscriber jnt_pos_sub;
+ ros::Subscriber cart_pos_sub;
+ ros::Subscriber ortn_pos_sub;
+
+ //Published Topics
+ sensor_msgs::JointState wam_joint_state, bhand_joint_state;
+ geometry_msgs::PoseStamped wam_pose;
+
+ //Publishers
+ ros::Publisher wam_joint_state_pub, bhand_joint_state_pub, wam_pose_pub;
+
+ //Services
+ ros::ServiceServer gravity_srv, go_home_srv, hold_jpos_srv, hold_cpos_srv;
+ ros::ServiceServer hold_ortn_srv, joint_move_srv, pose_move_srv;
+ ros::ServiceServer cart_move_srv, ortn_move_srv, hand_close_srv;
+ ros::ServiceServer hand_open_grsp_srv, hand_close_grsp_srv, hand_open_sprd_srv;
+ ros::ServiceServer hand_close_sprd_srv, hand_fngr_pos_srv, hand_fngr_vel_srv;
+ ros::ServiceServer hand_grsp_pos_srv, hand_grsp_vel_srv, hand_sprd_pos_srv;
+ ros::ServiceServer hand_sprd_vel_srv;
+
+ public:
+ WamNode(systems::Wam<DOF>& wam_) :
+ wam(wam_), hand(NULL), ramp(NULL, SPEED)
+ {
+ }
+ void
+ init(ProductManager& pm);
+
+ ~WamNode()
+ {
+ }
+
+ bool
+ gravity(wam_srvs::GravityComp::Request &req, wam_srvs::GravityComp::Response &res);
+ bool
+ goHome(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res);
+ bool
+ holdJPos(wam_srvs::Hold::Request &req, wam_srvs::Hold::Response &res);
+ bool
+ holdCPos(wam_srvs::Hold::Request &req, wam_srvs::Hold::Response &res);
+ bool
+ holdOrtn(wam_srvs::Hold::Request &req, wam_srvs::Hold::Response &res);
+ bool
+ jointMove(wam_srvs::JointMove::Request &req, wam_srvs::JointMove::Response &res);
+ bool
+ poseMove(wam_srvs::PoseMove::Request &req, wam_srvs::PoseMove::Response &res);
+ bool
+ cartMove(wam_srvs::CartPosMove::Request &req, wam_srvs::CartPosMove::Response &res);
+ bool
+ ortnMove(wam_srvs::OrtnMove::Request &req, wam_srvs::OrtnMove::Response &res);
+ bool
+ handOpenGrasp(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res);
+ bool
+ handCloseGrasp(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res);
+ bool
+ handOpenSpread(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res);
+ bool
+ handCloseSpread(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res);
+ bool
+ handFingerPos(wam_srvs::BHandFingerPos::Request &req, wam_srvs::BHandFingerPos::Response &res);
+ bool
+ handGraspPos(wam_srvs::BHandGraspPos::Request &req, wam_srvs::BHandGraspPos::Response &res);
+ bool
+ handSpreadPos(wam_srvs::BHandSpreadPos::Request &req, wam_srvs::BHandSpreadPos::Response &res);
+ bool
+ handFingerVel(wam_srvs::BHandFingerVel::Request &req, wam_srvs::BHandFingerVel::Response &res);
+ bool
+ handGraspVel(wam_srvs::BHandGraspVel::Request &req, wam_srvs::BHandGraspVel::Response &res);
+ bool
+ handSpreadVel(wam_srvs::BHandSpreadVel::Request &req, wam_srvs::BHandSpreadVel::Response &res);
+ void
+ cartVelCB(const wam_msgs::RTCartVel::ConstPtr& msg);
+ void
+ ortnVelCB(const wam_msgs::RTOrtnVel::ConstPtr& msg);
+ void
+ jntVelCB(const wam_msgs::RTJointVel::ConstPtr& msg);
+ void
+ jntPosCB(const wam_msgs::RTJointPos::ConstPtr& msg);
+ void
+ cartPosCB(const wam_msgs::RTCartPos::ConstPtr& msg);
+ void
+ publishWam(ProductManager& pm);
+ void
+ publishHand(void);
+ void
+ updateRT(ProductManager& pm);
+ };
+
+// Templated Initialization Function
+template<size_t DOF>
+ void WamNode<DOF>::init(ProductManager& pm)
+ {
+ ros::NodeHandle n_("wam"); // WAM specific nodehandle
+ ros::NodeHandle nh_("bhand"); // BarrettHand specific nodehandle
+
+ //Setting up real-time command timeouts and initial values
+ cart_vel_status = false; //Bool for determining cartesian velocity real-time state
+ ortn_vel_status = false; //Bool for determining orientation velocity real-time state
+ new_rt_cmd = false; //Bool for determining if a new real-time message was received
+ rt_msg_timeout.fromSec(0.3); //rt_status will be determined false if rt message is not received in specified time
+ cart_vel_mag = SPEED; //Setting default cartesian velocity magnitude to SPEED
+ ortn_vel_mag = SPEED;
+ pm.getExecutionManager()->startManaging(ramp); //starting ramp manager
+
+ ROS_INFO(" \n %zu-DOF WAM", DOF);
+ jp_home = wam.getJointPositions();
+
+ if (pm.foundHand()) //Does the following only if a BarrettHand is present
+ {
+ std::cout << "Barrett Hand" << std::endl;
+ hand = pm.getHand();
+
+ // 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
+
+ //Advertise the following services only if there is a BarrettHand present
+ hand_open_grsp_srv = nh_.advertiseService("open_grasp", &WamNode<DOF>::handOpenGrasp, this); // bhand/open_grasp
+ hand_close_grsp_srv = nh_.advertiseService("close_grasp", &WamNode::handCloseGrasp, this); // bhand/close_grasp
+ hand_open_sprd_srv = nh_.advertiseService("open_spread", &WamNode::handOpenSpread, this); // bhand/open_spread
+ hand_close_sprd_srv = nh_.advertiseService("close_spread", &WamNode::handCloseSpread, this); // bhand/close_spread
+ 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
+
+ //Set up the BarrettHand joint state publisher
+ const char* bhand_jnts[] = {"inner_f1", "inner_f2", "inner_f3", "spread", "outer_f1", "outer_f2", "outer_f3"};
+ std::vector < std::string > bhand_joints(bhand_jnts, bhand_jnts + 7);
+ bhand_joint_state.name.resize(7);
+ bhand_joint_state.name = bhand_joints;
+ bhand_joint_state.position.resize(7);
+ }
+
+ wam.gravityCompensate(true); // Turning on Gravity Compenstation by Default when starting the WAM Node
+
+ //Setting up WAM joint state publisher
+ const char* wam_jnts[] = {"wam_j1", "wam_j2", "wam_j3", "wam_j4", "wam_j5", "wam_j6", "wam_j7"};
+ std::vector < std::string > wam_joints(wam_jnts, wam_jnts + 7);
+ wam_joint_state.name = wam_joints;
+ wam_joint_state.name.resize(DOF);
+ wam_joint_state.position.resize(DOF);
+ wam_joint_state.velocity.resize(DOF);
+ wam_joint_state.effort.resize(DOF);
+
+ //Publishing the following rostopics
+ wam_joint_state_pub = n_.advertise < sensor_msgs::JointState > ("joint_states", 1); // wam/joint_states
+ wam_pose_pub = n_.advertise < geometry_msgs::PoseStamped > ("pose", 1); // wam/pose
+
+ //Subscribing to the following rostopics
+ cart_vel_sub = n_.subscribe("cart_vel_cmd", 1, &WamNode::cartVelCB, this); // wam/cart_vel_cmd
+ ortn_vel_sub = n_.subscribe("ortn_vel_cmd", 1, &WamNode::ortnVelCB, this); // wam/ortn_vel_cmd
+ jnt_vel_sub = n_.subscribe("jnt_vel_cmd", 1, &WamNode::jntVelCB, this); // wam/jnt_vel_cmd
+ jnt_pos_sub = n_.subscribe("jnt_pos_cmd", 1, &WamNode::jntPosCB, this); // wam/jnt_pos_cmd
+ cart_pos_sub = n_.subscribe("cart_pos_cmd", 1, &WamNode::cartPosCB, this); // wam/cart_pos_cmd
+
+ //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
+ 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
+
+ }
+
+// gravity_comp service callback
+template<size_t DOF>
+ bool WamNode<DOF>::gravity(wam_srvs::GravityComp::Request &req, wam_srvs::GravityComp::Response &res)
+ {
+ wam.gravityCompensate(req.gravity);
+ ROS_INFO("Gravity Compensation Request: %s", (req.gravity) ? "true" : "false");
+ return true;
+ }
+
+// goHome Function for sending the WAM safely back to its home starting position.
+template<size_t DOF>
+ 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);
+ jp_home[3] -= 0.3;
+ wam.moveTo(jp_home, true);
+ jp_home[3] += 0.3;
+ wam.moveTo(jp_home, true);
+ return true;
+ }
+
+//Function to hold WAM Joint Positions
+template<size_t DOF>
+ bool WamNode<DOF>::holdJPos(wam_srvs::Hold::Request &req, wam_srvs::Hold::Response &res)
+ {
+ ROS_INFO("Joint Position Hold request: %s", (req.hold) ? "true" : "false");
+
+ if (req.hold)
+ wam.moveTo(wam.getJointPositions());
+ else
+ wam.idle();
+ return true;
+ }
+
+//Function to hold WAM end effector Cartesian Position
+template<size_t DOF>
+ bool WamNode<DOF>::holdCPos(wam_srvs::Hold::Request &req, wam_srvs::Hold::Response &res)
+ {
+ ROS_INFO("Cartesian Position Hold request: %s", (req.hold) ? "true" : "false");
+
+ if (req.hold)
+ wam.moveTo(wam.getToolPosition());
+ else
+ wam.idle();
+ return true;
+ }
+
+//Function to hold WAM end effector Orientation
+template<size_t DOF>
+ bool WamNode<DOF>::holdOrtn(wam_srvs::Hold::Request &req, wam_srvs::Hold::Response &res)
+ {
+ ROS_INFO("Orientation Hold request: %s", (req.hold) ? "true" : "false");
+
+ if (req.hold)
+ {
+ orientationSetPoint.setValue(wam.getToolOrientation());
+ wam.trackReferenceSignal(orientationSetPoint.output);
+ }
+ else
+ wam.idle();
+ return true;
+ }
+
+//Function to command a joint space move to the WAM
+template<size_t DOF>
+ bool WamNode<DOF>::jointMove(wam_srvs::JointMove::Request &req, wam_srvs::JointMove::Response &res)
+ {
+ if (req.joints.size() != DOF)
+ {
+ ROS_INFO("Request Failed: %zu-DOF request received, must be %zu-DOF", req.joints.size(), DOF);
+ return false;
+ }
+ ROS_INFO("Moving Robot to Commanded Joint Pose");
+ for (size_t i = 0; i < DOF; i++)
+ jp_cmd[i] = req.joints[i];
+ wam.moveTo(jp_cmd, false);
+ return true;
+ }
+
+//Function to command a pose move to the WAM
+template<size_t DOF>
+ bool WamNode<DOF>::poseMove(wam_srvs::PoseMove::Request &req, wam_srvs::PoseMove::Response &res)
+ {
+ ROS_INFO("Moving Robot to Commanded Pose");
+
+ cp_cmd[0] = req.pose.position.x;
+ cp_cmd[1] = req.pose.position.y;
+ cp_cmd[2] = req.pose.position.z;
+ ortn_cmd.x() = req.pose.orientation.x;
+ ortn_cmd.y() = req.pose.orientation.y;
+ ortn_cmd.z() = req.pose.orientation.z;
+ ortn_cmd.w() = req.pose.orientation.w;
+
+ pose_cmd = boost::make_tuple(cp_cmd, ortn_cmd);
+
+ //wam.moveTo(pose_cmd, false); //(TODO:KM Update Libbarrett API for Pose Moves)
+ ROS_INFO("Pose Commands for WAM not yet supported by API");
+ return false;
+ }
+
+//Function to command a cartesian move to the WAM
+template<size_t DOF>
+ bool WamNode<DOF>::cartMove(wam_srvs::CartPosMove::Request &req, wam_srvs::CartPosMove::Response &res)
+ {
+ ROS_INFO("Moving Robot to Commanded Cartesian Position");
+
+ for (int i = 0; i < 3; i++)
+ cp_cmd[i] = req.position[i];
+ wam.moveTo(cp_cmd, false);
+ return true;
+ }
+
+//Function to command an orientation move to the WAM
+template<size_t DOF>
+ bool WamNode<DOF>::ortnMove(wam_srvs::OrtnMove::Request &req, wam_srvs::OrtnMove::Response &res)
+ {
+ ROS_INFO("Moving Robot to Commanded End Effector Orientation");
+
+ ortn_cmd.x() = req.orientation[0];
+ ortn_cmd.y() = req.orientation[1];
+ ortn_cmd.z() = req.orientation[2];
+ ortn_cmd.w() = req.orientation[3];
+
+ wam.moveTo(ortn_cmd, false);
+ return true;
+ }
+
+//Function to open the BarrettHand Grasp
+template<size_t DOF>
+ 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);
+ return true;
+ }
+
+//Function to close the BarrettHand Grasp
+template<size_t DOF>
+ 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);
+ return true;
+ }
+
+//Function to open the BarrettHand Spread
+template<size_t DOF>
+ 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);
+ return true;
+ }
+
+//Function to close the BarrettHand Spread
+template<size_t DOF>
+ 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);
+ return true;
+ }
+
+//Function to control a BarrettHand Finger Position
+template<size_t DOF>
+ bool WamNode<DOF>::handFingerPos(wam_srvs::BHandFingerPos::Request &req, wam_srvs::BHandFingerPos::Response &res)
+ {
+ 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);
+ return true;
+ }
+
+//Function to control the BarrettHand Grasp Position
+template<size_t DOF>
+ bool WamNode<DOF>::handGraspPos(wam_srvs::BHandGraspPos::Request &req, wam_srvs::BHandGraspPos::Response &res)
+ {
+ ROS_INFO("Moving BarrettHand Grasp: %.3f radians", req.radians);
+ hand->trapezoidalMove(Hand::jp_type(req.radians), Hand::GRASP, false);
+ return true;
+ }
+
+//Function to control the BarrettHand Spread Position
+template<size_t DOF>
+ bool WamNode<DOF>::handSpreadPos(wam_srvs::BHandSpreadPos::Request &req, wam_srvs::BHandSpreadPos::Response &res)
+ {
+ ROS_INFO("Moving BarrettHand Spread: %.3f radians", req.radians);
+ hand->trapezoidalMove(Hand::jp_type(req.radians), Hand::SPREAD, false);
+ return true;
+ }
+
+//Function to control a BarrettHand Finger Velocity
+template<size_t DOF>
+ bool WamNode<DOF>::handFingerVel(wam_srvs::BHandFingerVel::Request &req, wam_srvs::BHandFingerVel::Response &res)
+ {
+ 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);
+ return true;
+ }
+
+//Function to control a BarrettHand Grasp Velocity
+template<size_t DOF>
+ 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);
+ return true;
+ }
+
+//Function to control a BarrettHand Spread Velocity
+template<size_t DOF>
+ bool WamNode<DOF>::handSpreadVel(wam_srvs::BHandSpreadVel::Request &req, wam_srvs::BHandSpreadVel::Response &res)
+ {
+ ROS_INFO("Moving BarrettHand Spread: %.3f m/s", req.velocity);
+ usleep(5000);
+ hand->velocityMove(Hand::jv_type(req.velocity), Hand::SPREAD);
+ return true;
+ }
+
+//Callback function for RT Cartesian Velocity messages
+template<size_t DOF>
+ void WamNode<DOF>::cartVelCB(const wam_msgs::RTCartVel::ConstPtr& msg)
+ {
+ if (cart_vel_status)
+ {
+ for (size_t i = 0; i < 3; i++)
+ rt_cv_cmd[i] = msg->direction[i];
+ new_rt_cmd = true;
+ if (msg->magnitude != 0)
+ cart_vel_mag = msg->magnitude;
+ }
+ last_cart_vel_msg_time = ros::Time::now();
+
+ }
+
+//Callback function for RT Orientation Velocity Messages
+template<size_t DOF>
+ void WamNode<DOF>::ortnVelCB(const wam_msgs::RTOrtnVel::ConstPtr& msg)
+ {
+ if (ortn_vel_status)
+ {
+ for (size_t i = 0; i < 3; i++)
+ rt_ortn_cmd[i] = msg->angular[i];
+ new_rt_cmd = true;
+ if (msg->magnitude != 0)
+ ortn_vel_mag = msg->magnitude;
+ }
+ last_ortn_vel_msg_time = ros::Time::now();
+ }
+
+//Callback function for RT Joint Velocity Messages
+template<size_t DOF>
+ void WamNode<DOF>::jntVelCB(const wam_msgs::RTJointVel::ConstPtr& msg)
+ {
+ if (msg->velocities.size() != DOF)
+ {
+ ROS_INFO("Commanded Joint Velocities != DOF of WAM");
+ return;
+ }
+ if (jnt_vel_status)
+ {
+ for (size_t i = 0; i < DOF; i++)
+ rt_jv_cmd[i] = msg->velocities[i];
+ new_rt_cmd = true;
+ }
+ last_jnt_vel_msg_time = ros::Time::now();
+ }
+
+//Callback function for RT Joint Position Messages
+template<size_t DOF>
+ void WamNode<DOF>::jntPosCB(const wam_msgs::RTJointPos::ConstPtr& msg)
+ {
+ if (msg->joints.size() != DOF)
+ {
+ ROS_INFO("Commanded Joint Positions != DOF of WAM");
+ return;
+ }
+ if (jnt_pos_status)
+ {
+ for (size_t i = 0; i < DOF; i++)
+ {
+ rt_jp_cmd[i] = msg->joints[i];
+ rt_jp_rl[i] = msg->rate_limits[i];
+ }
+ new_rt_cmd = true;
+ }
+ last_jnt_pos_msg_time = ros::Time::now();
+ }
+
+//Callback function for RT Cartesian Position Messages
+template<size_t DOF>
+ void WamNode<DOF>::cartPosCB(const wam_msgs::RTCartPos::ConstPtr& msg)
+ {
+ if (cart_pos_status)
+ {
+ for (size_t i = 0; i < 3; i++)
+ {
+ rt_cp_cmd[i] = msg->position[i];
+ rt_cp_rl[i] = msg->rate_limits[i];
+ }
+ new_rt_cmd = true;
+ }
+ last_cart_pos_msg_time = ros::Time::now();
+ }
+
+//Function to update the WAM publisher
+template<size_t DOF>
+ void WamNode<DOF>::publishWam(ProductManager& pm)
+ {
+ //Current values to be published
+ jp_type jp = wam.getJointPositions();
+ jt_type jt = wam.getJointTorques();
+ jv_type jv = wam.getJointVelocities();
+ cp_type cp_pub = wam.getToolPosition();
+ Eigen::Quaterniond to_pub = wam.getToolOrientation();
+
+ //publishing sensor_msgs/JointState to wam/joint_states
+ for (size_t i = 0; i < DOF; i++)
+ {
+ wam_joint_state.position[i] = jp[i];
+ wam_joint_state.velocity[i] = jv[i];
+ wam_joint_state.effort[i] = jt[i];
+ }
+ wam_joint_state.header.stamp = ros::Time::now();
+ wam_joint_state_pub.publish(wam_joint_state);
+
+ //publishing geometry_msgs/PoseStamed to wam/pose
+ wam_pose.header.stamp = ros::Time::now();
+ wam_pose.pose.position.x = cp_pub[0];
+ wam_pose.pose.position.y = cp_pub[1];
+ wam_pose.pose.position.z = cp_pub[2];
+ wam_pose.pose.orientation.w = to_pub.w();
+ wam_pose.pose.orientation.x = to_pub.x();
+ wam_pose.pose.orientation.y = to_pub.y();
+ wam_pose.pose.orientation.z = to_pub.z();
+ wam_pose_pub.publish(wam_pose);
+ }
+
+//Function to update the real-time control loops
+template<size_t DOF>
+ void WamNode<DOF>::publishHand() //systems::PeriodicDataLogger<debug_tuple>& logger
+ {
+ while (ros::ok())
+ {
+ 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
+ }
+ }
+
+//Function to update the real-time control loops
+template<size_t DOF>
+ void WamNode<DOF>::updateRT(ProductManager& pm) //systems::PeriodicDataLogger<debug_tuple>& logger
+ {
+ //Real-Time Cartesian Velocity Control Portion
+ if (last_cart_vel_msg_time + rt_msg_timeout > ros::Time::now()) // checking if a cartesian velocity message has been published and if it is within timeout
+ {
+ if (!cart_vel_status)
+ {
+ cart_dir.setValue(cp_type(0.0, 0.0, 0.0)); // zeroing the cartesian direction
+ current_cart_pos.setValue(wam.getToolPosition()); // Initializing the cartesian position
+ current_ortn.setValue(wam.getToolOrientation()); // Initializing the orientation
+ systems::forceConnect(ramp.output, mult_linear.input1); // connecting the ramp to multiplier
+ systems::forceConnect(cart_dir.output, mult_linear.input2); // connecting the direction to the multiplier
+ systems::forceConnect(mult_linear.output, cart_pos_sum.getInput(0)); // adding the output of the multiplier
+ systems::forceConnect(current_cart_pos.output, cart_pos_sum.getInput(1)); // with the starting cartesian position offset
+ systems::forceConnect(cart_pos_sum.output, rt_pose_cmd.getInput<0>()); // saving summed position as new commanded pose.position
+ systems::forceConnect(current_ortn.output, rt_pose_cmd.getInput<1>()); // saving the original orientation to the pose.orientation
+ ramp.setSlope(cart_vel_mag); // setting the slope to the commanded magnitude
+ ramp.stop(); // ramp is stopped on startup
+ ramp.setOutput(0.0); // ramp is re-zeroed on startup
+ ramp.start(); // start the ramp
+ wam.trackReferenceSignal(rt_pose_cmd.output); // command WAM to track the RT commanded (500 Hz) updated pose
+ }
+ else if (new_rt_cmd)
+ {
+ ramp.reset(); // reset the ramp to 0
+ ramp.setSlope(cart_vel_mag);
+ cart_dir.setValue(rt_cv_cmd); // set our cartesian direction to subscribed command
+ current_cart_pos.setValue(wam.tpoTpController.referenceInput.getValue()); // updating the current position to the actual low level commanded value
+ }
+ cart_vel_status = true;
+ new_rt_cmd = false;
+ }
+
+ //Real-Time Angular Velocity Control Portion
+ else if (last_ortn_vel_msg_time + rt_msg_timeout > ros::Time::now()) // checking if a orientation velocity message has been published and if it is within timeout
+ {
+ if (!ortn_vel_status)
+ {
+ rpy_cmd.setValue(math::Vector<3>::type(0.0, 0.0, 0.0)); // zeroing the rpy command
+ current_cart_pos.setValue(wam.getToolPosition()); // Initializing the cartesian position
+ current_rpy_ortn.setValue(toRPY(wam.getToolOrientation())); // Initializing the orientation
+
+ systems::forceConnect(ramp.output, mult_angular.input1); // connecting the ramp to multiplier
+ systems::forceConnect(rpy_cmd.output, mult_angular.input2); // connecting the rpy command to the multiplier
+ systems::forceConnect(mult_angular.output, ortn_cmd_sum.getInput(0)); // adding the output of the multiplier
+ systems::forceConnect(current_rpy_ortn.output, ortn_cmd_sum.getInput(1)); // with the starting rpy orientation offset
+ systems::forceConnect(ortn_cmd_sum.output, to_quat.input);
+ systems::forceConnect(current_cart_pos.output, rt_pose_cmd.getInput<0>()); // saving the original position to the pose.position
+ systems::forceConnect(to_quat.output, rt_pose_cmd.getInput<1>()); // saving the summed and converted new quaternion commmand as the pose.orientation
+ ramp.setSlope(ortn_vel_mag); // setting the slope to the commanded magnitude
+ ramp.stop(); // ramp is stopped on startup
+ ramp.setOutput(0.0); // ramp is re-zeroed on startup
+ ramp.start(); // start the ramp
+ wam.trackReferenceSignal(rt_pose_cmd.output); // command the WAM to track the RT commanded up to (500 Hz) cartesian velocity
+ }
+ else if (new_rt_cmd)
+ {
+ ramp.reset(); // reset the ramp to 0
+ ramp.setSlope(ortn_vel_mag); // updating the commanded angular velocity magnitude
+ rpy_cmd.setValue(rt_ortn_cmd); // set our angular rpy command to subscribed command
+ current_rpy_ortn.setValue(toRPY(wam.tpoToController.referenceInput.getValue())); // updating the current orientation to the actual low level commanded value
+ }
+ ortn_vel_status = true;
+ new_rt_cmd = false;
+ }
+
+ //Real-Time Joint Velocity Control Portion
+ else if (last_jnt_vel_msg_time + rt_msg_timeout > ros::Time::now()) // checking if a joint velocity message has been published and if it is within timeout
+ {
+ if (!jnt_vel_status)
+ {
+ jv_type jv_start;
+ for (size_t i = 0; i < DOF; i++)
+ jv_start[i] = 0.0;
+ jv_track.setValue(jv_start); // zeroing the joint velocity command
+ wam.trackReferenceSignal(jv_track.output); // command the WAM to track the RT commanded up to (500 Hz) joint velocities
+ }
+ else if (new_rt_cmd)
+ {
+ jv_track.setValue(rt_jv_cmd); // set our joint velocity to subscribed command
+ }
+ jnt_vel_status = true;
+ new_rt_cmd = false;
+ }
+
+ //Real-Time Joint Position Control Portion
+ else if (last_jnt_pos_msg_time + rt_msg_timeout > ros::Time::now()) // checking if a joint position message has been published and if it is within timeout
+ {
+ if (!jnt_pos_status)
+ {
+ jp_type jp_start = wam.getJointPositions();
+ jp_track.setValue(jp_start); // setting initial the joint position command
+ jp_rl.setLimit(rt_jp_rl);
+ systems::forceConnect(jp_track.output, jp_rl.input);
+ wam.trackReferenceSignal(jp_rl.output); // command the WAM to track the RT commanded up to (500 Hz) joint positions
+ }
+ else if (new_rt_cmd)
+ {
+ jp_track.setValue(rt_jp_cmd); // set our joint position to subscribed command
+ jp_rl.setLimit(rt_jp_rl); // set our rate limit to subscribed rate to control the rate of the moves
+ }
+ jnt_pos_status = true;
+ new_rt_cmd = false;
+ }
+
+ //Real-Time Cartesian Position Control Portion
+ else if (last_cart_pos_msg_time + rt_msg_timeout > ros::Time::now()) // checking if a cartesian position message has been published and if it is within timeout
+ {
+ if (!cart_pos_status)
+ {
+ cp_track.setValue(wam.getToolPosition());
+ current_ortn.setValue(wam.getToolOrientation()); // Initializing the orientation
+ cp_rl.setLimit(rt_cp_rl);
+ systems::forceConnect(cp_track.output, cp_rl.input);
+ systems::forceConnect(cp_rl.output, rt_pose_cmd.getInput<0>()); // saving the rate limited cartesian position command to the pose.position
+ systems::forceConnect(current_ortn.output, rt_pose_cmd.getInput<1>()); // saving the original orientation to the pose.orientation
+ wam.trackReferenceSignal(rt_pose_cmd.output); //Commanding the WAM to track the real-time pose command.
+ }
+ else if (new_rt_cmd)
+ {
+ cp_track.setValue(rt_cp_cmd); // Set our cartesian positions to subscribed command
+ cp_rl.setLimit(rt_cp_rl); // Updating the rate limit to subscribed rate to control the rate of the moves
+ }
+ cart_pos_status = true;
+ new_rt_cmd = false;
+ }
+
+ //If we fall out of 'Real-Time', hold joint positions
+ else if (cart_vel_status | ortn_vel_status | jnt_vel_status | jnt_pos_status | cart_pos_status)
+ {
+ wam.moveTo(wam.getJointPositions()); // Holds current joint positions upon a RT message timeout
+ cart_vel_status = ortn_vel_status = jnt_vel_status = jnt_pos_status = cart_pos_status = ortn_pos_status = false;
+ }
+ }
+
+//wam_main Function
+template<size_t DOF>
+ int wam_main(int argc, char** argv, ProductManager& pm, systems::Wam<DOF>& wam)
+ {
+ BARRETT_UNITS_TEMPLATE_TYPEDEFS(DOF);
+ ros::init(argc, argv, "wam_node");
+ WamNode<DOF> wam_node(wam);
+ wam_node.init(pm);
+ ros::Rate pub_rate(PUBLISH_FREQ);
+
+ if (pm.getHand())
+ boost::thread handPubThread(&WamNode<DOF>::publishHand, &wam_node);
+
+ while (ros::ok() && pm.getSafetyModule()->getMode() == SafetyModule::ACTIVE)
+ {
+ ros::spinOnce();
+ wam_node.publishWam(pm);
+ wam_node.updateRT(pm);
+ pub_rate.sleep();
+ }
+
+ return 0;
+ }
--- /dev/null
+/*
+ Copyright 2012 Barrett Technology <support@barrett.com>
+
+ This file is part of barrett-ros-pkg.
+
+ This version of barrett-ros-pkg 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 barrett-ros-pkg 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 barrett-ros-pkg. If not, see
+ <http://www.gnu.org/licenses/>.
+
+ Barrett Technology holds all copyrights on barrett-ros-pkg. As the sole
+ copyright holder, Barrett reserves the right to release future versions
+ of barrett-ros-pkg under a different license.
+
+ File: wam_node.cpp
+ Date: 5 June, 2012
+ Author: Kyle Maroney
+ */
+
+#include <unistd.h>
+#include <math.h>
+
+#include <Eigen/Geometry>
+
+#include "ros/ros.h"
+#include "tf/transform_datatypes.h"
+
+#include "wam_msgs/RTJointPos.h"
+#include "wam_msgs/RTJointVel.h"
+#include "wam_msgs/RTCartPos.h"
+#include "wam_msgs/RTCartVel.h"
+#include "wam_msgs/RTOrtnPos.h"
+#include "wam_msgs/RTOrtnVel.h"
+#include "wam_srvs/GravityComp.h"
+#include "wam_srvs/Hold.h"
+#include "wam_srvs/JointMove.h"
+#include "wam_srvs/PoseMove.h"
+#include "wam_srvs/CartPosMove.h"
+#include "wam_srvs/OrtnMove.h"
+#include "wam_srvs/BHandFingerPos.h"
+#include "wam_srvs/BHandGraspPos.h"
+#include "wam_srvs/BHandSpreadPos.h"
+#include "wam_srvs/BHandFingerVel.h"
+#include "wam_srvs/BHandGraspVel.h"
+#include "wam_srvs/BHandSpreadVel.h"
+#include "std_srvs/Empty.h"
+#include "sensor_msgs/JointState.h"
+#include "geometry_msgs/PoseStamped.h"
+
+#include <wam_node_sim/rate_limiter.h>
+#include <wam_node_sim/wam.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
+
+//WamNode Class
+template<size_t DOF>
+ class WamNode
+ {
+ 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;
+// protected:
+ bool cart_vel_status, ortn_vel_status, jnt_vel_status;
+ bool jnt_pos_status, cart_pos_status, ortn_pos_status, new_rt_cmd;
+ double cart_vel_mag, ortn_vel_mag;
+ barrett::Wam<DOF> &wam;
+ jp_type jp, jp_cmd, jp_home;
+ jp_type rt_jp_cmd, rt_jp_rl;
+ jv_type rt_jv_cmd;
+ cp_type cp_cmd, rt_cv_cmd;
+ cp_type rt_cp_cmd, rt_cp_rl;
+
+ Eigen::Quaterniond ortn_cmd, rt_op_cmd, rt_op_rl;
+
+// pose_type pose_cmd;
+ Eigen::Vector3d rt_ortn_cmd;
+ wam_node_sim::RateLimiter<jp_type> jp_rl;
+ wam_node_sim::RateLimiter<cp_type> cp_rl;
+ Eigen::Quaterniond ortn_print;
+ ros::Time last_cart_vel_msg_time, last_ortn_vel_msg_time, last_jnt_vel_msg_time;
+ ros::Time last_jnt_pos_msg_time, last_cart_pos_msg_time, last_ortn_pos_msg_time;
+ ros::Duration rt_msg_timeout;
+
+ wam_msgs::RTCartVel cart_vel_cmd;
+ wam_msgs::RTOrtnVel ortn_vel_cmd;
+
+ ros::Subscriber cart_vel_sub;
+ ros::Subscriber ortn_vel_sub;
+ ros::Subscriber jnt_vel_sub;
+ ros::Subscriber jnt_pos_sub;
+ ros::Subscriber cart_pos_sub;
+ ros::Subscriber ortn_pos_sub;
+
+ sensor_msgs::JointState wam_joint_state;
+ sensor_msgs::JointState bhand_joint_state;
+ geometry_msgs::PoseStamped wam_pose;
+
+ ros::Publisher wam_joint_state_pub;
+ ros::Publisher bhand_joint_state_pub;
+ ros::Publisher wam_pose_pub;
+
+ ros::ServiceServer gravity_srv;
+ ros::ServiceServer go_home_srv;
+ ros::ServiceServer hold_jpos_srv;
+ ros::ServiceServer hold_cpos_srv;
+ ros::ServiceServer hold_ortn_srv;
+ ros::ServiceServer joint_move_srv;
+ ros::ServiceServer pose_move_srv;
+ ros::ServiceServer cart_move_srv;
+ ros::ServiceServer ortn_move_srv;
+ ros::ServiceServer hand_close_srv;
+ ros::ServiceServer hand_open_grsp_srv;
+ ros::ServiceServer hand_close_grsp_srv;
+ ros::ServiceServer hand_open_sprd_srv;
+ ros::ServiceServer hand_close_sprd_srv;
+ ros::ServiceServer hand_fngr_pos_srv;
+ ros::ServiceServer hand_fngr_vel_srv;
+ ros::ServiceServer hand_grsp_pos_srv;
+ ros::ServiceServer hand_grsp_vel_srv;
+ ros::ServiceServer hand_sprd_pos_srv;
+ ros::ServiceServer hand_sprd_vel_srv;
+
+ public:
+ WamNode(barrett::Wam<DOF> &wam_,bool foundHand);
+
+ ~WamNode(void) { }
+
+ bool gravity(wam_srvs::GravityComp::Request &req,wam_srvs::GravityComp::Response &res);
+ bool goHome(std_srvs::Empty::Request &req,std_srvs::Empty::Response &res);
+ bool holdJPos(wam_srvs::Hold::Request &req,wam_srvs::Hold::Response &res);
+ bool holdCPos(wam_srvs::Hold::Request &req,wam_srvs::Hold::Response &res);
+ bool holdOrtn(wam_srvs::Hold::Request &req,wam_srvs::Hold::Response &res);
+ bool jointMove(wam_srvs::JointMove::Request &req,wam_srvs::JointMove::Response &res);
+ bool poseMove(wam_srvs::PoseMove::Request &req,wam_srvs::PoseMove::Response &res);
+ bool cartMove(wam_srvs::CartPosMove::Request &req,wam_srvs::CartPosMove::Response &res);
+ bool ortnMove(wam_srvs::OrtnMove::Request &req,wam_srvs::OrtnMove::Response &res);
+ bool handOpenGrasp(std_srvs::Empty::Request &req,std_srvs::Empty::Response &res);
+ bool handCloseGrasp(std_srvs::Empty::Request &req,std_srvs::Empty::Response &res);
+ bool handOpenSpread(std_srvs::Empty::Request &req,std_srvs::Empty::Response &res);
+ bool handCloseSpread(std_srvs::Empty::Request &req,std_srvs::Empty::Response &res);
+ bool handFingerPos(wam_srvs::BHandFingerPos::Request &req,wam_srvs::BHandFingerPos::Response &res);
+ bool handGraspPos(wam_srvs::BHandGraspPos::Request &req,wam_srvs::BHandGraspPos::Response &res);
+ bool handSpreadPos(wam_srvs::BHandSpreadPos::Request &req,wam_srvs::BHandSpreadPos::Response &res);
+ bool handFingerVel(wam_srvs::BHandFingerVel::Request &req,wam_srvs::BHandFingerVel::Response &res);
+ bool handGraspVel(wam_srvs::BHandGraspVel::Request &req,wam_srvs::BHandGraspVel::Response &res);
+ bool handSpreadVel(wam_srvs::BHandSpreadVel::Request &req,wam_srvs::BHandSpreadVel::Response &res);
+
+ void cartVelCB(const wam_msgs::RTCartVel::ConstPtr& msg);
+ void ortnVelCB(const wam_msgs::RTOrtnVel::ConstPtr& msg);
+ void jntVelCB(const wam_msgs::RTJointVel::ConstPtr& msg);
+ void jntPosCB(const wam_msgs::RTJointPos::ConstPtr& msg);
+ void cartPosCB(const wam_msgs::RTCartPos::ConstPtr& msg);
+ void publishWam(void);
+ void publishHand(void);
+ void updateRT(void);
+ };
+
+template<size_t DOF>
+ WamNode<DOF>::WamNode(barrett::Wam<DOF> &wam_,bool foundHand): wam(wam_)
+ {
+ ros::NodeHandle n_("wam"); // WAM specific nodehandle
+ ros::NodeHandle nh_("bhand"); // BarrettHand specific nodehandle
+
+ //Setting up real-time command timeouts and initial values
+ cart_vel_status = false; //Bool for determining cartesian velocity real-time state
+ ortn_vel_status = false; //Bool for determining orientation velocity real-time state
+ new_rt_cmd = false; //Bool for determining if a new real-time message was received
+ rt_msg_timeout.fromSec(0.3); //rt_status will be determined false if rt message is not received in specified time
+ cart_vel_mag = SPEED; //Setting default cartesian velocity magnitude to SPEED
+ ortn_vel_mag = SPEED;
+
+ ROS_INFO(" \n %zu-DOF WAM", DOF);
+ jp_home = wam.getJointPositions();
+
+ if (foundHand) //Does the following only if a BarrettHand is present
+ {
+ std::cout << "Barrett Hand" << std::endl;
+
+ //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
+
+ //Advertise the following services only if there is a BarrettHand present
+ hand_open_grsp_srv = nh_.advertiseService("open_grasp", &WamNode::handOpenGrasp, this); // bhand/open_grasp
+ hand_close_grsp_srv = nh_.advertiseService("close_grasp", &WamNode::handCloseGrasp, this); // bhand/close_grasp
+ hand_open_sprd_srv = nh_.advertiseService("open_spread", &WamNode::handOpenSpread, this); // bhand/open_spread
+ hand_close_sprd_srv = nh_.advertiseService("close_spread", &WamNode::handCloseSpread, this); // bhand/close_spread
+ 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
+
+ //Set up the BarrettHand joint state publisher
+ const char* bhand_jnts[] = {"inner_f1", "inner_f2", "inner_f3", "spread", "outer_f1", "outer_f2", "outer_f3"};
+ std::vector < std::string > bhand_joints(bhand_jnts, bhand_jnts + 7);
+ bhand_joint_state.name.resize(7);
+ bhand_joint_state.name = bhand_joints;
+ bhand_joint_state.position.resize(7);
+ }
+
+ wam.gravityCompensate(true); // Turning on Gravity Compenstation by Default when starting the WAM Node
+
+ //Setting up WAM joint state publisher
+ const char* wam_jnts[] = {"wam_j1", "wam_j2", "wam_j3", "wam_j4", "wam_j5", "wam_j6", "wam_j7"};
+ std::vector < std::string > wam_joints(wam_jnts, wam_jnts + 7);
+ wam_joint_state.name = wam_joints;
+ wam_joint_state.name.resize(DOF);
+ wam_joint_state.position.resize(DOF);
+ wam_joint_state.velocity.resize(DOF);
+ wam_joint_state.effort.resize(DOF);
+
+ //Publishing the following rostopics
+ wam_joint_state_pub = n_.advertise < sensor_msgs::JointState > ("joint_states", 1); // wam/joint_states
+ wam_pose_pub = n_.advertise < geometry_msgs::PoseStamped > ("pose", 1); // wam/pose
+
+ //Subscribing to the following rostopics
+// cart_vel_sub = n_.subscribe("cart_vel_cmd", 1, &WamNode::cartVelCB, this); // wam/cart_vel_cmd
+// ortn_vel_sub = n_.subscribe("ortn_vel_cmd", 1, &WamNode::ortnVelCB, this); // wam/ortn_vel_cmd
+// jnt_vel_sub = n_.subscribe("jnt_vel_cmd", 1, &WamNode::jntVelCB, this); // wam/jnt_vel_cmd
+ jnt_pos_sub = n_.subscribe("jnt_pos_cmd", 1, &WamNode::jntPosCB, this); // wam/jnt_pos_cmd
+// cart_pos_sub = n_.subscribe("cart_pos_cmd", 1, &WamNode::cartPosCB, this); // wam/cart_pos_cmd
+
+
+ //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
+ 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
+
+ }
+
+
+// gravity_comp service callback
+template<size_t DOF>
+ bool WamNode<DOF>::gravity(wam_srvs::GravityComp::Request &req, wam_srvs::GravityComp::Response &res)
+ {
+ wam.gravityCompensate(req.gravity);
+ ROS_INFO("Gravity Compensation Request: %s", (req.gravity) ? "true" : "false");
+ return true;
+ }
+
+// goHome Function for sending the WAM safely back to its home starting position.
+template<size_t DOF>
+ 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);
+ jp_home[3] -= 0.3;
+ wam.moveTo(jp_home, true);
+ jp_home[3] += 0.3;
+ wam.moveTo(jp_home, true);
+
+ return true;
+ }
+
+//Function to hold WAM Joint Positions
+template<size_t DOF>
+ bool WamNode<DOF>::holdJPos(wam_srvs::Hold::Request &req, wam_srvs::Hold::Response &res)
+ {
+ ROS_INFO("Joint Position Hold request: %s", (req.hold) ? "true" : "false");
+
+ if (req.hold)
+ wam.moveTo(wam.getJointPositions());
+ else
+ wam.idle();
+
+ return true;
+ }
+
+//Function to hold WAM end effector Cartesian Position
+template<size_t DOF>
+ bool WamNode<DOF>::holdCPos(wam_srvs::Hold::Request &req, wam_srvs::Hold::Response &res)
+ {
+ ROS_INFO("Cartesian Position Hold request: %s", (req.hold) ? "true" : "false");
+
+ if (req.hold)
+ wam.moveTo(wam.getToolPosition());
+ else
+ wam.idle();
+
+ return true;
+ }
+
+//Function to hold WAM end effector Orientation
+template<size_t DOF>
+ bool WamNode<DOF>::holdOrtn(wam_srvs::Hold::Request &req, wam_srvs::Hold::Response &res)
+ {
+ ROS_INFO("Orientation Hold request: %s", (req.hold) ? "true" : "false");
+/*
+ if (req.hold)
+ {
+ orientationSetPoint.setValue(wam.getToolOrientation());
+ wam.trackReferenceSignal(orientationSetPoint.output);
+ }
+ else
+ wam.idle();
+*/
+ return true;
+ }
+
+//Function to command a joint space move to the WAM
+template<size_t DOF>
+ bool WamNode<DOF>::jointMove(wam_srvs::JointMove::Request &req, wam_srvs::JointMove::Response &res)
+ {
+ if (req.joints.size() != DOF)
+ {
+ ROS_INFO("Request Failed: %zu-DOF request received, must be %zu-DOF", req.joints.size(), DOF);
+ return false;
+ }
+ ROS_INFO("Moving Robot to Commanded Joint Pose");
+ for (size_t i = 0; i < DOF; i++)
+ jp_cmd[i] = req.joints[i];
+ wam.moveTo(jp_cmd, false);
+
+ return true;
+ }
+
+//Function to command a pose move to the WAM
+template<size_t DOF>
+ bool WamNode<DOF>::poseMove(wam_srvs::PoseMove::Request &req, wam_srvs::PoseMove::Response &res)
+ {
+ ROS_INFO("Moving Robot to Commanded Pose");
+/*
+ cp_cmd[0] = req.pose.position.x;
+ cp_cmd[1] = req.pose.position.y;
+ cp_cmd[2] = req.pose.position.z;
+ ortn_cmd.x() = req.pose.orientation.x;
+ ortn_cmd.y() = req.pose.orientation.y;
+ ortn_cmd.z() = req.pose.orientation.z;
+ ortn_cmd.w() = req.pose.orientation.w;
+
+ pose_cmd = boost::make_tuple(cp_cmd, ortn_cmd);
+*/
+ //wam.moveTo(pose_cmd, false); //(TODO:KM Update Libbarrett API for Pose Moves)
+ ROS_INFO("Pose Commands for WAM not yet supported by API");
+ return false;
+ }
+
+//Function to command a cartesian move to the WAM
+template<size_t DOF>
+ bool WamNode<DOF>::cartMove(wam_srvs::CartPosMove::Request &req, wam_srvs::CartPosMove::Response &res)
+ {
+ ROS_INFO("Moving Robot to Commanded Cartesian Position");
+
+ for (int i = 0; i < 3; i++)
+ cp_cmd[i] = req.position[i];
+ wam.moveTo(cp_cmd, false);
+
+ return true;
+ }
+
+//Function to command an orientation move to the WAM
+template<size_t DOF>
+ bool WamNode<DOF>::ortnMove(wam_srvs::OrtnMove::Request &req, wam_srvs::OrtnMove::Response &res)
+ {
+ ROS_INFO("Moving Robot to Commanded End Effector Orientation");
+
+ ortn_cmd.x() = req.orientation[0];
+ ortn_cmd.y() = req.orientation[1];
+ ortn_cmd.z() = req.orientation[2];
+ ortn_cmd.w() = req.orientation[3];
+
+ wam.moveTo(ortn_cmd, false);
+ return true;
+ }
+
+//Function to open the BarrettHand Grasp
+template<size_t DOF>
+ 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);
+ return true;
+ }
+
+//Function to close the BarrettHand Grasp
+template<size_t DOF>
+ 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);
+ return true;
+ }
+
+//Function to open the BarrettHand Spread
+template<size_t DOF>
+ 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);
+ return true;
+ }
+
+//Function to close the BarrettHand Spread
+template<size_t DOF>
+ 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);
+ return true;
+ }
+
+//Function to control a BarrettHand Finger Position
+template<size_t DOF>
+ bool WamNode<DOF>::handFingerPos(wam_srvs::BHandFingerPos::Request &req, wam_srvs::BHandFingerPos::Response &res)
+ {
+ 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);
+ return true;
+ }
+
+//Function to control the BarrettHand Grasp Position
+template<size_t DOF>
+ bool WamNode<DOF>::handGraspPos(wam_srvs::BHandGraspPos::Request &req, wam_srvs::BHandGraspPos::Response &res)
+ {
+ ROS_INFO("Moving BarrettHand Grasp: %.3f radians", req.radians);
+// hand->trapezoidalMove(Hand::jp_type(req.radians), Hand::GRASP, false);
+ return true;
+ }
+
+//Function to control the BarrettHand Spread Position
+template<size_t DOF>
+ bool WamNode<DOF>::handSpreadPos(wam_srvs::BHandSpreadPos::Request &req, wam_srvs::BHandSpreadPos::Response &res)
+ {
+ ROS_INFO("Moving BarrettHand Spread: %.3f radians", req.radians);
+// hand->trapezoidalMove(Hand::jp_type(req.radians), Hand::SPREAD, false);
+ return true;
+ }
+
+//Function to control a BarrettHand Finger Velocity
+template<size_t DOF>
+ bool WamNode<DOF>::handFingerVel(wam_srvs::BHandFingerVel::Request &req, wam_srvs::BHandFingerVel::Response &res)
+ {
+ 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);
+ return true;
+ }
+
+//Function to control a BarrettHand Grasp Velocity
+template<size_t DOF>
+ 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);
+ return true;
+ }
+
+//Function to control a BarrettHand Spread Velocity
+template<size_t DOF>
+ bool WamNode<DOF>::handSpreadVel(wam_srvs::BHandSpreadVel::Request &req, wam_srvs::BHandSpreadVel::Response &res)
+ {
+ ROS_INFO("Moving BarrettHand Spread: %.3f m/s", req.velocity);
+ usleep(5000);
+// hand->velocityMove(Hand::jv_type(req.velocity), Hand::SPREAD);
+ return true;
+ }
+
+//Callback function for RT Cartesian Velocity messages
+template<size_t DOF>
+ void WamNode<DOF>::cartVelCB(const wam_msgs::RTCartVel::ConstPtr& msg)
+ {
+ if (cart_vel_status)
+ {
+ for (size_t i = 0; i < 3; i++)
+ rt_cv_cmd[i] = msg->direction[i];
+ new_rt_cmd = true;
+ if (msg->magnitude != 0)
+ cart_vel_mag = msg->magnitude;
+ }
+ last_cart_vel_msg_time = ros::Time::now();
+
+ }
+
+//Callback function for RT Orientation Velocity Messages
+template<size_t DOF>
+ void WamNode<DOF>::ortnVelCB(const wam_msgs::RTOrtnVel::ConstPtr& msg)
+ {
+ if (ortn_vel_status)
+ {
+ for (size_t i = 0; i < 3; i++)
+ rt_ortn_cmd[i] = msg->angular[i];
+ new_rt_cmd = true;
+ if (msg->magnitude != 0)
+ ortn_vel_mag = msg->magnitude;
+ }
+ last_ortn_vel_msg_time = ros::Time::now();
+ }
+
+//Callback function for RT Joint Velocity Messages
+template<size_t DOF>
+ void WamNode<DOF>::jntVelCB(const wam_msgs::RTJointVel::ConstPtr& msg)
+ {
+ if (msg->velocities.size() != DOF)
+ {
+ ROS_INFO("Commanded Joint Velocities != DOF of WAM");
+ return;
+ }
+ if (jnt_vel_status)
+ {
+ for (size_t i = 0; i < DOF; i++)
+ rt_jv_cmd[i] = msg->velocities[i];
+ new_rt_cmd = true;
+ }
+ last_jnt_vel_msg_time = ros::Time::now();
+ }
+
+//Callback function for RT Joint Position Messages
+template<size_t DOF>
+ void WamNode<DOF>::jntPosCB(const wam_msgs::RTJointPos::ConstPtr& msg)
+ {
+ if (msg->joints.size() != DOF)
+ {
+ ROS_INFO("Commanded Joint Positions != DOF of WAM");
+ return;
+ }
+ if (jnt_pos_status)
+ {
+ for (size_t i = 0; i < DOF; i++)
+ {
+ rt_jp_cmd[i] = msg->joints[i];
+ rt_jp_rl[i] = msg->rate_limits[i];
+ }
+ new_rt_cmd = true;
+ }
+ last_jnt_pos_msg_time = ros::Time::now();
+ }
+
+//Callback function for RT Cartesian Position Messages
+template<size_t DOF>
+ void WamNode<DOF>::cartPosCB(const wam_msgs::RTCartPos::ConstPtr& msg)
+ {
+ if (cart_pos_status)
+ {
+ for (size_t i = 0; i < 3; i++)
+ {
+ rt_cp_cmd[i] = msg->position[i];
+ rt_cp_rl[i] = msg->rate_limits[i];
+ }
+ new_rt_cmd = true;
+ }
+ last_cart_pos_msg_time = ros::Time::now();
+ }
+
+//Function to update the WAM publisher
+template<size_t DOF>
+ void WamNode<DOF>::publishWam(void)
+ {
+
+ //Current values to be published
+ jp_type jp = wam.getJointPositions();
+ jt_type jt = wam.getJointTorques();
+ jv_type jv = wam.getJointVelocities();
+ cp_type cp_pub = wam.getToolPosition();
+// Eigen::Quaterniond to_pub = wam.getToolOrientation();
+
+ //publishing sensor_msgs/JointState to wam/joint_states
+ for (size_t i = 0; i < DOF; i++)
+ {
+ wam_joint_state.position[i] = jp[i];
+ wam_joint_state.velocity[i] = jv[i];
+ wam_joint_state.effort[i] = jt[i];
+ }
+
+ wam_joint_state.header.stamp = ros::Time::now();
+ wam_joint_state_pub.publish(wam_joint_state);
+
+ //publishing geometry_msgs/PoseStamed to wam/pose
+ wam_pose.header.stamp = ros::Time::now();
+/*
+ wam_pose.pose.position.x = cp_pub[0];
+ wam_pose.pose.position.y = cp_pub[1];
+ wam_pose.pose.position.z = cp_pub[2];
+ wam_pose.pose.orientation.w = to_pub.w();
+ wam_pose.pose.orientation.x = to_pub.x();
+ wam_pose.pose.orientation.y = to_pub.y();
+ wam_pose.pose.orientation.z = to_pub.z();
+*/
+ wam_pose_pub.publish(wam_pose);
+ }
+
+//Function to update the real-time control loops
+template<size_t DOF>
+ void WamNode<DOF>::publishHand() //systems::PeriodicDataLogger<debug_tuple>& logger
+ {
+ while (ros::ok())
+ {
+/* 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
+ }
+ }
+
+//Function to update the real-time control loops
+template<size_t DOF>
+ void WamNode<DOF>::updateRT(void) //systems::PeriodicDataLogger<debug_tuple>& logger
+ {
+ //Real-Time Cartesian Velocity Control Portion
+ if (last_cart_vel_msg_time + rt_msg_timeout > ros::Time::now()) // checking if a cartesian velocity message has been published and if it is within timeout
+ {
+ if (!cart_vel_status)
+ {
+/*
+ cart_dir.setValue(cp_type(0.0, 0.0, 0.0)); // zeroing the cartesian direction
+ current_cart_pos.setValue(wam.getToolPosition()); // Initializing the cartesian position
+ current_ortn.setValue(wam.getToolOrientation()); // Initializing the orientation
+ systems::forceConnect(ramp.output, mult_linear.input1); // connecting the ramp to multiplier
+ systems::forceConnect(cart_dir.output, mult_linear.input2); // connecting the direction to the multiplier
+ systems::forceConnect(mult_linear.output, cart_pos_sum.getInput(0)); // adding the output of the multiplier
+ systems::forceConnect(current_cart_pos.output, cart_pos_sum.getInput(1)); // with the starting cartesian position offset
+ systems::forceConnect(cart_pos_sum.output, rt_pose_cmd.getInput<0>()); // saving summed position as new commanded pose.position
+ systems::forceConnect(current_ortn.output, rt_pose_cmd.getInput<1>()); // saving the original orientation to the pose.orientation
+ ramp.setSlope(cart_vel_mag); // setting the slope to the commanded magnitude
+ ramp.stop(); // ramp is stopped on startup
+ ramp.setOutput(0.0); // ramp is re-zeroed on startup
+ ramp.start(); // start the ramp
+ wam.trackReferenceSignal(rt_pose_cmd.output); // command WAM to track the RT commanded (500 Hz) updated pose
+*/
+ }
+ else if (new_rt_cmd)
+ {
+/*
+ ramp.reset(); // reset the ramp to 0
+ ramp.setSlope(cart_vel_mag);
+ cart_dir.setValue(rt_cv_cmd); // set our cartesian direction to subscribed command
+ current_cart_pos.setValue(wam.tpoTpController.referenceInput.getValue()); // updating the current position to the actual low level commanded value
+*/
+ }
+ cart_vel_status = true;
+ new_rt_cmd = false;
+ }
+
+ //Real-Time Angular Velocity Control Portion
+ else if (last_ortn_vel_msg_time + rt_msg_timeout > ros::Time::now()) // checking if a orientation velocity message has been published and if it is within timeout
+ {
+ if (!ortn_vel_status)
+ {
+/* rpy_cmd.setValue(math::Vector<3>::type(0.0, 0.0, 0.0)); // zeroing the rpy command
+ current_cart_pos.setValue(wam.getToolPosition()); // Initializing the cartesian position
+ current_rpy_ortn.setValue(toRPY(wam.getToolOrientation())); // Initializing the orientation
+
+ systems::forceConnect(ramp.output, mult_angular.input1); // connecting the ramp to multiplier
+ systems::forceConnect(rpy_cmd.output, mult_angular.input2); // connecting the rpy command to the multiplier
+ systems::forceConnect(mult_angular.output, ortn_cmd_sum.getInput(0)); // adding the output of the multiplier
+ systems::forceConnect(current_rpy_ortn.output, ortn_cmd_sum.getInput(1)); // with the starting rpy orientation offset
+ systems::forceConnect(ortn_cmd_sum.output, to_quat.input);
+ systems::forceConnect(current_cart_pos.output, rt_pose_cmd.getInput<0>()); // saving the original position to the pose.position
+ systems::forceConnect(to_quat.output, rt_pose_cmd.getInput<1>()); // saving the summed and converted new quaternion commmand as the pose.orientation
+ ramp.setSlope(ortn_vel_mag); // setting the slope to the commanded magnitude
+ ramp.stop(); // ramp is stopped on startup
+ ramp.setOutput(0.0); // ramp is re-zeroed on startup
+ ramp.start(); // start the ramp
+ wam.trackReferenceSignal(rt_pose_cmd.output); // command the WAM to track the RT commanded up to (500 Hz) cartesian velocity
+*/
+ }
+ else if (new_rt_cmd)
+ {
+/*
+ ramp.reset(); // reset the ramp to 0
+ ramp.setSlope(ortn_vel_mag); // updating the commanded angular velocity magnitude
+ rpy_cmd.setValue(rt_ortn_cmd); // set our angular rpy command to subscribed command
+ current_rpy_ortn.setValue(toRPY(wam.tpoToController.referenceInput.getValue())); // updating the current orientation to the actual low level commanded value
+*/
+ }
+ ortn_vel_status = true;
+ new_rt_cmd = false;
+ }
+
+ //Real-Time Joint Velocity Control Portion
+ else if (last_jnt_vel_msg_time + rt_msg_timeout > ros::Time::now()) // checking if a joint velocity message has been published and if it is within timeout
+ {
+ if (!jnt_vel_status)
+ {
+/*
+ jv_type jv_start;
+ for (size_t i = 0; i < DOF; i++)
+ jv_start[i] = 0.0;
+ jv_track.setValue(jv_start); // zeroing the joint velocity command
+ wam.trackReferenceSignal(jv_track.output); // command the WAM to track the RT commanded up to (500 Hz) joint velocities
+*/
+ }
+ else if (new_rt_cmd)
+ {
+// jv_track.setValue(rt_jv_cmd); // set our joint velocity to subscribed command
+ }
+ jnt_vel_status = true;
+ new_rt_cmd = false;
+ }
+
+ //Real-Time Joint Position Control Portion
+ else if (last_jnt_pos_msg_time + rt_msg_timeout > ros::Time::now()) // checking if a joint position message has been published and if it is within timeout
+ {
+ if (!jnt_pos_status)
+ {
+ jp_type jp_start = wam.getJointPositions();
+ jp_rl.setCurVal(jp_start); // setting initial the joint position command
+ jp_rl.setLimit(rt_jp_rl);
+ wam.trackReferenceSignal(jp_rl.getLimit(jp_start)); // command the WAM to track the RT commanded up to (500 Hz) joint positions
+ }
+ else
+ {
+ if(new_rt_cmd) jp_rl.setLimit(rt_jp_rl); // set our rate limit to subscribed rate to control the rate of the moves
+ wam.trackReferenceSignal(jp_rl.getLimit(rt_jp_cmd));
+ }
+ jnt_pos_status = true;
+ new_rt_cmd = false;
+ }
+
+ //Real-Time Cartesian Position Control Portion
+ else if (last_cart_pos_msg_time + rt_msg_timeout > ros::Time::now()) // checking if a cartesian position message has been published and if it is within timeout
+ {
+ if (!cart_pos_status)
+ {
+/*
+ cp_track.setValue(wam.getToolPosition());
+ current_ortn.setValue(wam.getToolOrientation()); // Initializing the orientation
+ cp_rl.setLimit(rt_cp_rl);
+ systems::forceConnect(cp_track.output, cp_rl.input);
+ systems::forceConnect(cp_rl.output, rt_pose_cmd.getInput<0>()); // saving the rate limited cartesian position command to the pose.position
+ systems::forceConnect(current_ortn.output, rt_pose_cmd.getInput<1>()); // saving the original orientation to the pose.orientation
+ wam.trackReferenceSignal(rt_pose_cmd.output); //Commanding the WAM to track the real-time pose command.
+*/
+ }
+ else if (new_rt_cmd)
+ {
+// cp_track.setValue(rt_cp_cmd); // Set our cartesian positions to subscribed command
+// cp_rl.setLimit(rt_cp_rl); // Updating the rate limit to subscribed rate to control the rate of the moves
+ }
+ cart_pos_status = true;
+ new_rt_cmd = false;
+ }
+
+ //If we fall out of 'Real-Time', hold joint positions
+ else if (cart_vel_status | ortn_vel_status | jnt_vel_status | jnt_pos_status | cart_pos_status)
+ {
+ wam.moveTo(wam.getJointPositions()); // Holds current joint positions upon a RT message timeout
+ cart_vel_status = ortn_vel_status = jnt_vel_status = jnt_pos_status = cart_pos_status = ortn_pos_status = false;
+ }
+ }
+
+
+int main(int argc,char *argv[])
+{
+ ros::init(argc,argv,"wam_node_sim");
+
+ barrett::Wam<7> wam;
+ WamNode<7> wam_node(wam,true);
+
+ ros::Rate loop(PUBLISH_FREQ);
+
+ while (ros::ok())
+ {
+ ros::spinOnce();
+ wam_node.publishWam();
+ wam_node.updateRT();
+ loop.sleep();
+ }
+
+ return 0;
+}
projects / ufrgs_wam.git / commitdiff