#include <jaco_arm_trajectory_node.h>

Public Types
typedef actionlib::SimpleActionClient < control_msgs::GripperCommandAction >	GripperClient
typedef actionlib::SimpleActionServer < control_msgs::GripperCommandAction >	GripperServer
typedef actionlib::SimpleActionServer < wpi_jaco_msgs::HomeArmAction >	HomeArmServer
typedef actionlib::SimpleActionServer < control_msgs::FollowJointTrajectoryAction >	TrajectoryServer
Public Member Functions
void	execute_gripper (const control_msgs::GripperCommandGoalConstPtr &goal)
	Callback for the gripper_server_, executes a gripper command.
void	execute_gripper_radian (const control_msgs::GripperCommandGoalConstPtr &goal)
	Callback for the gripper_server_radian_, executes a gripper command with a goal representing the finger position in radians.
void	execute_joint_trajectory (const control_msgs::FollowJointTrajectoryGoalConstPtr &goal)
	Callback for the joint_velocity_controller, executes a smoothed trajectory with velocity control.
void	execute_smooth_trajectory (const control_msgs::FollowJointTrajectoryGoalConstPtr &goal)
	Callback for the smooth_arm_controller, executes a smoother Cartesian trajectory.
void	execute_trajectory (const control_msgs::FollowJointTrajectoryGoalConstPtr &goal)
	Callback for the arm_controller, executes a joint angle trajectory.
void	home_arm (const wpi_jaco_msgs::HomeArmGoalConstPtr &goal)
	move the arm to the home position using the Kinova API home call
	JacoArmTrajectoryController (ros::NodeHandle nh, ros::NodeHandle pnh)
	Constructor.
void	update_joint_states ()
	Reads joint states from the arm and publishes them as a JointState message.
virtual	~JacoArmTrajectoryController ()
	Destructor.
Private Member Functions
void	angularCmdCallback (const wpi_jaco_msgs::AngularCommand &msg)
	Callback for sending an angular command to the arm.
void	cartesianCmdCallback (const wpi_jaco_msgs::CartesianCommand &msg)
	Callback for sending a Cartesian command to the arm.
bool	eraseTrajectoriesCallback (std_srvs::Empty::Request &req, std_srvs::Empty::Response &res)
	Callback for erasing trajectories currently running on the arm.
bool	eStopCallback (wpi_jaco_msgs::EStop::Request &req, wpi_jaco_msgs::EStop::Response &res)
	Callback for enabling/disabling the software emergency stop.
void	executeAngularTrajectoryPoint (TrajectoryPoint point, bool erase)
	Stripped-down angular trajectory point sending to the arm.
void	executeCartesianTrajectoryPoint (TrajectoryPoint point, bool erase)
	Stripped-down Cartesian trajectory point sending to the arm.
void	fingerPositionControl (float f1, float f2, float f3)
	Control with finger velocity inputs to reach a given position.
bool	getAngularPosition (wpi_jaco_msgs::GetAngularPosition::Request &req, wpi_jaco_msgs::GetAngularPosition::Response &res)
	Service callback for getting the current joint positions of the arm and fingers.
bool	getCartesianPosition (wpi_jaco_msgs::GetCartesianPosition::Request &req, wpi_jaco_msgs::GetCartesianPosition::Response &res)
	Service callback for getting the current Cartesian pose of the end effector.
bool	loadParameters (const ros::NodeHandle n)
Private Attributes
ros::Publisher	angularCmdPublisher
	publisher for angular arm commands
ros::Subscriber	angularCmdSubscriber
	subscriber for angular arm commands
ros::ServiceServer	angularPositionServer
	service server to get the joint positions
boost::recursive_mutex	api_mutex
bool	arm_initialized
std::string	arm_name_
ros::Publisher	armHomedPublisher
	publisher for when the arm completes a kinova api home arm action
ros::Publisher	cartesianCmdPublisher
	publisher for Cartesian arm commands
ros::Subscriber	cartesianCmdSubscriber
	subscriber for Cartesian arm commands
ros::ServiceServer	cartesianPositionServer
	service server to get end effector pose
unsigned int	controlType
ros::ServiceServer	eraseTrajectoriesServer
bool	eStopEnabled
ros::ServiceServer	eStopServer
	service server for software estop and restart
double	finger_error_threshold_
double	finger_scale_
GripperClient *	gripper_client_
	gripper command action client, used for sending a converted goal from gripper_server_radian_ to execute on the gripper_server_
double	gripper_closed_
double	gripper_open_
GripperServer *	gripper_server_
	gripper command action server (goal in Kinova API units)
GripperServer *	gripper_server_radian_
	gripper command action server (goal in radians)
HomeArmServer *	home_arm_server_
ros::ServiceClient	jaco_fk_client
	forward kinematics client
std::vector< double >	joint_eff_
std::vector< std::string >	joint_names
std::vector< double >	joint_pos_
ros::Publisher	joint_state_pub_
	publisher for joint states
ros::Timer	joint_state_timer_
	timer for joint state publisher
std::vector< double >	joint_vel_
bool	kinova_gripper_
double	max_curvature_
double	max_speed_finger_
int	num_fingers_
int	num_joints_
ros::ServiceClient	qe_client
	quaternion to euler (XYZ) conversion client
TrajectoryServer *	smooth_joint_trajectory_server_
	smooth point-to-point trajectory follower based on joint velocity control
TrajectoryServer *	smooth_trajectory_server_
	smooth point-to-point trajectory follower based on Cartesian end effector positions
std::string	topic_prefix_
TrajectoryServer *	trajectory_server_
	point-to-point trajectory follower

Detailed Description

Definition at line 73 of file jaco_arm_trajectory_node.h.

Member Typedef Documentation

typedef actionlib::SimpleActionClient<control_msgs::GripperCommandAction> jaco::JacoArmTrajectoryController::GripperClient

Definition at line 80 of file jaco_arm_trajectory_node.h.

typedef actionlib::SimpleActionServer<control_msgs::GripperCommandAction> jaco::JacoArmTrajectoryController::GripperServer

Definition at line 77 of file jaco_arm_trajectory_node.h.

typedef actionlib::SimpleActionServer<wpi_jaco_msgs::HomeArmAction> jaco::JacoArmTrajectoryController::HomeArmServer

Definition at line 78 of file jaco_arm_trajectory_node.h.

typedef actionlib::SimpleActionServer<control_msgs::FollowJointTrajectoryAction> jaco::JacoArmTrajectoryController::TrajectoryServer

Definition at line 76 of file jaco_arm_trajectory_node.h.

Constructor & Destructor Documentation

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

Constructor.

Parameters:

nh	ROS node handle
pnh	ROS private node handle

Definition at line 7 of file jaco_arm_trajectory_node.cpp.

jaco::JacoArmTrajectoryController::~JacoArmTrajectoryController ( ) [virtual]

Destructor.

Definition at line 133 of file jaco_arm_trajectory_node.cpp.

Member Function Documentation

void jaco::JacoArmTrajectoryController::angularCmdCallback ( const wpi_jaco_msgs::AngularCommand & msg ) [private]

Callback for sending an angular command to the arm.

Parameters:

msg	angular command and info

Definition at line 1061 of file jaco_arm_trajectory_node.cpp.

void jaco::JacoArmTrajectoryController::cartesianCmdCallback ( const wpi_jaco_msgs::CartesianCommand & msg ) [private]

Callback for sending a Cartesian command to the arm.

Parameters:

msg	Cartesian command and info

Definition at line 1185 of file jaco_arm_trajectory_node.cpp.

bool jaco::JacoArmTrajectoryController::eraseTrajectoriesCallback	(	std_srvs::Empty::Request &	req,
		std_srvs::Empty::Response &	res
	)		`[private]`

Callback for erasing trajectories currently running on the arm.

Parameters:

req	empty service request
res	empty service response

Definition at line 1495 of file jaco_arm_trajectory_node.cpp.

bool jaco::JacoArmTrajectoryController::eStopCallback	(	wpi_jaco_msgs::EStop::Request &	req,
		wpi_jaco_msgs::EStop::Response &	res
	)		`[private]`

Callback for enabling/disabling the software emergency stop.

Parameters:

req	service request
res	service response

Returns:: true on success

Definition at line 1451 of file jaco_arm_trajectory_node.cpp.

void jaco::JacoArmTrajectoryController::execute_gripper ( const control_msgs::GripperCommandGoalConstPtr & goal )

Callback for the gripper_server_, executes a gripper command.

Parameters:

goal	action goal

Definition at line 785 of file jaco_arm_trajectory_node.cpp.

void jaco::JacoArmTrajectoryController::execute_gripper_radian ( const control_msgs::GripperCommandGoalConstPtr & goal )

Callback for the gripper_server_radian_, executes a gripper command with a goal representing the finger position in radians.

Parameters:

goal	action goal

Definition at line 890 of file jaco_arm_trajectory_node.cpp.

void jaco::JacoArmTrajectoryController::execute_joint_trajectory ( const control_msgs::FollowJointTrajectoryGoalConstPtr & goal )

Callback for the joint_velocity_controller, executes a smoothed trajectory with velocity control.

The trajectory is generated by interpolating a set of joint trajectory points and smoothing the corners using an acceleration constraint. The trajectory is then followed by using a velocity controller implemented in this node which sends joint velocity commands to the arm.

Parameters:

goal	action goal

Definition at line 565 of file jaco_arm_trajectory_node.cpp.

void jaco::JacoArmTrajectoryController::execute_smooth_trajectory ( const control_msgs::FollowJointTrajectoryGoalConstPtr & goal )

Callback for the smooth_arm_controller, executes a smoother Cartesian trajectory.

The trajectory is generated by converting joint angle trajectories to end effector cartesian trajectories and smoothed automatically by the JACO's Cartesian position controller.

NOTE: the trajectories must not fall within constraints defined internally on the JACO for singularity avoidance

Parameters:

goal	action goal

Definition at line 384 of file jaco_arm_trajectory_node.cpp.

void jaco::JacoArmTrajectoryController::execute_trajectory ( const control_msgs::FollowJointTrajectoryGoalConstPtr & goal )

Callback for the arm_controller, executes a joint angle trajectory.

Parameters:

goal	action goal

Definition at line 244 of file jaco_arm_trajectory_node.cpp.

void jaco::JacoArmTrajectoryController::executeAngularTrajectoryPoint	(	TrajectoryPoint	point,
		bool	erase
	)		`[private]`

Stripped-down angular trajectory point sending to the arm.

This is designed for trajectory followers, which need a quick response

Parameters:

point	angular trajectory point to send to the arm
erase	if true, clear the trajectory point stack before sending point

Definition at line 1384 of file jaco_arm_trajectory_node.cpp.

void jaco::JacoArmTrajectoryController::executeCartesianTrajectoryPoint	(	TrajectoryPoint	point,
		bool	erase
	)		`[private]`

Stripped-down Cartesian trajectory point sending to the arm.

This is designed for trajectory followers, which need a quick response trajectory followers that need very quick response

Parameters:

point	Cartesian trajectory point to send to the arm
erase	if true, clear the trajectory point stack before sending point

Definition at line 1403 of file jaco_arm_trajectory_node.cpp.

void jaco::JacoArmTrajectoryController::fingerPositionControl	(	float	f1,
		float	f2,
		float	f3
	)		`[private]`

Control with finger velocity inputs to reach a given position.

Parameters:

f1	position of finger 1
f2	position of finger 2
f3	position of finger 3

Definition at line 1276 of file jaco_arm_trajectory_node.cpp.

bool jaco::JacoArmTrajectoryController::getAngularPosition	(	wpi_jaco_msgs::GetAngularPosition::Request &	req,
		wpi_jaco_msgs::GetAngularPosition::Response &	res
	)		`[private]`

Service callback for getting the current joint positions of the arm and fingers.

This allows other nodes to request the joint positions when needed, rather than listening to the joint_state topic. The positions are given in radians on [-pi, pi].

Parameters:

req	empty service request
res	service response including joint positions

Returns:: true on success

Definition at line 1422 of file jaco_arm_trajectory_node.cpp.

bool jaco::JacoArmTrajectoryController::getCartesianPosition	(	wpi_jaco_msgs::GetCartesianPosition::Request &	req,
		wpi_jaco_msgs::GetCartesianPosition::Response &	res
	)		`[private]`