--- /dev/null
+#include <math.h>
+#include <ros/ros.h>
+
+#include "geometry_msgs/PoseStamped.h"
+#include "sensor_msgs/Joy.h"
+#include "std_srvs/Empty.h"
+
+#include "wam_msgs/RTCartVel.h"
+#include "wam_msgs/RTOrtnVel.h"
+#include "wam_srvs/GravityComp.h"
+#include "wam_srvs/Hold.h"
+#include "wam_srvs/BHandGraspVel.h"
+#include "wam_srvs/BHandSpreadVel.h"
+
+const int CNTRL_FREQ = 50; // Frequency at which we will publish our control messages.
+
+//WamTeleop Class
+class WamTeleop
+{
+public:
+ ros::NodeHandle n_, nw_, nh_; // NodeHandles for publishing / subscribing on topics "/... & /wam/..." & "/bhand/..."
+
+ // Boolean statuses for commanded states
+ bool pose_pubbed, grsp_publish, sprd_publish;
+ bool cart_publish, ortn_publish, home_publish;
+ bool hold_publish, home_st, hold_st, ortn_mode;
+
+ // Integer status for BarrettHand commanded state
+ int bh_cmd_st;
+
+ //variables to describe buttons on Joystick and their assignments
+ int deadman_btn, guardian_deadman_btn;
+ int gpr_open_btn, gpr_close_btn;
+ int sprd_open_btn, sprd_close_btn;
+ int ortn_btn, home_btn, hold_btn;
+ int axis_x, axis_y, axis_z;
+ int axis_r, axis_p, axis_yaw;
+
+ //variables to describe velocity commands
+ double max_grsp_vel, max_sprd_vel;
+ double cart_mag, ortn_mag;
+ double req_xdir, req_ydir, req_zdir;
+ double req_rdir, req_pdir, req_yawdir;
+ double bh_grsp_vel, bh_sprd_vel;
+
+ //Subscribers
+ ros::Subscriber joy_sub;
+
+ //Services
+ wam_srvs::BHandGraspVel grasp_vel;
+ wam_srvs::BHandSpreadVel spread_vel;
+ wam_srvs::Hold hold;
+ std_srvs::Empty go_home;
+
+ //Service Clients
+ ros::ServiceClient grasp_vel_srv, spread_vel_srv, go_home_srv;
+ ros::ServiceClient hold_srv;
+
+ //Messages
+ wam_msgs::RTCartVel cart_vel;
+ wam_msgs::RTOrtnVel ortn_vel;
+
+ //Publishers
+ ros::Publisher cart_vel_pub, ortn_vel_pub;
+
+ WamTeleop() :
+ nw_("wam"), nh_("bhand") // Name our nodehandle "wam" to preceed our messages/services
+ {
+ }
+
+ void init();
+ void joyCallback(const sensor_msgs::Joy::ConstPtr& joy_msg);
+ void update();
+
+ ~WamTeleop()
+ {
+ }
+};
+
+// WAM Teleoperation Initialization Function
+void WamTeleop::init()
+{
+ // We will check the parameter server for WAM Teleoperation configuration variables
+ n_.param("deadman_button", deadman_btn, 10);
+ n_.param("guardian_deadman_button", guardian_deadman_btn, 11);
+ n_.param("gripper_open_button", gpr_open_btn, 12);
+ n_.param("gripper_close_button", gpr_close_btn, 14);
+ n_.param("spread_open_button", sprd_open_btn, 13);
+ n_.param("spread_close_button", sprd_close_btn, 15);
+ n_.param("orientation_control_button", ortn_btn, 8);
+ n_.param("go_home_button", home_btn, 0);
+ n_.param("hold_joints_button", hold_btn, 3);
+ n_.param("grasp_max_velocity", max_grsp_vel, 1.0);
+ n_.param("spread_max_velocity", max_sprd_vel, 1.0);
+ n_.param("cartesian_magnitude", cart_mag, 0.05);
+ n_.param("orientation_magnitude", ortn_mag, 1.0);
+ n_.param("cartesian_x_axis", axis_x, 3);
+ n_.param("cartesian_y_axis", axis_y, 2);
+ n_.param("cartesian_x_axis", axis_z, 1);
+ n_.param("orientation_roll_axis", axis_r, 3);
+ n_.param("orientation_pitch_axis", axis_p, 2);
+ n_.param("orientation_yaw_axis", axis_yaw, 1);
+
+ hold.request.hold = false; // Default Start for joint hold command is false
+ cart_publish = ortn_publish = false; // Setting publisher states to false
+ bh_cmd_st = 0;// Initializing BarrettHand Command State to Zero
+
+ //Subscribers
+ joy_sub = n_.subscribe < sensor_msgs::Joy > ("joy", 1, &WamTeleop::joyCallback, this); // /joy
+
+ //Service Clients
+ grasp_vel_srv = nh_.serviceClient<wam_srvs::BHandGraspVel>("grasp_vel"); // /bhand/grasp_vel
+ spread_vel_srv = nh_.serviceClient<wam_srvs::BHandSpreadVel>("spread_vel"); // /bhand/spread_vel
+ go_home_srv = nw_.serviceClient<std_srvs::Empty>("go_home"); // /wam/go_home
+ hold_srv = nw_.serviceClient<wam_srvs::Hold>("hold_joint_pos"); // /wam/hold_joint_pos
+
+ //Publishers
+ cart_vel_pub = nw_.advertise<wam_msgs::RTCartVel>("cart_vel_cmd", 1); // /wam/cart_vel_cmd
+ ortn_vel_pub = nw_.advertise<wam_msgs::RTOrtnVel>("ortn_vel_cmd", 1); // /wam/ortn_vel_cmd
+}
+
+void WamTeleop::joyCallback(const sensor_msgs::Joy::ConstPtr& joy_msg)
+{
+ //Set our publishing states back to false for new commands
+ grsp_publish = sprd_publish = cart_publish = ortn_publish = home_publish = hold_publish = ortn_mode = false;
+
+ //Return with no deadman pressed or without first pose published yet.
+ if (!joy_msg->buttons[deadman_btn] | joy_msg->buttons[guardian_deadman_btn])
+ return;
+
+ if (joy_msg->buttons[ortn_btn]) //Checking our Orientation button state
+ ortn_mode = true;
+
+ // Gripper Grasp Command
+ if(joy_msg->buttons[gpr_open_btn]) //Checking to see if gripper open button is being pressed
+ {
+ bh_grsp_vel = -max_grsp_vel; // Set the velocity for the gripper command
+ grsp_publish = true; // set grasp publish state
+ }
+ else if(joy_msg->buttons[gpr_close_btn]) //Checking to see if gripper close button is being pressed
+ {
+ bh_grsp_vel = max_grsp_vel; // Set the velocity for the gripper command
+ grsp_publish = true; // set grasp publish state
+ }
+
+ // Gripper Spread Command
+ if (joy_msg->buttons[sprd_open_btn])
+ {
+ bh_sprd_vel = -max_sprd_vel;
+ sprd_publish = true;
+ }
+ else if (joy_msg->buttons[sprd_close_btn])
+ {
+ bh_sprd_vel = max_sprd_vel;
+ sprd_publish = true;
+ }
+
+ // Go Home Command
+ if (joy_msg->buttons[home_btn] && !home_st) // Checking to see if go home button is pressed and if it was pressed last callback loop
+ home_publish = true; // set true only when button is first pressed down
+ home_st = joy_msg->buttons[home_btn]; // setting the state for the next loop
+
+ // Hold Joints Command
+ if (joy_msg->buttons[hold_btn] && !hold_st)// Checking to see if hold position button is pressed and if it was pressed last callback loop
+ hold_publish = true;// set true only when button is first pressed down
+ hold_st = joy_msg->buttons[hold_btn];// setting the state for the next callback loop
+
+
+ //Cartesian Velocity Portion
+ if ((joy_msg->axes[axis_x] > 0.25 || joy_msg->axes[axis_x] < -0.25) && !ortn_mode)
+ {
+ req_xdir = joy_msg->axes[axis_x];
+ cart_publish = true;
+ }
+ else
+ req_xdir = 0.0;
+ if ((joy_msg->axes[axis_y] > 0.25 || joy_msg->axes[axis_y] < -0.25) && !ortn_mode)
+ {
+ req_ydir = joy_msg->axes[axis_y];
+ cart_publish = true;
+ }
+ else
+ req_ydir = 0.0;
+ if ((joy_msg->axes[axis_z] > 0.25 || joy_msg->axes[axis_z] < -0.25) && !ortn_mode)
+ {
+ req_zdir = joy_msg->axes[axis_z];
+ cart_publish = true;
+ }
+ else
+ req_zdir = 0.0;
+
+ //RPY Velocity Portion
+ if ((joy_msg->axes[axis_r] > 0.25 || joy_msg->axes[axis_r] < -0.25) && ortn_mode)
+ {
+ req_rdir = -joy_msg->axes[axis_r];
+ ortn_publish = true;
+ }
+ else
+ req_rdir = 0.0;
+ if ((joy_msg->axes[axis_y] > 0.25 || joy_msg->axes[axis_y] < -0.25) && ortn_mode)
+ {
+ req_pdir = -joy_msg->axes[axis_p];
+ ortn_publish = true;
+ }
+ else
+ req_pdir = 0.0;
+ if ((joy_msg->axes[axis_z] > 0.25 || joy_msg->axes[axis_z] < -0.25) && ortn_mode)
+ {
+ req_yawdir = joy_msg->axes[axis_yaw];
+ ortn_publish = true;
+ }
+ else
+ req_yawdir = 0.0;
+
+}
+
+// Function for updating the commands and publishing
+void WamTeleop::update()
+{
+ //Check our publish hand states and act accordingly
+ if (grsp_publish && bh_cmd_st == 0 && !sprd_publish && !cart_publish && !ortn_publish) // Only if grasp publish state is set
+ {
+ grasp_vel.request.velocity = bh_grsp_vel; // set grasp velocity to commanded
+ grasp_vel_srv.call(grasp_vel); // call grasp velocity service
+ bh_cmd_st = 1; // set the BarrettHand commanded state to signify grasp command
+ }
+ else if (sprd_publish && bh_cmd_st == 0 && !grsp_publish && !cart_publish && !ortn_publish) // only if spread publish state is set
+ {
+ spread_vel.request.velocity = bh_sprd_vel; // set spread velocity to commanded
+ spread_vel_srv.call(spread_vel); // call spread velocity service
+ bh_cmd_st = 2; // set the BarrettHand commanded state to signify spread command
+ }
+ else if (bh_cmd_st != 0 && !grsp_publish && !sprd_publish && !cart_publish && !ortn_publish) // only if nothing published and last bhand state !=0
+ {
+ if (bh_cmd_st == 1) // if BarrettHand state is in grasp mode
+ {
+ grasp_vel.request.velocity = 0.0; // Zero the velocity
+ grasp_vel_srv.call(grasp_vel); // Command zero velocity to grasp
+ }
+ if (bh_cmd_st == 2) // if Barretthand state is in spread mode
+ {
+ spread_vel.request.velocity = 0.0; // Zero the velocity
+ spread_vel_srv.call(spread_vel); // Command zero velocity to spread
+ }
+ bh_cmd_st = 0; // set the BarrettHand state to no mode (0)
+ }
+
+ //Check our publish hold joint position state and act accordingly
+ if (hold_publish && !cart_publish && !ortn_publish && !grsp_publish && !sprd_publish && !home_publish) // if only hold_publish state is set
+ {
+ if (!hold.request.hold) // Check previous holding state
+ hold.request.hold = true; // Setting the hold request to true;
+ else
+ hold.request.hold = false; // Setting the hold request to false
+ hold_srv.call(hold); // Call the commanded hold state
+ }
+
+ //Check our publish go home state and act accordingly
+ if(home_publish && !hold_publish && !cart_publish && !grsp_publish && !sprd_publish && !ortn_publish) // if only home_publish state is set
+ go_home_srv.call(go_home); // Command WAM to go home
+
+ //Check our published cartesian velocity state and act accordingly
+ if(cart_publish && !ortn_publish && !grsp_publish && !sprd_publish && !home_publish && !hold_publish) // if only cart_publish state is set
+ {
+ cart_vel.direction[0] = req_xdir;
+ cart_vel.direction[1] = req_ydir;
+ cart_vel.direction[2] = req_zdir;
+ cart_vel.magnitude = cart_mag;
+ cart_vel_pub.publish(cart_vel);
+ }
+
+ //Check our published orientation velocity state and act accordingly
+ if(ortn_publish && !cart_publish && !grsp_publish && !sprd_publish && !home_publish && !hold_publish) // if only cart_publish state is set
+ {
+ ortn_vel.angular[0] = req_rdir;
+ ortn_vel.angular[1] = req_pdir;
+ ortn_vel.angular[2] = req_yawdir;
+ ortn_vel.magnitude = ortn_mag;
+ ortn_vel_pub.publish(ortn_vel);
+ }
+}
+
+int main(int argc, char** argv)
+{
+ ros::init(argc, argv, "wam_teleop"); // Start our wam_node and name it "wam_teleop"
+ WamTeleop wam_teleop; // Declare a member of WamTeleop "wam_teleop"
+ wam_teleop.init(); // Initialize our teleoperation
+
+ ros::Rate pub_rate(CNTRL_FREQ); // Setting the publishing rate to CNTRL_FREQ (50Hz by default)
+
+ while (wam_teleop.n_.ok()) // Looping at the specified frequency while our ros node is ok
+ {
+ ros::spinOnce();
+ wam_teleop.update(); // Update and send commands to the WAM
+ pub_rate.sleep();
+ }
+ return 0;
+}
--- /dev/null
+#include <unistd.h>
+#include <math.h>
+
+#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 double SPEED = 0.03; // Default Cartesian Velocity
+
+using namespace barrett;
+
+//Some templated functions for 4-dof & 7-dof interchangeability
+template<size_t DOF>
+ systems::Wam<DOF>*
+ getWam(ProductManager& pm)
+ {
+ assert(false);
+ return NULL;
+ }
+template<>
+ systems::Wam<7>*
+ getWam(ProductManager& pm)
+ {
+ return pm.getWam7(false);
+ }
+template<>
+ systems::Wam<4>*
+ getWam(ProductManager& pm)
+ {
+ return pm.getWam4(false);
+ }
+
+//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
+ publish(ProductManager& pm);
+ 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();
+ // 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 publisher
+template<size_t DOF>
+ void WamNode<DOF>::publish(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);
+
+ //publishing sensor_msgs/JointState to bhand/joint_states if present
+ if (hand != NULL)
+ {
+ hand->update();
+ Hand::jp_type hi = hand->getInnerLinkPosition();
+ Hand::jp_type ho = hand->getOuterLinkPosition();
+ for (size_t i = 0; i < 4; i++)
+ 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();
+ bhand_joint_state_pub.publish(bhand_joint_state);
+ }
+ }
+
+//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)
+ {
+ BARRETT_SCOPED_LOCK(pm.getMutex());
+ //Forces us into real-time
+ 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)
+ {
+ BARRETT_SCOPED_LOCK(pm.getMutex());//Forces us into real-time
+ 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)
+ {
+ BARRETT_SCOPED_LOCK(pm.getMutex());//Forces us into real-time
+ 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)
+ {
+ BARRETT_SCOPED_LOCK(pm.getMutex());//Forces us into real-time
+ 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)
+ {
+ BARRETT_SCOPED_LOCK(pm.getMutex());//Forces us into real-time
+ 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);
+
+ while (ros::ok() && pm.getSafetyModule()->getMode() == SafetyModule::ACTIVE)
+ {
+ ros::spinOnce();
+ wam_node.publish(pm);
+ wam_node.updateRT(pm);
+ pub_rate.sleep();
+ }
+
+ return 0;
+ }
projects / barrett-ros-pkg.git / commitdiff