jaco_arm_trajectory_node.h

Go to the documentation of this file.

#ifndef JACO_ARM_TRAJECTORY_NODE_H_
#define JACO_ARM_TRAJECTORY_NODE_H_

#include <ros/ros.h>

#include <actionlib/client/simple_action_client.h>
#include <actionlib/server/simple_action_server.h>
#include <boost/foreach.hpp>
#include <boost/thread/recursive_mutex.hpp>
#include <control_msgs/FollowJointTrajectoryAction.h>
#include <control_msgs/GripperCommandAction.h>
#include <ecl/geometry.hpp>
#include <wpi_jaco_msgs/AngularCommand.h>
#include <wpi_jaco_msgs/CartesianCommand.h>
#include <wpi_jaco_msgs/EStop.h>
#include <wpi_jaco_msgs/GetAngularPosition.h>
#include <wpi_jaco_msgs/GetCartesianPosition.h>
#include <wpi_jaco_msgs/HomeArmAction.h>
#include <wpi_jaco_msgs/JacoFK.h>
#include <wpi_jaco_msgs/QuaternionToEuler.h>
#include <sensor_msgs/JointState.h>
#include <std_msgs/Bool.h>
#include <std_srvs/Empty.h>

#include <jaco_sdk/Kinova.API.UsbCommandLayerUbuntu.h>

#define NUM_JACO_JOINTS 6

#define LARGE_ACTUATOR_VELOCITY 0.8378 //maximum velocity of large actuator (joints 1-3) (rad/s)
#define SMALL_ACTUATOR_VELOCITY 1.0472 //maximum velocity of small actuator (joints 4-6) (rad/s)
#define TIME_SCALING_FACTOR 1.5 //keep the trajectory at a followable speed

#define DEG_TO_RAD (M_PI/180)
#define RAD_TO_DEG (180/M_PI)

//gains for trajectory follower
#define KP 300.0
#define KV 20.0
#define ERROR_THRESHOLD .03 //threshold in radians for combined joint error to consider motion a success

//gains for finger controller
#define KP_F 7.5
#define KV_F 0.05
#define KI_F 0.1

//control types
#define ANGULAR_CONTROL 1
#define CARTESIAN_CONTROL 2

#define NO_ERROR 1 //no error from Kinova API

namespace jaco
{

class JacoArmTrajectoryController
{
public:
  typedef actionlib::SimpleActionServer<control_msgs::FollowJointTrajectoryAction> TrajectoryServer;
  typedef actionlib::SimpleActionServer<control_msgs::GripperCommandAction>        GripperServer;
  typedef actionlib::SimpleActionServer<wpi_jaco_msgs::HomeArmAction>              HomeArmServer;

  typedef actionlib::SimpleActionClient<control_msgs::GripperCommandAction>        GripperClient;

  JacoArmTrajectoryController(ros::NodeHandle nh, ros::NodeHandle pnh);

  virtual ~JacoArmTrajectoryController();

  void update_joint_states();

  void home_arm(const wpi_jaco_msgs::HomeArmGoalConstPtr &goal);

  void execute_trajectory(const control_msgs::FollowJointTrajectoryGoalConstPtr &goal);

  void execute_smooth_trajectory(const control_msgs::FollowJointTrajectoryGoalConstPtr &goal);

  void execute_joint_trajectory(const control_msgs::FollowJointTrajectoryGoalConstPtr &goal);

  void execute_gripper(const control_msgs::GripperCommandGoalConstPtr &goal);

  void execute_gripper_radian(const control_msgs::GripperCommandGoalConstPtr &goal);

private:
  bool loadParameters(const ros::NodeHandle n);
  void angularCmdCallback(const wpi_jaco_msgs::AngularCommand& msg);

  void cartesianCmdCallback(const wpi_jaco_msgs::CartesianCommand& msg);

  void fingerPositionControl(float f1, float f2, float f3);

  void executeAngularTrajectoryPoint(TrajectoryPoint point, bool erase);

  void executeCartesianTrajectoryPoint(TrajectoryPoint point, bool erase);

  bool getAngularPosition(wpi_jaco_msgs::GetAngularPosition::Request &req, wpi_jaco_msgs::GetAngularPosition::Response &res);

  bool getCartesianPosition(wpi_jaco_msgs::GetCartesianPosition::Request &req,
                            wpi_jaco_msgs::GetCartesianPosition::Response &res);

  bool eStopCallback(wpi_jaco_msgs::EStop::Request &req, wpi_jaco_msgs::EStop::Response &res);

  bool eraseTrajectoriesCallback(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res);

  // Messages
  ros::Publisher joint_state_pub_; 
  ros::Publisher cartesianCmdPublisher; 
  ros::Publisher angularCmdPublisher; 
  ros::Publisher armHomedPublisher; 
  ros::Subscriber cartesianCmdSubscriber; 
  ros::Subscriber angularCmdSubscriber; 

  // Services
  ros::ServiceClient jaco_fk_client; 
  ros::ServiceClient qe_client; 
  ros::ServiceServer angularPositionServer; 
  ros::ServiceServer cartesianPositionServer; 
  ros::ServiceServer eStopServer; 
  ros::ServiceServer eraseTrajectoriesServer;

  ros::Timer joint_state_timer_; 

  // Actionlib
  TrajectoryServer*  trajectory_server_; 
  TrajectoryServer*  smooth_trajectory_server_; 
  TrajectoryServer*  smooth_joint_trajectory_server_; 
  GripperServer*     gripper_server_; 
  GripperServer*     gripper_server_radian_; 
  HomeArmServer*     home_arm_server_;

  GripperClient*     gripper_client_; 

  boost::recursive_mutex api_mutex;

  bool eStopEnabled;

  bool arm_initialized;

  // Parameters
  std::string   arm_name_;
  std::string   topic_prefix_;
  double        finger_scale_;
  double        finger_error_threshold_; //threshold in the JACO API's finger position units to consider a finger position reached
  double        max_curvature_;
  double        max_speed_finger_;
  double        gripper_open_;
  double        gripper_closed_;
  int           num_fingers_;
  int           num_joints_;
  bool          kinova_gripper_;

  std::vector<std::string> joint_names;
  std::vector<double>      joint_pos_;
  std::vector<double>      joint_vel_;
  std::vector<double>      joint_eff_;

  unsigned int controlType; //current state of control

};

}

#endif